Hi,
I’m coding the motors for an ROV. We’re using 3 motors, each controlled by a 2 input H-bridge. I cant test the motors as I wont be in the workshop for while. I was just wandering if my code looks okay… It compiles but not sure if it will actually work.
// Left Motor
int enA = 9; //PWM
int in1 = 8;
int in2 = 7;
// Right Motor
int enB = 10; //PWM
int in3 = 5;
int in4 = 4;
// Vertical Motor
int enC = 11; //PWM
int in5 = 13;
int in6 = 12;
// Joystick Input
int joy1Vert = A0; // Joystick 1 Vertical
int joy1Horz = A1; // Joystick 1 Horizontal
int joy2Horz = A2; // Joystick 2 Horizontal
// Motor Speed Values - Start at zero
int MotorSpeed1 = 0;
int MotorSpeed2 = 0;
int MotorSpeed3 = 0;
static const int Joy1Vert = 511; //Middle value for joysticks
static const int Joy1Horz = 511;
static const int Joy2Horz = 511;
void setup()
{
// Set all the motor control pins to outputs
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(enC, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(in5, OUTPUT);
pinMode(in6, OUTPUT);
// Start with motors disabled and direction forward
// Motor A
digitalWrite(enA, LOW);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// Motor B
digitalWrite(enB, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
// Vertical Motor
digitalWrite(enC, LOW);
digitalWrite(in5, LOW);
digitalWrite(in6, LOW);
}
void loop() {
// Read the Joystick X and Y positions
joy1Vert = analogRead(joy1Vert);
joy1Horz = analogRead(joy1Horz);
joy2Horz = analogRead(joy2Horz);
if (joy1Vert < 460)
{
// This is Backward
// Set Left Motor backward
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
// Set Right backward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW );
//Determine Motor Speeds
joy1Vert = joy1Vert - 460; // This produces a negative number
joy1Vert = joy1Vert * -1; // Make the number positive
MotorSpeed1 = map(joy1Vert, 0, 460, 0, 255);
MotorSpeed2 = map(joy1Vert, 0, 460, 0, 255);
}
else if (joy1Vert > 564)
{
// This is Forward
// Set Left Motor Forward
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// Set Right Motor forward
digitalWrite(in3,LOW);
digitalWrite(in4,LOW );
//Determine Motor Speeds
MotorSpeed1 = map(joy1Vert, 564, 1023, 0, 255);
MotorSpeed2 = map(joy1Vert, 564, 1023, 0, 255);
}
else
{
// Left & Right motors Stopped
MotorSpeed1 = 0;
MotorSpeed2 = 0;
}
// Steering
if (joy2Horz < 460)
{
// Move Left
// reverse readings
joy2Horz = joy2Horz - 460; // This produces a negative number
joy2Horz = joy2Horz * -1; // Make the number positive
// Map the number to a value of 255 maximum
joy2Horz = map(joy2Horz, 0, 460, 0, 255);
MotorSpeed1 = MotorSpeed1 - joy2Horz;
MotorSpeed2 = MotorSpeed2 + joy2Horz;
// Don't exceed range of 0-255
if (MotorSpeed1 < 0)MotorSpeed1 = 0;
if (MotorSpeed2 > 255)MotorSpeed2 = 300;
}
else if (joy2Horz > 564)
{
// Move Right
// Map the number to a value of 255 maximum
joy2Horz = map(joy2Horz, 564, 1023, 0, 255);
MotorSpeed1 = MotorSpeed1 + joy2Horz;
MotorSpeed2 = MotorSpeed2 - joy2Horz;
// Don't exceed range of 0-255 for motor speeds
if (MotorSpeed1 > 255)MotorSpeed1 = 300;
if (MotorSpeed2 < 0)MotorSpeed2 = 0;
}
if (joy1Horz < 460)
{
// This is Downwards
// Set vertical motor downwards
digitalWrite(in5, LOW);
digitalWrite(in6, LOW);
//Determine Motor Speeds
// As we are going backwards we need to reverse readings
joy1Horz = joy1Horz - 460; // This produces a negative number
joy1Horz = joy1Horz * -1; // Make the number positive
MotorSpeed3 = map(joy1Horz, 0, 460, 0, 255);
}
else if (joy1Horz > 564)
{
// This is Upwards
digitalWrite(in5, HIGH);
digitalWrite(in6, HIGH);
//Determine Motor Speeds
MotorSpeed3 = map(joy1Horz, 564, 1023, 0, 255);
}
else
{
// This is Stopped
MotorSpeed3 = 0;
// prevent buzzing at very low speeds
if (MotorSpeed1 < 8)MotorSpeed1 = 0;
if (MotorSpeed2 < 8)MotorSpeed2 = 0;
if (MotorSpeed3 < 8)MotorSpeed3 = 0;
}
// Set the motor speeds
analogWrite(enA, MotorSpeed1);
analogWrite(enB, MotorSpeed2);
analogWrite(enC, MotorSpeed3);
}