////////////////////////////////////////////// 
//        RemoteXY include library          // 
////////////////////////////////////////////// 

// RemoteXY select connection mode and include library  
#define REMOTEXY_MODE__SOFTSERIAL
#include <SoftwareSerial.h> 

#include <RemoteXY.h> 

// RemoteXY connection settings  
#define REMOTEXY_SERIAL_RX 3 
#define REMOTEXY_SERIAL_TX 2 
#define REMOTEXY_SERIAL_SPEED 9600 


// RemoteXY configurate   
#pragma pack(push, 1) 
uint8_t RemoteXY_CONF[] = 
  { 255,5,0,0,0,29,0,8,13,0,
  5,32,7,15,26,26,2,26,31,5,
  32,65,15,26,26,2,26,31,4,128,
  28,49,47,6,2,26 }; 
   
// this structure defines all the variables of your control interface  
struct { 

    // input variable
  int8_t joystick_1_x; // =-100..100 x-coordinate joystick position 
  int8_t joystick_1_y; // =-100..100 y-coordinate joystick position 
  int8_t joystick_2_x; // =-100..100 x-coordinate joystick position 
  int8_t joystick_2_y; // =-100..100 y-coordinate joystick position 
  int8_t slider_1; // =0..100 slider position 

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 

} RemoteXY; 
#pragma pack(pop) 

///////////////////////////////////////////// 
//           END RemoteXY include          // 
///////////////////////////////////////////// 

//////////////////////////////////////////////
//         START Motor Shield code          //
//////////////////////////////////////////////

// Include for Adafruit Motor Shield
#include <Wire.h>
#include <Adafruit_MotorShield.h>

#include "utility/Adafruit_MS_PWMServoDriver.h"

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 

// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
Adafruit_DCMotor *motor3 = AFMS.getMotor(3);
Adafruit_DCMotor *motor4 = AFMS.getMotor(4);

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


//////////////////////////////////////////////
//          END Motor Shield code           //
//////////////////////////////////////////////

///////////////////////////////////////////// 
//           START Servo include           // 
///////////////////////////////////////////// 

#include <Servo.h>
Servo myservo;

//////////////////////////////////////////// 
//           END Servo include             // 
///////////////////////////////////////////// 

void setup()  
{ 
  RemoteXY_Init ();  
  Serial.begin(9600);
  AFMS.begin();  // create with the default frequency 1.6KHz

   myservo.attach(9);
  
   
} 

void loop()  
{  
  RemoteXY_Handler (); 
   
    //Left Wheel to Motor 1 (CCW)
    runMotor(motor1,(map(RemoteXY.joystick_1_y,-100,100,255,-255)));
    //Right Wheel to Motor 3 (CW)
    runMotor(motor3,(map(RemoteXY.joystick_2_y,-100,100,-255,255)));

    //Slider Might be used to control a third motor, a servo, or another output.  Comment/uncomment the following lines accordingly

    //Slider to Motor 2 (CW)
     runMotor(motor2,(map(RemoteXY.slider_1,0,100,-255,255)));


    /*
     * 
    //Slider to Servo
    myservo.write(int(map(RemoteXY.slider_1,0,100,0,180)));

    */


  

}