/*
* Attach an Adafruit Motor Shield to the Arduino Uno/Mega.
* In addition to powering the RC Receiver (with GND and 5V), connect
* one of the RC Receiver's signal pins to the Arduino's digital pin 2.
* The duration of the high RC pulse will be used to control a motor
* attached to Motor Shield motor port 1.
*
* Author: David Wang, NuVu Studio
*/
#include <RCPulseIn.h>
#include <EnableInterrupt.h>


#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"

Adafruit_MotorShield AFMSbot(0x61); // Rightmost jumper closed
Adafruit_MotorShield AFMStop(0x60); // Default address, no jumpers

// On the top shield, connect two steppers, each with 200 steps
Adafruit_StepperMotor *myStepper2 = AFMStop.getStepper(200, 1);
Adafruit_StepperMotor *myStepper3 = AFMStop.getStepper(200, 2);

// On the bottom shield connect a stepper to port M3/M4 with 200 steps
//Adafruit_StepperMotor *myStepper1 = AFMSbot.getStepper(200, 2);
// And a DC Motor to port M1
Adafruit_DCMotor *myMotor1 = AFMStop.getMotor(1);


int val = 3;
// define an interrupt for digital pin 2.
// that will need to read an RC pulse train.
defineRC(val);


void setup() {
//while (!Serial);
Serial.begin(9600);           // set up Serial library at 9600 bps
// setup the RC interrupt for digital pin 2.
// (You need to use "defineRC(pin)" for each pin, see above.)
setupRC(val);

//AFMSbot.begin(); // Start the bottom shield
AFMStop.begin(); // Start the top shield
myStepper3->setSpeed(10);  // 10 rpm
myStepper2->setSpeed(10);  // 10 rpm

myStepper3->step(12, BACKWARD, MICROSTEP);
delay(500);
myStepper2->step(23, BACKWARD, MICROSTEP);

// turn on the DC motor
//myMotor1->setSpeed(5);
//myMotor1->run(RELEASE);
}

int i;

void loop() {
//myMotor1->run(FORWARD);

// Use the function "readRC(pinNumber)" to read the duration  
// of the high voltage pulse to digital pin 2.
// (You need to use "defineRC(pin)" and "setupRC(pin)" for each pin, see above.)
Serial.print(readRC(val));
Serial.println();
int SM = deadbandMap(readRC(val),1000,200,2000,-255,0,255);

if(SM>10){
//myStepper3->step(12, FORWARD, DOUBLE);
myStepper3->step(17, FORWARD, MICROSTEP);

delay(100);

for (i=0; i<16; i++) {
//myStepper2->setSpeed(1000);
//myStepper3->setSpeed(1000);
//myMotor1->setSpeed(i);  
//myStepper1->onestep(FORWARD, INTERLEAVE);
myStepper2->onestep(FORWARD, DOUBLE);
myStepper3->onestep(FORWARD, DOUBLE);
//myStepper2->onestep(FORWARD, MICROSTEP);
//myStepper3->onestep(FORWARD, MICROSTEP);
delay(100);
}

//myStepper2->step(15, FORWARD, DOUBLE);
myStepper2->step(15, FORWARD, MICROSTEP);

delay(100);

/////////////

//myStepper3->step(12, BACKWARD, DOUBLE);
myStepper3->step(17, BACKWARD, MICROSTEP);

delay(100);

for (i=16; i!=0; i--) {
//myStepper2->setSpeed(1000);
//myStepper3->setSpeed(i);
//myMotor1->setSpeed(i);  
//myStepper1->onestep(BACKWARD, INTERLEAVE);
myStepper2->onestep(BACKWARD, DOUBLE);
myStepper3->onestep(BACKWARD, DOUBLE);
//myStepper2->onestep(BACKWARD, MICROSTEP);
//myStepper3->onestep(BACKWARD, MICROSTEP);
delay(100);
}
//myStepper2->step(15, BACKWARD, DOUBLE);
myStepper2->step(15, BACKWARD, MICROSTEP);

delay(100);

//myMotor1->run(BACKWARD);

}
else if(SM<-10){
//myStepper3->step(12, FORWARD, DOUBLE);
myStepper3->step(20, FORWARD, MICROSTEP);

delay(100);

for (i=0; i<18; i++) {
//myStepper2->setSpeed(1000);
//myStepper3->setSpeed(1000);
//myMotor1->setSpeed(i);  
//myStepper1->onestep(FORWARD, INTERLEAVE);
myStepper2->onestep(FORWARD, DOUBLE);
myStepper3->onestep(FORWARD, DOUBLE);
//myStepper2->onestep(FORWARD, MICROSTEP);
//myStepper3->onestep(FORWARD, MICROSTEP);
delay(100);
}

//myStepper2->step(15, FORWARD, DOUBLE);
myStepper2->step(35, FORWARD, MICROSTEP);

delay(100);

/////////////

//myStepper3->step(12, BACKWARD, DOUBLE);
myStepper3->step(20, BACKWARD, MICROSTEP);

delay(100);

for (i=18; i!=0; i--) {
//myStepper2->setSpeed(1000);
//myStepper3->setSpeed(i);
//myMotor1->setSpeed(i);  
//myStepper1->onestep(BACKWARD, INTERLEAVE);
myStepper2->onestep(BACKWARD, DOUBLE);
myStepper3->onestep(BACKWARD, DOUBLE);
//myStepper2->onestep(BACKWARD, MICROSTEP);
//myStepper3->onestep(BACKWARD, MICROSTEP);
delay(100);
}
//myStepper2->step(15, BACKWARD, DOUBLE);
myStepper2->step(35, BACKWARD, MICROSTEP);

delay(100);

//myMotor1->run(BACKWARD);
}
else {
myStepper2->release();
myStepper3->release();
}
}