/*
Exercise Monster Motor Shield
Uses Serial Monitor window to issue commands for controlling the DC motors 
connected to the shield
S = Stop
F = Forward
R = Reverse
C = Returns the current reading of the motors
Pxxx (P0 - P255) sets the PWM speed value
P? = Returns the current PWM value
*/
#include <SoftwareSerial.h>
SoftwareSerial bluetooth(3, 4); // RX, TX
const int MOTOR_1 = 0;
const int MOTOR_2 = 1;
const int MOTOR_3 = 2;
const int MOTOR_4 = 3;
int Mot_no=-1;
const int BRAKE = 0;
const int CW = 1;
const int CCW = 2;
int st_robot = 0; //universal robot start bit. If 0, robot is OFF, if 1 robot is ON
int enm = 0; //universal motors start motors won't move if it is 0
/*
Note:  the pin definitions below are set by the shield pinout. If using
the board as a shield, these pins must remain as specified below.
If wiring the board rather than using as a shield, these can be changed. */
const int MOTOR1_A1_PIN = 6; //Motor 1 control inputs
const int MOTOR1_B1_PIN = 7; 

const int MOTOR2_A2_PIN = 8; // Motor 2 control inputs
const int MOTOR2_B2_PIN = 9;

const int MOTOR3_A1_PIN = 10; //Motor 3 control inputs
const int MOTOR3_B1_PIN = 11; 

const int MOTOR4_A2_PIN = 12; // Motor 4 control inputs
const int MOTOR4_B2_PIN = 13;

const int PWM_MOTOR_1 = 5;
const int PWM_MOTOR_2 = 4;
const int led_pin = 13;
const int LimitSw=A0;

//const int EN_PIN_1 = A0;
//const int EN_PIN_2 = A1;

int motor_Speed = 30;  //default motor speed
int motor_State = BRAKE;

