/*
Diplomski rad - Sustav za zadržavanje dubine

Created by Andrej Sokolić, 15, May 2015

Program runs Dynamixel AX12+ Servo motor in wheel mode.
It takes reading of AX12 apsolute encoder every 50ms and calculates
value of its own incremental encoder used for positioning piston
in balast tank. Program supports 2 balast tank limitters, upper and
lower.

Program takes readings of pressure and temperature from pressure sensor
Keller series 7LD whenever ready and uses data as input in PID regulator.
Using the PID regulator it calculates the position of piston inside balast tank.

Program displays current and set position of piston inside balast tank, 
current and maximal pressure, current temperature and AX12+ communication
error sum.

Program has primitive user interface which enables the user to set
mission depth and/or change PID parameters as well as stop the motor.
*/

#include <ax12_vol3.h>
#include <SoftwareSerial.h>
#include <TimerThree.h>
#include <I2C.h>
#include <PID_v1.h>

#define EOC 7

// PID variables
double Setpoint, Input, Output;
double P = 0.1;
double I = 0;
double D = 1;
int SampleTime = 1000;
int Offset = 290;

SoftwareSerial mySerial(11, 10); // RX, TX
AX12 ax(200000, 0x0E, false);
PID myPID(&Input, &Output, &Setpoint, P, I, D, DIRECT);

// Motor variables
volatile int kut = 0;
volatile int sector = 0;
volatile int sector_new;
volatile int suma = 0;
volatile int temp;
volatile int counter = 0;
volatile long time_is_now = 0;
byte ANGLE_FLAG = 1;
int Button = 0;
int Button_time;
int error_counter=0;
int Output_int;

// Limiters
long debounceDelay = 50;

long lastDebounceTime_l = 0;
int lastbuttonState_l = HIGH;
int buttonState_l;

long lastDebounceTime_h = 0;
int lastbuttonState_h = LOW;
int buttonState_h;

// Sensor variables
byte EOC_last_state = 1;

int pressure_int = 0;
int temperature_int = 0;

float temperature_float = 0;
float pressure_float = 0;

int current_pressure;
int max_pressure = 0;
int average_pressure = 0;
int pressure_data_ready;

int reference_pressure;

int pressure_buffer[5] = {0, 0, 0, 0, 0};
int pressure_buffer_position = 0;
double average_pressure_double = 0;

// Misc variables
int temp1;
String str = "";
char buf3[4] = {0, 0, 0, 0};
char buf4[5] = {0, 0, 0, 0, 0};

void setup() {
  delay(2000);

  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(EOC, INPUT);
  pinMode(13, OUTPUT);

  mySerial.begin(57600);
  mySerial.println("\n\rSoftware serial!");
  mySerial.println("\n\rPress any key to continue...");
  while (!mySerial.available());
  mySerial.read();
  mySerial.println("Bok!");

  // Pressure sensor initialisation
  I2c.begin();
  I2c.write(0x40,0xAC);
  while(!digitalRead(EOC))
  I2c.read(0x40, 3);
  reference_pressure = get_press();

  // Motor initialisation
  mySerial.println("\n\rReference pressure got. Initializing actuator.");
  mySerial.println("\n\rPlease wait...");
  motor_init();

  mySerial.println("\n\rInitialization complete. Please submerge the probe.");
  mySerial.println("\n\rPress any key to continue...");
  while (!mySerial.available());
  mySerial.read();

  Timer3.initialize(50000); //In microseconds
  Timer3.attachInterrupt(funkcija);
  delay(50);

  temp = 0;

  I2c.write(0x40,0xAC);
  while(!digitalRead(EOC))
  I2c.read(0x40, 3);
  Input = get_press()-reference_pressure;

  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(SampleTime);
  myPID.SetOutputLimits(-30, 30);     // +/- 25 ml of room in balast tank
}

