Sorry for taking so long to reply. My code and a drawing at at the bottom. Currently, I'm not using any special motor speed control. I thought about using PMW on the Arduino but those are disabled due to using the Timer to calculate the RPM.
I reckon you need a compass so you can command a turn of X degrees and get feedback from the compass to let you know when you have done that.
I thought about that but most Compass modules I see are quite expensive.
#include <math.h>
#include <TimerOne.h>
int wheelDia = 66;
int CarDia = 125;
int rForward = 10;
int rBack = 11;
int lForward = 6;
int lBack = 5;
int rEncoder = 3;
int lEncoder = 2;
int rSpeed, lSpeed;
int xVal, yVal;
unsigned int count1, count2;
float angle;
unsigned long previousMillis;
typedef enum { NONE, GOT_X, GOT_Y,END } states;
states state = NONE;
unsigned int currentValue;
bool WaitForCo = true;
int CarState = 0;
void setup() {
// put your setup code here, to run once:
pinMode(rForward, OUTPUT);
pinMode(rBack, OUTPUT);
pinMode(lForward, OUTPUT);
pinMode(lBack, OUTPUT);
pinMode(lEncoder, INPUT);
pinMode(rEncoder, INPUT);
state = NONE;
Timer1.initialize(1000000);
attachInterrupt(0, do_count, RISING);
attachInterrupt(1, do_count2, RISING);
Timer1.attachInterrupt(timerIsr);
}
void do_count(){
count1++;
}
void do_count2(){
count2++;
}
void timerIsr(){
Timer1.detachInterrupt();
rSpeed = (count2/20)*60;
lSpeed = (count1/20)*60;
count1 = 0; count2 = 0;
Timer1.attachInterrupt(timerIsr);
}
void StrDrive(){
long currentMillis = millis();
int SqRoot = sqrt(square(xVal) + square(yVal));
long interval = DrvCalc(rSpeed, SqRoot) ;
if ((currentMillis - previousMillis) >= interval){
StopDrive();
previousMillis = millis();
} else {
digitalWrite(rForward, HIGH);
digitalWrite(lForward, HIGH);
}
}
void LftTurn(){
long currentMillis = millis();
long interval = DrvCalc(lSpeed, DrvCalc(yVal, xVal)) ;
if ((currentMillis - previousMillis) >= interval){
StrDrive(); //Function to drive the car in a straight line after turning
previousMillis = millis();
} else {
digitalWrite(rForward, HIGH);
digitalWrite(lBack, HIGH);
}
}
void RghtTurn(){
long currentMillis = millis();
long interval = DrvCalc(rSpeed, DrvCalc(yVal, xVal)) ;
if ((currentMillis - previousMillis) >= interval){
StrDrive(); //Function to drive the car in a straight line after turning
previousMillis = millis();
} else {
digitalWrite(lForward, HIGH);
digitalWrite(rBack, HIGH);
}
}
void StopDrive(){
digitalWrite(lForward, LOW);
digitalWrite(lBack, LOW);
digitalWrite(rForward, LOW);
digitalWrite(rBack, LOW);
}
long DrvCalc(long RPM, long WantDis){
float DisPerSec = ((wheelDia * M_PI) * RPM / 60000);
float interval = WantDis *DisPerSec;
return interval;
}
long AngleCalc(int x, int y){
angle = atan2 (y, x) * ( 180/M_PI);
float distance = angle/360;
float dist_to_travel = (CarDia * M_PI) * distance;
return dist_to_travel;
}
void handlePreviousState (){
switch (state){
case GOT_X:
xVal = currentValue;
break;
case GOT_Y:
yVal = currentValue;
break;
case END:
if (xVal > 0) {
CarState = 3;
} else if (xVal < 0){
CarState = 1;
} else {
CarState = 2;
}
break;
}
currentValue = 0;
} // end of handlePreviousState
void processIncomingByte (const byte c)
{
if (isdigit (c)){
currentValue *= 10;
currentValue += c - '0';
} else {
handlePreviousState ();
switch (c){
case 'X':
state = GOT_X;
break;
case 'Y':
state = GOT_Y;
break;
case 'E':
state = END;
break;
default:
state = NONE;
break;
}
}
}
void loop (){
if (WaitForCo == true){
while (Serial.available ())
processIncomingByte (Serial.read ());
} else {
switch (CarState) {
case 1:
LftTurn();
break;
case 2:
StrDrive();
break;
case 3:
RghtTurn();
break;
case 0:
StopDrive();
break;
}
}
}
