johnwasser:
In Arduino C it would look something like this:////task main()
////{
#include <Servo.h>
//The powers we give to both motors. masterPower will remain constant while slavePower will change so that
//the right wheel keeps the same speed as the left wheel.
int masterPower = 30;
int slavePower = 30;
int SensorValue[2];
const int leftEncoder = 0;
const int rightEncoder = 1;
Servo motor[2];
const int leftServo = 0;
const int LeftServoPin = 4;
const int rightServo = 1;
const int RightServoPin = 5;
//Essentially the difference between the master encoder and the slave encoder. Negative if slave has
//to slow down, positive if it has to speed up. If the motors moved at exactly the same speed, this
//value would be 0.
int error = 0;
//'Constant of proportionality' which the error is divided by. Usually this is a number between 1 and 0 the
//error is multiplied by, but we cannot use floating point numbers. Basically, it lets us choose how much
//the difference in encoder values effects the final power change to the motor.
int kp = 5;
void setup() { ////
//Reset the encoders.
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
motor[leftServo].attach(LeftServoPin);
motor[rightServo].attach(RightServoPin);
}
//Repeat ten times a second.
void loop() ////while(true)
{
//Set the motor powers to their respective variables.
motor[leftServo].write(masterPower);
motor[rightServo].write(slavePower);
//This is where the magic happens. The error value is set as a scaled value representing the amount the slave
//motor power needs to change. For example, if the left motor is moving faster than the right, then this will come
//out as a positive number, meaning the right motor has to speed up.
error = SensorValue[leftEncoder] - SensorValue[rightEncoder];
//This adds the error to slavePower, divided by kp. The '+=' operator literally means that this expression really says
//"slavePower = slavepower + error / kp", effectively adding on the value after the operator.
//Dividing by kp means that the error is scaled accordingly so that the motor value does not change too much or too
//little. You should 'tune' kp to get the best value. For us, this turned out to be around 5.
slavePower += error / kp;
//Reset the encoders every loop so we have a fresh value to use to calculate the error.
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
//Makes the loop repeat ten times a second. If it repeats too much we lose accuracy due to the fact that we don't have
//access to floating point math, however if it repeats to little the proportional algorithm will not be as effective.
//Keep in mind that if this value is changed, kp must change accordingly.
delay(100); ////wait1Msec(100);
}
It looks like the sensor values are being updated in interrupt code. If the encoders are quadrature (A and B signals) there are lots of example of how to red them with an Arduino.
i have 2 encoders and 2 dc motors and trying to use this code to run the robot in a straight line.i dont know where to attach my motor and encoder signal pins ?