void loop()
{
  // Lower limmiter debunce routine
  int reading_l = digitalRead(5);
  if (reading_l != lastbuttonState_l)
    lastDebounceTime_l = millis();
  if ((millis() - lastDebounceTime_l) > debounceDelay) {
    if (reading_l != buttonState_l) {
      buttonState_l = reading_l;
      if(-suma>=10) {
        ax.set_speed(0);
        suma = 0;
      }
    }
  }
  lastbuttonState_l = reading_l;

  // Upper (high) limmiter debunce routine
  int reading_h = digitalRead(6);
  if (reading_h != lastbuttonState_h)
    lastDebounceTime_h = millis();  
  if ((millis() - lastDebounceTime_h) > debounceDelay) {
    if (reading_h != buttonState_h) {
      buttonState_h = reading_h;
      if(-suma<=650) {
        ax.set_speed(0);
        suma = -660;
      }
    }
  }
  lastbuttonState_h = reading_h;

  // Reading data from the pressure sensor
  if (digitalRead(EOC) && EOC_last_state)
  {
    I2c.write(0x40, 0xAC);
    EOC_last_state = 0;
  }

  if (digitalRead(EOC) && !EOC_last_state)
  {
    I2c.read(0x40, 5);
    pressure_data_ready = 1;
    EOC_last_state = 1;
  }

  // Pressure data averagingn and filtering
  if (pressure_data_ready)
  {
    current_pressure = get_press() - reference_pressure;
    if (current_pressure >= max_pressure)
      max_pressure = current_pressure;

    pressure_buffer[pressure_buffer_position] = current_pressure;
    pressure_buffer_position++;
    if(pressure_buffer_position>4)
      pressure_buffer_position = 0;
    average_pressure = 0;
    for (int i=0;i<5;i++)
    {
      average_pressure += pressure_buffer[i];
    }
    average_pressure = average_pressure/5;  // average
    average_pressure_double = (double) average_pressure;
    //
    //double average_pressure_double = average_pressure/10; // from mm to cm
    Input = average_pressure_double/10;               // Input for PD regulator
    pressure_data_ready = 0;
  }

  // Regulator part
  if (mySerial.available())
  {
    ax.set_speed(0);
    mySerial.readBytes(buf3, 3);
    str = String(buf3);
    if (str == "reg")
    {
      str = "";

      mySerial.print("\n\r");
      mySerial.print("Enter value of P:\n\r");
      while (!mySerial.available());
      mySerial.readBytes(buf4, 4);
      str = String(buf4);
      P = str.toFloat();
      mySerial.print("P set to: ");
      mySerial.print(P,3);
      mySerial.print("\n\r");
      str = "";

      mySerial.print("Enter value of I:\n\r");
      while (!mySerial.available());
      mySerial.readBytes(buf4, 4);
      str = String(buf4);
      I = str.toFloat();
      mySerial.print("I set to: ");
      mySerial.print(I,3);
      mySerial.print("\n\r");
      str = "";
      
      mySerial.print("Enter value of D:\n\r");
      while (!mySerial.available());
      mySerial.readBytes(buf4, 4);
      str = String(buf4);
      D = str.toFloat();
      mySerial.print("D set to: ");
      mySerial.print(D,3);
      mySerial.print("\n\r");
      str = "";

      myPID.SetSampleTime(SampleTime);
      myPID.SetTunings(P,I,D);
    }

    else if (str == "sam")
    {
      str = "";
      mySerial.print("\n\r");
      mySerial.print("Enter value of SampleTime in ms:\n\r");
      while (!mySerial.available());
      mySerial.readBytes(buf4, 4);
      str = String(buf4);
      SampleTime = str.toInt();
      mySerial.print("SampleTime set to: ");
      mySerial.print(SampleTime);
      mySerial.print("\n\r");
      str = "";
    }

    else if (str == "off")
    {
      str = "";
      mySerial.print("\n\r");
      mySerial.print("Enter value of Offset:\n\r");
      while (!mySerial.available());
      mySerial.readBytes(buf3, 3);
      str = String(buf3);
      Offset = str.toInt();
      mySerial.print("Offset set to: ");
      mySerial.print(Offset);
      mySerial.print("\n\r");
      str = "";
    }

    else if(str == "stp")
    {
      str = "";
      mySerial.print("\n\r");
      mySerial.print("P set to ");
      mySerial.print(myPID.GetKp(),3);
      mySerial.print(",\n\rI set to ");
      mySerial.print(myPID.GetKi(),3);
      mySerial.print(",\n\rD set to ");
      mySerial.print(myPID.GetKd(),3);
      mySerial.print(",\n\rSampleTime set to ");
      mySerial.print(SampleTime);
      mySerial.print(",\n\rOffset set to ");
      mySerial.print(Offset);
      mySerial.print(".\n\r");
      mySerial.print("Press any key to continue...\n\r");
      while (!mySerial.available());
      mySerial.read();
    }
    else if(str == "cal")
    {
      str = "";
      mySerial.print("\n\r");
      mySerial.print("Calibrating...");
      calibrate();
      myPID.ITerm = 0;  
    }
    else if(str == "rst")
    {
      str = "";
      mySerial.print("\n\r");
      mySerial.print("Reseting PID...");
      myPID.ITerm = 0;
    }
    else
    {
      temp = str.toInt();
      Setpoint = temp;
      str = "";
    }
  }


  if (myPID.Compute())
  {
    pressure_data_ready = 0;
    mySerial.print("\t");
    mySerial.print(Input);
    mySerial.print("\t");  

    Output_int = (int) Output*13.2+Offset; 
    mySerial.print(Output_int);
    mySerial.print("\t");

    mySerial.print(Setpoint);
    mySerial.print("\t");

    mySerial.print(myPID.kp*myPID.error);
    mySerial.print("\t");

    mySerial.print(myPID.ITerm);
    mySerial.print("\t");

    mySerial.print(myPID.kd*myPID.dInput);
    mySerial.print("\t");

    mySerial.print(millis());
  }

  temp1 = go_to(-Output_int);
  
  if (counter >= 3)
  {
    mySerial.print("\r\n");
    counter = 0;
    //mySerial.print(kut);
    //mySerial.print("\t");
    //mySerial.print(sector);
    //mySerial.print("\t");
    mySerial.print(suma);
    mySerial.print("\t");
    mySerial.print(temp1);
    mySerial.print("\t");
    mySerial.print(current_pressure);
    //mySerial.print(" mm");
    mySerial.print("\t");
    mySerial.print(average_pressure);
    //mySerial.print(" cm");
    mySerial.print("\t");
    mySerial.print(max_pressure);
    //mySerial.print(" mm");
    mySerial.print("\t");
    mySerial.print(get_temp());
    //mySerial.print(" °C");
    mySerial.print("\t");
    mySerial.print(error_counter);
    mySerial.print("\t");
    mySerial.print(millis());
  }
}

