//This is the version with speed control, 
//use pins 4,5,9,10 (pwm) (on teensy 2.0)

//motor drive pins
int m1FW=4;
int m1RV=5;
int m2FW=9;
int m2RV=10;

//kill all function halt
void halt();

//directional function prototypes
void fw();
void rv();
void left();
void right();


//run time==dtime
int dtime=1000; //used for turning
//sped control
unsigned int sped= 96;

void setup() {
 Serial.begin(9600);
 pinMode(m1FW, OUTPUT);
 pinMode(m1RV, OUTPUT);
 pinMode(m2FW, OUTPUT);
 pinMode(m2RV, OUTPUT);
}
// lets make a fire!
void loop() {

  char inByte = ' ';
      if(Serial.available()){ // only send data back if data has been sent
          char inByte = Serial.read(); // read the incoming data
          Serial.println(inByte); 
  switch (inByte) {
    //wasd ws=y ad=x
    case 'w':
      halt();
      fw(); 
      break;
      case 'a':
      halt();
      left();   
      break;    
      case 's':
      halt();
      rv();
      break;    
      case 'd':
      halt();
      right();  
      break;
      case 'b':
      halt();  
      break;
    default:
      halt();  
      break;
        }
    }
}

void left(){
  halt();
  analogWrite(m1FW, sped);
  analogWrite(m2RV, sped);   
  delay(dtime);               // wait for a second
  halt();
  }

void right(){
  halt();
  analogWrite(m1RV,sped);
  analogWrite(m2FW,sped);   
  delay(dtime);               // wait for a second
  halt();
  }

void rv(){
  analogWrite(m1RV,sped);
  analogWrite(m2RV,sped);   
  }

void fw(){
  analogWrite(m1FW,sped);
  analogWrite(m2FW,sped);   
  }

void halt(){
  //stop all that sht
   analogWrite(m1FW, 0); 
   analogWrite(m1RV, 0); 
   analogWrite(m2FW, 0); 
   analogWrite(m2RV, 0); 

  }