MT6835 Magnetic Encoder Programmer Sketch for Arduino

MT6835Programmer.ino

/*
  Depends on 'Simple FOC' and 'SimpleFOCDrivers' from Arduino Library Manager
  Uses default hardware  SPI pins for whatever board you run it on
  Should be pretty cross-compatible.
  
  Commands are outlined in cliGetCommandParse and cliSetCommandParse, and
  are preceeded by a 'g' or 's' to get or set <setting> and then configuration
  is ended with a 's save' to save to onboard registers
  
  e.g.
    'g bw' returns the value of the bandwidth register
    's abre 2500' sets the PPR to 2500
    's zcur' sets the zero to current position
*/
#include <Arduino.h>
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/mt6835/MagneticSensorMT6835.h>

#define SENSOR_nCS 10	// chip select pin

SPISettings myMT6835SPISettings(1000000, MT6835_BITORDER, SPI_MODE3);
MagneticSensorMT6835 sensor = MagneticSensorMT6835(SENSOR_nCS, myMT6835SPISettings);


void setup()
{
	Serial.begin(115200);
	sensor.init(&SPI);
}

void loop()
{
	if (Serial.available() > 0) {
		cliCheckContents();
	}
}

// Serial CLI components
#define MAX_SERIAL_BYTES 250
#define MAX_ARGUMENT_COUNT 3

void cliCheckContents() {
	char buffer[MAX_SERIAL_BYTES] = {'\0'};
	uint8_t length = 0;
	for (uint8_t i = 0; i < MAX_SERIAL_BYTES; i++) {
		char in_char = Serial.read();			
		if (in_char == '\n' || in_char == '\r'){
			cliParse(buffer, length);
			break;
		} else {
			buffer[length] = in_char;
		}
		length++;
	}
}

void cliParse(char* buffer_in, uint8_t length_in) {
	char* args[MAX_ARGUMENT_COUNT];
	if (length_in > 0) {
		for (uint8_t i = 0; i < MAX_ARGUMENT_COUNT; i++) {
			if (i == 0) {
				// First args gets the current buffer then delim by ascii space
				args[i] = strtok(buffer_in, " ");
			} else {
				// Second+ args get filled by anything else in the buffer, delim by ascii space
				args[i] = strtok(NULL, " ");
			}
		}
	}
	// Check command list
	if (!strcmp(args[0], "g")) {	// Getters
		cliGetCommandParse(args[1]);
	} else if (!strcmp(args[0], "s")) {	// Setters
		cliSetCommandParse(args[1], argToUInt(args[2]));
	} else {
		Serial.println("Incorrect command format");
	}
}

void cliGetCommandParse(char* arg) {
	if (!strcmp(arg, "bw")) {				// Bandwidth
		getBandwidth();
	} else if (!strcmp(arg, "hyst")) {		// Hysteresis
		getHysteresis();
	} else if (!strcmp(arg, "rotd")) {		// Rotation Direction
		getRotationDir();
	} else if (!strcmp(arg, "abre")) {		// ABZ Resolution
		getABZResolution();
	} else if (!strcmp(arg, "aben")) {		// ABZ Enable
		getABZEnable();
	} else if (!strcmp(arg, "absw")) {		// AB Swapped
		getABSwapped();
	} else if (!strcmp(arg, "zero")) {		// Zero Position
		getZeroPos();
	} else if (!strcmp(arg, "stat")) {		// Status
		getStatus();
	} else if (!strcmp(arg, "cals")) {		// Calibration Status
		getCalibrationStatus();
	} else if (!strcmp(arg, "angc")) {		// Angle - Current (float)
		getCurrentAngle();
	} else if (!strcmp(arg, "angr")) {		// Angle - Raw (21 bit)
		getCurrenRawAngle();
	} else {
		Serial.println("Incorrect command format");
	}
}

