Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
orion_main.cpp
- Committer:
- ahmed_lv
- Date:
- 2018-03-20
- Revision:
- 30:44676e1b38f8
- Parent:
- 29:a6a812ee83ea
File content as of revision 30:44676e1b38f8:
/** * @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**/*#LV*/ float bearingToWaypoint, robotFrame, prevError, speedChange; float k3, cmdL, cmdR, Error; //**end of new control **/*#LV*/ 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) { /*#LV*/ PID_B.setSpeedChange( &W_l, &W_r, &speedChange, &Error, target_waypoint, localization.position, imu.Pose[2], &robotFrame, waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3, 3.5 ); //new controller /*#LV*/ 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; /*#LV*/ PID_B.findRobotFrameDistance(target_waypoint, localization.position, &robotFrame); if(robotFrame <= waypointZone) waypointReached_flag = 1; else waypointReached_flag = 0; /*#LV*/ } 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", Error, 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); } }