PWM control of TI SN754410 from Arduino

[rbtying]

I've been trying to implement a closed-loop motor system on my robot with a TI SN754410 motor driver, two motors, an arduino, and a pair of Pololu wheel encoders.  While I can read in encoder data and run the motors, the motors pulse to the desired speed, drop down, and repeat, instead of just staying at a desired, lower speed.  The pulse is from the desired (50% power) to a 0% power to motors.  My guess is that there's something wrong with my code: (below)

Code: [Select]

#include <PololuWheelEncoders.h>

// sensors
unsigned char front_left_ir = 6;
unsigned char front_right_ir = 5;
unsigned char back_ir = 4;
// right encoder on 2,3
// left encoder on 18, 19 (analog 4,5)
PololuWheelEncoders enc;

// left motor
unsigned char left_pwm = 10;
unsigned char left_pin1 = 7;
unsigned char left_pin2 = 8;

// right motor
unsigned char right_pwm = 9;
unsigned char right_pin1 = 12;
unsigned char right_pin2 = 13;

// variables
boolean objR = false;
boolean objL = false;
boolean objB = false;

float lspeed = 0;
float rspeed = 0;
float lspd = 0;
float rspd = 0;

void setup() {
  enc.init(2,3,18,19);
  pinMode(front_left_ir, INPUT);
  pinMode(front_right_ir, INPUT);
  pinMode(back_ir, INPUT);
  pinMode(left_pin1, OUTPUT);
  pinMode(left_pin2, OUTPUT);
  pinMode(right_pin1, OUTPUT);
  pinMode(right_pin2, OUTPUT);
  Serial.begin(9600);
  setSpd(1, 0.5);
  setSpd(2, 0.5);
}

void loop() {
  // max on both motors is 15 ticks/read
  digitalWrite(left_pin1, HIGH);
  digitalWrite(left_pin2, LOW);
  digitalWrite(right_pin1, HIGH);
  digitalWrite(right_pin2, LOW);
  analogWrite(left_pwm, 255 * lspeed);
  analogWrite(right_pwm, 255 * rspeed);
  clamp(1, lspd);
  clamp(2, rspd);
}

void setSpd(unsigned char motor, float spd) {
  switch(motor) {
  case 1:
    lspd = lspeed = spd;
    break;
  case 2:
    rspd = rspeed = spd;
    break;
  }
}

int readRate(unsigned char motor) {
  switch(motor){
  case 1:
    enc.getCountsAndResetM1();
    delay(100);
    return enc.getCountsAndResetM1();
  case 2:
    enc.getCountsAndResetM1();
    delay(100);
    return enc.getCountsAndResetM1();
  }
}

void clamp(unsigned char motor, float percent) {
  int wantedSpeed = (int)( percent * 15.0);
  int realSpeed = readRate(motor); 
  if (wantedSpeed - realSpeed < 0)
    switch(motor){
    case 1:
      lspeed += 0.1;
      break;
    case 2:
      rspeed += 0.1;
      break;
    }
  else if (wantedSpeed - realSpeed > 0)
    switch(motor){
    case 1:
      lspeed -= 0.1;
      break;
    case 2:
      rspeed -= 0.1;
      break;
    }
}
void scan() {
  objR = !(boolean) digitalRead(front_right_ir);
  objL = !(boolean) digitalRead(front_left_ir);
  objB = !(boolean) digitalRead(back_ir);
}

[Soeren]

Hi,

My guess is that there's something wrong with my code: (below)

Then please post this in "Software" rather than here in "Electronics".
And please just post the code that you have questions on. Reading other peoples totally uncommented code is a daunting task that most people will shy away from, so:

* Add a generous amount of comments (then you will be able to understand it 6 months from now as well).
* Cut to the chase, we don't really need the entire hay stack, when you could just show us the needle.
* Post in the right forum.

You shouldn't do this to please me or anyone else, you should do this for the selfish reason to get the best possible answers.

