90 ton 1/14 metal excavator scratch build w/embedded PC

[SteinHDan]

/*  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;
  }
}