#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);
}