Code:
/* Based on the MonsterMoto Shield Example Sketch by: Jim Lindblom */
#include <limits.h>
int deviceId = 10; // TODO: Set this to a unique ID for each controller!
#define BRAKEVCC 0
#define CW 1
#define CCW 2
#define BRAKEGND 3
// VNH2SP30 pin definitions
int inApin[2] = {7, 4}; // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)
// Position sensors
int pospin[2] = {4, 5}; // Position sensors at ANALOG input 4,5
// Serial state
int inBytes[15];
int inBytesCount = 0;
int command = 0;
unsigned long lastCommandTime = 0;
unsigned long timeoutMillis = 1000;
// Timing
int timerDivisor = 8;
int defaultTimerDivisor = 64;
int millisDivisor = defaultTimerDivisor / timerDivisor;
// Motor state
int current[2] = {0, 0};
int currentLimit[2] = { 160, 160 };
int overcurrentDivisor[2] = {8, 8};
unsigned long overCurrentTime[2] = {0, 0};
int direction[2] = { BRAKEGND, BRAKEGND };
int directionWanted[2] = { BRAKEGND, BRAKEGND };
int speed[2] = { 0, 0 };
int speedPrev[2] = { 0, 0 };
int speedWanted[2] = { 0, 0 };
int acceleration[2] = { 100, 100 };
int position[2] = { 511, 511 };
int positionMin[2] = { INT_MIN, INT_MIN };
int positionMax[2] = { INT_MAX, INT_MAX };
bool positionMaxTriggered[2] = { false, false };
bool positionMinTriggered[2] = { false, false };
int positionHysteresis[2] = { 100, 100 };
// Loop state
unsigned long lastTime = 0;
unsigned long now = 0;
int speedScaler = 4;
void setup() {
setPwmFrequency(5, timerDivisor);
setPwmFrequency(6, timerDivisor);
// Initialize digital pins as outputs
for (int i=0; i<2; i++) {
pinMode(inApin[i], OUTPUT);
pinMode(inBpin[i], OUTPUT);
pinMode(pwmpin[i], OUTPUT);
}
// Initialize motors braked
motorGo(0, BRAKEGND, 255);
motorGo(1, BRAKEGND, 255);
// start serial port at 9600 bps:
Serial.begin(9600);
while (!Serial) {
// wait for serial port to connect. Needed for native USB port only
}
}
void loop() {
// Keep time
now = millis() / millisDivisor; // to account for the change of the timer by setPwmFrequency below
// Read commands from serial port
if (Serial.available() > 0) {
int inByte = Serial.read();
if (inByte > 127) {
inBytesCount = 0;
command = inByte;
switch(command) {
// 0x90 & 0x91: Get Motor M0 & M1 Current
case 0x90: Serial.write(current[0] / 4); command = 0; break;
case 0x91: Serial.write(current[1] / 4); command = 0; break;
// 0x94 & 0x95: Get Motor M0 & M1 position
case 0x94: Serial.write(position[0] / 4); command = 0; break;
case 0x95: Serial.write(position[1] / 4); command = 0; break;
}
lastCommandTime = now;
}
else if (command != 0) {
if (inBytesCount < 14) {
inBytes[inBytesCount] = inByte;
}
inBytesCount++;
switch(inBytesCount) {
case 1: {
int inSpeed = inByte * 2; // From 0-127 range to 0-254 range
int motor = -1;
int inDirection = CW;
switch(command) {
case 0x86: motor = 0; inDirection = BRAKEGND; break;
case 0x87: motor = 1; inDirection = BRAKEGND; break;
case 0x88: motor = 0; inDirection = CW; break;
case 0x8A: motor = 0; inDirection = CCW; break;
case 0x8C: motor = 1; inDirection = CW; break;
case 0x8E: motor = 1; inDirection = CCW; break;
case 0x83: {
if (inBytes[0] == 0) {
Serial.write(deviceId);
}
command = 0;
break;
}
}
if (motor != -1) {
speedWanted[motor] = inSpeed;
directionWanted[motor] = inDirection;
command = 0;
}
break;
}
case 4: {
// 0x84, parameter number, parameter value, 0x55, 0x2A // Set parameter
// Parameter number 8 and 9 are the current limits for motors 0 and 1 respectively
switch(command) {
case 0x84: {
int retVal = 1;
if (inBytes[2] == 0x55 && inBytes[3] == 0x2A) {
retVal = 0;
switch(inBytes[0]) {
case 4: acceleration[0] = inBytes[1]; break; // 0-127
case 5: acceleration[1] = inBytes[1]; break; // 0-127
case 8: currentLimit[0] = inBytes[1] * 8; break; // 0-1016
case 9: currentLimit[1] = inBytes[1] * 8; break; // 0-1016
case 72: positionMax[0] = inBytes[1] * 8; break; // 0-1016
case 73: positionMax[1] = inBytes[1] * 8; break; // 0-1016
case 74: positionMin[0] = inBytes[1] * 8; break; // 0-1016
case 75: positionMin[1] = inBytes[1] * 8; break; // 0-1016
case 76: positionHysteresis[0] = inBytes[1] * 8; break; // 0-1016
case 77: positionHysteresis[1] = inBytes[1] * 8; break; // 0-1016
default: retVal = 2; break;
}
}
Serial.write(retVal);
command = 0;
break;
}
}
break;
}
}
}
}
// Update motor state every 10ms
if (lastTime == 0 || now < lastTime) { // If the first time or when the millis() values wrap, we need to fix the lastTime to be before the now time.
lastTime = now;
}
if (now - lastTime >= 10) {
lastTime = now;
for (int i = 0; i < 2; i++) {
bool sameDirection = direction[i] == directionWanted[i];
direction[i] = directionWanted[i];
speed[i] = speedWanted[i];
if ((direction[i] == CW) || (direction[i] == CCW)) {
// Apply accelleration limiting
int accelSpeed = speedScaler * speed[i]; // 10ms loop time and *4 gives the 40ms as in the Pololu controller
if (direction[i] == CCW) {
accelSpeed = -accelSpeed;
}
if ((acceleration[i] > 0) &&
(!(sameDirection && (abs(accelSpeed) < abs(speedPrev[i])))) &&
(abs(accelSpeed - speedPrev[i]) > acceleration[i])) {
accelSpeed = speedPrev[i] + ((speedPrev[i] > accelSpeed) ? -acceleration[i] : acceleration[i]);
}
direction[i] = accelSpeed < 0 ? CCW : CW;
speed[i] = abs(accelSpeed / speedScaler);
speedPrev[i] = accelSpeed;
// Apply current limiting
current[i] = analogRead(cspin[i]);
// If overcurrent, kill the output
if (current[i] > currentLimit[i]) {
overcurrentDivisor[i] = 0;
overCurrentTime[i] = now;
}
// Slowly bring it back
if (now > overCurrentTime[i] + 1000 && overcurrentDivisor[i] < 8) {
overcurrentDivisor[i]++;
overCurrentTime[i] = now;
}
speed[i] = speed[i] * overcurrentDivisor[i] / 8;
}
// Apply position limits
int readPosition = analogRead(pospin[i]);
int changeLimit = positionHysteresis[i] / 2 + 10;
if (abs(readPosition - position[i]) > changeLimit) {
position[i] = position[i] + (position[i] > readPosition ? -changeLimit : changeLimit);
} else {
position[i] = readPosition;
}
if (position[i] > positionMax[i]) {
positionMaxTriggered[i] = true;
}
if (position[i] < positionMax[i] - positionHysteresis[i]) {
positionMaxTriggered[i] = false;
}
if (position[i] < positionMin[i]) {
positionMinTriggered[i] = true;
}
if (position[i] > positionMin[i] + positionHysteresis[i]) {
positionMinTriggered[i] = false;
}
if ((positionMaxTriggered[i] && direction[i] == CW) ||
(positionMinTriggered[i] && direction[i] == CCW)) {
direction[i] = BRAKEGND;
speed[i] = 255;
speedPrev[i] = 0;
}
// Stop on serial command timeout
if (now - lastCommandTime > timeoutMillis) {
direction[i] = BRAKEGND;
speed[i] = 0;
speedPrev[i] = 0;
}
motorGo(i, direction[i], speed[i]);
}
}
}
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm) {
if (motor >= 0 && motor < 2 && direct >= 0 && direct < 4 && pwm >= 0 && pwm < 256 ) {
digitalWrite(inApin[motor], (direct == BRAKEVCC) || (direct == CW) ? HIGH : LOW);
digitalWrite(inBpin[motor], (direct == BRAKEVCC) || (direct == CCW) ? HIGH : LOW);
analogWrite(pwmpin[motor], pwm);
}
}
void setPwmFrequency(int pin, int divisor) {
byte mode;
if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {
switch(divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 64: mode = 0x03; break;
case 256: mode = 0x04; break;
case 1024: mode = 0x05; break;
default: return;
}
if(pin == 5 || pin == 6) {
TCCR0B = TCCR0B & 0b11111000 | mode;
} else {
TCCR1B = TCCR1B & 0b11111000 | mode;
}
} else if(pin == 3 || pin == 11) {
switch(divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 32: mode = 0x03; break;
case 64: mode = 0x04; break;
case 128: mode = 0x05; break;
case 256: mode = 0x06; break;
case 1024: mode = 0x7; break;
default: return;
}
TCCR2B = TCCR2B & 0b11111000 | mode;
}
}