This is an applied use of dynamic calibration and motor3 board use for 2 brushed motors.
#define Debug//Comment out if you do not want debugging info sent to the serial monitor.
//for the sake of arguement I will assume the x axis is on A2 and the y axis on A3.
//These are the white 3 pin plugs on the Motor v3 board.
long X_Max;
long X_Min;
long Y_Min;
long Y_Max;
long X_Center;
long Y_Center;
boolean IsCalibrated_X;
boolean IsCalibrated_Y;
long mCalSampleX;
long mCalSampleY;
/*
Right Motor channel A, using PWM Pin 3, Brake Pin 9, and Direction Pin 12.
Left Motor channel B, using PWM Pin 11, Brake Pin 8, and Direction Pin 13.
*/
//Servo LeftDrive;
int LD_PWM = 11;
int LD_Brake = 8;
int LD_Dir = 13;
//Servo RightDrive;
int RD_PWM = 3;
int RD_Brake = 9;
int RD_Dir = 12;
long LD_Velocity;
long RD_Velocity;
long Drive_Deadband = 5;
void setup()
{
X_Max = 0;
Y_Max = 0;
X_Min = 4096;
Y_Min = 4096;
IsCalibrated_X = false;
IsCalibrated_Y = false;
pinMode(3,OUTPUT);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
pinMode(11,OUTPUT);
pinMode(13,OUTPUT);
digitalWrite(RD_Brake,HIGH);
digitalWrite(LD_Brake,HIGH);
#ifdef Debug
Serial.begin(9600);
#endif
}
void calibrate_X()
{
while ((mCalSampleX >= X_Max) || (mCalSampleX <= X_Min))
{//Read and test the A2 pin and store values in X_Max and X_Min. If mCalSample is greater than X_Min then store current Value in X_Min.
//Likewise if mCalSample is less than X_Min then store it in X_Min.
if (mCalSampleX > X_Max)
{X_Max = mCalSampleX;}
else if (mCalSampleX < X_Min)
{X_Min = mCalSampleX;}
mCalSampleX = analogRead(A2);
delay(1500);//delays the call so that the pot movement is captured. increase delay if you have troubles getting a calibration consistent.
}
IsCalibrated_X = true;
}
void calibrate_Y()
{
while ((mCalSampleY >= Y_Max) || (mCalSampleY <= Y_Min))
{//Read and test the A3 pin and store values in Y_Max and Y_Min. If mCalSample is greater than Y_Min then store current Value in Y_Min.
//Likewise if mCalSample is less than Y_Min then store it in Y_Min.
if (mCalSampleY > Y_Max)
{Y_Max = mCalSampleY;}
else if (mCalSampleY < Y_Min)
{Y_Min = mCalSampleY;}
mCalSampleY = analogRead(A3);
delay(1500);//delays the call so that the pot movement is captured. increase delay if you have troubles getting a calibration consistent.
}
IsCalibrated_Y = true;
}
void loop()
{
if ((IsCalibrated_X == false) || (IsCalibrated_Y == false))//Conditional statement for exiting the calibration routine.
{
#ifdef Debug
Serial.println("Beginning X Axis Calibration.");
#endif
calibrate_X();
X_Center = (X_Max-X_Min)/2 + X_Min;
#ifdef Debug
Serial.print("X Max=");
Serial.print(X_Max);
Serial.print("; X Min=");
Serial.print(X_Min);
Serial.print("; X Center=");
Serial.println(X_Center);
Serial.println(analogRead(A2));;
delay(3500);
Serial.println("Beginning Y Calibration");
#endif
calibrate_Y();
Y_Center = (Y_Max - Y_Min)/2 + Y_Min;
#ifdef Debug
Serial.print("Y Max=");
Serial.print(Y_Max);
Serial.print("; Y Min=");
Serial.print(Y_Min);
Serial.print("; Y Center=");
Serial.println(Y_Center);
Serial.println(analogRead(A3));
delay(3500);
Serial.println("Calibration Complete");
#endif
}
mCalSampleX = analogRead(A2);
mCalSampleY = analogRead(A3);
LD_Velocity = mCalSampleY - Y_Center;//Interpolates a 0 to 1023 value to +/- 511 (map function could also be used)
RD_Velocity = mCalSampleY - Y_Center;//Interpolates a 0 to 1023 value to +/- 511 (map function could also be used)
//generate velocity vectors whose magnitude are the margin beyond the Drive_Deadband.
//Adjusting forĀ Y Axis which is forward/reverse movement.
if (LD_Velocity > (Y_Center + Drive_Deadband))
{
LD_Velocity = LD_Velocity - Drive_Deadband;
}
else if (LD_Velocity < (Y_Center - Drive_Deadband))
{
LD_Velocity = LD_Velocity + Drive_Deadband;
}
if (RD_Velocity > (Y_Center + Drive_Deadband))
{
RD_Velocity = RD_Velocity - Drive_Deadband;
}
else if (RD_Velocity < (Y_Center - Drive_Deadband))
{
RD_Velocity = RD_Velocity + Drive_Deadband;
}
//Now we adjust each drive for any right or left turn control outside of Drive_Deadband.
if (mCalSampleX > (X_Center + Drive_Deadband))
{
LD_Velocity = LD_Velocity + mCalSampleX - Drive_Deadband;
RD_Velocity = RD_Velocity - mCalSampleX + Drive_Deadband;
}
else if (mCalSampleX < (X_Center - Drive_Deadband))
{
LD_Velocity = LD_Velocity - mCalSampleX + Drive_Deadband;
RD_Velocity = RD_Velocity + mCalSampleX - Drive_Deadband;
}
#ifdef Debug
Serial.print("Left Drive:");
Serial.println(LD_Velocity);
delay(1000);
Serial.print("Right Drive:");
Serial.println(RD_Velocity);
delay(1000);
#endif
//Determine motor direction and apply appropriate direction control signal.
if (sgn(LD_Velocity) > 0)
{
digitalWrite(LD_Dir,HIGH);
}
else
{
digitalWrite(LD_Dir,LOW);
}
if (sgn(RD_Velocity) > 0)
{
digitalWrite(RD_Dir,HIGH);
}
else
{
digitalWrite(RD_Dir,LOW);
}
//Determine if Braking is required.
if (abs(LD_Velocity) < Drive_Deadband)
{
digitalWrite(LD_Brake,HIGH);
}
else
{
digitalWrite(LD_Brake,LOW);
}
if (abs(RD_Velocity) < Drive_Deadband)
{
digitalWrite(RD_Brake,HIGH);
}
else
{
digitalWrite(RD_Brake,LOW);
}
//Scale up each absolute velocity and write PWM voltage.
#ifdef Debug
Serial.print("Left Drive Voltage:");
Serial.println(abs(LD_Velocity)*2);
Serial.print("Right Drive Voltage:");
Serial.println(abs(RD_Velocity)*2);
#else
analogWrite(RD_PWM,2*abs(RD_Velocity));
analogWrite(LD_PWM,2*abs(LD_Velocity));
#endif
}
int sgn(long ConvVal)
{
if (ConvVal > 0)
{
return 1;
}
else
{
return 0;
}
}
Any queries can be presented here or emailed to me at stgiiiovrdvn@hotmail.com