/* 
ROVPS2Control_Masterv2.ino 
Hamish Trolove - 13 Jan 2015 
www.techmonkeybusiness.com 
This sketch seeks takes control commands from a PS2 handset and transmits the 
commands using Bill Porter's EasyTransfer Library over a 9600 baud serial 
link (100m tether). 

This sketch is designed for an Arduino Nano with only one Serial Port. 

Pin assignments are: 

3.3V output to PS2 red Pin 
External 5V supply to 1Kohm pull up resistors for PS2 and
     to the four digit display. 
Pin D10 to PS2 yellow pin 
Pin D11 to PS2 orange pin 
Pin D12 to PS2 brown pin 
Pin D13 to PS2 blue pin 

A TM1637 based display is used in this development sketch to 
display data back from the ROV. 
  The Display is supplied with 5V from a seperate 5V regulator, 
  CLK is on Pin D8 
  DIO is on Pin D9. 

Pin D2 to LED Camera Photo Trigger Indicator 
Pin D3 to LED Camera Record Indicator 
Pin D4 to LED Main Lights Indicator 

Communications 
Serial Connection: Topside D1 (TX) to ROV D0 (RX) 
Serial Connection: Topside D0 (RX) to ROV D1 (TX) 
Connect the GND on both 

The coding pulls on the PSX library and the EasyTransfer library
developed by Bill Porter.  See www.billporter.info for the latest
from Bill Porter and to download the library. 

The controls for the ROV are; 
Left Stick - X-axis = Roll, Y-axis = Up/down 
Right Stick - X-axis = Yaw, Y-axis = forward/back 
Direction button pad left = LED Main lights On/Off toggle 
Direction button pad up = turn camera upwards 
Direction button pad down = turn camera downwards 
Triangle = Start/Stop video recording 
Circle = Take photo 


*/ 


#include <PS2X_lib.h> // Bill Porter's PS2 Library 
#include <EasyTransfer.h> // Bill Porter's Easy Transfer Library 
#include <TM1637Display.h> 

PS2X ps2x;  //The PS2 Controller Class 
EasyTransfer ETin, ETout;  //Create the two Easy transfer Objects for 
                            // Two way communication 

// TM1637 Display Module connection pins (Digital Pins) 
#define CLK 8 
#define DIO 9 

const int grnLEDpin = 4;  //green LED is on Digital pin 12 
const int redLEDpin = 3;  //red LED is on Digital pin 13. 
const int yelLEDpin = 2;  //yellow LED is on Digital pin 2 

int ForwardVal = 0;  //Value read off the PS2 Right Stick up/down. 
int YawLeftVal = 0;  //Value read off the PS2 Right Stick left/right 
int UpVal = 0; //Value read off the PS2 Left Stick up/down 
int RollLeftVal = 0; // Value read off the PS2 Left Stick left/right 

struct RECEIVE_DATA_STRUCTURE{ 
  int inmsg;  //messages back from the slave. 
}; 

struct SEND_DATA_STRUCTURE{ 
  int upLraw;  //Variables to carry the actual raw data for the ESCs 
  int upRraw; 
  int HLraw; 
  int HRraw; 
  int CamPitch; //Angle of the camera servo. 
  int CamPhotoInd; // a counter for controlling the camera trigger indicator 
  volatile boolean CamRec;  //Camera record function toggle 
  volatile boolean LEDHdlts; //LED headlights on/off toggle 
}; 

//give a name to the group of data 
RECEIVE_DATA_STRUCTURE rxdata; 
SEND_DATA_STRUCTURE txdata; 
TM1637Display display(CLK, DIO); //Set up the display. 

void setup() 
{ 
  ps2x.config_gamepad(13,11,10,12, false, false); 
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) 
  //We have disabled the pressure sensitivity and rumble in this instance and 
  //we know the controller type so we have not bothered with the error checks 
  pinMode(grnLEDpin, OUTPUT);  //Sets the grnLEDpin to output 
  pinMode(redLEDpin, OUTPUT);  //Sets the redLEDpin to output 
  pinMode(yelLEDpin, OUTPUT);  //Sets the yelLEDpin to output. 
  txdata.CamRec = false;  //Sets the Camera default to not recording 
  txdata.CamPitch =90;  //Sets the Camera Pitch to be level 
  
  delay(10000);    //The 10 second delay to allow opportunity to upload new programs. 
  Serial.begin(9600); //Begin Serial to talk to the Slave Arduino 
  ETin.begin(details(rxdata), &Serial); //Get the Easy Transfer Library happening through the Serial 
  ETout.begin(details(txdata), &Serial); 
  display.setBrightness(0x0a); //Set the TM1637 Display brightness 
  
} 

