File content as of revision 27:2db168d9fb18:

/**
 * @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"
#include "globalExterns.h"
#include "VL53L0X.h"
#define W 0.1

/**
 * Functions, Threads & General Definitions
 */
//*****************************************************************************

//** Ranging **//
I2C ItC_ranging(i2c2_SDA, i2c2_SCL);
Timer time_r;

DigitalOut xshut1(xs1);
DigitalOut xshut2(xs2);
DigitalOut xshut3(xs3);
DigitalOut xshut4(xs4);
DigitalOut xshut5(xs5);

VL53L0X sensor1(&ItC_ranging, &time_r);
VL53L0X sensor2(&ItC_ranging, &time_r);
VL53L0X sensor3(&ItC_ranging, &time_r);
VL53L0X sensor4(&ItC_ranging, &time_r);
VL53L0X sensor5(&ItC_ranging, &time_r);


struct RangeData{
     uint16_t exleft, left, front, right, exright;;
    };
RangeData RangeSensor; 


//** Drivetrain **
motorDriver drive;  //motor drive train
odometer odometry;  //odometer function
pidBearing PID_B; //PID control for bearing
pidControl PID_L, PID_R; //pidcontroller for left and right motors

//**new PID control**

float bearingToWaypoint;
float normalDistanceError;
float prevError;
float speedChange;

float k3, cmdL, cmdR, aError;
float dist[3]; 


//**end of new control **

Timer motorControl_t;
float rpm_cmd[2]; //drive motor rpm command
float rpm_compensated[2]; //rpm command compensated by acc limit
float targetAcceleration = 300.0; //RPM/s acceleration
float pwm_cmd[2]; //drive motor pwm command

/* THREAD */
void odometry_thread(void const *n);
void motorControl_thread(void const *n);
/*   **   */
//-------------

//** Localization **
IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu;    //MPU9150 / MPU6050 wrapper class
float imuTime;
Localization localization;  //localization function

/* FUNCTION */
bool imuInit_function();
/*   **   */

/* THREAD */
void imu_thread(void const *n);

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

//** Power Monitor **
BattGuage  battery; //Battery fuel gauge wrapper

/* THREAD */

/*   **   */
//-------------

//** Trajectory tracking **
purePursuit purePursuit;
//kinematics kinematics;

float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
//waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
uint8_t totalWaypoints = 5;
//kite pattern 200cm long, 100cm wide
int16_t waypoints_set[][4] = { {0,0,90,0},
    {100,100,90,0},
    {0,200,90,0},
    {-100,100,90,0},
    {0,0,90,0},
    {0,0,90,0},
    {0,0,90,0}
};

float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
float target_waypoint[2] = {0.0, 0.0}; //coordinates in millimeters for pure-pursuit's use. initialize with 0,0 this is necessary to prevent comparison to a garbage value
float target_velocity =0.0; //target velocity in mm/s
float distanceToWaypoint; //distance from robot to waypoint
uint8_t waypoint_index=0;
uint8_t go_cmd=0; //make robot run a waypoint set

/* THREAD */
void purePursuit_thread(void const *n);
void waypointCmd_thread(void const *n);
/*   **   */
//-------------

//** Attitude control **
attitudeControl attitudeControl;

float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
//** Declarations of misc functions **
Serial Debug(uart_TX, uart_RX); //Debug serial port
DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication

/* THREAD */
void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
void print_thread(void const *n); //Debug printing thread
void ranging_thread(void const *n); //ranging VL53L0X
/*   **   */

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


