File content as of revision 2:761e3c932ce0:

/**
 * @author Akash Vibhute 
 * < akash . roboticist [at] gmail . com >
 *
 * @section LICENSE
 *
 * Copyright (c) 2015 Akash Vibhute 
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * @section DESCRIPTION
 *
 * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
 * Robot controller software for SUTD Virgo version 3 robot
 * 
 */

/** 
 * Header file for including all necessary functions for robot and defining any
 * custom functions written in the main file.
 */
#include "main.h"


//** General Functions ************************************************************************
DigitalOut debugLED(debug_LED);
Serial debug(uart_TX, uart_RX);

DigitalOut cameraEN(cam_EN);


void Heartbeat_loop(void const *n); //heartbeat loop as a seperate thread

//---------------------------------------------------------------------------------------------

//** Function exectution rates ****************************************************************
const uint8_t Hearbeat_RateMS = 1000/Hearbeat_RateHz;// Heartbeat update rate in ms
const uint8_t imu_UpdatePeriodMS = 1000/imu_UpdateRateHz;// imu update rate in ms
const uint8_t motorControl_UpdatePeriodMS = 1000/motorControl_UpdateRateHz;// motorControl update rate in ms
const uint8_t odometry_UpdatePeriodMS = 1000/odometry_UpdateRateHz;//odometry update rate in ms
const uint8_t PrintLoop_PeriodMS = 1000/PrintLoop_RateHz; //print loop run rate in ms
const uint8_t PurePursuit_UpdatePeriodMS = 1000/PurePursuit_UpdateRateHz; //Pure pursuit update run rate in ms
const uint8_t WaypointCmd_UpdatePeriodMS = 1000/WaypointCmd_UpdateRateHz; //Waypoint commander update rate in ms
const uint8_t CommLoop_UpdatePeriodMS = 1000/CommLoop_UpdateRateHz; //Communications loop run rate in ms

//---------------------------------------------------------------------------------------------


//** IMU MPU9150 ******************************************************************************
MPU6050 mpu(i2c_SDA, i2c_SCL);  // sda, scl pin
InterruptIn mpu_int(imu_INT);   // INT0 pin


const int FIFO_BUFFER_SIZE = 250;
uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
uint16_t fifoCount;
uint16_t packetSize;
bool dmpReady;
uint8_t mpuIntStatus;


struct Offset 
{
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
}imu_Offset = {150, -350, 1000, -110, 5, 0};    // Measured values

struct MPU6050_DmpData 
{
    Quaternion q;
    VectorFloat gravity;    // g
    float roll, pitch, yaw;     // rad
    VectorInt16 aa; //acceleration
    VectorInt16 aaReal; //linear acceleration after removing gravity
    VectorInt16 aaWorld; //world acceleration after rotating using pose quaternion
    float roll_v, pitch_v, yaw_v; //filtered values calculated from DMP packet
    
    VectorFloat accW; //aaWorld stored as float after conversion into mm/s2
}imu_Data;

int16_t raw_gyro[3]; //variable to store data decoded from dmp packet
float filt_gyro[3]; //variable to store data recalculated values
float GyroV_window[3][GyroV_MWindowSize];
volatile uint8_t GyroV_MWindowIndex=0; //moving window average index tracker

Timer imu_timer;
float imu_time[2] = {0.00, 0.00}; //index 0 in array holds values to correct wrap around condition

float imu_initial_angles[3]; //array holds initial imu angles to offset
int8_t unstable_readings = 10;
bool imu_stabilized=false;

bool imu_Initialize();
void imu_Update(void const *n);

Mutex mutex_imuPublish;
//---------------------------------------------------------------------------------------------


//** Motor Control ****************************************************************************
const float motor_LimitRPM[2] = {motor_LimitRPM_min,motor_LimitRPM_max}; //min max motor RPM limits
const float motor_Acc = motor_Acc_limit; //RPM / s

