View Single Post
  #56  
Old 12-14-2015, 07:12 AM
SteinHDan SteinHDan is offline
Green Horn
 
Join Date: Sep 2015
Location: Oslo, Norway
Posts: 188
SteinHDan is on a distinguished road
Default Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

To program the motor drivers so that they are Pololu Qik compatible (for the commands "GetDeviceId", "SetMotor1Speed" and "SetMotor2Speed"), follow these steps:
1. Connect the USB cable to a PC
2. Download the Arduino programmer: https://www.arduino.cc/en/Main/Software
3. Copy and paste the code below into the programming tool
4. Set the device ID on the second line (any number from 0 to 255, used to identify the controller later) I'm using device IDs 10, 11 and 12 for this build.
5. Click the "Upload" button

Done!

I also added a serial command timeout, which means that the motor driver will stop the motor if it has not received a motor command for one second. Usually the Raspberry Pi will send a motor command 50 times per second, but in the case it loses power or is disconnected for some reason, the motor will stop.

I've not added current sensing or limiting yet.

Code:
/*  Based on the MonsterMoto Shield Example Sketch by: Jim Lindblom */
int deviceId = 10;

#define BRAKEVCC 0
#define CW   1
#define CCW  2
#define BRAKEGND 3
#define CS_THRESHOLD 100

// 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)

// Serial state
int inByte = 0;
int command = 0;
unsigned long lastCommandTime = 0;
unsigned long timeoutMillis = 1000;

void setup() {
  setPwmFrequency(5, 8);
  setPwmFrequency(6, 8);
  
  // 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, 0);
  motorGo(1, BRAKEGND, 0);
  
  // 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() {
  // if we get a valid byte, read analog ins:
  if (Serial.available() > 0) {
    // get incoming byte:
    inByte = Serial.read();
    if (inByte > 127)
      command = inByte;
    else if (command != 0) {
      int speed = inByte * 2; // From 0-127 range to 0-254 range
      int motor = -1;
      int direction = CW;
      switch(command) {
        case 0x88: motor = 0; direction = CW; break;
        case 0x8A: motor = 0; direction = CCW; break;
        case 0x8C: motor = 1; direction = CW; break;
        case 0x8E: motor = 1; direction = CCW; break;
        case 0x83: motor = -1; Serial.write(deviceId); break;
      }
      if (motor != -1) {
        motorGo(motor, direction, speed);
      }
      command = 0;
      lastCommandTime = millis();
    }
  } else {
    if (millis() - lastCommandTime > timeoutMillis) {
      motorGo(0, BRAKEGND, 0);
      motorGo(1, BRAKEGND, 0);
    }
  }
  
  // if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD))
}


/* motorGo() will set a motor going in a specific direction
 the motor will continue going in that direction, at that speed
 until told to do otherwise.
 
 motor: this should be either 0 or 1, will selet which of the two
 motors to be controlled
 
 direct: Should be between 0 and 3, with the following result
 0: Brake to VCC
 1: Clockwise
 2: CounterClockwise
 3: Brake to GND
 
 pwm: should be a value between 0 and 255 */
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
  if (motor <= 1)
  {
    if (direct <=4)
    {
      // Set inA[motor]
      if (direct <=1)
        digitalWrite(inApin[motor], HIGH);
      else
        digitalWrite(inApin[motor], LOW);

      // Set inB[motor]
      if ((direct==0)||(direct==2))
        digitalWrite(inBpin[motor], HIGH);
      else
        digitalWrite(inBpin[motor], LOW);

      analogWrite(pwmpin[motor], pwm);
    }
  }
}


/**
 * Divides a given PWM pin frequency by a divisor.
 * 
 * The resulting frequency is equal to the base frequency divided by
 * the given divisor:
 *   - Base frequencies:
 *      o The base frequency for pins 3, 9, 10, and 11 is 31250 Hz.
 *      o The base frequency for pins 5 and 6 is 62500 Hz.
 *   - Divisors:
 *      o The divisors available on pins 5, 6, 9 and 10 are: 1, 8, 64,
 *        256, and 1024.
 *      o The divisors available on pins 3 and 11 are: 1, 8, 32, 64,
 *        128, 256, and 1024.
 * 
 * PWM frequencies are tied together in pairs of pins. If one in a
 * pair is changed, the other is also changed to match:
 *   - Pins 5 and 6 are paired on timer0
 *   - Pins 9 and 10 are paired on timer1
 *   - Pins 3 and 11 are paired on timer2
 * 
 * Note that this function will have side effects on anything else
 * that uses timers:
 *   - Changes on pins 3, 5, 6, or 11 may cause the delay() and
 *     millis() functions to stop working. Other timing-related
 *     functions may also be affected.
 *   - Changes on pins 9 or 10 will cause the Servo library to function
 *     incorrectly.
 * 
 * Thanks to macegr of the Arduino forums for his documentation of the
 * PWM frequency divisors. His post can be viewed at:
 *   http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1235060559/0#4
 */
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;
  }
}
Reply With Quote