Alvee Ahmed
Waypoint Command + Obstacle Avoidance + Controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler MAX17048 Servo MODSERIAL
Fork of
Orion_PCB_test_Faulhaber_gear_ratio41_waypoint_cmd
by 
Team Virgo v3
Revision 30:3cfa8d7f84de, committed 2018-06-18
- Comitter:
- ahmed_lv
- Date:
- Mon Jun 18 02:51:56 2018 +0000
- Parent:
- 29:43056f5cd0db
- Commit message:
- Waypoint Command + Obstacle Avoidance + Controller
Changed in this revision
diff -r 43056f5cd0db -r 3cfa8d7f84de 00_ControllerCore/main.h --- a/00_ControllerCore/main.h Fri Jun 01 01:54:04 2018 +0000 +++ b/00_ControllerCore/main.h Mon Jun 18 02:51:56 2018 +0000 @@ -7,6 +7,8 @@ #ifndef VirgoMain_H #define VirgoMain_H +#include "pidBearing.h" +#include "MotionPlanning.h" #include "mbed.h" #include "rtos.h"
diff -r 43056f5cd0db -r 3cfa8d7f84de 00_ControllerCore/orion_pinmapping.h --- a/00_ControllerCore/orion_pinmapping.h Fri Jun 01 01:54:04 2018 +0000 +++ b/00_ControllerCore/orion_pinmapping.h Mon Jun 18 02:51:56 2018 +0000 @@ -49,14 +49,7 @@ #define rasp_TX PA_9 #define rasp_RX PA_10 -//#define debug_LED PC_8 //Ver 2 -#define debug_LED PC_1 //Ver 1 -#define ranging_i2c_SDA PB_4 //Ver 1 -#define ranging_i2c_SCL PA_8 - -//#define ranging_i2c_SDA PC_9 //Ver 2 -//#define ranging_i2c_SCL PA_8 //#define XS1 PC_5 @@ -66,18 +59,32 @@ //#define XS4 PB_0 //#define XS5 PB_2 -#define XS1 PB_1 //Ver1 -#define XS2 PA_5 -#define XS3 PA_7 +//Ver 1 +#define debug_LED PC_1 //Ver 1 + +#define ranging_i2c_SDA PB_4 //Ver 1 +#define ranging_i2c_SCL PA_8 -#define XS4 PB_0 -#define XS5 PB_2 +#define XS5 PB_1 +#define XS4 PA_5 +#define XS3 PA_7 +#define XS2 PB_0 +#define XS1 PB_2 + -//#define XS1 PB_1 //Ver2 -//#define XS2 PC_5 -//#define XS3 PA_7 -// -//#define XS4 PB_0 -//#define XS5 PB_2 +//Ver 2 + +//#define debug_LED PC_8 //Ver 2 + +//#define ranging_i2c_SDA PC_9 //Ver 2 +//#define ranging_i2c_SCL PA_8 + +//#define XS5 PB_0 +//#define XS4 PB_1 +//#define XS3 PB_2 +//#define XS2 PA_7 +//#define XS1 PC_5 + + #endif \ No newline at end of file
diff -r 43056f5cd0db -r 3cfa8d7f84de 01_DriveTrain/pidBearing.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/01_DriveTrain/pidBearing.cpp Mon Jun 18 02:51:56 2018 +0000
@@ -0,0 +1,49 @@
+ #include "pidBearing.h"
+
+pidBearing::pidBearing()
+ {
+// kp = 0.32;
+// kd = 20;
+// ki = 0.0;
+ }
+
+void pidBearing::setSpeedChange(float* wl, float* wr,
+ float mplan[2],
+ float local[2],
+ float yaw,
+ float* linearSpeed, int targetV, float kp, float kd, int GTG)
+ {
+ bTW = 90 - 180/M_PI * atan2((mplan[1]-local[1]),(mplan[0]-local[0])); //angle due X-axis of IMU
+
+ if (bTW>=0.0) iTA = 360 - bTW; //Since YAW decreases in the clockwise direction, if target is to the right of IMU to achieve should be 360 minus the bearing to the waypoint
+ else iTA = 0 - bTW; //Since YAW decreases in the anticlockwise direction, if target is to the left of IMU to acieve should 0 minus (negative sign) bearing to the waypoint
+
+ offset = yaw - iTA; //this is to calculate how far off the bot is from it's current IMU yaw to the one it needs to be.
+
+ if (offset >=180) aTR = offset - 360; //this is to determine the shortest angle to rotate.
+ else aTR = offset - 0;
+
+ if (generalFunctions::abs_f(aTR)>180) Error = 360 - generalFunctions::abs_f(aTR);
+ else Error = aTR;
+
+ sumError = sumError + Error;
+
+ speedChange = -1 *(kd * (Error - prevError) + kp * (Error));
+ prevError = Error;
+
+ if (GTG == 1){
+ Error = generalFunctions::abs_f(Error);
+ Error = generalFunctions::constrain_f(Error, 0.1, 180);
+ *linearSpeed = generalFunctions::constrain_f((*linearSpeed/Error)*80, 0, 17.5);}
+
+
+ *wl = -1 * targetV * (*linearSpeed - speedChange); // linear component - rotation component
+ *wr = -1 * targetV * (*linearSpeed + speedChange); // linear component + rotation component
+ }
+
+
+
+void pidBearing::findRobotFrameDistance(float target[2], float local[2])
+ {
+ robotFrame = sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2));
+ }
\ No newline at end of file
diff -r 43056f5cd0db -r 3cfa8d7f84de 01_DriveTrain/pidBearing.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/01_DriveTrain/pidBearing.h Mon Jun 18 02:51:56 2018 +0000
@@ -0,0 +1,34 @@
+#ifndef pidBearing_H
+#define pidBearing_H
+
+#include "generalFunctions.h"
+#include "mbed.h"
+#include "orion_pinmapping.h"
+#include "config.h"
+#include "math.h"
+
+class pidBearing
+{
+public:
+ //motorDriver(PinName mPlus, PinName mMinus, int freq_khz);
+ //void setPWM(float val);
+
+ pidBearing();
+ float Error, robotFrame, RF, linearSpeed, speedChange;
+ //float kp, kd, ki;
+ void setSpeedChange(float* wl, float* wr,
+ float mplan[2],
+ float local[2],
+ float yaw,
+ float* linearSpeed, int targetV, float kp, float kd, int GTG);
+
+ void findRobotFrameDistance(float target[2], float local[2]);
+
+private:
+
+ float iTA, offset, aTR, prevError, bTW, sumError, max_in, minDist;
+
+};
+
+
+#endif
\ No newline at end of file
diff -r 43056f5cd0db -r 3cfa8d7f84de 02_Localization/virgo3_imuHandler.lib --- a/02_Localization/virgo3_imuHandler.lib Fri Jun 01 01:54:04 2018 +0000 +++ b/02_Localization/virgo3_imuHandler.lib Mon Jun 18 02:51:56 2018 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/harrynguyen/code/virgo3_imuHandler_Orion_PCB/#1422f990b462 +https://os.mbed.com/users/ahmed_lv/code/virgo3_imuHandler/#52a455fb107a
diff -r 43056f5cd0db -r 3cfa8d7f84de Configuration/config.h --- a/Configuration/config.h Fri Jun 01 01:54:04 2018 +0000 +++ b/Configuration/config.h Mon Jun 18 02:51:56 2018 +0000 @@ -19,7 +19,7 @@ #define imu_UpdateRateHz 110 // imu update rate in Hz #define imu_UpdatePeriodMS (1000/imu_UpdateRateHz) // imu update rate in ms -#define motorControl_UpdateRateHz 50 // motorControl update rate in Hz +#define motorControl_UpdateRateHz 100//50 // motorControl update rate in Hz #define motorControl_UpdatePeriodMS (1000/motorControl_UpdateRateHz) // motorControl update rate in ms #define odometry_UpdateRateHz 125 //odometry update rate in Hz @@ -108,17 +108,17 @@ //* ** * //* Localization * -#define wheel_dia 30*1/4.0 //wheel dia in mm * effective hypocyclic gear ratio -#define track_width 78 //wheel dia in mm +#define wheel_dia 42.26*1/4.0//30*1/4.0 //wheel dia in mm * effective hypocyclic gear ratio +#define track_width 82.57//78 //wheel dia in mm //------------------------------------------------------------------------------------------------- //** PurePursuit controller ************************************************************************* -#define track_width 78 //track width in mm +#define track_width 82.57//78 //track width in mm #define driveTrain_minV 100//175 //min velocity of individual drive in mm/s -#define driveTrain_maxV 500//235 //max velocity of individual drive in mm/s +#define driveTrain_maxV 700//500//235 //max velocity of individual drive in mm/s //-------------------------------------------------------------------------------------------------
diff -r 43056f5cd0db -r 3cfa8d7f84de MotionPlanning/MotionPlanning.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionPlanning/MotionPlanning.cpp Mon Jun 18 02:51:56 2018 +0000
@@ -0,0 +1,231 @@
+#include "MotionPlanning.h"
+
+
+motionPlanning::motionPlanning()
+ {limit = 350;
+ limitFW = 280;//was 350
+
+ Vmax = 17.5;;
+ alpha = 1; //for GTG
+
+ c = 12000000;
+ epsilon = 550000; //these numbers were determined in excel
+ timeAllowed = 2.0;
+ }
+
+
+void motionPlanning::planTrack(uint16_t max[5],
+ float target[2],
+ float local[2],
+ float dueXaxis, //starting with extreme right sensor
+ float robotF, float Error, float time)
+
+
+
+ {
+ obstacleDist(max);
+ //trapped(max, robotF, time);
+
+// if (obstacleDistance < limit && FW == false) mode = 'T';
+// else if (FW == true) mode = 'F';
+// else mode = 'G';
+ mode = 'G';
+
+ switch(mode) {
+ case 'F'://Follow
+ clearShot(target, local, dueXaxis);
+ if (max[CS] > limitFW && compareProgress > robotF) {FW = false;}
+ else {
+ n = 0;
+ pTrack(max, local, dueXaxis);
+ followBoundary(max, local, robotF, time);
+
+ switch(n0) {
+ case -90://LV
+ if (rs == 2) n = n0, sFW = 2;
+ else if (rs > 2) n = angleCW, sFW = 4;
+ else if (rs < 2 && angleCCW > 0) n = angleCCW, sFW = 0;
+ break;
+ case 90://LV
+ if (rs == 2) n = n0, sFW = 2;
+ else if (rs < 2) n = angleCCW, sFW = 0;
+ else if (rs > 2 && angleCW < 0) n = angleCW, sFW = 4;
+ break;
+ }
+
+
+
+ pTrack(max, local, dueXaxis);
+ AOSpeed(sFW, local);
+
+ plan[0] = obs[2][0];
+ plan[1] = obs[2][1];
+ }
+ break;
+
+ case 'T'://Turn
+ if(rs > 2 || (rs==2 && rs2 < 2 /*Error >0*/)) n = -90;//LV//-90; //clockwise
+ else if (rs < 2 || (rs==2 && rs2 > 2/*Error <0*/)) n = 90;//LV//90; //counter-clockwise
+
+ wallTurn(target,local);
+ pTrack(max, local, dueXaxis);
+ AOSpeed(rs, local);
+
+
+ plan[0] = obs[rs][0];
+ plan[1] = obs[rs][1];
+
+
+ break;
+
+ case 'G'://Goal
+ GTSpeed(target,local);
+ plan[0] = target[0];//target[0];
+ plan[1] = target[1];//target[1];
+ break;
+
+
+ }
+ }
+
+
+void motionPlanning::pTrack(uint16_t max[5],
+ float local[2],
+ float dueXaxis) {
+
+
+ for (int r = 0; r < 5; r++)
+ {
+ point = generalFunctions::constrain_f(max[r], 40, 500);
+
+ /*0 for 0th, 45 for 1st and so on ending with 180 for 4th in Robot Frame*/
+ sensorAngle = (45*r);
+
+ /*dueXaxis by itself is the deviation of the 0th sensor from the X axis of global frame
+ e.g. if 0th sensor is 270 degrees from X axis then 1st is 315, 2nd is 0, 3rd is 45 and 4th is 90 all in global frame and cc about X axis
+
+ n can be -90 or 90 depending on which direction to rotate*/
+ float sensorAngleGlobal = (dueXaxis + 45*(r) + n);
+ if (sensorAngleGlobal > 360){
+ sensorAngleGlobal = sensorAngleGlobal - 360;}
+
+
+ /*in global frame the waypoint is at right angles (if n != 0) to the obstacle set using sensorAngleGlobal and the distance of the obstacle*/
+ obs[r][0] = local[0] + point*cos(sensorAngleGlobal*(M_PI/180));
+ obs[r][1] = local[1] + point*sin(sensorAngleGlobal*(M_PI/180));
+
+ /*in robot frame*/
+ obsRF[r][0] = point*cos(sensorAngle*(M_PI/180));
+ obsRF[r][1] = point*sin(sensorAngle*(M_PI/180));
+
+ }
+ }
+
+
+void motionPlanning::obstacleDist(uint16_t max[5]) {
+
+ /*compare all sensor readings for the one with the closest obstacle*/
+ obstacleDistance = limit;
+ for (int a = 0; a < 5; a++){
+ if (max[a] < obstacleDistance){
+ rs = a;
+ obstacleDistance = max[a];}
+ }
+
+ farthest = 0;
+ for (int b = 0; b < 5; b++){
+ if (max[b] > farthest){
+ rs2 = b;
+ farthest = max[b];}
+ }
+
+
+ }
+
+void motionPlanning::wallTurn(float target[2],
+ float local[2]) {
+
+ n0 = n; //remember which direction I turned to follow wall
+ compareProgress = sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2)); //to compare and determine whether to keep following wall or go-to-goal
+ FW = true; //set to determine robot mode
+}
+
+
+void motionPlanning::GTSpeed(float target[2], float local[2]){
+
+
+ /***____________________________________***/
+ /*determine linear speed for Go-To-Goal*/
+ float RF = sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2)); //distance between target
+
+ //Go To Goal
+ GTGTerm = -1*alpha*(pow((RF/1000),2));
+ kGTG = ( Vmax * (1-exp(GTGTerm)))/(RF);
+ uGTG = kGTG * (RF);
+
+// if (RF <= 30)
+// linearSpeed = 0.0;
+// else
+ linearSpeed = uGTG;
+ linearSpeed = generalFunctions::constrain_f(linearSpeed, Vmax*0.5, Vmax);
+
+ kp = 1.0;
+ kd = 26;
+ GTGTrue = 1;
+ /***____________________________________***/
+ }
+
+void motionPlanning::AOSpeed(int r, float local[2]){
+
+
+ /***____________________________________***/
+ /*determine linear speed for Avoid Obstacle.*/
+ float e = sqrt(pow((obs[r][0] - local[0]),2) + pow((obs[r][1] - local[1]),2)); //distance from obstacle to robot
+
+ AOTerm = c/(e*e + epsilon);
+ kAO = (1/e) * AOTerm;
+ uAO = kAO * e;
+
+ linearSpeed = uAO;
+
+ kp = linearSpeed/80;// change to 90 for normal floor;
+ kp = generalFunctions::constrain_f(kp, 0.2, 1.0);
+ kd = 26;// change to 22 for normal floor;
+ GTGTrue = 1;//was 0
+ /***____________________________________***/
+
+
+ }
+
+void motionPlanning::clearShot(float target[2], float local[2], float dueXaxis){
+
+ bearingTT = 180/M_PI * atan2((target[1]-local[1]),(target[0]-local[0])); //angle due X-axis of IMU
+
+ if (bearingTT > 337.5 || bearingTT < 22.5) bearingTT = 0;
+
+
+ //find the sensor facing imuToTarget
+ for (int r = 0; r < 5; r++){
+ float sensorCS = (dueXaxis + 45*r);
+
+ if (sensorCS > 360){
+ sensorCS = sensorCS - 360;}
+
+ if (sensorCS < bearingTT+22.5 && sensorCS > bearingTT-22.5){CS = r; r = 5;}
+ else {CS = 5;}
+ }
+ }
+
+
+
+
+void motionPlanning::followBoundary(uint16_t max[5], float local[2], float robotF, float time){
+
+ obsFWRX = obsRF[1][0] - (obsRF[0][0] - (max[0] - limitFW));
+ obsFWRY = obsRF[1][1] - obsRF[0][1];
+ angleCCW = -1*(90 - 180/M_PI * atan2(obsFWRY, obsFWRX));
+
+ obsFWLX = obsRF[3][0] - (obsRF[4][0] + (max[4] - limitFW));
+ obsFWLY = obsRF[3][1] - obsRF[4][1];
+ angleCW = -1*(90 - 180/M_PI * atan2(obsFWLY, obsFWLX));
+}
\ No newline at end of file
diff -r 43056f5cd0db -r 3cfa8d7f84de MotionPlanning/MotionPlanning.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionPlanning/MotionPlanning.h Mon Jun 18 02:51:56 2018 +0000
@@ -0,0 +1,49 @@
+#ifndef motionPlanning_H
+#define motionPlanning_H
+
+#include "generalFunctions.h"
+#include "mbed.h"
+#include "orion_pinmapping.h"
+#include "config.h"
+#include "math.h"
+
+
+class motionPlanning
+{
+public:
+ motionPlanning();
+ float plan[2], linearGain, obstacleDistance, largest, targetDistance[5], compareProgress, linearSpeed, bearingTT, imuTT, limit, limitFW, avgSum, prevRobotF, farthest, spent, count, angleCW, angleCCW;
+ int sensorDir, rs, rs2, sFW, n, n0, CS, AOreverse, GTGreverse, rev, cheat;
+ float kp, kd, GTGTrue;
+ bool FW, Trap, s;
+ char mode;
+ float obsFWLX, obsFWLY, obsFWRX, obsFWRY, obs[5][2], obsRF[5][2], target[2], compareAngle;
+ void planTrack(uint16_t max[5],
+ float target[2],
+ float local[2],
+ float dueXaxis,
+ float robotF, float Error, float time);
+
+
+
+private:
+ float point;
+ float dueNorth, sensorAngle, theta;
+ int sensorNo;
+ float alpha;
+ float Vmax, timeAllowed;
+ float GTGTerm,kLin, sigma, cAO, kGTG, uGTG, AOTerm, c, kAO, epsilon, uAO;
+
+
+ void followBoundary(uint16_t max[5], float local[2], float robotF, float time);
+ void pTrack(uint16_t max[5], float local[2], float dueXaxis);
+ void obstacleDist(uint16_t max[5]);
+ void wallTurn(float target[2], float local[2]);
+ void GTSpeed(float target[2], float local[2]);
+ void AOSpeed(int r, float local[2]);
+ void clearShot(float target[2], float local[2], float dueXaxis);
+ void trapped(uint16_t max[5], float robotF, float time);
+};
+
+
+#endif
\ No newline at end of file
diff -r 43056f5cd0db -r 3cfa8d7f84de main.cpp
--- a/main.cpp Fri Jun 01 01:54:04 2018 +0000
+++ b/main.cpp Mon Jun 18 02:51:56 2018 +0000
@@ -28,14 +28,12 @@
DigitalOut xshut1(XS1);
DigitalOut xshut2(XS2);
DigitalOut xshut3(XS3);
-
DigitalOut xshut4(XS4);
DigitalOut xshut5(XS5);
-VL53L0X sensor(&ItC_ranging, &time_r);
+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);
@@ -43,9 +41,23 @@
struct RangeData{
uint16_t fwd,right,right_d,left_d,left;
double theta_idx;
+
+ uint16_t range[5];
+ float sensorValues[5][5];
};
RangeData RangeSensor;
+/*LV*/
+
+pidBearing PID_B; //PID control for bearing
+motionPlanning MP;
+//void ranging_thread(void const *n);
+void motionPlanning_thread(void const *n);
+
+/*LV*/
+
+
+
//** IMU **
/* THREAD */
void imu_thread(void const *n);
@@ -71,7 +83,7 @@
Timer motorControl_t;
float rpm_cmd[2]; //drive motor rpm command
float rpm_compensated[2]; //rpm command compensated by acc limit
-float targetAcceleration = 150.0; //RPM/s acceleration
+float targetAcceleration = 350.0; //RPM/s acceleration
float pwm_cmd[2]; //drive motor pwm command
bool motor_enable = 0;
@@ -235,7 +247,12 @@
// //** start OdometryUpdate function as Thread **
Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
debugprint.printf("* Odometry routine started *\n");
-//
+//
+
+ //** start Motion Planning funtion as Thread **
+ Thread MotionPlanning_function(motionPlanning_thread, NULL, osPriorityNormal);
+ debugprint.printf("* Motion Planning routine started *\n");
+
// //** start MotorControl function as Thread **
Thread MotorControl_function(motorControl_thread, NULL, osPriorityHigh); // should be osPriorityHigh
debugprint.printf("* Motor control routine started *\n");
@@ -284,11 +301,11 @@
debugprint.printf("Sensor 1: \nXSHUT ON\n");
Thread::wait(W);
-sensor.init();
+sensor1.init();
debugprint.printf("S1 Initialized...\n");
Thread::wait(W);
-sensor.setAddress((uint8_t)22);
+sensor1.setAddress((uint8_t)22);
debugprint.printf("S1 Address set...\n");
@@ -340,13 +357,13 @@
//////////////////////////////////////////////////
-sensor.startContinuous();
+sensor1.startContinuous();
sensor2.startContinuous();
sensor3.startContinuous();
sensor4.startContinuous();
sensor5.startContinuous();
-debugprint.printf("S5 Address set... %u \n",sensor.readRangeContinuousMillimeters());
+debugprint.printf("S5 Address set... %u \n",sensor1.readRangeContinuousMillimeters());
debugprint.printf("S5 Address set... %u \n",sensor2.readRangeContinuousMillimeters());
debugprint.printf("S5 Address set... %u \n",sensor3.readRangeContinuousMillimeters());
@@ -436,23 +453,67 @@
*/
void static_ranging_thread(void const *n)
{
- while(true)
+// while(true)
+// {
+//// if(imu.imu_stabilized[1] ==1){
+// RangeSensor.right = sensor.readRangeContinuousMillimeters();
+// RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
+// RangeSensor.fwd = sensor3.readRangeContinuousMillimeters();
+//
+// RangeSensor.left_d = sensor4.readRangeContinuousMillimeters();
+// RangeSensor.left = sensor5.readRangeContinuousMillimeters();
+//// if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())
+//// debugprint.printf(" TIMEOUT\r\n");
+//// }
+// Thread::wait(10);
+// }
+
+
+
+ while(1)
{
-// if(imu.imu_stabilized[1] ==1){
- RangeSensor.right = sensor.readRangeContinuousMillimeters();
- RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
- RangeSensor.fwd = sensor3.readRangeContinuousMillimeters();
+ //if(imu.imu_stabilized[1] ==1){
+
+ for (int n=0; n<5; n++){//stores 5 values from each sensor
+ RangeSensor.sensorValues[0][n] = sensor1.readRangeContinuousMillimeters();//extreme right
+ RangeSensor.sensorValues[1][n] = sensor2.readRangeContinuousMillimeters();
+ RangeSensor.sensorValues[2][n] = sensor3.readRangeContinuousMillimeters();
+ RangeSensor.sensorValues[3][n] = sensor4.readRangeContinuousMillimeters();
+ RangeSensor.sensorValues[4][n] = sensor5.readRangeContinuousMillimeters();}//extreme left
- RangeSensor.left_d = sensor4.readRangeContinuousMillimeters();
- RangeSensor.left = sensor5.readRangeContinuousMillimeters();
-// if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())
-// debugprint.printf(" TIMEOUT\r\n");
-// }
- Thread::wait(10);
- }
+
+ for (int i=0; i<5; i++){
+ RangeSensor.range[i] = generalFunctions::moving_window(RangeSensor.sensorValues[i], 5);}
+
+ //}
+ Thread::wait(1.0);
+
+ }
+
}
-
+/**/
+/**
+ * motion planning loop as an individual thread
+ */
+
+ void motionPlanning_thread(void const *n){
+
+ while(true) {
+
+ if(imu.imu_stabilized[1] ==1){
+
+ MP.planTrack(RangeSensor.range,
+ target_waypoint,
+ localization.position,
+ imu.Pose[2],
+ PID_B.robotFrame, PID_B.Error, imuTime);
+ }
+ Thread::wait(4.0);
+
+ }
+ }
+/**/
void odometry_thread(void const *n)
{
@@ -484,77 +545,48 @@
{
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) {
-// debugprint.printf("%.2f Running 3 motorControl \n", imuTime);
+
+
//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);
+ PID_B.setSpeedChange( &W_l, &W_r,
+ MP.plan,
+ localization.position,
+ imu.Pose[2],
+ &MP.linearSpeed, target_velocity, MP.kp, MP.kd, MP.GTGTrue); //new controller
- wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
- wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
-
-// Rasp.printf("%.2f \n", purePursuit_velocity);
- ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
- ref_gamma = purePursuit_gamma;
- ref_dgamma = purePursuit_omega;
+ if(motor_enable == 1) {
+ 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]=700;
+// rpm_cmd[1]=700;
- 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);
-
+// if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) ) //was 500 and 475
+// 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;
-// Rasp.printf("Reach %d \n", waypointReached_flag);
-// Rasp.printf("Finsih %d \n", waypointSetFinish_flag);
-// if(waypointSetFinish_flag == 0 && waypointReached_flag == 0 ) {
-// Rasp.printf("Motor enable: %d \n", motor_enable);
- if (motor_enable == 1){
- rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);// Virgo
- rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
-// Rasp.printf("%.2f \n", rpm_cmd[0]);
-// rpm_cmd[0]=W_l*60/(2*M_PI)*(1.0); //Orion
-// rpm_cmd[1]=W_r*60/(2*M_PI)*(1.0);
-// rpm_cmd[0]=0;
-// rpm_cmd[1]=0;
-
- if( (generalFunctions::abs_f(rpm_cmd[0]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 10.0) )
- rpm_cmd[0] = 47.5*generalFunctions::sign_f(rpm_cmd[0]);
- else if(generalFunctions::abs_f(rpm_cmd[0]) <= 10.0)
- rpm_cmd[0] = 0;
-
- if( (generalFunctions::abs_f(rpm_cmd[1]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 10.0) )
- rpm_cmd[1] = 47.5*generalFunctions::sign_f(rpm_cmd[1]);
- else if(generalFunctions::abs_f(rpm_cmd[1]) <= 10.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 {
+ }
+ else {
rpm_cmd[0]=0;
rpm_cmd[1]=0;
@@ -567,7 +599,8 @@
drive.setPWM_L(pwm_cmd[0]);
drive.setPWM_R(pwm_cmd[1]);
-
+
+
}
motorControl_t.reset();
@@ -587,9 +620,13 @@
//// Rasp.printf("%.2f,%.2f,%.2f; ",imu.AngVel[0], imu.AngVel[1], imu.AngVel[2]); //ang vel x,y,z
//// Rasp.printf("%.2f,%.2f,%.2f; ",imu.LinAcc[0], imu.LinAcc[1], imu.LinAcc[2]); //Linear acc
Rasp.printf("%.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
- Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+ Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);
+
Rasp.printf("%.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
Rasp.printf("%d,%d,%d; ",trilateration.range_array[0], trilateration.range_array[1], trilateration.range_array[2]); //uwb range data
+
+ //Rasp.printf("%.2f; ",PID_B.robotFrame); //LV RF
+
// Rasp.printf("%.2f,%.2f; ",odometry.revolutions[0] * 2*M_PI, odometry.revolutions[1] * 2*M_PI); //Wheel Position L,R
Rasp.printf("%.2f,%.2f; ",odometry.rpm[0] * 2*M_PI / 60, odometry.rpm[1] * 2*M_PI / 60); //Wheel Speed L,R
Rasp.printf("%.2f,%.2f; ", target_waypoint[0], target_waypoint[1]); //Waypoint heading to
@@ -965,12 +1002,23 @@
// debugprint.printf("%.2f Running 7 purePursuit \n", imuTime);
if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 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]));
+
+// 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;
- if(purePursuit.robotFrame_targetDistance <= waypointZone)
+/*#LV*/
+ PID_B.findRobotFrameDistance(target_waypoint, localization.position);
+
+ if(PID_B.robotFrame <= waypointZone)
waypointReached_flag = 1;
else
- waypointReached_flag = 0;
+ waypointReached_flag = 0;
+/*#LV*/
+
}
Thread::wait(imu_UpdatePeriodMS);
}
@@ -985,7 +1033,7 @@
// debugprint.printf("%.2f waypoint cmd \n", imuTime);
//if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
-
+
// if(waypoint_index > totalWaypoints) {
// target_velocity = 0.0; //stop the robot
// waypointSetFinish_flag = 1;
@@ -995,7 +1043,7 @@
target_velocity = 0.0; //stop the robot;
motor_enable = 0;
}
-
+
// debugprint.printf("waypointReached_flag = %d, waypoint_ready = %d \n", waypointReached_flag,waypoint_ready);
// debugprint.printf("target waypoint %.2f %.2f \n", target_waypoint[0], target_waypoint[1]);
if(waypointReached_flag == 1 && waypointSetFinish_flag == 0 && waypoint_ready == 1) {
@@ -1003,7 +1051,8 @@
// 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
+ //target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
+ target_velocity = 1.0;
//target_velocity = 90*(driveTrain_maxV/100.0);
waypoint_ready = 0;
// waypoint_index++;
@@ -1147,7 +1196,9 @@
// debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready);
// //debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
// debugprint.printf("Raspberry waypoint: %s \n\e[K", rasp_data);
- debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+
+ debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);
+
// debugprint.printf("UWB ranging: %s %s %s\n\e[K", rangestring_array[0],rangestring_array[1], rangestring_array[2]);
debugprint.printf("UWB ranging: %d %d %d \n\e[K", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
debugprint.printf("x : %f , y : %f , z : %f \n\e[K", trilateration.robot_pos.x, trilateration.robot_pos.y, trilateration.robot_pos.z);