tank.c

#define USBSERIAL 9600
#define BTGPSRATE 38400

#define SPDA             5          //Speed Control
#define DIR1A            9          //Direction
#define DIR2A            8          //Direction
#define SPDB             3          //Speed Control
#define DIR1B           12          //Direction
#define DIR2B           11          //Direction
#define STANDBY         10          //Standby Pin
#define STATUS_LED      13          //Led Pin
#define HAXXXOR          6          //spinning wheel of death 

#define MOTA             0
#define MOTB             3

#define FORWARD          1
#define BACKWARD         2
#define STOP             3

#define CMD_DRIVE       'c'
#define CMD_STOP        's'

#include <SoftwareServo.h> 


char MOTs[] = {SPDA, DIR1A, DIR2A, SPDB, DIR1B, DIR2B};
char x,y,z;
char status = CMD_STOP;
SoftwareServo myservo;  // create servo object to control a servo 

char readBT() { while(!Serial1.available()) sdelay(20); return (char)Serial1.read(); }

void sendBT(char command[]) {
    Serial1.println();
    sdelay(250);
    Serial1.print(command);
    sdelay(250);
    Serial1.println();
    Serial1.flush();
    sdelay(250);
}

void setupBT(int setupdelay) {
    sendBT("+STWMOD=0");            // client mode
    sendBT("+STNA=m0wb0t");         // device name
    sendBT("+STAUTO=0");            // auto connect forbidden
    sendBT("+STOAUT=1");            // permit paired device to connect me
    sendBT("+STPIN=1337");          // bluetooth pin 
    delay(setupdelay);              // This delay is required.
    sendBT("+INQ=1");               // enable inqure the device 
    delay(setupdelay);              // This delay is required.
}

void stopMotor(char mot) { setMotor(mot, STOP, 0); }

void setMotor(char mot, char dir, int spd) {
    analogWrite (MOTs[mot + 0], spd);
    digitalWrite(MOTs[mot + 1], dir & 0x01);
    digitalWrite(MOTs[mot + 2], dir & 0x02);
    myservo.write(spd/2);
}

void fullStop() {
    digitalWrite(STANDBY,    LOW);
    digitalWrite(STATUS_LED, LOW); // indicate status
    stopMotor(MOTA);
    stopMotor(MOTB);
    myservo.write(0);
    SoftwareServo::refresh();
    status = CMD_STOP;
}

void sdelay(int servodelay)
  {
    unsigned long previousMillis = 0; // last time update
    int sloop = 0;
    for (sloop = 0; sloop < servodelay; sloop +=1)
      {
        unsigned long currentMillis = millis();  
          if(currentMillis - previousMillis > 40) {  
           previousMillis = currentMillis;  
           SoftwareServo::refresh();  // keep the softservo pbm alive 
           }
        delay(1);
      }
  }
  
void initservo() {
    int pos = 0;   
    for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    sdelay(20);
  } 
  sdelay(100);
  for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    sdelay(20);
    SoftwareServo::refresh();
  } 
  
}

void setup() {
    pinMode( SPDA       , OUTPUT );
    pinMode( SPDB       , OUTPUT );
    pinMode( STANDBY    , OUTPUT );
    pinMode( STATUS_LED , OUTPUT );
    pinMode( DIR1A      , OUTPUT );
    pinMode( DIR2A      , OUTPUT );
    pinMode( DIR1B      , OUTPUT );
    pinMode( DIR2B      , OUTPUT );
                         
    myservo.attach(HAXXXOR);  // attaches the servo pin
    initservo();
    
    fullStop(); //init's all previous outputs
    
    Serial.begin(USBSERIAL);        // init USB Serial (console)
    Serial1.begin(BTGPSRATE);       // init pin0/1 Serial (bluetooth)
    delay(1234); 
    setupBT(3210);
    while(readBT() != CMD_STOP);    // safety frist? ¯\(°_o)/¯ I DUNNO LOL
}

int runDelay = 0;
void loop() {
    Serial1.print('r');             // request data from droid
    char bt = readBT();
    if(bt == CMD_STOP)
        fullStop();
    else if(bt == CMD_DRIVE) {
        if(status == CMD_STOP) {
            status = CMD_DRIVE;
            digitalWrite(STANDBY,    HIGH);
            digitalWrite(STATUS_LED, HIGH); // indicate status
        }
        x = readBT(); y = readBT(); z = readBT();
        int l = z + y, r = z - y;
        int L = (l < 0 ? -l : l)<<1;
        int R = (r < 0 ? -r : r)<<1;
        L = L > 255 ? 255 : L < 0 ? 0 : L;
        R = R > 255 ? 255 : R < 0 ? 0 : R;
        
        setMotor(MOTA, -l > 0 ? FORWARD : BACKWARD, L);
        setMotor(MOTB, -r > 0 ? FORWARD : BACKWARD, R);
    }
}