void loop() 
{ 
  ps2x.read_gamepad(); //This needs to be called at least once a second 
                        // to get data from the controller. 
  if(ps2x.Button(PSB_PAD_UP))  //Pressed and held 
  { 
    txdata.CamPitch = txdata.CamPitch + 2; //increase the camera pitch 
  } 
  
  if(ps2x.ButtonPressed(PSB_PAD_LEFT))  //Pressed 
  { 
    txdata.LEDHdlts = !txdata.LEDHdlts; //Toggle the LED light flag 
  } 
        
 
  if(ps2x.Button(PSB_PAD_DOWN))  //Pressed and Held 
  { 
    txdata.CamPitch = txdata.CamPitch - 2; //decrease the camera pitch 
  } 
  txdata.CamPitch = constrain(txdata.CamPitch,20,160);  //Constrain the camera pitch 
     //to within range servo can handle. 


  if(ps2x.ButtonPressed(PSB_GREEN))  //Triangle pressed 
  { 
    txdata.CamRec = !txdata.CamRec; //Toggle the Camera recording Status 
  } 

  if(ps2x.ButtonPressed(PSB_RED))  //Circle pressed 
  { 
    txdata.CamPhotoInd = 70;  //Set to indicate photo taken for about 1 second. 
  } 

//Analogue Stick readings 
  ForwardVal = ps2x.Analog(PSS_RY); 
  YawLeftVal = ps2x.Analog(PSS_RX); 
  UpVal = ps2x.Analog(PSS_LY); 
  RollLeftVal = ps2x.Analog(PSS_LX); 

//Translate the Stick readings to servo instructions 
//Readings from PS2 Controller Sticks are from 0 to 255 
//with the neutral being 128.  The zero positions are to 
//the left for X-axis movements and up for Y-axis movements. 

//Variables to carry the actual raw data for the ESCs 
  txdata.upLraw = (128-UpVal)+(128-RollLeftVal)/2;  //This will be up to a value of 192 
  txdata.upRraw = (128-UpVal)-(128-RollLeftVal)/2;  //This will be up to a value of 192 
  txdata.HLraw = (128-ForwardVal)+(128-YawLeftVal);  //This will be up to a value of 256 
  txdata.HRraw = (128-ForwardVal)-(128-YawLeftVal);  //This will be up to a value of 256 

//Scale the values to be suitable for ESCs and Servos 
// These values will be able to be written directly to the ESCs and Servos 
  txdata.upLraw=map(txdata.upLraw,-193,193,0,179); 
  txdata.upRraw=map(txdata.upRraw,-193,198,0,179); 
  txdata.HLraw=map(txdata.HLraw,-256,256,0,179); 
  txdata.HRraw=map(txdata.HRraw,-256,256,0,179); 



  // Send the message to the serial port for the ROV Arduino  
   ETout.sendData(); 
 
 //Based on Bill Porter's example for the Two Way Easy Transfer Library 
 //We will include a loop here to make sure the receive part of the 
 //process runs smoothly. 
  for(int i=0; i<5; i++){ 
    ETin.receiveData(); 
    // We'll do something properly with the returned data at a later s 
    display.showNumberDec(rxdata.inmsg); // Send the data to the TM1637 Display. 
    //The factor of 10 in the data from the ROV gives a 0.1V precision 
    // unfortunately there is no decimal point to use so the user needs 
    // to imagine it is there. 
    delay(18); 
  } 

// Signalling the probable status of the camera. 

  if(txdata.CamPhotoInd >= 1) 
  { 
    txdata.CamPhotoInd = txdata.CamPhotoInd - 1; //countdown for the CamIndicator 
    digitalWrite(grnLEDpin,HIGH);  //Light the Green LED to indicate phototriggered 
  } 
  if(txdata.CamPhotoInd <= 0) 
  { 
    digitalWrite(grnLEDpin,LOW);  //Photo event indication over. 
  } 
  digitalWrite(redLEDpin,txdata.CamRec); //Light the redLED based on camera recording status flag 
  digitalWrite(yelLEDpin,txdata.LEDHdlts); //Light the LED based on headlights status flag 
  delay(18); 
} 