char readString[4];  // String array to hold PWM value typed in on keyboard
//===============================================================================
//  Initialization
//===============================================================================
void setup() {
  pinMode(MOTOR1_A1_PIN, OUTPUT);
  pinMode(MOTOR1_B1_PIN, OUTPUT);
  pinMode(MOTOR2_A2_PIN, OUTPUT);
  pinMode(MOTOR2_B2_PIN, OUTPUT);
//  pinMode(0,INPUT);
//  pinMode(1,INPUT);
  pinMode(MOTOR3_A1_PIN, OUTPUT);
  pinMode(MOTOR3_B1_PIN, OUTPUT);
  pinMode(MOTOR4_A2_PIN, OUTPUT);
  pinMode(MOTOR4_B2_PIN, OUTPUT);
  pinMode(PWM_MOTOR_1, OUTPUT);
  pinMode(LimitSw,INPUT);
  pinMode(led_pin, OUTPUT);
  
    
  Serial.begin(9600);           // Initialize serial monitor
  Serial.println("Enter command:");    // Printout commands
  Serial.println("S = STOP");
  Serial.println("U = FORWARD");
  Serial.println("D = REVERSE");
  Serial.println("Pxxx = PWM SPEED (P000 - P254)");
  Serial.println("P? = RETURNS CURRENT PWM SPEED");
  //attachInterrupt(digitalPinToInterrupt(LimitSw), blink, CHANGE);
bluetooth.begin(38400);
bluetooth.println("Hello, world?");
  if (st_robot == 0){
    motor_State = BRAKE;
    blink();
  }
}
void blink() {
  Motor_Cmd(MOTOR_1, motor_State, 0);
  Motor_Cmd(MOTOR_2, motor_State, 0);
  Motor_Cmd(MOTOR_3, motor_State, 0);
  Motor_Cmd(MOTOR_4, motor_State, 0);
}
//===============================================================================
//  Main
//===============================================================================
void loop() {
// Just loop while monitoring the serial port and then jump to DoSerial to
// handle incoming characters and act on them
if (bluetooth.available())
  {
    char ch=bluetooth.available();
    Serial.println(ch);
  }
if (Serial.available()) 
  DoSerial();
}
//===============================================================================
//  Subroutine to handle characters typed via Serial Monitor Window
//===============================================================================
void DoSerial()
{
  int index = 0;
  int pwm_Value = 0;

  char ch = Serial.read();  // Read the character we know we have
  Serial.println(ch);       // Echo character typed to show we got it

  // Use Switch/Case statement to handle the different commands

  switch (ch) {
    
    case 'O':   // Robot START command
    case 'o':
      st_robot = 1;
      Serial.println(st_robot);
      break;
    case 'n':   // Robot STOP command
    case 'N':
      motor_State = BRAKE;
      blink();   
      Serial.println("Motors Stop");
      st_robot = 0;
      Mot_no = -1;
      enm = 0;
      Serial.println(st_robot);
      ledoff();
      break;
          
    case 's':   // Motor STOP command
    case 'S':
      motor_State = BRAKE;
      blink();  
      Serial.println("Motors Stop");
      Serial.println(st_robot);
      ledoff();
      break;
      
    case'w':
    case'W':
      //Arm_off();
      Link1_off();
      Link2_off();
      Link3_off();
      //blink(); 
      Link3_off();
      if(st_robot ==0)
      {
        break;
      }
      Mot_no=MOTOR_1;
      motor_Speed=255;
//      bluetooth.println("arm base Selected");
      Serial.println("arm base selected");
      Serial.println(Mot_no);
      break;
    case'x':
    case'X':
      Arm_off();
      //Link1_off();
      Link2_off();
      Link3_off();
      
      //blink(); 
      if(st_robot ==0)
      {
        break;
      }
      Mot_no=MOTOR_2;
      motor_Speed=255;
      Serial.println("L1 selected");
      Serial.println(Mot_no);
      break;
  
    case'y':
    case'Y':
      Arm_off();
      Link1_off();
      //Link2_off();
      Link3_off();
      //blink(); 
      if(st_robot ==0)
      {
        break;
      }
      Mot_no=MOTOR_3;
      motor_Speed=40;
      Serial.println("L2 selected");
      Serial.println(Mot_no);
      break;    
    case'z':
    case'Z':
      Arm_off();
      Link1_off();
      Link2_off();
      //Link3_off();
      //blink(); 
      if(st_robot ==0)
      {
        break;
      }
      Mot_no=MOTOR_4;
      motor_Speed=20;
      Serial.println("L3 selected");
      Serial.println(Mot_no);
      break;
      
    case 'u':   // Motor FORWARD command
    case 'U':   // This fall-through case statement accepts upper and lower case
      if(st_robot ==0)
      {
            break;
      }
      motor_State = CW;
      
      if (Mot_no>=0)
      {
        Serial.println("Motors Forward");
        Serial.println(Mot_no);
        Motor_Cmd(Mot_no, motor_State, motor_Speed);
      }
      else {
        Serial.println("Motors Not selected");
      }
      break;
  
    case 'd':   // Motor REVERSE command
    case 'D':
      if(st_robot ==0)
      {
        break;
      }
      motor_State = CCW;
            
      if (Mot_no>=0)
      {
        Serial.println("Motors Reverse");
        Serial.println(Mot_no);
        Motor_Cmd(Mot_no, motor_State, motor_Speed);
      }
      else {
         Serial.println("Motors Not selected");
      }
      break;
    
    case 'p':  // Motor SPEED command
    case 'P':
      // This command is a little trickier.  We are looking for a number from 0-255
      // to follow this command so we can set the PWM speed.  If we see a '?'
      // we will report our current speed setting, otherwise we start collecting chars
      // into the readString array.
      delay(2);  // Give time for more characters to arrive.
      for (int i; i<4; i++) readString[i] = ' ';  // Clear string array
      while (Serial.available())  // Read what we get and put into the string array
      {
        char c = Serial.read();
        readString[index] = c;
        index++;
        delay(2);
      }
      readString[3] = '\0'; // Append null to end of string array to make it a valid string
      index = 0;            // Reset our index back to the start of the string
      if (readString[index] == '?')   // ? means report our current speed setting and exit.
      {
        Serial.print("Current PWM Setting: ");
        Serial.println(motor_Speed);
        break;
      }
      pwm_Value = atoi(readString);  // Try to convert string into integer
      // We assume a 0 value is because of a non-valid input and ignore the command
      if(pwm_Value!=0) {   
        if (pwm_Value > 255) pwm_Value = 255;     // Cap WPM setting at 255
        Serial.println(pwm_Value);        // Echo what we end up with to confirm we got it
        motor_Speed = pwm_Value;
        Motor_Cmd(Mot_no, motor_State, motor_Speed);
        
      }
      break;
    
    default:
      break;
  }
}

