/* _______________________________________________________
 *  C Y T R O N   M O T O R   C O N T R O L L E R  
 *               Dual Motor Test  
 *              Created  6/14/2021
 *                 Ron Kessler
 *               UCI Critter Class
 *             Locomotion Controller
 *  
 * _______________________________________________________
 * Updated:
 *  6/14/2021  First build and test
 * 
 * Supporting Documentation:
 *  This folder contains the datasheet and overview of the
 *  motor shield and images of my setup demo.
 *  
 * LIBRARY REQUIRED: 
 *  "CytronMotorDriver.h"
 *  1. Go to Tools | Manage Libraries | Type in "Cytron 3A Motor Drivber Shield". 
 *  2. Install the shield driver and the drivers Library list below it
 *  3. Restart Arduino IDE.
 * 
 * DESCRIPTION:
 * This is a test of the Cytron DC motor controller shield
 * that is attached to an Arduino UNO.
 * This sketch will test the direction and speed of
 * 2 DC brushed motors.
 * Tests forward|backward|left|right|stop functions
 * 
 * The board is powered by 1500mAh AA.Drone2 Li-Ion battery
 * The motors move forward/backwards and at different speeds.
 * The battery powers the UNO and the motor shield without the
 * need for external wires. 
 * 
 * The battery connections and motor connections are made on the
 * shield. Take care to wire the motors so they run the direction
 * you want. Simply reverse the motor wires if needed.
 *  
 * Each motor function ramps speed up and down for easier control
 * than just using full-on signals.
 * 
 * The motors are controlled via PWM.
 * 
 * OUR JUMPERS CONNECTIONS SETUP:
 *  The jumpers on the shield are set as follows:
 *  Make sure the jumpers on the board are connected as follows:
 *    Motor 1 PW1 = D3
 *    Motor 1 DIR = D4
 *    
 *    Motor 2 PWM = D6
 *    Motor 2 DIR = D7   
 *  
 *
 ****************S T A R T   O F   C O D E***********************
 */
//---make sure to add the new library
 #include "CytronMotorDriver.h"


  //---Create motor objects and configure them. PWM_DIR is defined in the Cytron Library.
CytronMD leftMotor(PWM_DIR, 3, 4);  // Use PWM and Direction control. PWM 1 = Pin 3, DIR 1 = Pin 4.)
CytronMD rightMotor(PWM_DIR, 6, 7); // PWM 2 = Pin 6, DIR 2 = Pin 7.

 //---define rpm and direction -255 = 100% duty cycle CCW. 255 = 100% CW
  const int haltMotor = 0;
  const int fullSpeedCW = 255;
  const int fullSpeedCCW = -255;
  const int timeDelay = 5000;     //5 seconds between actions
  

void setup() 
{
  //---open serial monitor to check motor rotation. Reverse motor wires before changing code
  Serial.begin(9600);
}

void loop()
{
  //---Make it so!
  Forward();
  delay (timeDelay);
  Stop();
   
  Backwards();
  delay (timeDelay);
  Stop();

  TurnRight();
  delay(timeDelay);
  Stop();
 
  TurnLeft();
  delay(timeDelay);
  Stop();
}
//---------------Helper Functions----------------

//---move bot motors forward
void Forward()
{
  leftMotor.setSpeed(fullSpeedCCW);
  rightMotor.setSpeed(fullSpeedCW);
   Serial.println("Forward");
  return;
}

//---move bot motors backwards
void Backwards()
{
  leftMotor.setSpeed(fullSpeedCW);
  rightMotor.setSpeed(fullSpeedCCW);
  Serial.println("Backwards");
  return;
}

void TurnRight()
{
  leftMotor.setSpeed(haltMotor);
  rightMotor.setSpeed(fullSpeedCW);
  Serial.println("Turn Right");
  return;
}

void TurnLeft()
{
  leftMotor.setSpeed(fullSpeedCCW);
  rightMotor.setSpeed(haltMotor);
  Serial.println("Turn Left");
  return;  
 }

void Stop()
{
  leftMotor.setSpeed(haltMotor);
  rightMotor.setSpeed(haltMotor);
  Serial.println("Stopped");
  delay(timeDelay);
  return;
 
}

//------------E N D   O F   C O D E------------------