   return 1;
 }
 
-void controlMotor(int speed,int direction,float bias){
-  
+void controlMotor(int speed,int direction,float bias,int rotate){
   int biasSpeedL=0.0;
   int biasSpeedR=0.0;
   if(bias==0){

  ch1 = pulseIn(5, HIGH, 25000); // Read the pulse width of 
  ch2 = pulseIn(6, HIGH, 25000); // each channel
  ch3 = pulseIn(7, HIGH, 25000);
  float bias=0;
  //Serial.print("CH3=");Serial.println(ch3);
  if(ch3<1500&&ch3>1490)
    bias=0;
  if(ch3<=1490)
    bias=-((ch3-1490)/190.0)*0.8;
  if(ch3>=1500)
    bias=((ch3-1500)/200.0)*0.8;
  //Serial.print("Bias=");Serial.println(bias);

  if(ch1<996){ // This sets SWB as the master motor cut off
    controlMotor(0,0,bias);
  } else {
     if(ch2<1400){
        controlMotor(SPEED_FASTER,DIRECTION_BACKWARD,bias);
     }
     if(ch2>=1400&&ch2<1425){
        controlMotor(SPEED_FAST,DIRECTION_BACKWARD,bias);
      }
      if(ch2>=1425&&ch2<1450){
          controlMotor(SPEED_SLOW,DIRECTION_BACKWARD,bias);
      }
      if(ch2>=1450&&ch2<1500){
        controlMotor(0,DIRECTION_STOP,bias);
      }
      if(currentRange>12.0){
        if(ch2>=1500&&ch2<1525){
            controlMotor(SPEED_SLOW,DIRECTION_FORWARD,bias);
        }
        if(ch2>=1525&&ch2<1550){
            controlMotor(SPEED_FAST,DIRECTION_FORWARD,bias);
         }
         if(ch2>=1550&&ch2<1575){
            controlMotor(SPEED_FASTER,DIRECTION_FORWARD,bias);
         }
         if(ch2>=1575&&ch2<1600){
            controlMotor(SPEED_FASTEST,DIRECTION_FORWARD,bias);
         }
         if(ch2>=1600&&ch2<1625){
            controlMotor(SPEED_RIDICULOUS,DIRECTION_FORWARD,bias);
         }
         if(ch2>=1625){
            controlMotor(SPEED_LUDICROUS,DIRECTION_FORWARD,bias);
         }
       }
      

}

void loop() {
  //Serial.println(".");
  // To drive the motor in H-bridge mode
  // the power chip inputs must be opposite polarity
  // and the Enable input must be HIGH
