orion_main.cpp

00001 /**
00002  * @author Akash Vibhute
00003  * < akash . roboticist [at] gmail . com >
00004  *
00005  * @section LICENSE
00006  *
00007  * Copyright (c) 2015 Akash Vibhute
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014  *
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  *
00021  * The above copyright notice and this permission notice shall be included in
00022  * all copies or substantial portions of the Software.
00023  *
00024  * @section DESCRIPTION
00025  *
00026  * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
00027  * Robot controller software for SUTD Virgo version 3 robot
00028  *
00029  */
00030 
00031 /**
00032  * Header file for including all necessary functions for robot and defining any
00033  * custom functions written in the main file.
00034  */
00035 #include "main.h"
00036 #include "globalExterns.h"
00037 #include "VL53L0X.h"
00038 #define W 0.1
00039 
00040 /**
00041  * Functions, Threads & General Definitions
00042  */
00043 //*****************************************************************************
00044 
00045 //** Ranging **//
00046 I2C ItC_ranging(i2c2_SDA, i2c2_SCL);
00047 Timer time_r;
00048 
00049 DigitalOut xshut1(xs1);
00050 DigitalOut xshut2(xs2);
00051 DigitalOut xshut3(xs3);
00052 DigitalOut xshut4(xs4);
00053 DigitalOut xshut5(xs5);
00054 
00055 VL53L0X sensor1(&ItC_ranging, &time_r);
00056 VL53L0X sensor2(&ItC_ranging, &time_r);
00057 VL53L0X sensor3(&ItC_ranging, &time_r);
00058 VL53L0X sensor4(&ItC_ranging, &time_r);
00059 VL53L0X sensor5(&ItC_ranging, &time_r);
00060 
00061 
00062 struct RangeData{
00063      uint16_t exleft, left, front, right, exright;;
00064     };
00065 RangeData RangeSensor; 
00066 
00067 
00068 //** Drivetrain **
00069 motorDriver drive;  //motor drive train
00070 odometer odometry;  //odometer function
00071 pidBearing PID_B; //PID control for bearing
00072 pidControl PID_L, PID_R; //pidcontroller for left and right motors
00073 
00074 //**new PID control**/*#LV*/
00075 float bearingToWaypoint, robotFrame, prevError, speedChange;
00076 float k3, cmdL, cmdR, Error;
00077 //**end of new control **/*#LV*/
00078 
00079 Timer motorControl_t;
00080 float rpm_cmd[2]; //drive motor rpm command
00081 float rpm_compensated[2]; //rpm command compensated by acc limit
00082 float targetAcceleration = 300.0; //RPM/s acceleration
00083 float pwm_cmd[2]; //drive motor pwm command
00084 
00085 /* THREAD */
00086 void odometry_thread(void const *n);
00087 void motorControl_thread(void const *n);
00088 /*   **   */
00089 //-------------
00090 
00091 //** Localization **
00092 IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu;    //MPU9150 / MPU6050 wrapper class
00093 float imuTime;
00094 Localization localization;  //localization function
00095 
00096 /* FUNCTION */
00097 bool imuInit_function();
00098 /*   **   */
00099 
00100 /* THREAD */
00101 void imu_thread(void const *n);
00102 
00103 //-------------
00104 
00105 //** Power Monitor **
00106 BattGuage  battery; //Battery fuel gauge wrapper
00107 
00108 /* THREAD */
00109 
00110 /*   **   */
00111 //-------------
00112 
00113 //** Trajectory tracking **
00114 purePursuit purePursuit;
00115 //kinematics kinematics;
00116 
00117 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
00118 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
00119 uint8_t totalWaypoints = 5;
00120 //kite pattern 200cm long, 100cm wide
00121 int16_t waypoints_set[][4] = { {0,0,90,0},
00122     {100,100,90,0},
00123     {0,200,90,0},
00124     {-100,100,90,0},
00125     {0,0,90,0},
00126     {0,0,90,0},
00127     {0,0,90,0}
00128 };
00129 
00130 float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
00131 uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
00132 uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
00133 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
00134 float target_velocity =0.0; //target velocity in mm/s
00135 float distanceToWaypoint; //distance from robot to waypoint
00136 uint8_t waypoint_index=0;
00137 uint8_t go_cmd=0; //make robot run a waypoint set
00138 
00139 /* THREAD */
00140 void purePursuit_thread(void const *n);
00141 void waypointCmd_thread(void const *n);
00142 /*   **   */
00143 //-------------
00144 
00145 //** Attitude control **
00146 attitudeControl attitudeControl;
00147 
00148 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
00149 //** Declarations of misc functions **
00150 Serial Debug(uart_TX, uart_RX); //Debug serial port
00151 DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication
00152 
00153 /* THREAD */
00154 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
00155 void print_thread(void const *n); //Debug printing thread
00156 void ranging_thread(void const *n); //ranging VL53L0X
00157 /*   **   */
00158 
00159 //-----------------------------------------------------------------------------
00160 
00161 
00162 int main()
00163 {
00164     Debug.baud(PC_BAUDRATE);
00165 
00166     debugLED =1;
00167 
00168     //wait_ms(5000);
00169 
00170     Debug.printf("** Starting Virgo v3 Routines *************\n\n");
00171 
00172     //** start Hearbeat loop as a thread **
00173     Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
00174     Debug.printf("* Hearbeat loop started *\n");
00175 
00176     //** start IMU funtion as Thread **
00177     Thread IMU_function(imu_thread, NULL, osPriorityHigh);
00178     Debug.printf("* IMU routine started *\n");
00179 
00180     //** start OdometryUpdate function as Thread **
00181     Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
00182     Debug.printf("* Odometry routine started *\n");
00183 
00184     //** start MotorControl function as Thread **
00185     Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
00186     Debug.printf("* Motor control routine started *\n");
00187 
00188     //** start PurePursuit controller as Thread **
00189     Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
00190     Debug.printf("* PurePursuit controller routine started *\n");
00191 
00192     //** start Waypoint commander function as Thread **
00193     Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
00194     Debug.printf("* Waypoint commander routine started *\n");
00195 
00196 
00197     
00198        
00199 /*    xshut1 = 0;
00200     xshut2 = 0;
00201     xshut3 = 0;
00202     xshut4 = 0;
00203     xshut5 = 0;
00204 
00205     Thread::wait(W);
00206 
00207 /////////////////////////    
00208     xshut1 = 1;
00209     Debug.printf("Sensor 1: \nXSHUT ON\n");
00210     Thread::wait(W);
00211      
00212     sensor1.init();
00213     Debug.printf("S1 Initialized...\n");
00214     Thread::wait(W);
00215      
00216     sensor1.setAddress((uint8_t)23);
00217     Debug.printf("S1 Address set...\n");   
00218 /////////////////////////      
00219     xshut2 = 1;
00220     Debug.printf("Sensor 2: \nXSHUT ON\n");
00221     Thread::wait(W);
00222      
00223     sensor2.init();
00224     Debug.printf("S2 Initialized...\n");
00225     Thread::wait(W);
00226      
00227     sensor2.setAddress((uint8_t)25);
00228     Debug.printf("S2 Address set...\n");
00229 /////////////////////////      
00230     xshut3 = 1;
00231     Debug.printf("Sensor 3: \nXSHUT ON\n");
00232     Thread::wait(W);
00233      
00234     sensor3.init();
00235     Debug.printf("S3 Initialized...\n");
00236     Thread::wait(W);
00237      
00238     sensor3.setAddress((uint8_t)27);
00239     Debug.printf("S3 Address set...\n");   
00240 /////////////////////////      
00241     xshut4 = 1;
00242     Debug.printf("Sensor 4: \nXSHUT ON\n");
00243     Thread::wait(W);
00244      
00245     sensor4.init();
00246     Debug.printf("S4 Initialized...\n");
00247     Thread::wait(W);
00248      
00249     sensor4.setAddress((uint8_t)29);
00250     Debug.printf("S4 Address set...\n");
00251 /////////////////////////    
00252     xshut5 = 1;
00253     Debug.printf("Sensor 5: \nXSHUT ON\n");
00254     Thread::wait(W);
00255      
00256     sensor5.init();
00257     Debug.printf("S5 Initialized...\n");
00258     Thread::wait(W);
00259      
00260     sensor5.setAddress((uint8_t)22);
00261     Debug.printf("S5 Address set...\n");    
00262     
00263 /////////////////////////      
00264     sensor1.startContinuous(); 
00265     sensor2.startContinuous(); 
00266     sensor3.startContinuous(); 
00267     sensor4.startContinuous();
00268     sensor5.startContinuous();     
00269     //** start Ranging funtion as Thread **
00270     Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
00271     Debug.printf("* Ranging routine started *\n"); */     
00272     
00273     
00274 
00275     //** start Debug print loop as a thread **
00276     Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
00277     Debug.printf("* Print loop started *\n\n\n");
00278 
00279     Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
00280 
00281     while(1) {
00282 
00283     }
00284 }
00285 
00286 /**
00287  * heartbeat loop as an individual thread
00288  */
00289 void heartbeat_thread(void const *n)
00290 {
00291     while(true) {
00292         if(imu.imu_stabilized[0] ==1) {
00293             debugLED = !debugLED;
00294             Thread::wait(Hearbeat_RateMS-50);
00295             debugLED = !debugLED;
00296             Thread::wait(50);
00297         } else
00298             debugLED = 1;
00299     }
00300 }
00301 
00302 /**
00303  * imu initialization function
00304  */
00305 bool imuInit_function()
00306 {
00307     return (imu.imuInit());
00308 }
00309 
00310 /**
00311  * imu update loop as an individual thread
00312  */
00313 void imu_thread(void const *n)
00314 {
00315     bool init_status = imuInit_function();
00316     Thread::wait(100);
00317 
00318     while(init_status) {
00319         imu.imuUpdate();
00320 
00321         //Usage:
00322         //imu.Pose[0, 1, 2]; //euler x, y, z
00323         //imu.AngVel[0, 1, 2]; //AngVel x, y, z
00324         //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
00325         //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
00326 
00327         imuTime = imu.time_s;
00328 
00329         Thread::wait(imu_UpdatePeriodMS);
00330     }
00331 }
00332 
00333 
00334 /**
00335  * Ranging sensor loop as an individual thread
00336  */
00337  
00338 void ranging_thread(void const *n)
00339 {
00340 
00341         
00342 
00343     
00344     while(1)
00345     {
00346       if(imu.imu_stabilized[1] ==1){            
00347             RangeSensor.exleft = sensor1.readRangeContinuousMillimeters();
00348             RangeSensor.left = sensor2.readRangeContinuousMillimeters();            
00349             RangeSensor.front = sensor3.readRangeContinuousMillimeters();
00350             RangeSensor.right = sensor4.readRangeContinuousMillimeters(); 
00351             RangeSensor.exright = sensor5.readRangeContinuousMillimeters();                                   
00352             //debugprint.printf("in ranging %u \n\e[K", RangeSensor.left);
00353       }      
00354             Thread::wait(1.0);  
00355               
00356     }
00357 }
00358 
00359 
00360 /**
00361  * odometry update loop as an individual thread
00362  */
00363 void odometry_thread(void const *n)
00364 {
00365     odometry.init();
00366     Thread::wait(50);
00367 
00368     while(true) {
00369         odometry.update();
00370 
00371         //Usage:
00372         //odometer.revolutions[0, 1]; //revolutions left, right
00373         //odometer.rpm[0, 1]; //rpm left, right
00374 
00375         localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);
00376 
00377         //Usage:
00378         //localization.position[0, 1] //x, y
00379 
00380         Thread::wait(odometry_UpdatePeriodMS);
00381     }
00382 }
00383 
00384 /**/
00385 float rpm_smc = 500;
00386 float ref_dtheta = 0;
00387 float ref_theta = 0;
00388 
00389 float ref_dgamma = 0;
00390 float ref_gamma = 0;
00391 
00392 float ref_beta = DEG_TO_RAD(0.0);
00393 float ref_dbeta = 0;
00394 
00395 float u1, u2;
00396 /**/
00397 /**
00398  * motor control loop as an individual thread
00399  */
00400 void motorControl_thread(void const *n)
00401 {
00402     motorControl_t.start();
00403 
00404     float pitch_th, pitch_om, yaw_th, yaw_om;
00405     float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
00406     float W_l, W_r;
00407 
00408     while(true) {
00409 
00410         //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
00411         if(imu.imu_stabilized[1] ==1) {
00412 
00413 /*#LV*/            
00414             PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &Error, 
00415                                     target_waypoint,
00416                                     localization.position,
00417                                     imu.Pose[2], 
00418                                     &robotFrame,
00419                                     waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], 
00420                                     &k3,
00421                                     3.5 ); //new controller
00422 /*#LV*/
00423 
00424 
00425             if(waypointSetFinish_flag == 0) {
00426                 rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
00427                 rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
00428 //                rpm_cmd[0]=-800;
00429 //                rpm_cmd[1]=-800;
00430 
00431                 if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) )
00432                     rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);
00433                 else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0)
00434                     rpm_cmd[0] = 0;
00435 
00436                 if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) )
00437                     rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]);
00438                 else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0)
00439                     rpm_cmd[1] = 0;
00440 
00441                 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
00442                 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
00443                 
00444                 //rpm_compensated[0]= rpm_cmd[0];
00445                 //rpm_compensated[1]= rpm_cmd[1];
00446 
00447             } else {
00448                 rpm_cmd[0]=0;
00449                 rpm_cmd[1]=0;
00450                 
00451                 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read());
00452                 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read());
00453             }
00454 
00455             pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read());
00456             pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read());
00457 
00458             drive.setPWM_L(pwm_cmd[0]);
00459             drive.setPWM_R(pwm_cmd[1]);
00460                      
00461             
00462         }
00463 
00464         motorControl_t.reset();
00465 
00466         Thread::wait(motorControl_UpdatePeriodMS);
00467     }
00468 }
00469 
00470 /**
00471  * purepursuit loop as an individual thread
00472  */
00473 void purePursuit_thread(void const *n)
00474 {
00475     while(true) {
00476         if(imu.imu_stabilized[0] ==1) {
00477             //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
00478 //            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
00479 //
00480 //            if(purePursuit.robotFrame_targetDistance <= waypointZone)
00481 //                waypointReached_flag = 1;
00482 //            else
00483 //                waypointReached_flag = 0;
00484 
00485 /*#LV*/
00486         PID_B.findRobotFrameDistance(target_waypoint, localization.position, &robotFrame);   
00487                   
00488             if(robotFrame <= waypointZone)
00489                 waypointReached_flag = 1;
00490             else
00491                 waypointReached_flag = 0; 
00492 /*#LV*/                
00493 
00494         }
00495         Thread::wait(imu_UpdatePeriodMS);
00496     }
00497 }
00498 
00499 /**
00500  * waypoint tracking loop as individual thread
00501  */
00502 void waypointCmd_thread(void const *n)
00503 {
00504     while(true) {
00505         //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
00506         if(imu.imu_stabilized[0] ==1) {
00507             if(waypoint_index > totalWaypoints) {
00508                 target_velocity = 0.0; //stop the robot
00509                 waypointSetFinish_flag = 1;
00510             }
00511 
00512             if(waypointReached_flag == 1 && waypointSetFinish_flag == 0) {
00513                 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
00514                 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
00515                 target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
00516                 waypoint_index++;
00517             }
00518         }
00519         Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
00520     }
00521 }
00522 
00523 
00524 
00525 
00526 
00527 
00528 
00529 /**
00530  * Debug data print loop as an individual thread
00531  */
00532 #define print_lines 15 //number of info lines being printed on screen
00533 void print_thread(void const *n)
00534 {
00535     //clear 14 lines while going up, these are the initilization lines printed on screen
00536     for(int l=14; l>0; l--) {
00537         Debug.printf("\e[1A"); //go up 1 line
00538         Debug.printf("\e[K"); //clear line
00539     }
00540 
00541     Debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
00542     for(int l=print_lines; l>0; l--) Debug.printf("\n");
00543     Debug.printf("\n===================================================");
00544     Debug.printf("\e[1A"); //go up 1 line
00545 
00546     while(true) {
00547         //move cursor up # of lines printed to create a static display and clear the first line
00548         for(int l=print_lines; l>0; l--) Debug.printf("\e[1A");
00549         Debug.printf("\e[K");
00550 
00551         Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
00552         Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
00553         Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
00554         Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
00555         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]);
00556 
00557 //        Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
00558         Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
00559         Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
00560         Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", Error, speedChange, k3);
00561         Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
00562 //        Debug.printf("Ranging: %u, %u, %u, %u, %u \n\e[K", RangeSensor.exleft, RangeSensor.left, RangeSensor.front, RangeSensor.right, RangeSensor.exright);
00563 
00564 
00565         Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
00566         //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
00567         Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
00568         Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
00569 
00570         //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);
00571         //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);
00572 
00573 
00574         //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
00575         
00576         Thread::wait(PrintLoop_PeriodMS);
00577     }
00578 }