/***********************************************************/
// INTERRUPT FUNCTION
/***********************************************************/

// Function is called every 50 ms on TIMER3 interrupt.
// It calculates the current sector the motor is in and how many it has passed.
// Each sector is 60°.

void funkcija(void)
{
  // Arduino doesn't support nested interrupts, sei() enables interrupts.
  sei();                                                
  while (ANGLE_FLAG)
  {
    //ax.readData(PRESENT_POSITION, 2);
    //delayMicroseconds(1000);
    //mySerial.print(ax.readData(PRESENT_POSITION, 2), DEC);
    //mySerial.print(" ");
    //kut = ax.status_data;
    kut = ax.get_position();
    //delay(1);
    if (kut != -1)
      ANGLE_FLAG = 0;
    else
      error_counter ++;
  }
  ANGLE_FLAG = 1;
  sector_new = get_sector(sector, kut);
  cli();

  if ((sector_new == 1) && (sector == 6))
    suma += 1;
  else if ((sector_new == 6) && (sector == 1))
    suma -= 1;
  else
    suma += (sector_new - sector);
  sector = sector_new;

  counter++;
}

/***********************************************************/
// MOTOR FUNCTIONS
/***********************************************************/

// Function for incremental encoder. It has 6 sectors derived from 300° degrees of AX12 apsolute encoder.
// Input is last known sector and current angle

  byte get_sector (byte sector_old, int angle)
  {
  // Kvadant 1 ->   0°- 60°
    if (((angle > 0) && (angle <= 205 )) && ((sector_old == 6) || (sector_old == 2)))
    {
      return 1;
    }

  // Kvadant 2 ->  60°-120°
    if (((angle > 205) && (angle <= 410 )) && ((sector_old == 1) || (sector_old == 3)))
    {
      return 2;
    }

  // Kvadant 3 -> 120°-180°
    if (((angle > 410) && (angle <= 615 ))  && ((sector_old == 2) || (sector_old == 4)))
    {
      return 3;
    }

  // Kvadant 4 -> 180°-240°
    if (((angle > 615) && (angle <= 820 )) && ((sector_old == 3) || (sector_old == 5)))
    {
      return 4;
    }

  // Kvadant 5 -> 240°-300°
    if (((angle > 820) && (angle < 1023)) && ((sector_old == 4) || (sector_old == 6)))
    {
      return 5;
    }

  // Kvadant 6 -> 300°-360°
    if (((angle == 1023) || (angle == 0)) && ((sector_old == 5) || (sector_old == 1)))
    {
      return 6;
    }

  // If none of the above conditions are met the sector didnt change
    return sector_old;
  }

