1 #define USBSERIAL 9600
   2 #define BTGPSRATE 38400
   3 
   4 #define SPDA             5          //Speed Control
   5 #define DIR1A            9          //Direction
   6 #define DIR2A            8          //Direction
   7 #define SPDB             3          //Speed Control
   8 #define DIR1B           12          //Direction
   9 #define DIR2B           11          //Direction
  10 #define STANDBY         10          //Standby Pin
  11 #define STATUS_LED      13          //Led Pin
  12 
  13 #define HAXXXOR          6          //spinning wheel of death ^^ macfags
  14 #define HAX0RSPEEDMIN    0          
  15 #define HAX0RSPEEDMAX    90
  16 
  17 #define MOTA             0
  18 #define MOTB             3
  19 
  20 #define FORWARD          1
  21 #define BACKWARD         2
  22 #define STOP             3
  23 
  24 #define CMD_DRIVE       'c'
  25 #define CMD_STOP        's'
  26 
  27 #include <Servo.h> 
  28 
  29 
  30 char MOTs[] = {SPDA, DIR1A, DIR2A, SPDB, DIR1B, DIR2B};
  31 char x,y,z;
  32 char status = CMD_STOP;
  33 Servo myservo;  // create servo object to control a servo 
  34 int hax0rspeed=0;
  35 int oldmillis;
  36 
  37 char readBT() { while(!Serial1.available()) delay(20); return (char)Serial1.read(); }
  38 
  39 void sendBT(char command[]) {
  40     Serial1.println();
  41     delay(250);
  42     Serial1.print(command);
  43     delay(250);
  44     Serial1.println();
  45     Serial1.flush();
  46     delay(250);
  47 }
  48 
  49 void setupBT(int setupdelay) {
  50     sendBT("+STWMOD=0");            // client mode
  51     sendBT("+STNA=m0wb0t");         // device name
  52     sendBT("+STAUTO=0");            // auto connect forbidden
  53     sendBT("+STOAUT=1");            // permit paired device to connect me
  54     sendBT("+STPIN=1337");          // bluetooth pin 
  55     delay(setupdelay);              // This delay is required.
  56     sendBT("+INQ=1");               // enable inqure the device 
  57     delay(setupdelay);              // This delay is required.
  58 }
  59 
  60 void stopMotor(char mot) { setMotor(mot, STOP, 0); }
  61 
  62 void setMotor(char mot, char dir, int spd) {
  63     analogWrite (MOTs[mot + 0], spd);
  64     digitalWrite(MOTs[mot + 1], dir & 0x01);
  65     digitalWrite(MOTs[mot + 2], dir & 0x02);
  66 }
  67 
  68 void fullStop() {
  69     digitalWrite(STANDBY,    LOW);
  70     digitalWrite(STATUS_LED, LOW); // indicate status
  71     stopMotor(MOTA);
  72     stopMotor(MOTB);
  73     status = CMD_STOP;
  74 }
  75 
  76 void servoup() {
  77     int pos = 0;   
  78     for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  79   {                                  // in steps of 1 degree 
  80     myservo.write(pos);              // tell servo to go to position in variable 'pos' 
  81     delay(20);
  82   } 
  83 }
  84 void servodown() {
  85   int pos = 180;
  86   for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
  87   {                                
  88     myservo.write(pos);              // tell servo to go to position in variable 'pos' 
  89     delay(20);
  90   }
  91 }
  92 
  93 
  94   
  95 void initservo() {
  96   servoup();
  97   delay(50);
  98   servodown(); 
  99 }
 100 
 101 void setup() {
 102     pinMode( SPDA       , OUTPUT );
 103     pinMode( SPDB       , OUTPUT );
 104     pinMode( STANDBY    , OUTPUT );
 105     pinMode( STATUS_LED , OUTPUT );
 106     pinMode( DIR1A      , OUTPUT );
 107     pinMode( DIR2A      , OUTPUT );
 108     pinMode( DIR1B      , OUTPUT );
 109     pinMode( DIR2B      , OUTPUT );
 110                          
 111     myservo.attach(6);  // attaches the servo on pin 6 to the servo object 
 112     initservo();
 113     
 114     fullStop(); //init's all previous outputs
 115     
 116     Serial.begin(USBSERIAL);        // init USB Serial (console)
 117     Serial1.begin(BTGPSRATE);       // init pin0/1 Serial (bluetooth)
 118     delay(1234); 
 119     setupBT(3210);
 120     while(readBT() != CMD_STOP);    // safety frist? ¯\(°_o)/¯ I DUNNO LOL
 121 }
 122 
 123 int runDelay = 0;
 124 void loop() {
 125     Serial1.print('r');             // request data from droid
 126     char bt = readBT();
 127     if(bt == CMD_STOP)
 128         {
 129         fullStop();
 130         }
 131     else if(bt == CMD_DRIVE) {
 132         if(status == CMD_STOP) {
 133             status = CMD_DRIVE;
 134             digitalWrite(STANDBY,    HIGH);
 135             digitalWrite(STATUS_LED, HIGH); // indicate status
 136         }
 137         x = readBT(); y = readBT(); z = readBT();
 138         int l = z + y, r = z - y;
 139         int L = (l < 0 ? -l : l)<<1;
 140         int R = (r < 0 ? -r : r)<<1;
 141         L = L > 255 ? 255 : L < 0 ? 0 : L;
 142         R = R > 255 ? 255 : R < 0 ? 0 : R;
 143         
 144         setMotor(MOTA, -l > 0 ? FORWARD : BACKWARD, L);
 145         setMotor(MOTB, -r > 0 ? FORWARD : BACKWARD, R);
 146     }
 147     if (millis()-oldmillis >20) {
 148       oldmillis=millis();
 149       if (status==CMD_DRIVE && hax0rspeed<HAX0RSPEEDMAX){
 150         hax0rspeed+=2;
 151       }    
 152       if (status==CMD_STOP && hax0rspeed>HAX0RSPEEDMIN){
 153         hax0rspeed--;
 154       }    
 155       myservo.write(hax0rspeed);
 156 
 157     }
 158 }