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
Diff: orion_main.cpp
- Revision:
- 27:2db168d9fb18
- Parent:
- 26:32eaf3c3ac2e
- Child:
- 28:39d694b0e998
diff -r 32eaf3c3ac2e -r 2db168d9fb18 orion_main.cpp --- a/orion_main.cpp Thu Mar 15 09:14:43 2018 +0000 +++ b/orion_main.cpp Thu Mar 15 10:14:47 2018 +0000 @@ -34,11 +34,37 @@ */ #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 @@ -81,18 +107,7 @@ /* THREAD */ void imu_thread(void const *n); -/* ** */ -//------------- -//** Communications ** -//nRF24NetworkHandler comm; //nRF24 radio and network layer handler function -//uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted -//uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, - -//cmdParser wirelessCmd; - - -/* ** */ //------------- //** Power Monitor ** @@ -137,29 +152,8 @@ //** Attitude control ** attitudeControl attitudeControl; -//pidAttitudeControl pidPitchControl; -//pidAttitudeControl pidRollControl; float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control - -/* THREAD */ - -/* ** */ -//------------- - - -/* ** */ -//------------- - -////** Robot data recorder ** -//RDR virgoRDR; -//uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled - -/* THREAD */ - -/* ** */ -//------------- - //** Declarations of misc functions ** Serial Debug(uart_TX, uart_RX); //Debug serial port DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication @@ -167,8 +161,9 @@ /* 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 /* ** */ -//------------- + //----------------------------------------------------------------------------- @@ -206,6 +201,85 @@ 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"); @@ -264,6 +338,33 @@ } } + +/** + * 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 */ @@ -317,43 +418,10 @@ //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) { if(imu.imu_stabilized[1] ==1) { - pitch_th = DEG_TO_RAD(imu.Pose[0]); - if(pitch_th < -1*M_PI) pitch_th += 2*M_PI; - if(pitch_th > M_PI) pitch_th -= 2*M_PI; - if(pitch_th < -1*M_PI) pitch_th += 2*M_PI; - - - pitch_om = DEG_TO_RAD(imu.AngVel[0]); - - yaw_th = DEG_TO_RAD(imu.Pose[2]); - - yaw_om = DEG_TO_RAD(imu.AngVel[2]); - - wheelTh_l = odometry.revolutions[0]*(-2*M_PI); - wheelTh_r = odometry.revolutions[1]*(-2*M_PI); - - wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI); - wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI); - - //ref_dtheta = rpm_smc*(-2*M_PI/60.0); //*generalFunctions::abs_f(sin(2*M_PI*imuTime/(15*2))); - //ref_theta = ref_dtheta * imuTime; - //ref_gamma = DEG_TO_RAD(30.0) ;//* sin(2*M_PI*imuTime/(30*2)); - - ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0); - ref_gamma = purePursuit_gamma; - ref_dgamma = purePursuit_omega; - -// attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(), -// pitch_th, pitch_om, yaw_th , yaw_om, -// wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r, -// 0, ref_dbeta, ref_beta, -// 0, ref_dtheta, ref_theta, -// 0, ref_dgamma, ref_gamma, -// &u1, &u2); 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 ); //new controller + waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3, 3.5); //new controller @@ -484,7 +552,7 @@ 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]);