#include <RCPulseIn.h>
#include <EnableInterrupt.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>

#include <Servo.h>
Servo myservo1;
Servo myservo2;
int servo1;
int servo2;



Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);

defineRC(2);
defineRC(3);
defineRC(4);





void setup() {

Serial.begin(9600);

setupRC(2);
setupRC(3);
setupRC(4);

AFMS.begin();

myservo1.attach(8);
myservo2.attach(9);

}





void loop() {
Serial.print(readRC(2));
Serial.print(readRC(3));
Serial.print(readRC(4));
Serial.println();

int motor1Speed = deadbandMap(readRC(2),1000,200,2000,-255,0,255);
Serial.println(motor1Speed);
runMotor(motor1,motor1Speed);
runMotor(motor2,motor1Speed);
}


void runMotor(Adafruit_DCMotor *myMotor, int spd){
if(spd>0){
myMotor->run(FORWARD);
myMotor->setSpeed(spd);
}else if(spd<0){
myMotor->run(BACKWARD);
myMotor->setSpeed(-1*spd);
}else{
myMotor->run(BRAKE);
myMotor->setSpeed(0);
}



servo1 = deadbandMap(readRC(3),1000,200,2000,80,90,100);  
servo2 = deadbandMap(readRC(4),1000,200,2000,80,90,100);  
myservo1.write(servo1);  
myservo2.write(servo2);          
delay(15);                      

}