/* 
ROVPS2Control_Slavev2.ino 
Hamish Trolove - 13 Jan 2015 
www.techmonkeybusiness.com 
This sketch takes commands sent to it from the Master unit with 
the PS2 Controller attached and converts it to motor commands, 
servo commands, light controls etc.  The data is sent from 
the handset (Master) to the ROV(Slave) using Bill Porter's EasyTransfer 
Library over a 9600 baud serial link (100m tether). 

Data sent from the Master are raw settings for the ESC control. 

This sketch is designed for an Arduino Nano with only one Serial Port. 

The pin assignments are; 
D13 = RED LED pin. 
D12 = Headlight Control 
D11 = Jumper pin 
D8 = ESC Vertical Left 
D7 = ESC Vertical Right 
D6 = ESC Horizontal Left 
D5 = ESC Horizontal Right 
D4 = Camera Pitch Servo 
D3 = Video Trigger 
D2 = Photo Trigger 

A5 = Voltage Divider connection 

Communications 
Serial Connection: Topside D1 (TX) to ROV D0 (RX) 
Serial Connection: Topside D0 (RX) to ROV D1 (TX) 
Connect the GND on both 


Please note that the ESCs will all have been programmed by this 
point in the project. 

In this edition, the onboard voltage is sent through the Serial 
back to the Master for display on a 4 digit LED display. 

In the future edition we will also include the ESC calibration sequence. 


*/ 

#include <Servo.h> 
#include <EasyTransfer.h> // Bill Porter's Easy Transfer Library 

EasyTransfer ETin, ETout;  //Create the two Easy transfer Objects for 
                            // Two way communication 

Servo ESCVL;  // Create Servo Object ESC Vertical Left 
Servo ESCVR;  // Create Servo Object ESC Vertical Right 
Servo ESCHL;  // Create Servo Object ESC Horizontal Left 
Servo ESCHR;  // Create Servo Object ESC Horizontal Right 
Servo CamAng; // Create Servo Object for the Camera Pitch Servo. 

const int RedLEDpin = 13; // The indicator LED pin is 13. 
const int HeadLts = 12; // The Headlight Control is on pin 12 
const int CamRecTrig = 3; //Camera video recorder trigger is on pin D3 
const int CamPhotTrig = 2; //Camera photo trigger is on pin D2 

volatile boolean CamRecd;  //Camera record function toggle 
volatile boolean CamPhoto;  //Camera photo function toggle 

const int Voltpin = A5; // analog pin used to read the battery voltage 
int volts;    // variable to read the voltage from the analog pin 
const float RefVolts = 5.0; // 5-Volt board reference voltage on Nano 
const float ResistFactor = 319.68; //Calculated from 1023.0*(R2/(R1 + R2) 
     //where R1 = 2200 ohms and R2 = 1000 ohms for a 15V max voltage. 
                  
struct RECEIVE_DATA_STRUCTURE{ 
  int upLraw;  //Variables to carry the actual raw data for the ESCs 
  int upRraw; 
  int HLraw; 
  int HRraw; 
  int CamPitch; //Angle of the camera servo. 
  int CamPhotoInd; // a counter for controlling the camera trigger indicator 
  volatile boolean CamRec;  //Camera record function toggle 
  volatile boolean LEDHdlts; //LED headlights on/off toggle 
}; 

struct SEND_DATA_STRUCTURE{ 
  int inmsg;  //messages to the Master. 
}; 

//give a name to the group of data 
RECEIVE_DATA_STRUCTURE rxdata; 
SEND_DATA_STRUCTURE txdata; 



