#include <Servo.h>
Servo myservo;
int pos = 0; 
const int buttonPin = 7;
void motor1(int m1);

String angle = "";
String strength = "";
String button = "";
int buttonState = 0;
char command = 'S';
 int speed =  200 ;
void setup() {
  // put your setup code here, to run once:
  pinMode(6, OUTPUT);//led
  pinMode(5, OUTPUT);
  pinMode(buttonPin, INPUT);
  Serial.begin(9600);
  myservo.attach(2);
 
}

void loop() {
  // put your main code here, to run repeatedly:
//for ( int i = 50; i < 255 ; i = i + 5 )  {
//Serial.println(-100);
//setSpeed(-100,0);
buttonState = digitalRead(buttonPin);

while(buttonState) {
if(Serial.available()>0)
{
command = Serial.read();
Serial.print(command);
if( command == 'F' ) {
  //Serial.print("FWD");
  Serial.println(speed);
  motor1(speed);
  }
  if( command == 'B' ) {
  //Serial.print("REV");
  Serial.println(speed);
  motor1((-1) * speed);
  }
  if(  command == 'L' ) {
  //Serial.print("LEFT");
  myservo.write(45);
  }
  if(  command == 'R' ) {
  //Serial.print("RIGHT");
  myservo.write(105);
  }
  if(  command == 'G' ) {
  //Serial.print("FWD_LEFT");
  motor1(speed);
  myservo.write(45);
  }
  if(  command == 'H' ) {
  //Serial.print("BW_LEFT");
  motor1((-1) * speed);
  myservo.write(45);
  }
  if(  command == 'I' ) {
  //Serial.print("FWD_RIGHT");
  motor1(speed);
  myservo.write(105);
  }
  if(  command == 'J' ) {
  //Serial.print("BWD_RIGHT");
  motor1((-1) * speed);
  myservo.write(105);
  }
  if(  command == 'S' ) {
  motor1(0);
  myservo.write(75);
  }
  if(command=='q') {
  speed = 255;  //Full velocity
  }
  else {
  if((command >= 48) && (command <= 57)) {
  speed = (command - 48)*25;      
  }
 }
}
}


Serial.println("BT disconnected");
motor1(0);
myservo.write(75);
}

void motor1(int m1)
{
  if (m1 > 0)
  {
    analogWrite(5, 255);
    analogWrite(6, 255 - m1);  //B+
    
  }
  else if (m1 < 0)
  {
    analogWrite(5, 255 + m1 );
    analogWrite(6, 255);  //B+
 
  }
  else
  {
    analogWrite(6, 0);
    //digitalWrite(7, HIGH); //B-
    analogWrite(5, 0); //B+
  }
  delay(1);
}