3 brushed motor rov code

General Discussion
1

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);
}
3

@soyboy97 how did it go?