[rbtying]

Oops, I thought I'd posted this in software... (Admin/Mod please move?)

commented code:

Code: [Select]

#include <PololuWheelEncoders.h>

// sensors
unsigned char front_left_ir = 6;
unsigned char front_right_ir = 5;
unsigned char back_ir = 4;
// right encoder on 2,3
// left encoder on 18, 19 (analog 4,5)
PololuWheelEncoders enc;

// left motor
unsigned char left_pwm = 10;
unsigned char left_pin1 = 7;
unsigned char left_pin2 = 8;

// right motor
unsigned char right_pwm = 9;
unsigned char right_pin1 = 12;
unsigned char right_pin2 = 13;

// variables
// if theres object at Right, Left, and Back
boolean objR = false;
boolean objL = false;
boolean objB = false;

// *speed is actual speed, [0, 1.0]
// *spd is desired speed, [0, 1.0]
float lspeed = 0;
float rspeed = 0;
float lspd = 0;
float rspd = 0;

void setup() {
  // init the encoders
  enc.init(2,3,18,19);
  // setup IR sensors
  pinMode(front_left_ir, INPUT);
  pinMode(front_right_ir, INPUT);
  pinMode(back_ir, INPUT);
  // setup motor pins
  pinMode(left_pin1, OUTPUT);
  pinMode(left_pin2, OUTPUT);
  pinMode(right_pin1, OUTPUT);
  pinMode(right_pin2, OUTPUT);
  // serial
  Serial.begin(9600);
  // test with each motor at half speed
  setSpd(1, 0.5);
  setSpd(2, 0.5);
}

void loop() {
  // max on both motors is 15 ticks/read
 
  // set left motor to move forward
  digitalWrite(left_pin1, HIGH);
  digitalWrite(left_pin2, LOW);
  // set right motor to move forward;
  digitalWrite(right_pin1, HIGH);
  digitalWrite(right_pin2, LOW);
 
  // use pwm to control speed, (l || r)speed is between 0 and 1.0
  analogWrite(left_pwm, 255 * lspeed);
  analogWrite(right_pwm, 255 * rspeed);
 
  // adjust the pwm so that the speed is the same on left and right wheels
  clamp(1, lspd);
  clamp(2, rspd);
}

void setSpd(unsigned char motor, float spd) {
  // set the motor's desired speed and actual speed to a given value
  switch(motor) {
  case 1:
    lspd = lspeed = spd;
    break;
  case 2:
    rspd = rspeed = spd;
    break;
  }
}

int readRate(unsigned char motor) {
// count the number of ticks the encoder moves in 100ms
  switch(motor){
  case 1:
    enc.getCountsAndResetM1(); // reset the count
    delay(100); // wait 100 ms
    return enc.getCountsAndResetM1(); // get the value
  case 2:
    enc.getCountsAndResetM1(); // reset the count
    delay(100); // wait 100 ms
    return enc.getCountsAndResetM1(); // get the value
  }
}

void clamp(unsigned char motor, float percent) {
  // find the desired speed in ticks/100ms, which is the percent * max(ticks/100ms)
  int wantedSpeed = (int)( percent * 15.0);
  // find the actual speed of the motor  in ticks/100ms
  int realSpeed = readRate(motor); 
  // if the realspeed is > the wanted speed
  if (wantedSpeed - realSpeed < 0)
    switch(motor){
    case 1:
      lspeed -= 0.1;
      break;
    case 2:
      rspeed -= 0.1;
      break;
    }
  // if the realspeed is < the wanted speed
  else if (wantedSpeed - realSpeed > 0)
    switch(motor){
    case 1:
      lspeed += 0.1;
      break;
    case 2:
      rspeed += 0.1;
      break;
    }
}
// unused
void scan() {
  objR = !(boolean) digitalRead(front_right_ir);
  objL = !(boolean) digitalRead(front_left_ir);
  objB = !(boolean) digitalRead(back_ir);
}