I am trying to build a robot car to move forward a prescribed distance using 20 slot encoder disks and a IR interrupters. I am getting much higher values of ticks than I am supposed to for a rotation. Is my code correct?
///VARIABLE SETUP FOR ARDUINO I/O AND SENSORS
////////MOTION CONTROL VARIABLES///////
//Motor 1 which is the Right MOTOR
int enA=9;
int in1=8;
int in2=7;
//Motor 2 which is the Left MOTOR
int enB=4;
int in3=6;
int in4=5;
////////ENCODER VARIABLES////////
//Constants for Interrupt Pins (Used for the Wheel Encoders)
const byte MOTOR1=21; //Motor 1 Interrupt Pin
const byte MOTOR2=20; //Motor 2 Interrupt Pin
//Float for number of state changes in a revolution for the encoder disk
float enc_calib=40; //20 slots per disk so hence 40 state changes
//Constant for Wheel Diameter
const float wheeldiameter=66.10; // In mm
//Integers to pulse counter (Used for the Wheel Encoders)
volatile int counter1=0;
volatile int counter2=0;
//////INTERRUPT SERVICE ROUTINE FOR ENCODERS//////
//Motor 1 pulse count
void ISR_count1(){
counter1++;
Serial.println("ISR_COUNTER1=");
Serial.println(counter1);
}
//Motor 2 pulse count
void ISR_count2(){
counter2++;
Serial.println("ISR_COUNTER2=");
Serial.println(counter2);
}
// Function to convert cm to Steps
int CMtoSteps(float cm) {
int stepcount;
float circumference= (wheeldiameter*3.14)/10; //Calculate Circumference in Cm
float cm_step=circumference/enc_calib; // Cm per step
float f_stepcount= (cm/cm_step); //Convert cm/step to cm as a floating point
stepcount= (int) f_stepcount; //Convert float to integer....not rounded off.
return stepcount;
}
//FUNCTION TO MOVE FORWARD
void MoveForward(int steps,int mspeed){
counter1=0; //Reset Counter 1
counter2=0; //Reset Counter 2
//Set Motor 1 to move forward
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
//Set Motor 2 to move forward
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
while (steps> counter1 && steps> counter2){
if (steps>counter1) {
analogWrite(enA,mspeed);
}else{
analogWrite(enA,0);
}
if (steps>counter2) {
analogWrite(enB,mspeed);
}else{
analogWrite(enB,0);
}
}
//Stop Motors when distance reached
analogWrite(enA,0);
analogWrite(enB,0);
counter1=0;
counter2=0;
}
void setup()
{
Serial.begin(115200);
//Set Motor control pins to output
pinMode(enA,OUTPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(enB,OUTPUT);
//Encoder Timer
attachInterrupt(digitalPinToInterrupt (MOTOR1), ISR_count1,CHANGE); //Increase counter 1 when speed sensor pin changes state
attachInterrupt(digitalPinToInterrupt (MOTOR2), ISR_count2,CHANGE); //Increase counter 2 when speed sensor pin changes state
// ///MOTION COMMAND//////
//
MoveForward(CMtoSteps(20.77),200);
}
void loop()
{
}