arduinodrivejj

#include <Servo.h>
const uint8_t servoPin[4] = {4, 5, 6, 7}; /* Servo (1 - 4) Pins */
Servo servo_SVM[4];
int RH, RV, LV, LH;
int speedleft, speedright, a, b, c, d;

const uint8_t speedMotorPin_PWM[4] = {12, 11, 10, 9};   //PWM pins
const uint8_t directPin_INA[4] = {43, 41, 36, 35};      //INA pins
const uint8_t directPin_INB[4] = {47, 40, 38, 34};      //INB pins
const uint8_t enablePin_EN[4] = {42, 39, 37, 33};       //EN pins
const uint8_t currentSensePin_CS[4] = {A0, A3, A2, A1}; //CS pins

uint16_t currentRead[4];

void setup()
{
  Serial.begin(9600);
  pinMode(22, INPUT);
  pinMode(23, INPUT);
  pinMode(24, INPUT);
  pinMode(25, INPUT);

  for (uint8_t i = 0 ; i < 4; i++)
  {
    servo_SVM[i].attach(servoPin[i], 1000, 2200); //for servo 996R
    servo_SVM[i].write(0); //SetUp Servo is 0 degree
  }
  for (uint8_t i = 0; i < 4; i++)
  {
    pinMode(directPin_INA[i], OUTPUT);
    digitalWrite(directPin_INA[i], LOW);

    pinMode(directPin_INB[i], OUTPUT);
    digitalWrite(directPin_INB[i], LOW);

    pinMode(speedMotorPin_PWM[i], OUTPUT);
    analogWrite(speedMotorPin_PWM[i], 0);

    pinMode(enablePin_EN[i], OUTPUT);
    digitalWrite(enablePin_EN[i], HIGH); //Enable open drain ouput
  }
}
void loop()
{
  RH = pulseIn(22,HIGH);
  RV = pulseIn(23,HIGH);
  LV = pulseIn(24,HIGH);
  LH = pulseIn(25,HIGH);



  Serial.print(RH);
  Serial.print("  ");
  Serial.print(RV);
  Serial.print("  ");
  Serial.print(LV);
  Serial.print("  ");
  Serial.print(LH);
  Serial.print("  ");
  Serial.print(speedleft);
  Serial.print("  ");
  Serial.println(speedright);
/*

RH = constrain(RH,1000,2000);
RV = constrain(RV,1000,2000);
LV = constrain(LV,1000,2000);
LH = constrain(LH,1000,2000);

  speedleft = constrain(speedleft,0,255);
  speedright= constrain(speedright,0,255);
  q = constrain(q,0,255);
  w = constrain(w,0,255);
  e = constrain(e,0,255);
  r = constrain(r,0,255);
  */
  if(RV > 1575)
  {
    speedleft = map(RV, 1575, 2000, 0, 255);
    speedright = speedleft;
    if(RH > 1575)
    {
      a = map(RH, 1575, 2000, 0, 255);
      speedright = speedright - a;
    }
     if(RH < 1425)
    {
      b = map(RH, 1425, 1000, 0, 255);
      speedleft = speedleft - b;
    }
      digitalWrite(directPin_INA[0], HIGH);
      digitalWrite(directPin_INB[0], LOW);
      analogWrite(speedMotorPin_PWM[0], speedleft);
      digitalWrite(directPin_INA[2], LOW);
      digitalWrite(directPin_INB[2], HIGH);
      analogWrite(speedMotorPin_PWM[2], speedright);
  }
  else if(RV < 1425)
  {
    speedleft = map(RV, 1425, 1000, 0, 255);
    speedright = speedleft;
    if(RH > 1530)
    {
      c = map(RH, 1575, 2000, 0, 255);
      speedright = speedright - c;
    }
    else if(RH < 1425)
    {
      d = map(RH, 1425, 1000, 0, 255);
      speedleft = speedleft - d;
    }


      speedleft = constrain(speedleft,0,255);
      speedright= constrain(speedright,0,255);
      digitalWrite(directPin_INA[0], LOW);
      digitalWrite(directPin_INB[0], HIGH);
      analogWrite(speedMotorPin_PWM[0], speedleft);
      digitalWrite(directPin_INA[2], HIGH);
      digitalWrite(directPin_INB[2], LOW);
      analogWrite(speedMotorPin_PWM[2], speedright);
  }

  else if(RV < 1575 && RV > 1425)
  {
    if(RH < 1575 && RH > 1425)
    {
      digitalWrite(directPin_INA[0], LOW);
      digitalWrite(directPin_INB[0], LOW);
      analogWrite(speedMotorPin_PWM[0], 0);
      digitalWrite(directPin_INA[2], LOW);
      digitalWrite(directPin_INB[2], LOW);
      analogWrite(speedMotorPin_PWM[2], 0);
    }
    else if(RH > 1575)
    {
      speedleft = map(RV, 1575, 2000, 0, 255);
      speedright = speedleft;
      digitalWrite(directPin_INA[0], HIGH);
      digitalWrite(directPin_INB[0], LOW);
      analogWrite(speedMotorPin_PWM[0], speedleft);
      digitalWrite(directPin_INA[2], HIGH);
      digitalWrite(directPin_INB[2], LOW);
      analogWrite(speedMotorPin_PWM[2], speedright);
    }
    else if(RH < 1425)
    {
      speedleft = map(RV, 1425, 1000, 0, 255);
      speedright = speedleft;
      digitalWrite(directPin_INA[0], LOW);
      digitalWrite(directPin_INB[0], HIGH);
      analogWrite(speedMotorPin_PWM[0], speedleft);
      digitalWrite(directPin_INA[2], LOW);
      digitalWrite(directPin_INB[2], HIGH);
      analogWrite(speedMotorPin_PWM[2], speedright);
    }
  }

  //delay(5);
}