Society of Robots - Robot Forum
Electronics => Electronics => Topic started by: rbtying on May 05, 2010, 07:35:51 PM
-
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)
#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);
}
-
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.
-
Oops, I thought I'd posted this in software... (Admin/Mod please move?)
commented code:
#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);
}