// AX12 init function
// Positions the balast tank to its bouyant state and sets a referent position

  void motor_init()
  {
    // If the motor was moving stop it.
    ax.set_speed(0);
    ax.setEndlessTurnMode(1);

    calibrate();
}

// Function tells the motor to wich position to go.
// Every 6 positions motor makes a full circle.
// Every circle is equal to 5/110 ml (0.4545 ml)

int go_to(int where)
{
  int speed;
  int e;

  // Bounds restriction
  if (where <= -660)
    where = -660;
  else if (where >= 0)
    where = 0;

  e = abs(suma - where);
  if (e > 5)
    speed = 1023;
  else
  {
    speed = e * 200;
  }

  if (suma > where)
    ax.set_speed(-speed);
  if (suma < where)
    ax.set_speed(speed);
  if (suma == where)
    ax.set_speed(0);

  return where;
}

// Function for calibrating balast tank position
// Balast tank piston goes to empty state.

void calibrate()
{
  if (!button_pressed(5))
  {
    ax.set_speed(1023);
    while (!button_pressed(5))
    {
      if (mySerial.available())
      {
        ax.set_speed(0);
        break;
      }
    }
    mySerial.read();
    ax.set_speed(0);
  }
  ax.setEndlessTurnMode(0);
  delay(1);
  ax.writeInfo(MOVING_SPEED, 150);
  ax.writeInfo(GOAL_POSITION, 0x1FF);
  delay(1000);
  ax.setEndlessTurnMode(1);
  delay(1);
  sector = 3;

  suma = 0;
  temp = 0;
  lastbuttonState_l = HIGH;
  error_counter = 0;
}

/***********************************************************/
// PRESSURE SENSOR FUNCTIONS
/***********************************************************/

// Function for calculating pressure
// Function returns hPa (mBar) witch is equal to cm underwater.

int get_press()
{
  pressure_int = I2c.data[1] << 8;
  pressure_int += I2c.data[2];
  pressure_float = (pressure_int - 16384) * 10;
  pressure_float = pressure_float / 32768;
  pressure_float = pressure_float * 10000;
  pressure_int = (int) pressure_float;
  return pressure_int;
}

// Function for calculating temperature
// Function returns °C

float get_temp()
{
  temperature_int = I2c.data[3] << 8;
  temperature_int += I2c.data[4];
  temperature_float = ((temperature_int >> 4) - 24) * 0.05 - 50;
  return temperature_float;
}

/***********************************************************/
// MISC FUNCTIONS
/***********************************************************/

void test(int broj)
{
  mySerial.print("Tu sam, ");
  mySerial.print(broj);
  mySerial.print("\n\r");
}

byte button_pressed(int button)
{
  if (digitalRead(button))
  {
    delay(50);
    if (digitalRead(button))
      return 1;
  }
  else
    return 0;
}