//=====================================================================================================
// SimpleRobot.cpp
//=====================================================================================================
//
// This code supports a simple self-balancing robot based on the the Arduino 101 board
// It uses the Keyestudio Motor shield and 2 DC geared motors for positioning (12V, 266 RPM with encoders)
//
// Date      Author          Notes
// 03/17/2016 LCross        Initial release
//
//---------------------------------------------------------------------------------------------------

#ifndef SimpleRobot_h
#define SimpleRobot_h
#include <Arduino.h>

//----------------------------------------------------------------------------------------------------
// Defines

// PID controller Constants
#define BALANCE_KP 20.4                   // Balance PID (Complementary filter) Constants
#define BALANCE_KI 0.02
#define BALANCE_KD -0.7
#define BALANCE_PID_MIN -200              // Balance PID range
#define BALANCE_PID_MAX 200
#define SPEED_KP 8.6                      // Speed PID Constants
#define SPEED_KI 0.1
#define SPEED_KD 0.2
#define SPEED_PID_MIN -90                 // Speed PID range
#define SPEED_PID_MAX 90
#define PID_P_INC 0.1                     // inc/dec value for PID tuning
#define PID_I_INC 0.01                     // inc/dec value for PID tuning
#define PID_D_INC 0.1                     // inc/dec value for PID tuning

// Timer Definitions
#define BALANCEPIDTIMER 5                // Interval for balance PID eval in mS
#define NAVIGATEPIDTIMER 20              // Interval for speed/steer/turn PID eval in mS
#define IMUTIMER 5                       // Interval to check IMU data in mS
#define HEARTBEATTIMER 1000              // Interval for heart beat indicator in mS
#define DATALOGTIMER 5                   // Interval for data logging in mS
#define MONITORTIMER 200                 // Interval for robot data monitoring on Android App in mS
 
// IMU Definitions
#define IMU_SETTLE 1000                   // ms to allow IMU before calibration
#define ROLLERR 0.5                       // Error in pitch from IMU/robot construction
#define ROLLERR_INC 0.1                   // inc/dec value for roll error calibration
#define UPRIGHT 4                         // Angle, in deg, to indicate robot is upright, and balancing can begin
#define OFFBALANCE 30                     // Angle, in deg, beyond which attempt to recover balance is futile
#define GYRO_CONV 0.01525879              // Convert Gryo output to deg/sec : 250 deg/sec @ 32768
#define ACC_CONV  0.000061035             // Convert Accelerometer output to g : 2g @ 32768
#define AY_OFFSET 224                     // Offset used for ay in complementary filter
#define GX_OFFSET -3                      // Offset used for gx in complementary filter
#define CF_A 0.970                        // Complementary Filter coefficient A  : tau = A*dt/(1-A)
#define M_PI 3.14159265359                // PI

// Arduino Pin Mapping
#define MOTOR_A_DIR 12                    // Motor shield pin mapping
#define MOTOR_A_PWM 3
#define MOTOR_A_ENC_A 2
#define MOTOR_A_ENC_B 4
#define MOTOR_B_DIR 13
#define MOTOR_B_PWM 9
#define MOTOR_B_ENC_A 7
#define MOTOR_B_ENC_B 8
#define LED_HEARTBEAT 5                   // LED pin mapping
#define PIN_DEBUG 10                      // Used for debugging
#define VBATTERY A2
           
// Motor Misc
#define MOTORSLACK 28                     // Compensate for motor slack range (low PWM values which result in no motor engagement)
#define MOTOR_A_SLACKMOD -2               // Motor A slack calibration value
#define MOTOR_B_SLACKMOD 0                // Motor A slack calibration value
#define MOTORSLACK_INC 1                  // Motor slack inc/dec value for slack compensation tuning
#define MOTOR_PWM_MAX 255                 // Max PWM value
#define MOTOR_A_SCALE 255                 // Motor A calibration value
#define MOTOR_B_SCALE 240                 // Motor B calibration value
#define CM_PER_ENC 3.7436                 // cm per encoder tick (35.56 cm circumference wheel. 955 counts per rev) x 100

// Robot FSM states
#define RESET 0
#define STANDBY 1
#define BALANCE 2

// Misc defines
#define ROBOTMAXSPEED 8                    // Maximum robot speed requested from BT

// Debug switches
//#define OUTPUT_READABLE_YAWPITCHROLL          // Debug print switch
//#define OUTPUT_READABLE_YAWPITCHROLL_HB       // Debug print switch
//#define OUTPUT_PID_DEBUG                      // PID internals print switch
//#define OUTPUT_DEBUG_PIN_BALANCE              // Enable toggle of debug pin to monitor internal timing
//#define OUTPUT_BT_MPU                         // Send MPU data to BT on regular interval

//----------------------------------------------------------------------------------------------------
// PID Controller Class
class PropIntDiff {

// Variable declarations
    public:                 
        volatile float ITerm, lastInput, dInput, Output;
        volatile float kp, ki, kd;
        volatile float error, MIN, MAX, ITermThreshold;
        bool inAuto;    

// Function prototypes
             PropIntDiff(float MINlimit, float MAXlimit, float Threshold);
        void initialize();
        void calculate(float Setpoint, float Input);
        void calculate(float Setpoint, float Input, float dInput);
};


//----------------------------------------------------------------------------------------------------
// Motor Control Class
class MotorEncoderControl {
// Variable declarations
    public:  
        volatile int enc_counta;
        volatile int enc_countb;
        float wheelAspeed, wheelBspeed, robotSpeed, robotDistance;
        int MotorSlack;

// Function prototypes
            MotorEncoderControl();
       void setPWM(int *MotorAPWM, int *MotorBPWM);
       void calcSpeed();
};

//----------------------------------------------------------------------------------------------------
// Bluetooth Function prototypes
void btSendPID(PropIntDiff functionPID);
void btBatteryVoltage(float voltage);
void btFirmware(float fw_version);
void btRollError();
void btMotorSlack();
void btHeading(float heading);
void btPitch(float pitch);
void btSpeed(float robotSpeed);
void btComm ();

// PID Multiplier Management function prototypes - not members of the PID class
void setPIDmulti(int PIDindex, float kp, float ki, float kd);
void setPIDmulti(int PIDindex);
#endif