Hi all,
I am a beginner in coding and I have to create a project for my university. I have to build a robot car and its control system. In this project, I am using 2 DC motors with encoders.
Control system design: the robot must move forward and backward 1 meter and then turn 90 degrees and move further a half meter.
So far, the robot can move 1m forward and backward, but one side of the motors is always off, which means that does not move straight perfectly ( always off to the right side).
So, the question is how can I make it move straight or how can I control the differences between both encoders' pulses? For instance, which functions I should use or which equation or formula I should configure in my code.
Subsequently, due to new in coding, I have less idea of making the robot turns 90 degrees. Therefore, I do need you guys a favor and advice of turning the robot. So, if you guys have some example codes that I can use as a guide, or at least please, point it out for me a little bit of doing that, I would be really appreciated.
This is the code that I have been made so far.
// Motor A //
const int ENCA1 = 2;
const int ENA = 5;
const int In1 = 7;
const int In2 = 8;
const int ENCA2 = 11;
// Motor B //
const int ENCB1 = 3;
const int ENB = 6;
const int In3 = 9;
const int In4 = 10;
const int ENCB2 = 12;
//Parameter //
const float HallRatio = 341.2 / 34.02; // Hall x Ratio 34.02 = 341.2PPR
const float pulse_per_fullrev = 34 * HallRatio; // (34:1 gearbox) * (HallRatio (10.3)) Hall x Ratio 34.02 = 341.2PPR
const int Diawheel = 44; // Diameter of the wheel (mm)
//the globle variable
volatile float A_pulse = 0;
volatile float B_pulse = 0;
// Function to convert from millimetre to steps
int MMtoPulses(float mm){
int result; // Final calculation result
const float Cirwheel = PI*Diawheel; // Calculate wheel circumference in mm
float mm_step = Cirwheel / pulse_per_fullrev; // MM per Step
float f_result = mm / mm_step; // Calculate result as a float
result = (int) f_result; // Convert to an integer (note this is NOT rounded)
return result; // End and return result
}
void MoveForward(int distance, int mspeed){
A_pulse = 0;
B_pulse = 0;
// Set Motor A forward
digitalWrite(In1, HIGH);
digitalWrite(In2, LOW);
// Set Motor B forward
digitalWrite(In3, HIGH);
digitalWrite(In4, LOW);
// Go forward until step value is reached
while (distance > abs(A_pulse) && distance > abs(B_pulse)) {
Serial.print("Motor A is ");
Serial.print(A_pulse);
Serial.print(" pulses and Motor B ");
Serial.print(B_pulse);
Serial.println(" pulses");
if (distance > abs(A_pulse)) {
analogWrite(ENA, mspeed);
}
else {
analogWrite(ENA, 0);
}
if (distance > abs(B_pulse)) {
analogWrite(ENB, mspeed);
} else
{
analogWrite(ENB, 0);
}
}
//Stop///
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
void MoveBackward(int distance, int mspeed){
A_pulse = 0;
B_pulse = 0;
// Set Motor A Backward
digitalWrite(In1, LOW);
digitalWrite(In2, HIGH);
// Set Motor B Backward
digitalWrite(In3, LOW);
digitalWrite(In4, HIGH);
// Go Backward until step value is reached
while (distance > abs(A_pulse) && distance > abs(B_pulse)) {
Serial.print("Motor A is ");
Serial.print(A_pulse);
Serial.print(" pulses and Motor B ");
Serial.print(B_pulse);
Serial.println(" pulses");
if (distance > abs(A_pulse)) {
//abs()
analogWrite(ENA, mspeed);
}
else {
analogWrite(ENA, 0);
}
if (distance > abs(B_pulse)) {
analogWrite(ENB, mspeed);
} else
{
analogWrite(ENB, 0);
}
}
//Stop///
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
void readA(){
int A2 = digitalRead(ENCA2);
if(A2 > 0){
A_pulse--;
}
else{
A_pulse++;
}
}
void readB(){
int B2 = digitalRead(ENCB2);
if(B2 > 0){
B_pulse++;
}
else{
B_pulse--;
}
}
void setup(){
Serial.begin(9600);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(In1, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
pinMode(ENCA1, INPUT);
pinMode(ENCB1, INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA1), readA, RISING);
attachInterrupt(digitalPinToInterrupt(ENCB1), readB, RISING);
delay(3000);
MoveForward(MMtoPulses(1000), 100); // Forward 1 metre at 100 speed
delay(2000); // Wait two second
MoveBackward(MMtoPulses(1000), 100); // Backward 1 metre at 100 speed
}
void loop(){
// put your main code here, to run repeatedly:
Serial.print("Motor A is ");
Serial.print(A_pulse);
Serial.print(" pulses and Motor B ");
Serial.print(B_pulse);
Serial.println(" pulses");
//20/05/22
}