/*
 * Current Sensing with Arduino Motor Shield
 * 
 * Written by Ahmad Shamshiri for Robojax.com on August 31, 2018 at 17:40 in Ajax, Ontario, Canada
 * Watch video instruction for this code: https://youtu.be/-uQKBDTWHPM
 * This code requires a viewing of the introduction to the Arduino Motor Shield,
 * which you can watch here: https://youtu.be/kIgbjyqNrV8
 
     This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
const int MotorPinA = 12; 
const int MotorSpeedPinA = 3;
const int MotorBrakePinA = 9;
const int currentSenseA =A0;


const int MotorPinB = 13;
const int MotorSpeedPinB = 11;
const int MotorBrakePinB = 8;
const int currentSenseB =A1;

const int CW  = HIGH;// variable for clockwise rotation
const int CCW = LOW;// variable for counterclockwise rotation

const int showComments = 1;// show comments in serial monitor
const double currentFactor = 2/3.3;// 3.3V per 2A which is 0.909

void setup() {
  // motor A pin assignment
  pinMode(MotorPinA, OUTPUT);// define motor pin as output
  pinMode(MotorSpeedPinA, OUTPUT);//define motor speed control pin
  pinMode(MotorBrakePinA, OUTPUT);// define motor brake pin

  // motor B pin assignment
  pinMode(MotorPinB, OUTPUT);
  pinMode(MotorSpeedPinB, OUTPUT);
  pinMode(MotorBrakePinB, OUTPUT); 


  Serial.begin(9600);//  serial monitor initialized 

}


void loop() {
  
  brake('B', 0); // release brake

  moveMotor('B', CW, 255);// start motor B in CW direction with 255 PWM value
  Serial.print("Current at 255:");
  double currentVB = map(analogRead(currentSenseB), 0, 1024, 0, 5000)/1000;
  double currentB = currentVB*currentFactor;// get channel B current from voltage and current factor (from data sheet)

  double currentVA = map(analogRead(currentSenseA), 0, 1024, 0, 5000)/1000;
  double currentA = currentVA*currentFactor;// get channel A current from voltage and current factor (from data sheet)
    
  if(currentB >1.25)
  {
    brake('B', 1);// apply brake
    brake('B',0);// release brake
    // change the rotation direction to CCW
    moveMotor('B', CCW, 255);
  }
  Serial.println(currentB);// print the current
  delay(300);


}// loop end



/*
 * 
 * Written by Ahmad Shamshiri August 29, 2018 at 20:59 in Ajax, Ontario, Canada 
 * moveMotor controls the motor
  @param motor is char A or B referring to motor A or B.
  @param dir is motor direction, CW or CCW
  @param speed is PWM value between 0 to 255

  Example 1: to start moving motor A in CW direction with 135 PWM value
  moveMotor('A', CW, 135);

  Example 2: to start moving motor B in CCW direction with 200 PWM value
  moveMotor('B', CCW, 200);  
 */

void moveMotor(char motor, int dir, int speed)
{
  int motorPin;
  int motorSpeedPin;
  
  if(motor =='A')
  {
    motorPin      = MotorPinA;
    motorSpeedPin = MotorSpeedPinA;  
  }else{
    motorPin      = MotorPinB;
    motorSpeedPin = MotorSpeedPinB;     
  }
   digitalWrite(motorPin, dir);// set direction for motor
   analogWrite(motorSpeedPin, speed);// set speed of motor   
}//moveMotor end

/*
 * brake, stops the motor, or releases the brake
 * @param motor is character A or B
 * @param brk if 1, brake; if 0, release brake
 * example of usage:
 * brake('A', 1);// applies brake to motor A
 * brake('A', 0);// releases brake from motor A
 */
void brake(char motor, int brk)
{
  if(motor =='A')
  {
    digitalWrite(MotorBrakePinA, brk);// brake
    delay(1000);
  }else{
    digitalWrite(MotorBrakePinB, brk);// brake
    delay(1000);
   
  }
}