My robot is moving in X & -X direction only using 2 motors, how to drive it in Y & -y direction ?
#define ENCA1 2
#define ENCB1 3
#define PWM1 5
#define IN1_1 25
#define IN2_1 24
#define ENCA2 18
#define ENCB2 19
#define PWM2 6
#define IN1_2 27
#define IN2_2 26
#define ENCA3 20
#define ENCB3 21
#define PWM3 4
#define IN1_3 22
#define IN2_3 23
int pos1 = 0;
int pos2 = 0;
int pos3 = 0;
long prevT1 = 0;
long prevT2 = 0;
long prevT3 = 0;
float eprev1 = 0;
float eintegral1 = 0;
float eprev2 = 0;
float eintegral2 = 0;
float eprev3 = 0;
float eintegral3 = 0;
int targetX = 0; // Set the desired target position in X-direction
int targetY = 800; // Set the desired target position in Y-direction
bool stopping1 = false; // Flag for motor 1 stopping
bool stopping2 = false; // Flag for motor 2 stopping
bool stopping3 = false; // Flag for motor 3 stopping
void setup() {
Serial.begin(9600);
pinMode(IN1_1, OUTPUT);
pinMode(IN2_1, OUTPUT);
pinMode(IN1_2, OUTPUT);
pinMode(IN2_2, OUTPUT);
pinMode(IN1_3, OUTPUT);
pinMode(IN2_3, OUTPUT);
pinMode(PWM1, OUTPUT);
pinMode(PWM2, OUTPUT);
pinMode(PWM3, OUTPUT);
analogWrite(PWM1, 0);
analogWrite(PWM2, 0);
analogWrite(PWM3, 0);
pinMode(ENCA1, INPUT);
pinMode(ENCB1, INPUT);
pinMode(ENCA2, INPUT);
pinMode(ENCB2, INPUT);
pinMode(ENCA3, INPUT);
pinMode(ENCB3, INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA1), readEncoder1, RISING);
attachInterrupt(digitalPinToInterrupt(ENCA2), readEncoder2, RISING);
attachInterrupt(digitalPinToInterrupt(ENCA3), readEncoder3, RISING);
Serial.println("target pos");
}
void loop() {
// PID constants
float kp = 1;
float kd = 0.025;
float ki = 0.0;
// time differences
long currT1 = micros();
long currT2 = micros();
long currT3 = micros();
float deltaT1 = ((float)(currT1 - prevT1)) / (1.0e6);
float deltaT2 = ((float)(currT2 - prevT2)) / (1.0e6);
float deltaT3 = ((float)(currT3 - prevT3)) / (1.0e6);
prevT1 = currT1;
prevT2 = currT2;
prevT3 = currT3;
// Calculate the average position of all encoders
long averagePositionX = (pos1 + pos2) / 2;
long averagePositionY = pos3;
// Calculate the error as the difference between the target positions
int errorX = targetX - averagePositionX;
int errorY = targetY - averagePositionY;
// Calculate derivatives and integrals for both axes
float dedt1 = (errorX - eprev1) / deltaT1;
float dedt2 = (errorX - eprev2) / deltaT2;
float dedt3 = (errorY - eprev3) / deltaT3;
eintegral1 = eintegral1 + errorX * deltaT1;
eintegral2 = eintegral2 + errorX * deltaT2;
eintegral3 = eintegral3 + errorY * deltaT3;
// Calculate control signals for all motors
float u1 = kp * errorX + kd * dedt1 + ki * eintegral1;
float u2 = kp * errorX + kd * dedt2 + ki * eintegral2;
float u3 = kp * errorY + kd * dedt3 + ki * eintegral3;
// Calculate motor powers
float pwr1 = fabs(u1);
float pwr2 = fabs(u2);
float pwr3 = fabs(u3);
if (pwr1 > 255) {
pwr1 = 255;
}
if (pwr2 > 255) {
pwr2 = 255;
}
if (pwr3 > 255) {
pwr3 = 255;
}
// Set motor directions
int dir1 = (u1 >= 0) ? 1 : -1;
int dir2 = (u2 >= 0) ? 1 : -1;
int dir3 = (u3 >= 0) ? 1 : -1;
// Signal the motors
setMotor(dir1, pwr1, PWM1, IN1_1, IN2_1);
setMotor(dir2, pwr2, PWM2, IN1_2, IN2_2);
setMotor(dir3, pwr3, PWM3, IN1_3, IN2_3);
// Store previous errors
eprev1 = errorX;
eprev2 = errorX;
eprev3 = errorY;
Serial.print(targetX);
Serial.print(" ");
Serial.print(targetY);
Serial.print(" ");
Serial.print(averagePositionX);
Serial.print(" ");
Serial.print(averagePositionY);
Serial.println();
// Check if stopping is needed for all motors
if (targetX == averagePositionX && targetY == averagePositionY && !stopping1 && !stopping2 && !stopping3) {
stopping1 = true;
stopping2 = true;
stopping3 = true;
}
// Gradually reduce motor speed for smoother stopping
if (stopping1) {
pwr1 -= 5; // Adjust the decrement value for desired stopping speed
if (pwr1 <= 0) {
pwr1 = 0;
stopping1 = false;
}
analogWrite(PWM1, pwr1);
}
if (stopping2) {
pwr2 -= 5; // Adjust the decrement value for desired stopping speed
if (pwr2 <= 0) {
pwr2 = 0;
stopping2 = false;
}
analogWrite(PWM2, pwr2);
}
if (stopping3) {
pwr3 -= 5; // Adjust the decrement value for desired stopping speed
if (pwr3 <= 0) {
pwr3 = 0;
stopping3 = false;
}
analogWrite(PWM3, pwr3);
}
}
void setMotor(int dir, int pwmVal, int pwm, int in1, int in2) {
analogWrite(pwm, pwmVal);
if (dir == 1) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
} else if (dir == -1) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
} else {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
}
}
void readEncoder1() {
int b = digitalRead(ENCB1);
if (b > 0) {
pos1++;
} else {
pos1--;
}
}
void readEncoder2() {
int b = digitalRead(ENCB2);
if (b > 0) {
pos2++;
} else {
pos2--;
}
}
void readEncoder3() {
int b = digitalRead(ENCB3);
if (b > 0) {
pos3++;
} else {
pos3--;
}
}