const float PIDFf_k[4] = {PIDFf_kP, PIDFf_kI, PIDFf_kD, PIDFf_kFf}; //P, I, D and Ff constants
const float PIDFf_TermsLimit[2] = {PIDFf_TermsLimit_min, PIDFf_TermsLimit_max}; //min max values of individual terms of controller
const float control_SummLimit[2] = {control_SummLimit_min, control_SummLimit_max}; //min max value of PIDFf controller summ


PwmOut motor_LeftP(mot_LP), motor_LeftM(mot_LM); //left motor pins
PwmOut motor_RightP(mot_RP), motor_RightM(mot_RM); //right motor pins

float motor_CmdRPM[2] = {0.0, 0.0}; //commanded motor RPM

float motor_ControlError[2][3]; //control error left and right for (t) and (t-1) and dE

float PIDFfS_terms[2][6]; //pid controller terms: P, I, D, Ff, Summ, Filtered Summ

Timer PID_timer;
volatile uint16_t dt_PID;

volatile float pid_MWindowStore[2][pid_MWindowSize];
volatile float pid_FilterCalc[2];
volatile uint8_t pid_MWindowIndex=0;

void MotorControl(void const *n);

Mutex mutex_pidPublish;
//---------------------------------------------------------------------------------------------


//** Encoder **********************************************************************************



volatile int32_t encoder_counter[2][2]={{0,0},{0,0}}; //volatile 2x2 array to store left, right counts at time (t) and (t-1)
float odometry[2][2]={{0.0,0.0},{0.0,0.0}}; //2x2 array to store left, right revolutions and filtered rev/sec

volatile int32_t encoder_MWindowStore[2][encoder_MWindowSize];
volatile uint8_t encoder_MWindowIndex=0;

const float encoder_SpeedCalcFactor = ((float)(encoder_MWindowSize * encoder_cpr)/(1000.0 * 1000.0));

Timer enc_timer;
volatile int enc_calc_time;


void OdometryUpdate(void const *n);

Mutex mutex_OdometryPublish;

//---------------------------------------------------------------------------------------------


//** Localization *****************************************************************************
float robot_pos[2] = {0.0,0.0}; //calculated position of robot
float robot_speed;

void LocalizationUpdate(); 
//---------------------------------------------------------------------------------------------


//** Pure-Pursuit *****************************************************************************
//float robotFrame_target[3]= {0.0,0.0,0.0}; //target waypoint rotated to robot frame x, y, distance


Mutex mutex_PurePursuitPublish;

void PurePursuitUpdate(void const *n); 
//---------------------------------------------------------------------------------------------


//** Drivetrain commander *********************************************************************
float drive_velocity[2]= {0.0, 0.0};//left and right velocity in mm/s
uint16_t motor_cmd_cnt=0; //command updater for testing step response of PID

Mutex mutex_DrivetrainCmdPublish;

void DrivetrainCmd(); 
//---------------------------------------------------------------------------------------------


//** Waypoint commander ***********************************************************************
float waypoint_target[3]= {0.0, 0.0, 0.0};//x,y, velocity

Mutex mutex_WaypointCmdPublish;

void WaypointCmdUpdate(void const *n); 

const uint16_t waypoint_len = 2;
float waypoint_set[waypoint_len][3] =   {{0.0, 10000.0, 200.0},
                                         {0.0, 50000.0, 300.0}}; //x,y, velocity in mm

//---------------------------------------------------------------------------------------------

//** Print Loop thread ************************************************************************
void print_loop(void const *n);
Mutex mutex_PrintLoopThread;

//---------------------------------------------------------------------------------------------

//** Communications Loop thread ***************************************************************
RF24 nrf24_wireless(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE); //nrf object declaration
RF24Network virgo_network(nrf24_wireless); // Network uses that radio
const uint16_t cmd_node = 0; // Address of the commander node
const uint16_t virgo3_nodeAddr = virgo3_robot_address; // Address of this robot node
const uint16_t network_channel = virgo3_network_channel; // wireless channel of our network
uint16_t uploadRateMS = nRfDefault_uploadRateMS; //ms // How often to send data to the other unit
volatile bool transmit_status;