void cliSetCommandParse(char* arg, uint16_t val) {

	if (!strcmp(arg, "bw")) {				// Bandwidth
		setBandwidth(val);
	} else if (!strcmp(arg, "hyst")) {		// Hysteresis
		setHysteresis(val);
	} else if (!strcmp(arg, "rotd")) {		// Rotation Direction
		setRotationDir(val);
	} else if (!strcmp(arg, "abre")) {		// ABZ Resolution
		setABZResolution(val);
	} else if (!strcmp(arg, "aben")) {		// ABZ Enable
		setABZEnable(val);
	} else if (!strcmp(arg, "absw")) {		// AB Swapped
		setABSwapped(val);
	} else if (!strcmp(arg, "zero")) {		// Zero Position
		setZeroPos(val);
	} else if (!strcmp(arg, "zcur")) {		// Zero Position At Current Position
		setZeroCurrentPos();
	} else if (!strcmp(arg, "save")) {		// write to EEPROM
		writeEEPROM();
	} else {
		Serial.println("Incorrect command format");
	}
}

uint16_t argToUInt(char* arg) {
	uint16_t outval = atoi(arg);
	return outval;
}

void getBandwidth() {
	Serial.print("Bandwidth: ");
	Serial.println(sensor.getBandwidth());
}

void setBandwidth(uint16_t bw) {
	sensor.setBandwidth(bw);
	getBandwidth();
}

void getHysteresis() {
	Serial.print("Hysteresis: ");
	Serial.println(sensor.getHysteresis());
}

void setHysteresis(uint16_t hs) {
	sensor.setHysteresis(hs);
	getHysteresis();
}

void getRotationDir() {
	Serial.print("Rotation Direction: ");
	Serial.println(sensor.getRotationDirection());
}

void setRotationDir(uint16_t rotdir) {
	sensor.setRotationDirection(rotdir);
	getRotationDir();
}

void getABZResolution() {
	Serial.print("ABZResolution: ");
	Serial.println(sensor.getABZResolution());
}

void setABZResolution(uint16_t res) {
	sensor.setABZResolution(res);
	getABZResolution();
}

void getABZEnable() {
	Serial.print("isABZEnable: ");
	Serial.println(sensor.isABZEnabled());
}

void setABZEnable(uint16_t aben) {
	sensor.setABZEnabled((aben > 0 ? true : false));
	getABZEnable();
}

void getABSwapped() {
	Serial.print("ABZSwapped: ");
	Serial.println(sensor.isABSwapped());
}

void setABSwapped(uint16_t absw) {
	sensor.setABSwapped((absw > 0 ? true : false));
	getABSwapped();
}

void getZeroPos() {
	Serial.print("ZeroPos: ");
	Serial.println(sensor.getZeroPosition());
}

void setZeroPos(uint16_t zero) {
	sensor.setZeroPosition(zero);
	getZeroPos();
}

void setZeroCurrentPos() {
	sensor.setZeroFromCurrentPosition();
	getZeroPos();
}

void getStatus() {
	Serial.print("Status: ");
	Serial.println(sensor.getStatus());
}

void getCalibrationStatus() {
	Serial.print("CalibrationStatus: ");
	Serial.println(sensor.getCalibrationStatus());
}

void getCurrentAngle() {
	float angleRads = sensor.getCurrentAngle();
	Serial.print("Current Angle Radians: ");
	Serial.println(angleRads);
	Serial.print("Current Angle Degrees: ");
	Serial.println(angleRads * (180/PI));
}

void getCurrenRawAngle() {
	uint32_t rawAngle = sensor.readRawAngle21();
	Serial.print("21 bit angle: ");
	Serial.println(rawAngle);
}

void writeEEPROM() {
	sensor.writeEEPROM();
	Serial.println("Saving, do not power off...");
	delay(1000);
	Serial.println("5..");
	delay(1000);
	Serial.println("4..");
	delay(1000);
	Serial.println("3..");
	delay(1000);
	Serial.println("2..");
	delay(1000);
	Serial.println("1..");
	delay(1000);
	Serial.println("EEPROM Write Complete!");
}