/*
   -- submarine --
   
  This code is used to control a submarine using the RemoteXY smartphone app.
  Joystick1 on the app controls the propulsion and the rudder.
  Joystick 2 on the app controls the peristaltic pump and the solenoids used to submerge or emerge.
  The battery voltage is measured and shown in the app. If the voltage is too low the propulsion and pump stops and the springs in the tanks press the water out of them to emerge.

   To compile this code using RemoteXY library 4.1.4 or later version 
   download by link http://remotexy.com/en/library/
   To connect using RemoteXY mobile app by link http://remotexy.com/en/download/                   
     - for ANDROID 4.16.02 or later version;
     - for iOS 1.13.1 or later version;
    
   To interact with the GUI, please refer to the manual: 
   https://remotexy.com/ru/help/code/interaction/

   This source code was automatically generated by the RemoteXY editor and 
   is an example for the RemoteXY library. 
   Licensed under the MIT License. See the LICENSE file in the RemoteXY library
   root (https://github.com/RemoteXY/RemoteXY-Arduino-library) for full license 
   details.             
*/


//RemoteXY definitions
//#define REMOTEXY__DEBUGLOG  // you can enable debug logging to Serial at 115200  
#define REMOTEXY_MODE__HARDSERIAL // RemoteXY select connection mode and include library 
#define REMOTEXY_SERIAL Serial // RemoteXY connection settings 
#define REMOTEXY_SERIAL_SPEED 9600 // RemoteXY connection settings

//Loading libraries
#include <RemoteXY.h> //Loading RemoteXY library for smartphone control
#include <Servo.h> //Loading Servo library to control the rudder servo

//Definitions
#define valve 2 //Solenoid valves are controled by digital pin 2
#define ENABLE_Drive 3 //Propulsion is enabled by digital pin 3
#define DIRA_Drive A1 //Propulsion direction is controled by analog pins A1 and A2
#define DIRB_Drive A2 //Propulsion direction is controled by analog pins A1 and A2
#define ENABLE_Submerge 5 //Pump is enabled by digital pin 5
#define DIRA_Submerge A4 //Analog pins A4 and A5 control the rotation direction of the pump and therefore if the sub submerges or emerges
#define DIRB_Submerge A5 //Analog pins A4 and A5 control the rotation direction of the pump and therefore if the sub submerges or emerges
#define battery A6 //Battery voltage is measured using analog pin 6

//Declaration and initialisation of variables
Servo RudderServo; //Servo object to control the rudder servo
float u_battery = 0.0; //battery voltage in V
bool discharged = false; //variable which turns true if the battery voltage was too low once an the battery is considered as discharged
int drive_velocity = 0; //set propulsion power
int submerge_velocity = 0; //set power for the pump to submerge or emerge
int rudder = 90; //rudder position, 90 is the middle posion in degree
int upper_rudder_limit = 145; //limit the rudder rotation to prevent damage to 35 to 145 degree
int lower_rudder_limit = 35; //limit the rudder rotation to prevent damage to 35 to 145 degree
float battery_calibration_factor = 0.909; //calibration faktor=true voltage/voltage measuerd by the arduino

// RemoteXY GUI configuration. Writen automatically from the RemoteXY Web-editor 
#pragma pack(push, 1)  
uint8_t RemoteXY_CONF[] =   // 66 bytes
  { 255,4,0,4,0,59,0,19,0,0,0,0,31,1,106,200,1,1,4,0,
  5,9,106,88,88,32,2,26,31,67,61,95,40,10,78,2,26,2,129,5,
  96,40,8,64,8,86,111,108,116,97,103,101,32,47,32,86,0,5,10,6,
  87,87,32,2,26,31 };
  
// this structure defines all the variables and events of your control interface 
struct {

    // input variables
  int8_t joystick_01_x; // Joystick value from smartphone for rudder from -100 to 100
  int8_t joystick_01_y; // Joystick value from smartphone for propulsion from -100 to 100
  int8_t joystick_02_x; // Joystick value from smartphone, which is not used
  int8_t joystick_02_y; // Joystick value from smartphone for pump from -100 to 100

    // output variables
  float value_01; //battery voltage in V which is displayed on the smartphone

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

} RemoteXY;   
#pragma pack(pop)
 
/////////////////////////////////////////////
//           END RemoteXY include          //
/////////////////////////////////////////////