void Arm_off()
  {
    digitalWrite(MOTOR1_A1_PIN, LOW);
    digitalWrite(MOTOR1_B1_PIN, LOW);
  }
void Link1_off()
  {
    digitalWrite(MOTOR2_A2_PIN, LOW);
    digitalWrite(MOTOR2_B2_PIN, LOW);
  }
void Link2_off()
  {
    digitalWrite(MOTOR3_A1_PIN, LOW);
    digitalWrite(MOTOR3_B1_PIN, LOW);
  }
void Link3_off()
  {
    digitalWrite(MOTOR4_A2_PIN, LOW);
    digitalWrite(MOTOR4_B2_PIN, LOW);
  }
void Motor_Cmd(int motor, int DIR, int PWM)     //Function that writes to the motors
{
  if(motor == MOTOR_1)
  { Serial.println("Motor ab selected");
    if(DIR == CW)    {
      digitalWrite(MOTOR1_A1_PIN, LOW); 
      digitalWrite(MOTOR1_B1_PIN, HIGH);
    }
    else if(DIR == CCW)    {
      digitalWrite(MOTOR1_A1_PIN, HIGH);
      digitalWrite(MOTOR1_B1_PIN, LOW);      
    }
    else    {
      Arm_off();           
    }
    analogWrite(PWM_MOTOR_1, PWM); 
  }
  else if(motor == MOTOR_2)
  {Serial.println("Motor L1 selected");
    if(DIR == CW)    {
      digitalWrite(MOTOR2_A2_PIN, LOW);
      digitalWrite(MOTOR2_B2_PIN, HIGH);
    }
    else if(DIR == CCW)    {
      digitalWrite(MOTOR2_A2_PIN, HIGH);
      digitalWrite(MOTOR2_B2_PIN, LOW);      
    }
    else    {
      Link1_off();            
    } 
    analogWrite(PWM_MOTOR_1, PWM);
  }
   else if(motor == MOTOR_3)
  { Serial.println("Motor L2 selected");
    if(DIR == CW)    {
      digitalWrite(MOTOR3_A1_PIN, LOW);
      digitalWrite(MOTOR3_B1_PIN, HIGH);
    }
    else if(DIR == CCW)    {
      digitalWrite(MOTOR3_A1_PIN, HIGH);
      digitalWrite(MOTOR3_B1_PIN, LOW);      
    }
    else    {
      Link2_off();            
    } 
    analogWrite(PWM_MOTOR_1, PWM);
  }
   else if(motor == MOTOR_4)
  {Serial.println("Motor L3 selected");
    if(DIR == CW)    {
      digitalWrite(MOTOR4_A2_PIN, LOW);
      digitalWrite(MOTOR4_B2_PIN, HIGH);
    }
    else if(DIR == CCW)    {
      digitalWrite(MOTOR4_A2_PIN, HIGH);
      digitalWrite(MOTOR4_B2_PIN, LOW);      
    }
    else    {
      Link3_off();            
    } 
    analogWrite(PWM_MOTOR_1, PWM);
  }
}

void ledon()
{
      if(st_robot)
      {
        digitalWrite(led_pin, HIGH);   // turn the LED on 
        delay(500);                       // wait for half a second 
      }
      
}

void ledoff()
{
      digitalWrite(led_pin, LOW);   // turn the LED on 
      delay(500);                       // wait for half a second 
}