// Structure of transmit payload
struct TxPayload_t {
    float timestamp;
    uint8_t parameter;
    float TxVector[3];
};

// Structure of receive payload
struct RxPayload_t {
    uint8_t cmd;
    float RxVector[4];
};

RxPayload_t RxData;
TxPayload_t TxData;

const uint8_t TxChunks=4;
float TxPackets[TxChunks][3];

Timer comm_timer; // Timer to maintain communication loop rate

void comm_initialize(); //initialization of comm radio
void comm_loop(void const *n);


//---------------------------------------------------------------------------------------------

//** Attitude Control function ****************************************************************
float filt_WheelV[2];
float WheelV_window[2][wheelV_MWindowSize];
volatile uint8_t wheelV_MWindowIndex;

void attitudeController(float* w_L, float* w_R, int flag, float tc,
                        float beta, float dbeta, float gamma, float dgamma,
                        float theta_L, float theta_R, float dtheta_L, float dtheta_R,
                        float ref_ddbeta, float ref_dbeta, float ref_beta,
                        float ref_ddtheta, float ref_dtheta, float ref_theta,
                        float ref_ddgamma,  float ref_dgamma, float ref_gamma);
                        
//attitude controller
    float W_l, W_r; //left, right wheel velocity in rad/s. direction inverted to correspond to IMU XYZ frame
    float pitch_th, pitch_om; //pitch angle, velocity
    float yaw_th, yaw_om; //heading angle, velocity
    float wheelTh_l, wheelTh_r; //wheel position left, right
    float wheelOm_l, wheelOm_r; //wheel velocity left, right                        
                        
//---------------------------------------------------------------------------------------------

//** Camera control thread ********************************************************************
void CameraControl_loop(void const *n);
DigitalOut camEN(cam_EN);
uint32_t camera_OnRateS = camera_OnRateMinutes * 60; //camera switch on frequency in sec
uint32_t camera_RunTimeS = camera_OnS(camera_CaptureTimeS); //camera total runtime in sec

Timer Camera_timer;
volatile uint32_t dt_Camera;

//---------------------------------------------------------------------------------------------

// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //


int main() 
{
    motor_LeftP = 1;
    motor_LeftM = 1;
            
    motor_RightP = 1;
    motor_RightM = 1;


    debug.baud(PC_BAUDRATE);
    debugLED =1;
    
    debug.printf("** Starting Virgo v3 Routines *************\n\n");
    
    //** start main loop as a thread **********************************************************
    Thread Heartbeat_function(Heartbeat_loop, NULL, osPriorityNormal);
    debug.printf("* Hearbeat loop started *\n"); 

    //** start IMU funtion as Thread **********************************************************
    MBED_ASSERT(imu_Initialize() == true);
    Thread IMU_function(imu_Update, NULL, osPriorityHigh);
    debug.printf("* IMU routine started *\n");
        
    //** start OdometryUpdate function as Thread **********************************************
    enc_timer.start();
    Thread Odometry_function(OdometryUpdate, NULL, osPriorityNormal);
    debug.printf("* Odometry routine started *\n");
    
    //** start MotorControl function as Thread ************************************************
    PID_timer.start();
    Thread MotorControl_function(MotorControl, NULL, osPriorityNormal);
    debug.printf("* Motor control routine started *\n");    
    
    //** start PurePursuit controller as Thread ***********************************************
    Thread PurePursuitUpdate_function(PurePursuitUpdate, NULL, osPriorityNormal);
    debug.printf("* PurePursuit controller routine started *\n"); 
    
    //** start Waypoint commander function as Thread ******************************************
    Thread WaypointCmdUpdate_function(WaypointCmdUpdate, NULL, osPriorityNormal);
    debug.printf("* Waypoint commander routine started *\n");  
    
    //** start comm loop as a thread **********************************************************
    //comm_initialize();
    //Thread CommLoop_function(comm_loop, NULL, osPriorityNormal);
    //debug.printf("* Communications loop started *\n"); 
    
    //** start ccamera control loop as a thread ***********************************************
    //Thread CameraControl_function(CameraControl_loop, NULL, osPriorityNormal);
    //debug.printf("* Camera control loop started *\n"); 

    //** start print loop as a thread **********************************************************
    //Thread PrintLoop_function(print_loop, NULL, osPriorityNormal);
    //debug.printf("* Print loop started *\n\n\n");
    
    debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
    
    camEN = 1;
    
    while(1) //Program does not halt for any runtime errors, atleast for now. Plan to add a watchdog later.
    { 
        virgo_network.update(); // Pump the network regularly
    }
}

