/* incluse all libraries */



#include <Servo.h>

#include <AccelStepper.h>

#include <SoftwareSerial.h>



#define DIR_PIN 9  /*connect arduino pin 9 to motor driver direction pin*/

#define STEP_PIN 8 /*connect arduino pin 9 to motor driver step pin*/



#define SERVO_PIN 3 /*connect arduino pin 3 to servo pin*/

#define UPPER_BRUSHLESS_PIN 5 /*connect arduino pin 5 to brushless motor 1 pin*/

#define LOWER_BRUSHLESS_PIN 6  /*connect arduino pin 6 to brushless motor 2 pin*/



#define STEPS_PER_BALL 700 /* steps per ball*/



#define motorInterfaceType 1



#define SERVO_INIT_VALUE 55 /*initial value for servo motor */

#define SERVO_MAX_VALUE 65 /*max value for servo motor */

#define SERVO_MIN_VALUE 35  /*min value for servo motor */





#define MAX_MOTOR_SPEED  180 /*scale motor speed from 0 t0 180 */

#define MIN_MOTOR_SPEED  0



#define SPEED_MOTOR_STEPS 9 /*increase and decrease motor speed to be 5% of the all speed*/



#define ANGLE_STEP 5 /*increase and decrease servo motor steps to be 5 */



SoftwareSerial Bluetooth(4,2); /*connect Arduino pin 4 , 2 to Bluetooth module*/

AccelStepper stepper(motorInterfaceType, STEP_PIN, DIR_PIN); /*create stepper motor object */

Servo myservo;    /*create servo object to control a servo*/

Servo UpperESC;  /*create servo object to control a ESC*/

Servo LowerESC;   /*create servo object to control a servo*/









char incomingByte = 0; /* for incoming serial data */



int servoVal=SERVO_INIT_VALUE;   

int upperVal=0;    

int lowerVal=0;    

long current_Position = 0;

int stepperSpeed = 6000;

uint8_t counter = 0;

uint8_t set = 0;

uint8_t style = 0;

uint8_t paused = 0;



int sets[10][2]={{85,25},{50,20},{60,20},{70,20},{75,25},{85,25},{75,25},{75,25},{60,20},{50,20}};

void setup() {



  stepper.setMaxSpeed(6000); /*initial speed for stepper*/

  stepper.setAcceleration(1000); /*initial acceleration for stepper*/

  

  myservo.attach(SERVO_PIN);  /* attaches  servo */

  myservo.write(servoVal); /* write init value  servo */



  

  UpperESC.attach(UPPER_BRUSHLESS_PIN,1000,2000); /* (pin, min pulse width, max pulse width in microseconds)*/

  UpperESC.write(0); /* staet will zero spped*/



  LowerESC.attach(LOWER_BRUSHLESS_PIN,1000,2000); /* (pin, min pulse width, max pulse width in microseconds) */

  LowerESC.write(0); /* staet will zero spped*/



  Serial.begin(9600); /* just for debuging



    /* set the data rate for the SoftwareSerial port*/

  Bluetooth.begin(9600);

}



void loop() {

    if (Bluetooth.available())

    {

    /* read the incoming byte from Bluetooth */

    incomingByte = Bluetooth.read();



    

    if(incomingByte == 0X61)

    {

      Serial.println("BACKWORD");

      style = 0;

    }

    else if(incomingByte == 0X60 )

    {

      Serial.println("FORWARD");

      style = 1;

    }

    else if(incomingByte == 0X62)

    {

      Serial.println("HYBRID");

      style = 2;

    }

    else if(incomingByte == 0x30)

    {

      counter = 0;

      Serial.println("RESET");

      UpperESC.write(0); 

      LowerESC.write(0);

    }

    else if (incomingByte == 0x10)

    {

      counter = 30; /*number of balls */

      paused = 0;

      Serial.println("START");

      delay(5000);

    }

    else if (incomingByte == 0x20)

    {

      paused = 1;

      Serial.println("stop");

    }

    else if ((incomingByte & 0xF0) == 0x50)

    {

      Serial.print("spin = ");

      Serial.println(incomingByte & 0x0F);

    }

     else if ((incomingByte & 0xF0) == 0x40)

    {

      Serial.print("RATE = ");

      Serial.println(incomingByte & 0x0F); 

      /* update steeper speed to increase number of balls per second*/

        stepper.setMaxSpeed(6000+((incomingByte & 0x0F)*1000));

        stepper.setAcceleration(2000+((incomingByte & 0x0F)*1000));

    }

    

   

  }

  if((counter!= 0) &&( paused== 0 ))

  {

    if(style ==0)

    {

        servoVal= random(45,60);

    }

    else if (style ==1)

    {

      servoVal= random(35,45);

    }

    else 

    {

      servoVal= random(35,60);

    }



    /*select motor speed and ball direction randomly */

    set = random(0,10);

    upperVal = (sets[set][0])*1.8;

    lowerVal = (sets[set][1])*1.8;

    UpperESC.write(upperVal); 

    LowerESC.write(lowerVal); 

    myservo.write(servoVal); 

    Serial.print("servoVal = ");

    Serial.print(servoVal);

    Serial.print("C , uppervalue = ");  

    Serial.print(upperVal/(1.8));

    Serial.print("% , lowervalue = ");

    Serial.print(lowerVal/(1.8));

    Serial.print("%");

    Serial.println("");

    current_Position+=STEPS_PER_BALL;

    stepper.runToNewPosition(current_Position);

    stepper.runSpeed();

    counter --;

  }

  else

  {

    UpperESC.write(0); 

    LowerESC.write(0); 

    

  }

}