I am trying to code a drivetrain using two servo motors with differential style of turing and rotation using PWM signals. I have a contoller and im trying to achieve forward/backward movement and left/right movement on the right joystick and rotation (the motors spin in opposite directions so that it can turn in place) when moving the left joystick horizontally. The problem im getting is in my code i have three PWM channels but i cant seem to get the rotation and other movements working at the same time. For example the right josystick movement works perfectly but it doesnt respond to the left joystick movement, meaning it doesnt rotate. In terms of the motors im using big industrial ones. What could be the problem?
Heres my code:
#include <Servo.h>
#define RCPinFWD 2
#define RCPinSide 3
#define RCPinRotation 18
Servo leftMotor;
Servo rightMotor;
long PulsesFWD = 0;
long PulsesSide = 0;
long PulsesRotation = 0;
void setup() {
// setup serial, pin mode
Serial.begin(9600);
pinMode(RCPinFWD, INPUT_PULLUP);
pinMode(RCPinSide, INPUT_PULLUP);
pinMode(RCPinRotation, INPUT_PULLUP);
leftMotor.attach(5);
rightMotor.attach(6);
}
void loop() {
// Read pulse lengths
PulsesFWD = pulseIn(RCPinFWD, HIGH);
PulsesSide = pulseIn(RCPinSide, HIGH);
PulsesRotation = pulseIn(RCPinRotation, HIGH);
// save pulse lengths that are <2000ms
int PulseWidthFWD = min(PulsesFWD, 2000);
int PulseWidthSide = min(PulsesSide, 2000);
int PulseWidthRotation = min(PulsesRotation, 2000);
// Determine forward/backward speed and direction
int leftMotorAngle = 90;
int rightMotorAngle = 90;
// Get initial value from forward channel
leftMotorAngle = map(PulseWidthFWD, 1000, 2000, 0, 180);
rightMotorAngle = map(PulseWidthFWD, 1000, 2000, 0, 180);
// Correct for turning channel value
int directionOfTurnSlowDown = 1;
if (PulseWidthSide < 1500) {
// Turning left
if (leftMotorAngle > 90) {
// If going forward
directionOfTurnSlowDown = map(PulseWidthSide, 1000, 1499, 95, 85);
} else {
// If going backward
directionOfTurnSlowDown = map(PulseWidthSide, 1000, 1499, 105, 115);
}
leftMotorAngle = (leftMotorAngle * directionOfTurnSlowDown) / 100;
} else if (PulseWidthSide > 1500) {
// Turning right
if (rightMotorAngle > 90) {
// If going forward
directionOfTurnSlowDown = map(PulseWidthSide, 1501, 2000, 95, 85);
} else {
// If going backward
directionOfTurnSlowDown = map(PulseWidthSide, 1501, 2000, 105, 115);
}
rightMotorAngle = (rightMotorAngle * directionOfTurnSlowDown) / 100;
}
// Calculate rotation angles
int leftRotationAngle = 90;
int rightRotationAngle = 90;
// Update motor angles for left joystick control (rotation)
if (PulseWidthRotation < 1500) {
// Move motors in opposite directions for rotation left
int rotationAmount = map(PulseWidthRotation, 1000, 1499, -45, 0); // Adjust angle range as needed
leftRotationAngle -= rotationAmount;
rightRotationAngle += rotationAmount;
} else if (PulseWidthRotation > 1500) {
// Move motors in opposite directions for rotation right
int rotationAmount = map(PulseWidthRotation, 1501, 2000, 0, 45); // Adjust angle range as needed
leftRotationAngle += rotationAmount;
rightRotationAngle -= rotationAmount;
}
// Control the servo motors
leftMotor.write(leftMotorAngle);
rightMotor.write(rightMotorAngle);
// Debugging output
Serial.print("PulseWidthFWD: ");
Serial.print(PulseWidthFWD);
Serial.print(" PulseWidthSide: ");
Serial.print(PulseWidthSide);
Serial.print(" PulseWidthRotation: ");
Serial.println(PulseWidthRotation);
Serial.print("Left motor angle: ");
Serial.print(leftMotorAngle);
Serial.print(" Right motor angle: ");
Serial.println(rightMotorAngle);
// Add a delay to control the update rate if needed
delay(10); // Adjust this delay as needed
}