/*
made by Alex
the King of all KIs
hat sogar Skynet gemacht
*/

#include "DualVNH5019MotorShield.h"
DualVNH5019MotorShield md;



char dataIn='S';
char determinant;
char det;
int vel = 200; //Bluetooth zeug

int overdrive = 13; //Press Toggle Switch #1, Pin 13 qird Led erleuchten

void setup(){
Serial.begin(9600);md.init();

/*
myservo.attach(6);delay(100);
myservo.write(90);delay(100);
*/

}

void loop(){ det = check();

      while (det == 'F')
       {md.setSpeeds(vel,vel);det = check();}

      while (det == 'B')
       {md.setSpeeds(-vel,-vel);det = check();}

      while (det == 'L')
       {md.setSpeeds(-vel,vel);det = check();}

      while (det == 'R')
       {md.setSpeeds(vel,-vel);det = check();}

      while (det == 'I')
       {md.setSpeeds(vel,vel/2);det = check();}

      while (det == 'J')
       {md.setSpeeds(-vel,-vel/2);det = check();}

      while (det == 'G')
       {md.setSpeeds(vel/2,vel);det = check();}

      while (det == 'H')
       {md.setSpeeds(-vel/2,-vel);det = check();}

      while (det == 'S')   // S, stop
       {md.setSpeeds(0,0);det = check();}


      /*while (det == 'W'){myservo.write(180);delay(100);det = check();}
      while (det == 'w'){myservo.write(90);delay(100);det = check();}

      while (det == 'U'){myservo.write(0);delay(100);det = check();}
      while (det == 'u'){myservo.write(90);delay(100);det = check();}
      */

}

int check()
{if (Serial.available() > 0) {dataIn = Serial.read();
        if (dataIn == 'F'){determinant = 'F';}
        else if (dataIn == 'B'){determinant = 'B';}else if (dataIn == 'L'){determinant = 'L';}
        else if (dataIn == 'R'){determinant = 'R';}else if (dataIn == 'I'){determinant = 'I';}
        else if (dataIn == 'J'){determinant = 'J';}else if (dataIn == 'G'){determinant = 'G';}
        else if (dataIn == 'H'){determinant = 'H';}else if (dataIn == 'S'){determinant = 'S';}
        else if (dataIn == '0'){vel = 400;}else if (dataIn == '1'){vel = 380;}
        else if (dataIn == '2'){vel = 340;}else if (dataIn == '3'){vel = 320;}
        else if (dataIn == '4'){vel = 280;}else if (dataIn == '5'){vel = 240;}
        else if (dataIn == '6'){vel = 200;}else if (dataIn == '7'){vel = 160;}
        else if (dataIn == '8'){vel = 120;}else if (dataIn == '9'){vel = 80;}
        else if (dataIn == 'q'){vel = 40;}
        else if (dataIn == 'U'){determinant = 'U';}else if (dataIn == 'u'){determinant = 'u';}
        else if (dataIn == 'W'){determinant = 'W';}else if (dataIn == 'w'){determinant = 'w';}

        }return determinant;}