MotorContoller

/*
IBT-2 Motor Control Board driven by Arduino.

Speed and direction controlled by a potentiometer attached to analog input 0.
One side pin of the potentiometer (either one) to ground; the other side pin to +5V

Connection to the RIGHT IBT-2 board:
IBT-2 pin 1 (RPWM) to Arduino pin 5(PWM)
IBT-2 pin 2 (LPWM) to Arduino pin 6(PWM)
IBT-2 pins 3 (R_EN), 4 (L_EN), 7 (VCC) to Arduino 5V pin
IBT-2 pin 8 (GND) to Arduino GND
IBT-2 pins 5 (R_IS) and 6 (L_IS) not connected

Connection to the LEFT IBT-2 board:
IBT-2 pin 1 (RPWM) to Arduino pin 9(PWM)
IBT-2 pin 2 (LPWM) to Arduino pin 10(PWM)
IBT-2 pins 3 (R_EN), 4 (L_EN), 7 (VCC) to Arduino 5V pin
IBT-2 pin 8 (GND) to Arduino GND
IBT-2 pins 5 (R_IS) and 6 (L_IS) not connected
*/

int R_SENSOR_PIN = 0; // center pin of the potentiometer
int L_SENSOR_PIN = 1; // center pin of the potentiometer

//Right Motor
int R_RPWM_Output = 5; // Arduino PWM output pin 5; connect to IBT-2 pin 1 (RPWM)
int R_LPWM_Output = 6; // Arduino PWM output pin 6; connect to IBT-2 pin 2 (LPWM)

//Left Motor
int L_RPWM_Output = 9; // Arduino PWM output pin 5; connect to IBT-2 pin 1 (RPWM)
int L_LPWM_Output = 10; // Arduino PWM output pin 6; connect to IBT-2 pin 2 (LPWM)

#define CH1_PIN  2
#define CH2_PIN  3

double RAWPWM_CH1;
double RAWPWM_CH2;

double PWM_CH1;
double PWM_CH2;

const int ledPin =  13;      // the number of the LED pin
int ledState = LOW;             // ledState used to set the LED
unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 100;           // interval at which to blink (milliseconds)


void setup()
{
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);

  pinMode(CH1_PIN, INPUT);
  pinMode(CH2_PIN, INPUT);

//Right Motor
  pinMode(R_RPWM_Output, OUTPUT);
  pinMode(R_LPWM_Output, OUTPUT);

//Left Motor
  pinMode(L_RPWM_Output, OUTPUT);
  pinMode(L_LPWM_Output, OUTPUT);
}


void loop()
{
//Read Right & Left POT AnalogIN //

  int R_sensorValue = analogRead(R_SENSOR_PIN);
  int L_sensorValue = analogRead(L_SENSOR_PIN);

// Get PWM data & Contrain values //

  RAWPWM_CH1 = pulseIn(CH1_PIN, HIGH);
  PWM_CH1 = constrain(RAWPWM_CH1, 0, 1970);

  RAWPWM_CH2 = pulseIn(CH2_PIN, HIGH);
  PWM_CH2 = constrain(RAWPWM_CH2, 0, 1970);

  Serial.print("PWM_CH1 - ");
  Serial.print(PWM_CH1);
  Serial.print(" - ");

  Serial.print("PWM_CH2 - ");
  Serial.print(PWM_CH2);
  Serial.print(" - ");


// FAILSAFE //

   if (PWM_CH1 == 0 || PWM_CH2 == 0)
  {
analogWrite(R_LPWM_Output, 0);
analogWrite(R_RPWM_Output, 0);
analogWrite(L_LPWM_Output, 0);
analogWrite(L_RPWM_Output, 0);
Serial.println("!! FAILSAFE !!");
Blink();
  }
 else
 {

// Stick Centered //

 if (PWM_CH1 >= 1464 && PWM_CH1 <= 1530)
  {
analogWrite(R_LPWM_Output, 0);
analogWrite(R_RPWM_Output, 0);
Serial.print("CH1 Centered - ");
  }
 if (PWM_CH2 >= 1464 && PWM_CH2 <= 1530)
  {
analogWrite(L_LPWM_Output, 0);
analogWrite(L_RPWM_Output, 0);
Serial.print("CH2 Centered - ");
  }



// Right Motor Control Loop //

  if (PWM_CH1 >=1 && PWM_CH1 <= 1463)
  {
    // reverse rotation
    int R_reversePWM = -(PWM_CH1 - 1495) / 2;
    analogWrite(R_LPWM_Output, 0);
    analogWrite(R_RPWM_Output, R_reversePWM);
    Serial.print("R_Reverse - ");
    Serial.print(R_reversePWM);
    Serial.print(" ");
  }
  else if (PWM_CH1 >=1531)
  {
    // forward rotation
    int R_forwardPWM = (PWM_CH1 - 1462) / 2;
    analogWrite(R_LPWM_Output, R_forwardPWM);
    analogWrite(R_RPWM_Output, 0);
    Serial.print("R_Forward - ");
    Serial.print(R_forwardPWM);
    Serial.print(" ");
  }

// Left Motor Control Loop //

  if (PWM_CH2 >=1 && PWM_CH2 <= 1463)
  {
    // reverse rotation
    int L_reversePWM = -(PWM_CH2 - 1495) / 2;
    analogWrite(L_LPWM_Output, 0);
    analogWrite(L_RPWM_Output, L_reversePWM);
    Serial.print("L_Reverse - ");
    Serial.print(L_reversePWM);
    Serial.print(" ");

  }
  else if (PWM_CH2 >=1531)
  {
    // forward rotation
    int L_forwardPWM = (PWM_CH2 - 1462) / 2;
    analogWrite(L_LPWM_Output, L_forwardPWM);
    analogWrite(L_RPWM_Output, 0);
    Serial.print("L_Forward - ");
    Serial.print(L_forwardPWM);
    Serial.print(" ");
  }
  }
  Serial.println(" ");
  }


void Blink()
{
  // here is where you'd put code that needs to be running all the time.

  // check to see if it's time to blink the LED; that is, if the
  // difference between the current time and last time you blinked
  // the LED is bigger than the interval at which you want to
  // blink the LED.
  unsigned long currentMillis = millis();

  if(currentMillis - previousMillis >= interval) {
    // save the last time you blinked the LED
    previousMillis = currentMillis;

    // if the LED is off turn it on and vice-versa:
    if (ledState == LOW)
      ledState = HIGH;
    else
      ledState = LOW;

    // set the LED with the ledState of the variable:
    digitalWrite(ledPin, ledState);
  }}