/*
    NCB_wobble_board_controller.ino
    Author: Andrew Williams

    Acquires data from inertial measurement unit and user interface;
    Actuates vibrotactile elements and transmits data to user interface

    Tested with "Arduino MEGA" connected to "x-IMU" and "Lilypad Vibe Boards"
*/

//------------------------------------------------------------------------------
// Libraries

#include "Quaternion.h"
#include "XimuReceiver.h"
#include "elapsedMillis.h"

//------------------------------------------------------------------------------
// Variables

XimuReceiver ximuReceiver;

float pitch_calib=0.19;
float roll_calib=-0.06;

int lillypad_1=31;
int lillypad_2=32;
int lillypad_3=33;
int lillypad_4=34;
int lillypad_5=35;
int lillypad_6=36;
int lillypad_7=37;
int lillypad_8=38;
int daq_pin=2;

float roll_tol = 1.5;
float pitch_tol = 1.5;

int feedback_toggle = 0;
int pitch_tol_one = 3;
int pitch_tol_tenth = 0;
int roll_tol_one = 2;
int roll_tol_tenth = 5;

float roll=0.0;
float oldRoll = 0.0;
float oldoldRoll = 0.0;
float rollSpeed = 0.0;
float rollPD = 0.0;
float pitch=0.0;
float oldPitch = 0.0;
float oldoldPitch = 0.0;
float pitchSpeed = 0.0;
float pitchPD = 0.0;

float front=0.0;
float back=0.0;
float left=0.0;
float right=0.0;

float position_weight=1;
float velocity_weight=0.5;

unsigned long start = 0;
unsigned long finish = 0;
unsigned long delta = 0;
elapsedMillis timeElapsed;

//------------------------------------------------------------------------------
// Functions

void setup() {
    Serial.begin(115200);   // for sending data to computer via USB
    Serial1.begin(115200);  // for receiving data from x-IMU
   pinMode((lillypad_1),OUTPUT); 
   pinMode((lillypad_2),OUTPUT);
   pinMode((lillypad_3),OUTPUT);
   pinMode((lillypad_4),OUTPUT);
   pinMode((lillypad_5),OUTPUT); 
   pinMode((lillypad_6),OUTPUT); 
   pinMode((lillypad_7),OUTPUT); 
   pinMode((lillypad_8),OUTPUT); 
}

void loop() {

    ErrorCode e = ERR_NO_ERROR;

    // Process recieved data
    while(Serial1.available() > 0) {
        e = ximuReceiver.processNewChar(Serial1.read());
    }

    // read feedback settings from user interface
    if ( Serial.available() ){
    feedback_toggle = Serial.parseInt();
    pitch_tol = Serial.parseInt();
    roll_tol = Serial.parseInt();
    pitch_tol=pitch_tol/10;
    roll_tol=roll_tol/10;
    }

    // Print quaternion data as Euler angles
    if(ximuReceiver.isQuaternionGetReady()) {
        QuaternionStruct quaternionStruct = ximuReceiver.getQuaternion();
        Quaternion quaternion = Quaternion(quaternionStruct.w, quaternionStruct.x, quaternionStruct.y, quaternionStruct.z);
        EulerAnglesStruct eulerAnglesStruct = quaternion.getEulerAngles();

        Serial.print(feedback_toggle);
        Serial.print("\t");
        Serial.print(pitch_calib);
        Serial.print("\t");
        Serial.print(roll_calib);
        Serial.print("\t");
        Serial.print(pitch_tol);
        Serial.print("\t");
        Serial.print(roll_tol);
        Serial.print("\t");
        
        // get elapsed time between loops (sampling rate)
        finish = millis();
        delta=finish-start;
        start=finish;
        Serial.print(timeElapsed); 
        Serial.print("\t");

        // IMU defines roll, pitch in reverse of wobble board

        pitch=eulerAnglesStruct.roll-pitch_calib;
        roll=eulerAnglesStruct.pitch-roll_calib;


        // calculate pitch speed
        pitchSpeed=(oldoldPitch-2*oldPitch+pitch)*1000/(delta);
        oldPitch = (pitch);
        oldoldPitch = oldPitch;
        Serial.print(pitch);
        Serial.print("\t"); 
        pitchPD = position_weight*pitch + velocity_weight*pitchSpeed;
        Serial.print(pitchPD);
        Serial.print("\t");     
                   
        // calculate roll speed based on two previous readings
        rollSpeed=(oldoldRoll-2*oldRoll+roll)*1000/(delta);
        oldRoll = (roll);
        oldoldRoll = oldRoll;
        Serial.print(roll);
        Serial.print("\t"); ;
        rollPD = position_weight*roll + velocity_weight*rollSpeed;
        Serial.print(rollPD);
        Serial.print("\t"); 
        
        // print weights
        Serial.print(position_weight);
        Serial.print("\t"); 
        Serial.print(velocity_weight);
        Serial.print("\t"); 
                
        if(pitchPD>(+pitch_tol)) 
        {
          digitalWrite(lillypad_8,feedback_toggle);
          digitalWrite(lillypad_1,feedback_toggle);
          digitalWrite(lillypad_2,feedback_toggle);
          front = 1;
        }
        else{
          front=0;
        }
        
        if(pitchPD<(-pitch_tol)) // excessive backwards pitch
        {
          digitalWrite(lillypad_4,feedback_toggle);
          digitalWrite(lillypad_5,feedback_toggle);
          digitalWrite(lillypad_6,feedback_toggle);
          back=1;
        }
        else{
          back=0;
        }

        if(rollPD>(+roll_tol)) // excessive left roll
        {
          digitalWrite(lillypad_2,feedback_toggle);
          digitalWrite(lillypad_3,feedback_toggle);
          digitalWrite(lillypad_4,feedback_toggle);
          left=1;
        }
        else{
          left=0;
        }

        if(rollPD<(-roll_tol)) // excessive right roll
        {
          digitalWrite(lillypad_6,feedback_toggle);
          digitalWrite(lillypad_7,feedback_toggle);
          digitalWrite(lillypad_8,feedback_toggle);
          right=1;
        }
        else{
          right=0;
        }

        // print status of vibrators
        Serial.print(front);
        Serial.print("\t"); 
        Serial.print(back);
        Serial.print("\t"); 
        Serial.print(left);
        Serial.print("\t"); 
        Serial.print(right);
        Serial.print("\r"); 

        
        // turn off vibrators below threshold or if feedback_toggle is turned off

        
        if(roll<=(+roll_tol) && pitch>=(-pitch_tol))
        {
          digitalWrite(lillypad_3,LOW);
          digitalWrite(lillypad_4,LOW);
          digitalWrite(lillypad_5,LOW);
        }

        if(pitch<=(+pitch_tol) && roll>=(-roll_tol))
        {
          digitalWrite(lillypad_1,LOW);
          digitalWrite(lillypad_7,LOW);
          digitalWrite(lillypad_8,LOW);
        }

        if(pitch<=(+pitch_tol) && roll<=(+roll_tol))
        {
          digitalWrite(lillypad_1,LOW);
          digitalWrite(lillypad_2,LOW);
          digitalWrite(lillypad_3,LOW);
        }

        if(roll>=(-roll_tol) && pitch>=(-pitch_tol))
        {
          digitalWrite(lillypad_5,LOW);
          digitalWrite(lillypad_6,LOW);
          digitalWrite(lillypad_7,LOW);
        }
    }
}

//------------------------------------------------------------------------------
// End of file