Untitled

// dau vao
const int CH_1_PIN = 13;
const int CH_2_PIN = 12;
const int CH_3_PIN = 1;
// moto sau
const int STBY_PIN = 0;
const int AIN1_PIN = 2;
const int AIN2_PIN = 4;
const int APWM_PIN = 5;
const int BIN1_PIN = 7;
const int BIN2_PIN = 8;
const int BPWM_PIN = 6;
//moto truoc
const int CIN1_PIN = 3;
const int CIN2_PIN = 10;
const int DIN1_PIN = 9;
const int DIN2_PIN = 11;


// Parameters
const int deadzone = 20;  // Anything between -20 and 20 is stop

void setup() {

  // Configure pins
  pinMode(STBY_PIN, OUTPUT);
  pinMode(AIN1_PIN, OUTPUT);
  pinMode(AIN2_PIN, OUTPUT);
  pinMode(APWM_PIN, OUTPUT);
  pinMode(BIN1_PIN, OUTPUT);
  pinMode(BIN2_PIN, OUTPUT);
  pinMode(BPWM_PIN, OUTPUT);
  pinMode(CIN1_PIN, OUTPUT);
  pinMode(CIN2_PIN, OUTPUT);
  pinMode(DIN1_PIN, OUTPUT);
  pinMode(DIN2_PIN, OUTPUT);
  
  // Enable motor driver
  digitalWrite(STBY_PIN, HIGH);
}

void loop() {
  // moto truoc
  int z=pulseIn(CH_3_PIN, HIGH);
  int t=map(z,1000,2000,0,255);
  if (t<deadzone)
  {
    digitalWrite(3,0);
    digitalWrite(9,0);
    digitalWrite(11,0);
    digitalWrite(10,0);
  }
  else
    { 
      analogWrite(3,t);
      analogWrite(9,t);
  
    }
  // Read pulse width from receiver
  int y = pulseIn(CH_2_PIN, HIGH);
  delay(10);
  int x = pulseIn(CH_1_PIN, HIGH);
  

  // Convert to PWM value (-255 to 255)
  y = pulseToPWM(y);
  delay(10);
  x = pulseToPWM(x);

  // Mix for arcade drive
  int left = y + x;
  int right = y - x;

  // Drive motor
  drive(left, right);

  delay(5);
}

// Positive for forward, negative for reverse
void drive(int speed_a, int speed_b) {

  // Limit speed between -255 and 255
  speed_a = constrain(speed_a, -255, 255);
  speed_b = constrain(speed_b, -255, 255);

  // Set direction for motor A
  if ( speed_a == 0 ) {
    digitalWrite(AIN1_PIN, LOW);
    digitalWrite(AIN2_PIN, LOW);
  } else if ( speed_a > 0 ) {
    digitalWrite(AIN1_PIN, HIGH);
    digitalWrite(AIN2_PIN, LOW);
  } else {
    digitalWrite(AIN1_PIN, LOW);
    digitalWrite(AIN2_PIN, HIGH);
  }

  // Set direction for motor B
  if ( speed_b == 0 ) {
    digitalWrite(BIN1_PIN, LOW);
    digitalWrite(BIN2_PIN, LOW);
  } else if ( speed_b > 0 ) {
    digitalWrite(BIN1_PIN, HIGH);
    digitalWrite(BIN2_PIN, LOW);
  } else {
    digitalWrite(BIN1_PIN, LOW);
    digitalWrite(BIN2_PIN, HIGH);
  }

  // Set speed
  analogWrite(APWM_PIN, abs(speed_a));
  analogWrite(BPWM_PIN, abs(speed_b));
}

// Convert RC pulse value to motor PWM value
int pulseToPWM(int pulse) {
  
  // If we're receiving numbers, convert them to motor PWM
  if ( pulse > 1000 ) {
    pulse = map(pulse, 1000, 2000, -500, 500);
    pulse = constrain(pulse, -255, 255);
  } else {
    pulse = 0;
  }

  // Anything in deadzone should stop the motor
  if ( abs(pulse) <= deadzone ) {
    pulse = 0;
  }

  return pulse;
}