void setup() 
{ 
  pinMode(RedLEDpin,OUTPUT); 
  pinMode(HeadLts,OUTPUT); 
  pinMode(CamRecTrig,OUTPUT); 
  pinMode(CamPhotTrig,OUTPUT); 
  ESCVL.attach(8,600,2250); //attach the ESCVL to pin 8 
  ESCVR.attach(7,600,2250); //attach the ESCVR to pin 7 
  ESCHL.attach(6,600,2250); //attach the ESCHL to pin 6 
  ESCHR.attach(5,600,2250); //attach the ESCHR to pin 5 
    //Due to problems with the ESC recognising the maximum 
    //position at the default settings, the figures after 
    //the pin number are the microsecond signals for the 
    //minimum and maximum that the ESC will recognise. 
    // 600 and 2250 work. 
  CamAng.attach(4); //Attach the camera Pitch Servo to pin 4 

//  throttle = 90;  //Set throttle to the neutral position. 
  ESCVL.write(90);  //Set the ESCVL signal to the neutral position. 
  ESCVR.write(90);  //Set the ESCVL signal to the neutral position. 
  ESCHL.write(90);  //Set the ESCVL signal to the neutral position. 
  ESCHR.write(90);  //Set the ESCVL signal to the neutral position. 
  CamAng.write(90); //Set the camera servo pitch to be level. 
  digitalWrite(HeadLts, LOW); //Set the Headlights to Off 
  CamRecd = false;  //Sets the Camera default to not recording 
  CamPhoto = false; // No photos triggered. 
  digitalWrite(RedLEDpin,LOW); 
  digitalWrite(CamRecTrig,HIGH); //Both camera functions are controlled 
  digitalWrite(CamPhotTrig,HIGH); // by making the pin low. 
  
  delay(10000);   //Ten second delay 
    //The ESC should now be initialised and ready to run. 

  
  Serial.begin(9600); //Begin Serial to talk to the Master Arduino 
  ETin.begin(details(rxdata), &Serial); //Get the Easy Transfer Library happening through the Serial 
  ETout.begin(details(txdata), &Serial); 
  
} 

void loop() 
{ 
  // Send the message to the serial port for the ROV Arduino  
  ETout.sendData(); 

 //Based on Bill Porter's example for the Two Way Easy Transfer Library 
 //We will include a loop here to make sure the receive part of the 
 //process runs smoothly. 
  for(int i=0; i<5; i++){ 
    ETin.receiveData(); 
    // We'll do something properly with the returned data at a later s 
    ESCVL.write(rxdata.upLraw);  //Set the ESCVL signal to the defined throttle position. 
    ESCVR.write(rxdata.upRraw);  //Set the ESCVR signal to the defined throttle position. 
    ESCHL.write(rxdata.HLraw);  //Set the ESCHL signal to the defined throttle position. 
    ESCHR.write(rxdata.HRraw);  //Set the ESCHR signal to the defined throttle position. 
    CamAng.write(rxdata.CamPitch); //Set the camera servo pitch to the defined angle. 
    digitalWrite(HeadLts,rxdata.LEDHdlts);//Light the headlights based on the Message data 
    delay(18); 
  } 

//The camera settings are status flags so we will need to trigger the events based on 
//changes in the data. 
  if(rxdata.CamRec != CamRecd)  //If the held value of CamRec is different from the new 
  // value for CamRecd then the function has been triggered. 
  { 
    digitalWrite(CamRecTrig,LOW); //Trip the record toggle. 
    delay(20); 
    digitalWrite(CamRecTrig,HIGH); 
    CamRecd = rxdata.CamRec; //update the flag. 
  } 
  if(rxdata.CamPhotoInd != CamPhoto)  //If the held value of CamRec is different from the new 
  // value for CamRec then the function has been triggered. 
  // Note there may be problems with conflict between the two camera functions.  We 
  //may need to disable this function if video is being recorded. 
  { 
    digitalWrite(CamPhotTrig,LOW); //Trip the photo trigger. 
    delay(20); 
    digitalWrite(CamPhotTrig,HIGH); 
    CamPhoto = rxdata.CamPhotoInd; //update the flag. 
  } 


  delay(18);  //This delay is added to give the ROV a chance to 
                //return data 
  volts = analogRead(Voltpin)/ResistFactor*RefVolts*10;  //Read the voltage 
  //from the battery through the voltage divider.  Factor of 10 used 
  //to help achieve an integer with 0.1V accuracy. 
  txdata.inmsg = volts; //Send back the onboard battery voltage. 
}