int main()
{
    Debug.baud(PC_BAUDRATE);

    debugLED =1;

    //wait_ms(5000);

    Debug.printf("** Starting Virgo v3 Routines *************\n\n");

    //** start Hearbeat loop as a thread **
    Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
    Debug.printf("* Hearbeat loop started *\n");

    //** start IMU funtion as Thread **
    Thread IMU_function(imu_thread, NULL, osPriorityHigh);
    Debug.printf("* IMU routine started *\n");

    //** start OdometryUpdate function as Thread **
    Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
    Debug.printf("* Odometry routine started *\n");

    //** start MotorControl function as Thread **
    Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
    Debug.printf("* Motor control routine started *\n");

    //** start PurePursuit controller as Thread **
    Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
    Debug.printf("* PurePursuit controller routine started *\n");

    //** start Waypoint commander function as Thread **
    Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
    Debug.printf("* Waypoint commander routine started *\n");


    
       
    xshut1 = 0;
    xshut2 = 0;
    xshut3 = 0;
    xshut4 = 0;
    xshut5 = 0;

    Thread::wait(W);

/////////////////////////    
    xshut1 = 1;
    Debug.printf("Sensor 1: \nXSHUT ON\n");
    Thread::wait(W);
     
    sensor1.init();
    Debug.printf("S1 Initialized...\n");
    Thread::wait(W);
     
    sensor1.setAddress((uint8_t)23);
    Debug.printf("S1 Address set...\n");   
/////////////////////////      
    xshut2 = 1;
    Debug.printf("Sensor 2: \nXSHUT ON\n");
    Thread::wait(W);
     
    sensor2.init();
    Debug.printf("S2 Initialized...\n");
    Thread::wait(W);
     
    sensor2.setAddress((uint8_t)25);
    Debug.printf("S2 Address set...\n");
/////////////////////////      
    xshut3 = 1;
    Debug.printf("Sensor 3: \nXSHUT ON\n");
    Thread::wait(W);
     
    sensor3.init();
    Debug.printf("S3 Initialized...\n");
    Thread::wait(W);
     
    sensor3.setAddress((uint8_t)27);
    Debug.printf("S3 Address set...\n");   
/////////////////////////      
    xshut4 = 1;
    Debug.printf("Sensor 4: \nXSHUT ON\n");
    Thread::wait(W);
     
    sensor4.init();
    Debug.printf("S4 Initialized...\n");
    Thread::wait(W);
     
    sensor4.setAddress((uint8_t)29);
    Debug.printf("S4 Address set...\n");
/////////////////////////    
    xshut5 = 1;
    Debug.printf("Sensor 5: \nXSHUT ON\n");
    Thread::wait(W);
     
    sensor5.init();
    Debug.printf("S5 Initialized...\n");
    Thread::wait(W);
     
    sensor5.setAddress((uint8_t)22);
    Debug.printf("S5 Address set...\n");    
    
/////////////////////////      
    sensor1.startContinuous(); 
    sensor2.startContinuous(); 
    sensor3.startContinuous(); 
    sensor4.startContinuous();
    sensor5.startContinuous();     
    //** start Ranging funtion as Thread **
    Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
    Debug.printf("* Ranging routine started *\n");      
    
    

    //** start Debug print loop as a thread **
    Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
    Debug.printf("* Print loop started *\n\n\n");

    Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");

    while(1) {

    }
}

/**
 * heartbeat loop as an individual thread
 */
void heartbeat_thread(void const *n)
{
    while(true) {
        if(imu.imu_stabilized[0] ==1) {
            debugLED = !debugLED;
            Thread::wait(Hearbeat_RateMS-50);
            debugLED = !debugLED;
            Thread::wait(50);
        } else
            debugLED = 1;
    }
}

/**
 * imu initialization function
 */
bool imuInit_function()
{
    return (imu.imuInit());
}

/**
 * imu update loop as an individual thread
 */
void imu_thread(void const *n)
{
    bool init_status = imuInit_function();
    Thread::wait(100);

    while(init_status) {
        imu.imuUpdate();

        //Usage:
        //imu.Pose[0, 1, 2]; //euler x, y, z
        //imu.AngVel[0, 1, 2]; //AngVel x, y, z
        //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
        //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z

        imuTime = imu.time_s;

        Thread::wait(imu_UpdatePeriodMS);
    }
}


/**
 * Ranging sensor loop as an individual thread
 */
 
void ranging_thread(void const *n)
{

        

    
    while(1)
    {
      if(imu.imu_stabilized[1] ==1){            
            RangeSensor.exleft = sensor1.readRangeContinuousMillimeters();
            RangeSensor.left = sensor2.readRangeContinuousMillimeters();            
            RangeSensor.front = sensor3.readRangeContinuousMillimeters();
            RangeSensor.right = sensor4.readRangeContinuousMillimeters(); 
            RangeSensor.exright = sensor5.readRangeContinuousMillimeters();                                   
            //debugprint.printf("in ranging %u \n\e[K", RangeSensor.left);
      }      
            Thread::wait(1.0);  
              
    }
}


/**
 * odometry update loop as an individual thread
 */