//heatbeat loop as a seperate thread
void Heartbeat_loop(void const *n)
{
    while(true) {
        debugLED = !debugLED;
        Thread::wait(Hearbeat_RateMS);
    }
}

//CameraControl loop as a seperate thread
void CameraControl_loop(void const *n)
{
    camEN = 0;
    Camera_timer.start();
    while(true) {
        if(Camera_timer.read() <= camera_RunTimeS)
            camEN = 1;
            
        if((Camera_timer.read() > camera_RunTimeS) && (Camera_timer.read() <= (camera_OnRateS+camera_RunTimeS)))
            camEN = 0;
            
        if(Camera_timer.read() > (camera_OnRateS+camera_RunTimeS))
            Camera_timer.reset();
            
        Thread::wait(100); //proocess thread every 100ms
    }
}


//print loop as a seperate thread
#define print_lines 2 //number of info lines being printed on screen
void print_loop(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    
    //clear 14 lines while going up, these are the initilization lines printed on screen
    for(int l=14; l>0; l--) {
        debug.printf("\e[1A"); //go up 1 line
        debug.printf("\e[K"); //clear line
    }

    debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
    for(int l=print_lines; l>0; l--) debug.printf("\n");
    debug.printf("\n===================================================");
    debug.printf("\e[1A"); //go up 1 line

    while(true) {
        //move cursor up # of lines printed to create a static display and clear the first line
        for(int l=print_lines; l>0; l--) debug.printf("\e[1A");
        debug.printf("\e[K");
        
        //mutex_PrintLoopThread.lock();      

        
        debug.printf("Elapsed time: %.2f s\n\e[K", imu_time[1]);
        //debug.printf("Status: Heading %.2f, CmdRPM <%.1f , %.1f>\n\e[K", RAD_TO_DEG(imu_Data.yaw), motor_CmdRPM[0], motor_CmdRPM[1]);
        //debug.printf("Orientation (YPR): (%.2f , %.2f , %.2f)\n\e[K", RAD_TO_DEG(imu_Data.yaw), RAD_TO_DEG(imu_Data.pitch), RAD_TO_DEG(imu_Data.roll));
        //debug.printf("Command: Position <%.1f , %.1f> \tVelocity <%.1f>\n\e[K", waypoint_target[0], waypoint_target[1], waypoint_target[2]);
        //debug.printf("Current: Position <%.1f , %.1f> \tVelocity <%.1f>\n\e[K", robot_pos[0], robot_pos[1], (drive_velocity[0]+drive_velocity[1])/2);
        //debug.printf("Distance to go: %.1f\n\e[K", robotFrame_target[2]);
        //debug.printf("Command RPM (L,R): %.1f, %.1f\n\e[K", motor_CmdRPM[0], motor_CmdRPM[1]);
        //debug.printf("Received Command 0x%x :: <%.1f, %.1f, %.1f, %.1f>\n\e[K", RxData.cmd, RxData.RxVector[0], RxData.RxVector[1], RxData.RxVector[2], RxData.RxVector[3]);
        
        //debug.printf("Controller params: \n\e[K");
        //debug.printf("\tW_l: %.1f, W_r: %.1f\n\e[K", W_l, W_r);
        //debug.printf("\tpitch_th: %.1f, pitch_om: %.1f, yaw_th: %.1f, yaw_om: %.1f,\n\e[K", pitch_th, pitch_om, yaw_th, yaw_om);
        //debug.printf("\twheelTh_l: %.1f, wheelTh_r: %.1f, wheelOm_l: %.1f, wheelOm_r: %.1f\n\e[K", wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r);
        
        
        //debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry[0][1]*60, odometry[1][1]*60);
        //debug.printf("PurePursuit: ld %.1f, r %.1f\n\e[K", purePursuit_ld, purePursuit_r);
        //debug.printf("PurePursuit: hdgErr %.1f, omega %.1f\n\e[K", RAD_TO_DEG(purePursuit_headingE), RAD_TO_DEG(purePursuit_omega));
        //debug.printf("PWM Percent (L,R): %.2f, %.2f\n\e[K", PIDFfS_terms[0][4]*100.0, PIDFfS_terms[1][4]*100.0);
        //debug.printf("Error RPM (L,R): %.1f, %.1f\n\e[K", motor_ControlError[0][1], motor_ControlError[1][1]);
        //debug.printf("Individual terms (Percent) P= %.2f, I= %.2f, D= %.2f, FF= %.2f\n", PIDFfS_terms[0][0]*100.0, PIDFfS_terms[0][1]*100.0, PIDFfS_terms[0][2]*100.0, PIDFfS_terms[0][3]*100.0);
        //debug.printf("%.2f:%.2f,%.2f,%.2f,%.2f:%.2f\n", odometry[0][1]*60,PIDFfS_terms[0][0]*100.0, PIDFfS_terms[0][1]*100.0, PIDFfS_terms[0][2]*100.0, PIDFfS_terms[0][3]*100.0,PIDFfS_terms[0][4]*100.0);
        //debug.printf("%.2f %.2f %.2f %.2f\n", motor_CmdRPM[0], odometry[0][1]*60, PIDFfS_terms[0][4]*100.0, PIDFfS_terms[0][5]*100.0);
        
        //mutex_PrintLoopThread.unlock();
        

        
        Thread::wait(PrintLoop_PeriodMS);
    }
}