//This part of the code is runned once for initialization/ setup
void setup() 
{
  RemoteXY_Init (); //initialise remoteXY for smartphone control
  pinMode(valve,OUTPUT); //set pinmode of pins for the valves to output
  pinMode(ENABLE_Drive,OUTPUT); //set pinmode of pin for the propulsion power to output
  pinMode(DIRA_Drive,OUTPUT); //set pinmode of pin for propulsion direction to output
  pinMode(DIRB_Drive,OUTPUT); //set pinmode of pin for propulsion direction to output
  pinMode(ENABLE_Submerge,OUTPUT); //set pinmode of pin for the pump power to output
  pinMode(DIRA_Submerge,OUTPUT); //set pinmode of pin for the pump direction to output
  pinMode(DIRB_Submerge,OUTPUT); //set pinmode of pin for the pump direction to output
  pinMode(battery,INPUT); //set pinmode of pin for the battery voltage to input
  RudderServo.attach(6); //attach control pin D6 to the servo object
  RudderServo.write(90);// move servos to center position -> 90°
  Serial.begin(9600); //start serial communication with a baud rate of 9600
}

//This part of the code is runned repeatedly during the operation
void loop() 
{ 
  RemoteXY_Handler ();

  u_battery=analogRead(battery)*battery_calibration_factor*15.0/1023.0; //measurement of the voltage of the voltage divider, correction with a calibration factor and estimation of the battery pack voltage
  RemoteXY.value_01=u_battery; //send battery voltage to the smartphone
  
  //If the battery is not discharged, propulsion and pump are allowed to operate
  if(u_battery > 9.9 && discharged == false){
    
    //Propulsion
    //If the joystick is pushed in one direction, the drive direction of the propulsion is set accordingly
    if(RemoteXY.joystick_01_y<0){
      digitalWrite(DIRA_Drive,LOW); 
      digitalWrite(DIRB_Drive,HIGH);
    }
    //If the joystick is pushed in the other direction, the drive direction of the propulsion is set accordingly
    if(RemoteXY.joystick_01_y>=0){
      digitalWrite(DIRA_Drive,HIGH); 
      digitalWrite(DIRB_Drive,LOW);
    }
    //Calculate drive velocity/ power using the absolute value of the joystick reading and a conversion of the joystick scale to the output pin range of 2.55
    drive_velocity=abs(RemoteXY.joystick_01_y)*2.55;
    //Limit propulsion to a physically possible range
    if(drive_velocity>255){
      drive_velocity=255;
    }
    if(drive_velocity<0){
      drive_velocity=0;
    }
    analogWrite(ENABLE_Drive,drive_velocity); //power the propulsion
    
    //Submerge or emerge using the pump and the valves
    //If the joystick is pushed in one direction, the drive direction of the pump is set accordingly
    if(RemoteXY.joystick_02_y<=0){
      digitalWrite(DIRA_Submerge,LOW); 
      digitalWrite(DIRB_Submerge,HIGH);
    }
    //If the joystick is pushed in the other direction, the drive direction of the propulsion is set accordingly
    if(RemoteXY.joystick_02_y>0){
      digitalWrite(DIRA_Submerge,HIGH); 
      digitalWrite(DIRB_Submerge,LOW);
    }
    //Calculate submerge velocity/ pump power using the absolute value of the joystick reading and a conversion of the joystick scale to the output pin range of 3*2.55
    submerge_velocity=abs(RemoteXY.joystick_02_y)*3*2.55;
    //Limit pump to a physically possible range
    if(submerge_velocity>255){
      submerge_velocity=255;
    }
    if(submerge_velocity<0){
      submerge_velocity=0;
    }
    //Close the solenoid valves if the pump is not working
    if(submerge_velocity==0){
      digitalWrite(valve,HIGH);
    }
    //Open the valves if the pump is working
    else{
      digitalWrite(valve,LOW);
    }
    RemoteXY_delay(5); //wait 5 ms for the valves to fully open before activating the pump
    analogWrite(ENABLE_Submerge,submerge_velocity); //activate the pump with the calculated power
  }
  //if the battery is discharged, stop the propulsion and pump and open the solenoid valves to emergency resurface
  else{
    digitalWrite(DIRA_Drive,LOW); 
    digitalWrite(DIRB_Drive,LOW);
    analogWrite(ENABLE_Drive,0);
    digitalWrite(DIRA_Submerge,LOW); 
    digitalWrite(DIRB_Submerge,LOW);
    analogWrite(ENABLE_Submerge,0); 
    digitalWrite(valve,LOW);
    discharged = true;
  }
  
  //rudder
  rudder=(lower_rudder_limit+upper_rudder_limit)/2+(upper_rudder_limit-lower_rudder_limit)*RemoteXY.joystick_01_x/200; //calculate the rudder value using the joystick value and a conversion in the according range
  //limit the rudder value to a physically possible range
  if(rudder>upper_rudder_limit){
    rudder=upper_rudder_limit;
  }
  if(rudder<lower_rudder_limit){
    rudder=lower_rudder_limit;
  }
  RudderServo.write(rudder); //activate the rudder
}