void odometry_thread(void const *n)
{
    odometry.init();
    Thread::wait(50);

    while(true) {
        odometry.update();

        //Usage:
        //odometer.revolutions[0, 1]; //revolutions left, right
        //odometer.rpm[0, 1]; //rpm left, right

        localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);

        //Usage:
        //localization.position[0, 1] //x, y

        Thread::wait(odometry_UpdatePeriodMS);
    }
}

/**/
float rpm_smc = 500;
float ref_dtheta = 0;
float ref_theta = 0;

float ref_dgamma = 0;
float ref_gamma = 0;

float ref_beta = DEG_TO_RAD(0.0);
float ref_dbeta = 0;

float u1, u2;
/**/
/**
 * motor control loop as an individual thread
 */
void motorControl_thread(void const *n)
{
    motorControl_t.start();

    float pitch_th, pitch_om, yaw_th, yaw_om;
    float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
    float W_l, W_r;

    while(true) {

        //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
        if(imu.imu_stabilized[1] ==1) {

            
            PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &aError, target_waypoint[0], target_waypoint[1], 
                        localization.position[0], localization.position[1], imu.Pose[2], purePursuit.robotFrame_targetDistance,
                        waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3, 3.5); //new controller



            if(waypointSetFinish_flag == 0) {
                rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
                rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
//                rpm_cmd[0]=-800;
//                rpm_cmd[1]=-800;

                if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) )
                    rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);
                else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0)
                    rpm_cmd[0] = 0;

                if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) )
                    rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]);
                else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0)
                    rpm_cmd[1] = 0;

                rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
                rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
                
                //rpm_compensated[0]= rpm_cmd[0];
                //rpm_compensated[1]= rpm_cmd[1];

            } else {
                rpm_cmd[0]=0;
                rpm_cmd[1]=0;
                
                rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read());
                rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read());
            }

            pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read());
            pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read());

            drive.setPWM_L(pwm_cmd[0]);
            drive.setPWM_R(pwm_cmd[1]);
                     
            
        }

        motorControl_t.reset();

        Thread::wait(motorControl_UpdatePeriodMS);
    }
}

/**
 * purepursuit loop as an individual thread
 */
void purePursuit_thread(void const *n)
{
    while(true) {
        if(imu.imu_stabilized[0] ==1) {
            //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));

            if(purePursuit.robotFrame_targetDistance <= waypointZone)
                waypointReached_flag = 1;
            else
                waypointReached_flag = 0;
        }
        Thread::wait(imu_UpdatePeriodMS);
    }
}

/**
 * waypoint tracking loop as individual thread
 */
void waypointCmd_thread(void const *n)
{
    while(true) {
        //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
        if(imu.imu_stabilized[0] ==1) {
            if(waypoint_index > totalWaypoints) {
                target_velocity = 0.0; //stop the robot
                waypointSetFinish_flag = 1;
            }

            if(waypointReached_flag == 1 && waypointSetFinish_flag == 0) {
                target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
                target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
                target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
                waypoint_index++;
            }
        }
        Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
    }
}







/**
 * Debug data print loop as an individual thread
 */
#define print_lines 15 //number of info lines being printed on screen
void print_thread(void const *n)
{
    //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");

        Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
        Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
        Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
        Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
        Debug.printf("Calib Status (M-A-G-S-O): (%d , %d , %d , %d , %d)\n\e[K", imu.calib_stat[0], imu.calib_stat[1], imu.calib_stat[2], imu.calib_stat[3], imu.calib_stat[4]);

        //Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
        Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
        Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
        Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", aError, speedChange, k3);
        Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
        Debug.printf("Ranging: %u, %u, %u, %u, %u \n\e[K", RangeSensor.exleft, RangeSensor.left, RangeSensor.front, RangeSensor.right, RangeSensor.exright);


        Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
        //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
        Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
        //Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);

        //Debug.printf("PID_L: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_L.PIDFf_terms[0], PID_L.PIDFf_terms[1], PID_L.PIDFf_terms[2], PID_L.PIDFf_terms[3], PID_L.Summ_term);
        //Debug.printf("PID_R: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_R.PIDFf_terms[0], PID_R.PIDFf_terms[1], PID_R.PIDFf_terms[2], PID_R.PIDFf_terms[3], PID_R.Summ_term);


        //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
        
        Thread::wait(PrintLoop_PeriodMS);
    }
}