void comm_initialize()
{

    
    comm_timer.start();
}

//Communications thread running at CommLoop_UpdateRateHz, data transmission at uploadRateMS
void comm_loop(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    while(true) {

        Thread::wait(CommLoop_UpdatePeriodMS);
    }
}

//Pure pursuit controller thread running at PurePursuit_UpdateRateHz
void PurePursuitUpdate(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    while(true) {
        
        
        DrivetrainCmd();
        mutex_PurePursuitPublish.unlock();
        
        Thread::wait(PurePursuit_UpdatePeriodMS);
    }
}

//Drive train commander and kinematics solver
void DrivetrainCmd()
{
    
    //attitude controller
    
    
    //motor_CmdRPM[0];
    //motor_CmdRPM[1];
}

void WaypointCmdUpdate(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    while(true) {
        mutex_WaypointCmdPublish.lock();

        for(int i=0; i<3; i++) waypoint_target[i] = waypoint_set[0][i];

        mutex_WaypointCmdPublish.unlock();
        
        Thread::wait(WaypointCmd_UpdatePeriodMS);
    }
}

//Motor control thread running at motorControl_UpdateRateHz
void MotorControl(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    while(true) {
        
        Thread::wait(motorControl_UpdatePeriodMS);
    }
}


void OdometryUpdate(void const *n)
{
    while(!imu_stabilized) Thread::wait(10); //wait for IMU to stabilize
    
    /***/while(true)/***/ { 
        

        Thread::wait(odometry_UpdatePeriodMS);
    }
}

float distance_increment;
//Localization loop called after every odometry update
void LocalizationUpdate()
{
    
}

bool imu_Initialize()
{
    

    return true;
}

void imu_Update(void const *n)
{
    /***/while(true) { /***/
    
    
        Thread::wait(imu_UpdatePeriodMS);
    }
}