Alvee Ahmed / Orion_PCB_test_Faulhaber_gr41_wptcmd_V1

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

main.cpp@27:c813451bfb4d, 2018-05-21 (annotated)

Committer:
harrynguyen
Date:
Mon May 21 12:02:23 2018 +0000
Revision:
27:c813451bfb4d
Parent:
26:cd503e08a218
Child:
28:edb5e3770ae1
Add change so that the robot accepts angular velocity cmd from Raspberry Pi as degrees instead of radians.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrynguyen 26:cd503e08a218 1 #include "mbed.h"
harrynguyen 26:cd503e08a218 2 #include "imuHandler.h"
harrynguyen 26:cd503e08a218 3 #include "main.h"
harrynguyen 26:cd503e08a218 4
harrynguyen 26:cd503e08a218 5 #include "VL53L0X.h"
harrynguyen 26:cd503e08a218 6 #include "Servo.h"
harrynguyen 26:cd503e08a218 7
harrynguyen 26:cd503e08a218 8 #define W 0.1 //time to initiate the lidar
harrynguyen 26:cd503e08a218 9
harrynguyen 26:cd503e08a218 10 //TODO
harrynguyen 26:cd503e08a218 11 //1,Rewrite the controller core for the robot - currently using Virgo controller
harrynguyen 26:cd503e08a218 12 //2,Add time-out for UWB
harrynguyen 26:cd503e08a218 13
harrynguyen 26:cd503e08a218 14
harrynguyen 26:cd503e08a218 15
harrynguyen 26:cd503e08a218 16 DigitalOut led(NC);
harrynguyen 26:cd503e08a218 17
harrynguyen 26:cd503e08a218 18
harrynguyen 26:cd503e08a218 19 //**Ranging Module**
harrynguyen 26:cd503e08a218 20 /* THREAD */
harrynguyen 26:cd503e08a218 21 void static_ranging_thread(void const *n);
harrynguyen 26:cd503e08a218 22 void ranging_thread(void const *n);
harrynguyen 26:cd503e08a218 23 /* VARIABLE */
harrynguyen 26:cd503e08a218 24 I2C ItC_ranging(ranging_i2c_SDA, ranging_i2c_SCL);
harrynguyen 26:cd503e08a218 25 Timer time_r;
harrynguyen 26:cd503e08a218 26
harrynguyen 26:cd503e08a218 27
harrynguyen 26:cd503e08a218 28 DigitalOut xshut1(XS1);
harrynguyen 26:cd503e08a218 29 DigitalOut xshut2(XS2);
harrynguyen 26:cd503e08a218 30 DigitalOut xshut3(XS3);
harrynguyen 26:cd503e08a218 31
harrynguyen 26:cd503e08a218 32 DigitalOut xshut4(XS4);
harrynguyen 26:cd503e08a218 33 DigitalOut xshut5(XS5);
harrynguyen 26:cd503e08a218 34
harrynguyen 26:cd503e08a218 35 VL53L0X sensor(&ItC_ranging, &time_r);
harrynguyen 26:cd503e08a218 36 VL53L0X sensor2(&ItC_ranging, &time_r);
harrynguyen 26:cd503e08a218 37 VL53L0X sensor3(&ItC_ranging, &time_r);
harrynguyen 26:cd503e08a218 38
harrynguyen 26:cd503e08a218 39 VL53L0X sensor4(&ItC_ranging, &time_r);
harrynguyen 26:cd503e08a218 40 VL53L0X sensor5(&ItC_ranging, &time_r);
harrynguyen 26:cd503e08a218 41
harrynguyen 26:cd503e08a218 42 uint16_t data;
harrynguyen 26:cd503e08a218 43 struct RangeData{
harrynguyen 26:cd503e08a218 44 uint16_t fwd,right,right_d,left_d,left;
harrynguyen 26:cd503e08a218 45 double theta_idx;
harrynguyen 26:cd503e08a218 46 };
harrynguyen 26:cd503e08a218 47 RangeData RangeSensor;
harrynguyen 26:cd503e08a218 48
harrynguyen 26:cd503e08a218 49 //** IMU **
harrynguyen 26:cd503e08a218 50 /* THREAD */
harrynguyen 26:cd503e08a218 51 void imu_thread(void const *n);
harrynguyen 26:cd503e08a218 52
harrynguyen 26:cd503e08a218 53 float imuTime;
harrynguyen 26:cd503e08a218 54 IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class
harrynguyen 26:cd503e08a218 55 DigitalOut imu_reset(imu_RST);
harrynguyen 26:cd503e08a218 56
harrynguyen 26:cd503e08a218 57 /* FUNCTION */
harrynguyen 26:cd503e08a218 58 void imuReset();
harrynguyen 26:cd503e08a218 59 bool imuInit_function();
harrynguyen 26:cd503e08a218 60
harrynguyen 26:cd503e08a218 61 //** ODOMETRY **
harrynguyen 26:cd503e08a218 62 /* THREAD */
harrynguyen 26:cd503e08a218 63 void odometry_thread(void const *n);
harrynguyen 26:cd503e08a218 64
harrynguyen 26:cd503e08a218 65 odometer odometry; //odometer function
harrynguyen 26:cd503e08a218 66 Localization localization; //localization function
harrynguyen 26:cd503e08a218 67
harrynguyen 26:cd503e08a218 68 motorDriver drive; //motor drive train
harrynguyen 26:cd503e08a218 69 pidControl PID_L, PID_R; //pidcontroller for left and right motors
harrynguyen 26:cd503e08a218 70
harrynguyen 26:cd503e08a218 71 Timer motorControl_t;
harrynguyen 26:cd503e08a218 72 float rpm_cmd[2]; //drive motor rpm command
harrynguyen 26:cd503e08a218 73 float rpm_compensated[2]; //rpm command compensated by acc limit
harrynguyen 26:cd503e08a218 74 float targetAcceleration = 150.0; //RPM/s acceleration
harrynguyen 26:cd503e08a218 75 float pwm_cmd[2]; //drive motor pwm command
harrynguyen 26:cd503e08a218 76 bool motor_enable = 0;
harrynguyen 26:cd503e08a218 77
harrynguyen 26:cd503e08a218 78 //** Trajectory tracking **
harrynguyen 26:cd503e08a218 79 /* THREAD */
harrynguyen 26:cd503e08a218 80 void purePursuit_thread(void const *n);
harrynguyen 26:cd503e08a218 81 void waypointCmd_thread(void const *n);
harrynguyen 26:cd503e08a218 82
harrynguyen 26:cd503e08a218 83 purePursuit purePursuit;
harrynguyen 26:cd503e08a218 84 bool purePursuit_enable = 1;
harrynguyen 26:cd503e08a218 85 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
harrynguyen 26:cd503e08a218 86 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
harrynguyen 26:cd503e08a218 87 uint8_t totalWaypoints = 6;
harrynguyen 26:cd503e08a218 88 //kite pattern 200cm long, 100cm wide
harrynguyen 26:cd503e08a218 89 //int16_t waypoints_set[][4] = { {0,0,90,0},
harrynguyen 26:cd503e08a218 90 // {100,100,90,0},
harrynguyen 26:cd503e08a218 91 // {0,200,90,0},
harrynguyen 26:cd503e08a218 92 // {-100,100,90,0},
harrynguyen 26:cd503e08a218 93 // {0,0,90,0},
harrynguyen 26:cd503e08a218 94 // {0,0,90,0},
harrynguyen 26:cd503e08a218 95 // {0,0,90,0}
harrynguyen 26:cd503e08a218 96 //};
harrynguyen 26:cd503e08a218 97
harrynguyen 26:cd503e08a218 98 int16_t waypoints_set[][4] = { {0,0,0,0},
harrynguyen 26:cd503e08a218 99 {0,0,0,0},
harrynguyen 26:cd503e08a218 100 };
harrynguyen 26:cd503e08a218 101
harrynguyen 26:cd503e08a218 102 float waypointZone = 150.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
harrynguyen 26:cd503e08a218 103 uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
harrynguyen 26:cd503e08a218 104 uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
harrynguyen 26:cd503e08a218 105 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
harrynguyen 26:cd503e08a218 106 float target_velocity =0.0; //target velocity in mm/s
harrynguyen 26:cd503e08a218 107 float distanceToWaypoint; //distance from robot to waypoint
harrynguyen 26:cd503e08a218 108 uint8_t waypoint_index=0;
harrynguyen 26:cd503e08a218 109 uint8_t go_cmd=0; //make robot run a waypoint set
harrynguyen 26:cd503e08a218 110
harrynguyen 26:cd503e08a218 111
harrynguyen 26:cd503e08a218 112
harrynguyen 26:cd503e08a218 113 /*Motor Control*/
harrynguyen 26:cd503e08a218 114 /* THREAD */
harrynguyen 26:cd503e08a218 115 void motorControl_thread(void const *n);
harrynguyen 26:cd503e08a218 116 attitudeControl attitudeControl;
harrynguyen 26:cd503e08a218 117
harrynguyen 26:cd503e08a218 118 float rpm_smc = 500;
harrynguyen 26:cd503e08a218 119 float ref_dtheta = 0;
harrynguyen 26:cd503e08a218 120 float ref_theta = 0;
harrynguyen 26:cd503e08a218 121
harrynguyen 26:cd503e08a218 122 float ref_dgamma = 0;
harrynguyen 26:cd503e08a218 123 float ref_gamma = 0;
harrynguyen 26:cd503e08a218 124
harrynguyen 26:cd503e08a218 125 float ref_beta = DEG_TO_RAD(0.0);
harrynguyen 26:cd503e08a218 126 float ref_dbeta = 0;
harrynguyen 26:cd503e08a218 127
harrynguyen 26:cd503e08a218 128 float u1, u2;
harrynguyen 26:cd503e08a218 129
harrynguyen 26:cd503e08a218 130
harrynguyen 26:cd503e08a218 131 //void waypoint_parser(void const *n);
harrynguyen 26:cd503e08a218 132
harrynguyen 26:cd503e08a218 133 //** Communications **
harrynguyen 26:cd503e08a218 134 //nRF24NetworkHandler comm; //nRF24 radio and network layer handler function
harrynguyen 26:cd503e08a218 135 //uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
harrynguyen 26:cd503e08a218 136 //uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status,
harrynguyen 26:cd503e08a218 137 //
harrynguyen 26:cd503e08a218 138 //cmdParser wirelessCmd;
harrynguyen 26:cd503e08a218 139
harrynguyen 26:cd503e08a218 140 /* THREAD */
harrynguyen 26:cd503e08a218 141 //void comm_thread(void const *n);
harrynguyen 26:cd503e08a218 142 /* ** */
harrynguyen 26:cd503e08a218 143 //-------------
harrynguyen 26:cd503e08a218 144 //-----------------------------------------------------------------------------
harrynguyen 26:cd503e08a218 145
harrynguyen 26:cd503e08a218 146 ////** UWB **
harrynguyen 26:cd503e08a218 147 /* THREAD */
harrynguyen 26:cd503e08a218 148 void uwb_thread(void const *n);
harrynguyen 26:cd503e08a218 149 void uwbtriangulation_thread(void const *n);
harrynguyen 26:cd503e08a218 150 /* VARIABLE */
harrynguyen 26:cd503e08a218 151 MODSERIAL uwb(uwb_TX, uwb_RX);
harrynguyen 26:cd503e08a218 152 //RawSerial uwb(uwb_TX, uwb_RX);
harrynguyen 26:cd503e08a218 153 char uwb_data[67];
harrynguyen 26:cd503e08a218 154 char uwb_data1[67];
harrynguyen 26:cd503e08a218 155 volatile bool newline_detected = false;
harrynguyen 26:cd503e08a218 156 //char rangestring_array[3][10];
harrynguyen 26:cd503e08a218 157 //int range_array[4];
harrynguyen 26:cd503e08a218 158 //char range[9];
harrynguyen 26:cd503e08a218 159 //****Trilateration configuration
harrynguyen 26:cd503e08a218 160 Trilateration trilateration;
harrynguyen 26:cd503e08a218 161 bool uwb_data_flag = 0;
harrynguyen 26:cd503e08a218 162 void rxUwbCallback(MODSERIAL_IRQ_INFO *q);
harrynguyen 26:cd503e08a218 163 //void rxUwbCallback();
harrynguyen 26:cd503e08a218 164 //vec3d bestsolution;
harrynguyen 26:cd503e08a218 165 //int distanceArray[4];
harrynguyen 26:cd503e08a218 166 //vec3d anchorArray[3];
harrynguyen 26:cd503e08a218 167 /*FUNCTION*/
harrynguyen 26:cd503e08a218 168 void uwbtriangulation_fn(char* uwb_data);
harrynguyen 26:cd503e08a218 169 ////** End UWB **
harrynguyen 26:cd503e08a218 170
harrynguyen 26:cd503e08a218 171 //***Raspberry Pi Communication***
harrynguyen 26:cd503e08a218 172 /* THREAD */
harrynguyen 26:cd503e08a218 173 void raspberryrx_thread(void const *n);
harrynguyen 26:cd503e08a218 174 void raspberrytx_thread(void const *n);
harrynguyen 26:cd503e08a218 175 /* VARIALE */
harrynguyen 26:cd503e08a218 176 //RawSerial Rasp(PA_9,PA_10);
harrynguyen 26:cd503e08a218 177 MODSERIAL Rasp(rasp_TX, rasp_RX);
harrynguyen 26:cd503e08a218 178 volatile bool rasp_newline_detected = false;
harrynguyen 26:cd503e08a218 179 bool rasp_data_flag = 0;
harrynguyen 26:cd503e08a218 180 char letter[15];
harrynguyen 26:cd503e08a218 181 char rasp_data[30];
harrynguyen 26:cd503e08a218 182 char waypoint_data[30];
harrynguyen 26:cd503e08a218 183 bool waypoint_ready =0;
harrynguyen 26:cd503e08a218 184 /* FUNCTION */
harrynguyen 26:cd503e08a218 185 void waypoint_parser_fn(char* waypoint_data);
harrynguyen 26:cd503e08a218 186 void rxRaspCallback(MODSERIAL_IRQ_INFO *q);
harrynguyen 26:cd503e08a218 187 //void rxRaspCallback();
harrynguyen 26:cd503e08a218 188 //******Debug******
harrynguyen 26:cd503e08a218 189 DigitalOut debugLED(debug_LED);
harrynguyen 26:cd503e08a218 190 Serial debugprint(uart_TX,uart_RX); //debug serial port
harrynguyen 26:cd503e08a218 191 /* THREAD */
harrynguyen 26:cd503e08a218 192 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
harrynguyen 26:cd503e08a218 193 void print_thread(void const *n); //debug printing thread
harrynguyen 26:cd503e08a218 194
harrynguyen 26:cd503e08a218 195
harrynguyen 26:cd503e08a218 196
harrynguyen 26:cd503e08a218 197 int main() {
harrynguyen 26:cd503e08a218 198 debugprint.baud(PC_BAUDRATE);
harrynguyen 26:cd503e08a218 199 Rasp.baud(115200);
harrynguyen 26:cd503e08a218 200
harrynguyen 26:cd503e08a218 201 debugLED =1;
harrynguyen 26:cd503e08a218 202
harrynguyen 26:cd503e08a218 203 //wait_ms(5000);
harrynguyen 26:cd503e08a218 204
harrynguyen 26:cd503e08a218 205 debugprint.printf("** Starting Virgo v3 Routines *************\n\n");
harrynguyen 26:cd503e08a218 206
harrynguyen 26:cd503e08a218 207 // //** start Hearbeat loop as a thread **
harrynguyen 26:cd503e08a218 208 Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 209 debugprint.printf("* Hearbeat loop started *\n");
harrynguyen 26:cd503e08a218 210
harrynguyen 26:cd503e08a218 211 //** start IMU funtion as Thread **
harrynguyen 26:cd503e08a218 212 Thread IMU_function(imu_thread, NULL, osPriorityHigh);
harrynguyen 26:cd503e08a218 213 debugprint.printf("* IMU routine started *\n");
harrynguyen 26:cd503e08a218 214
harrynguyen 26:cd503e08a218 215 // //** start Ranging funtion as Thread **
harrynguyen 26:cd503e08a218 216 // Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 217 // debugprint.printf("* Ranging routine started *\n");
harrynguyen 26:cd503e08a218 218
harrynguyen 26:cd503e08a218 219 // //** start UwbUpdate function as Thread **
harrynguyen 26:cd503e08a218 220 Thread UwbUpdate_function(uwb_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 221 debugprint.printf("* Uwb Update routine started *\n");
harrynguyen 26:cd503e08a218 222
harrynguyen 26:cd503e08a218 223 // //** start Uwb Triangulation function as Thread **
harrynguyen 26:cd503e08a218 224 // Thread UwbTriangulation_function(uwbtriangulation_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 225 // debugprint.printf("* Uwb Triangulation routine started *\n");
harrynguyen 26:cd503e08a218 226
harrynguyen 26:cd503e08a218 227 // //** start Raspberrypi receive function as Thread **
harrynguyen 26:cd503e08a218 228 Thread Raspberryrx_function(raspberryrx_thread, NULL, osPriorityNormal,1024);
harrynguyen 26:cd503e08a218 229 debugprint.printf("* Raspberrypi routine started *\n");
harrynguyen 26:cd503e08a218 230
harrynguyen 26:cd503e08a218 231 // //** start Raspberrypi transmit function as Thread **
harrynguyen 26:cd503e08a218 232 Thread Raspberrytx_function(raspberrytx_thread, NULL, osPriorityNormal,1024);
harrynguyen 26:cd503e08a218 233 debugprint.printf("* Raspberrypi routine started *\n");
harrynguyen 26:cd503e08a218 234
harrynguyen 26:cd503e08a218 235 // //** start OdometryUpdate function as Thread **
harrynguyen 26:cd503e08a218 236 Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
harrynguyen 26:cd503e08a218 237 debugprint.printf("* Odometry routine started *\n");
harrynguyen 26:cd503e08a218 238 //
harrynguyen 26:cd503e08a218 239 // //** start MotorControl function as Thread **
harrynguyen 26:cd503e08a218 240 Thread MotorControl_function(motorControl_thread, NULL, osPriorityHigh); // should be osPriorityHigh
harrynguyen 26:cd503e08a218 241 debugprint.printf("* Motor control routine started *\n");
harrynguyen 26:cd503e08a218 242
harrynguyen 26:cd503e08a218 243 // //** start PurePursuit controller as Thread **
harrynguyen 26:cd503e08a218 244 Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 245 debugprint.printf("* PurePursuit controller routine started *\n");
harrynguyen 26:cd503e08a218 246
harrynguyen 26:cd503e08a218 247 //* //** start Waypoint commander function as Thread **
harrynguyen 26:cd503e08a218 248 Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 249 debugprint.printf("* Waypoint commander routine started *\n");
harrynguyen 26:cd503e08a218 250
harrynguyen 26:cd503e08a218 251 //* //** start Waypoint parser function as Thread **
harrynguyen 26:cd503e08a218 252 // Thread WaypointParser_function(waypoint_parser, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 253 // debugprint.printf("* Waypoint commander routine started *\n");
harrynguyen 26:cd503e08a218 254
harrynguyen 26:cd503e08a218 255 //* //** start comm loop as a thread **
harrynguyen 26:cd503e08a218 256 // Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024);
harrynguyen 26:cd503e08a218 257 // debugprint.printf("* Communications loop started *\n");
harrynguyen 26:cd503e08a218 258
harrynguyen 26:cd503e08a218 259 //* ** start debug print loop as a thread **
harrynguyen 27:c813451bfb4d 260 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
harrynguyen 27:c813451bfb4d 261 debugprint.printf("* Print loop started *\n\n\n");
harrynguyen 26:cd503e08a218 262
harrynguyen 26:cd503e08a218 263 // Thread GetLoop_function(get_thread,NULL,osPriorityNormal, 1024);
harrynguyen 26:cd503e08a218 264 // debugprint.printf("* Get loop started *\n\n\n");
harrynguyen 26:cd503e08a218 265 debugprint.printf("Start\n");
harrynguyen 26:cd503e08a218 266 // pinMode(5, OUTPUT);
harrynguyen 26:cd503e08a218 267 // pinMode(4, OUTPUT);
harrynguyen 26:cd503e08a218 268 xshut1 = 0;
harrynguyen 26:cd503e08a218 269 xshut2 = 0;
harrynguyen 26:cd503e08a218 270 xshut3 = 0;
harrynguyen 26:cd503e08a218 271
harrynguyen 26:cd503e08a218 272 xshut4 = 0;
harrynguyen 26:cd503e08a218 273 xshut5 = 0;
harrynguyen 26:cd503e08a218 274
harrynguyen 26:cd503e08a218 275 debugprint.printf("\n======== Orion v1: Multiple Range Monitor ========\n");
harrynguyen 26:cd503e08a218 276 debugprint.printf("\nXSHUT OFF\n");
harrynguyen 26:cd503e08a218 277
harrynguyen 26:cd503e08a218 278
harrynguyen 26:cd503e08a218 279 Thread::wait(W);
harrynguyen 26:cd503e08a218 280
harrynguyen 26:cd503e08a218 281
harrynguyen 26:cd503e08a218 282
harrynguyen 26:cd503e08a218 283 xshut1 = 1;
harrynguyen 26:cd503e08a218 284 debugprint.printf("Sensor 1: \nXSHUT ON\n");
harrynguyen 26:cd503e08a218 285 Thread::wait(W);
harrynguyen 26:cd503e08a218 286
harrynguyen 26:cd503e08a218 287 sensor.init();
harrynguyen 26:cd503e08a218 288 debugprint.printf("S1 Initialized...\n");
harrynguyen 26:cd503e08a218 289 Thread::wait(W);
harrynguyen 26:cd503e08a218 290
harrynguyen 26:cd503e08a218 291 sensor.setAddress((uint8_t)22);
harrynguyen 26:cd503e08a218 292 debugprint.printf("S1 Address set...\n");
harrynguyen 26:cd503e08a218 293
harrynguyen 26:cd503e08a218 294
harrynguyen 26:cd503e08a218 295 xshut2 = 1;
harrynguyen 26:cd503e08a218 296 debugprint.printf("\nSensor 2: \nXSHUT ON\n");
harrynguyen 26:cd503e08a218 297 Thread::wait(W);
harrynguyen 26:cd503e08a218 298
harrynguyen 26:cd503e08a218 299 sensor2.init();
harrynguyen 26:cd503e08a218 300 debugprint.printf("S2 Initialized...\n");
harrynguyen 26:cd503e08a218 301 Thread::wait(W);
harrynguyen 26:cd503e08a218 302
harrynguyen 26:cd503e08a218 303 sensor2.setAddress((uint8_t)23);
harrynguyen 26:cd503e08a218 304 debugprint.printf("S2 Address set...\n");
harrynguyen 26:cd503e08a218 305
harrynguyen 26:cd503e08a218 306 xshut3 = 1;
harrynguyen 26:cd503e08a218 307 debugprint.printf("\nSensor 3: \nXSHUT ON\n");
harrynguyen 26:cd503e08a218 308 Thread::wait(W);
harrynguyen 26:cd503e08a218 309
harrynguyen 26:cd503e08a218 310 sensor3.init();
harrynguyen 26:cd503e08a218 311 debugprint.printf("S3 Initialized...\n");
harrynguyen 26:cd503e08a218 312 Thread::wait(W);
harrynguyen 26:cd503e08a218 313
harrynguyen 26:cd503e08a218 314 sensor3.setAddress((uint8_t)25);
harrynguyen 26:cd503e08a218 315 debugprint.printf("S3 Address set...\n");
harrynguyen 26:cd503e08a218 316
harrynguyen 26:cd503e08a218 317 //////////////////////////////////////////////////
harrynguyen 26:cd503e08a218 318 xshut4 = 1;
harrynguyen 26:cd503e08a218 319 debugprint.printf("\nSensor 4: \nXSHUT ON\n");
harrynguyen 26:cd503e08a218 320 Thread::wait(W);
harrynguyen 26:cd503e08a218 321
harrynguyen 26:cd503e08a218 322 sensor4.init();
harrynguyen 26:cd503e08a218 323 debugprint.printf("S4 Initialized...\n");
harrynguyen 26:cd503e08a218 324 Thread::wait(W);
harrynguyen 26:cd503e08a218 325
harrynguyen 26:cd503e08a218 326 sensor4.setAddress((uint8_t)27);
harrynguyen 26:cd503e08a218 327 debugprint.printf("S4 Address set...\n");
harrynguyen 26:cd503e08a218 328
harrynguyen 26:cd503e08a218 329 //////////////////////////////////////////////////
harrynguyen 26:cd503e08a218 330 xshut5 = 1;
harrynguyen 26:cd503e08a218 331 debugprint.printf("\nSensor 5: \nXSHUT ON\n");
harrynguyen 26:cd503e08a218 332 Thread::wait(W);
harrynguyen 26:cd503e08a218 333
harrynguyen 26:cd503e08a218 334 sensor5.init();
harrynguyen 26:cd503e08a218 335 debugprint.printf("S5 Initialized...\n");
harrynguyen 26:cd503e08a218 336 Thread::wait(W);
harrynguyen 26:cd503e08a218 337
harrynguyen 26:cd503e08a218 338 sensor5.setAddress((uint8_t)31);
harrynguyen 26:cd503e08a218 339 debugprint.printf("S5 Address set...\n");
harrynguyen 26:cd503e08a218 340
harrynguyen 26:cd503e08a218 341 //////////////////////////////////////////////////
harrynguyen 26:cd503e08a218 342
harrynguyen 26:cd503e08a218 343 sensor.startContinuous();
harrynguyen 26:cd503e08a218 344 sensor2.startContinuous();
harrynguyen 26:cd503e08a218 345 sensor3.startContinuous();
harrynguyen 26:cd503e08a218 346
harrynguyen 26:cd503e08a218 347 sensor4.startContinuous();
harrynguyen 26:cd503e08a218 348 sensor5.startContinuous();
harrynguyen 26:cd503e08a218 349 debugprint.printf("S5 Address set... %u \n",sensor.readRangeContinuousMillimeters());
harrynguyen 26:cd503e08a218 350 debugprint.printf("S5 Address set... %u \n",sensor2.readRangeContinuousMillimeters());
harrynguyen 26:cd503e08a218 351 debugprint.printf("S5 Address set... %u \n",sensor3.readRangeContinuousMillimeters());
harrynguyen 26:cd503e08a218 352
harrynguyen 26:cd503e08a218 353 debugprint.printf("S5 Address set... %u \n",sensor4.readRangeContinuousMillimeters());
harrynguyen 26:cd503e08a218 354 debugprint.printf("S5 Address set... %u \n",sensor5.readRangeContinuousMillimeters());
harrynguyen 26:cd503e08a218 355 // //** start Ranging funtion as Thread **
harrynguyen 26:cd503e08a218 356 Thread Static_Ranging_function(static_ranging_thread, NULL, osPriorityNormal);
harrynguyen 26:cd503e08a218 357 debugprint.printf("* Ranging routine started *\n");
harrynguyen 26:cd503e08a218 358 debugprint.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
harrynguyen 26:cd503e08a218 359
harrynguyen 26:cd503e08a218 360 while(1) {
harrynguyen 26:cd503e08a218 361
harrynguyen 26:cd503e08a218 362 }
harrynguyen 26:cd503e08a218 363
harrynguyen 26:cd503e08a218 364
harrynguyen 26:cd503e08a218 365
harrynguyen 26:cd503e08a218 366 }
harrynguyen 26:cd503e08a218 367
harrynguyen 26:cd503e08a218 368 //****END MAIN*****************************
harrynguyen 26:cd503e08a218 369 //****THREAD AND FUNCTION******************
harrynguyen 26:cd503e08a218 370
harrynguyen 26:cd503e08a218 371 /**
harrynguyen 26:cd503e08a218 372 * heartbeat loop as an individual thread
harrynguyen 26:cd503e08a218 373 */
harrynguyen 26:cd503e08a218 374 void heartbeat_thread(void const *n)
harrynguyen 26:cd503e08a218 375 {
harrynguyen 26:cd503e08a218 376 while(true) {
harrynguyen 26:cd503e08a218 377 if(imu.imu_stabilized[0] ==1) {
harrynguyen 26:cd503e08a218 378 debugLED = !debugLED;
harrynguyen 26:cd503e08a218 379 Thread::wait(Hearbeat_RateMS-50);
harrynguyen 26:cd503e08a218 380 debugLED = !debugLED;
harrynguyen 26:cd503e08a218 381 Thread::wait(50);
harrynguyen 26:cd503e08a218 382 } else
harrynguyen 26:cd503e08a218 383 debugLED = 1;
harrynguyen 26:cd503e08a218 384 }
harrynguyen 26:cd503e08a218 385 }
harrynguyen 26:cd503e08a218 386
harrynguyen 26:cd503e08a218 387 /**
harrynguyen 26:cd503e08a218 388 * imu loop as an individual thread
harrynguyen 26:cd503e08a218 389 */
harrynguyen 26:cd503e08a218 390
harrynguyen 26:cd503e08a218 391 void imu_thread(void const *n)
harrynguyen 26:cd503e08a218 392 {
harrynguyen 26:cd503e08a218 393 bool init_status = imuInit_function();
harrynguyen 26:cd503e08a218 394 Thread::wait(100);
harrynguyen 26:cd503e08a218 395
harrynguyen 26:cd503e08a218 396 while(init_status) {
harrynguyen 26:cd503e08a218 397 // debugprint.printf("%.2f Running 1 imu \n", imuTime);
harrynguyen 26:cd503e08a218 398 imu.imuUpdate();
harrynguyen 26:cd503e08a218 399
harrynguyen 26:cd503e08a218 400 //Usage:
harrynguyen 26:cd503e08a218 401 //imu.Pose[0, 1, 2]; //euler x, y, z
harrynguyen 26:cd503e08a218 402 //imu.AngVel[0, 1, 2]; //AngVel x, y, z
harrynguyen 26:cd503e08a218 403 //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
harrynguyen 26:cd503e08a218 404 //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
harrynguyen 26:cd503e08a218 405
harrynguyen 26:cd503e08a218 406 imuTime = imu.time_s;
harrynguyen 26:cd503e08a218 407
harrynguyen 26:cd503e08a218 408 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 409 }
harrynguyen 26:cd503e08a218 410 }
harrynguyen 26:cd503e08a218 411
harrynguyen 26:cd503e08a218 412 bool imuInit_function()
harrynguyen 26:cd503e08a218 413 {
harrynguyen 26:cd503e08a218 414 //Set Reset pin low and then high to reset the imu
harrynguyen 26:cd503e08a218 415 imuReset(); //Physical reset
harrynguyen 26:cd503e08a218 416 return (imu.imuInit());
harrynguyen 26:cd503e08a218 417 }
harrynguyen 26:cd503e08a218 418
harrynguyen 26:cd503e08a218 419 void imuReset() //Physical reset
harrynguyen 26:cd503e08a218 420 {
harrynguyen 26:cd503e08a218 421 imu_reset = 0;
harrynguyen 26:cd503e08a218 422 wait_ms(50);
harrynguyen 26:cd503e08a218 423 imu_reset = 1;
harrynguyen 26:cd503e08a218 424 return;
harrynguyen 26:cd503e08a218 425 }
harrynguyen 26:cd503e08a218 426
harrynguyen 26:cd503e08a218 427
harrynguyen 26:cd503e08a218 428
harrynguyen 26:cd503e08a218 429
harrynguyen 26:cd503e08a218 430
harrynguyen 26:cd503e08a218 431 /**
harrynguyen 26:cd503e08a218 432 * Ranging sensor loop as an individual thread
harrynguyen 26:cd503e08a218 433 */
harrynguyen 26:cd503e08a218 434 /**
harrynguyen 26:cd503e08a218 435 * Ranging sensor loop as an individual thread
harrynguyen 26:cd503e08a218 436 */
harrynguyen 26:cd503e08a218 437 void static_ranging_thread(void const *n)
harrynguyen 26:cd503e08a218 438 {
harrynguyen 26:cd503e08a218 439 while(true)
harrynguyen 26:cd503e08a218 440 {
harrynguyen 26:cd503e08a218 441 // if(imu.imu_stabilized[1] ==1){
harrynguyen 26:cd503e08a218 442 RangeSensor.right = sensor.readRangeContinuousMillimeters();
harrynguyen 26:cd503e08a218 443 RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
harrynguyen 26:cd503e08a218 444 RangeSensor.fwd = sensor3.readRangeContinuousMillimeters();
harrynguyen 26:cd503e08a218 445
harrynguyen 26:cd503e08a218 446 RangeSensor.left_d = sensor4.readRangeContinuousMillimeters();
harrynguyen 26:cd503e08a218 447 RangeSensor.left = sensor5.readRangeContinuousMillimeters();
harrynguyen 26:cd503e08a218 448 // if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())
harrynguyen 26:cd503e08a218 449 // debugprint.printf(" TIMEOUT\r\n");
harrynguyen 26:cd503e08a218 450 // }
harrynguyen 26:cd503e08a218 451 Thread::wait(10);
harrynguyen 26:cd503e08a218 452 }
harrynguyen 26:cd503e08a218 453 }
harrynguyen 26:cd503e08a218 454
harrynguyen 26:cd503e08a218 455
harrynguyen 26:cd503e08a218 456
harrynguyen 26:cd503e08a218 457 void odometry_thread(void const *n)
harrynguyen 26:cd503e08a218 458 {
harrynguyen 26:cd503e08a218 459 odometry.init();
harrynguyen 26:cd503e08a218 460 Thread::wait(50);
harrynguyen 26:cd503e08a218 461
harrynguyen 26:cd503e08a218 462 while(true) {
harrynguyen 26:cd503e08a218 463 // debugprint.printf("%.2f Running 2 odometry \n", imuTime);
harrynguyen 26:cd503e08a218 464 odometry.update();
harrynguyen 26:cd503e08a218 465
harrynguyen 26:cd503e08a218 466 //Usage:
harrynguyen 26:cd503e08a218 467 //odometer.revolutions[0, 1]; //revolutions left, right
harrynguyen 26:cd503e08a218 468 //odometer.rpm[0, 1]; //rpm left, right
harrynguyen 26:cd503e08a218 469
harrynguyen 26:cd503e08a218 470 localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);
harrynguyen 26:cd503e08a218 471
harrynguyen 26:cd503e08a218 472 //Usage:
harrynguyen 26:cd503e08a218 473 //localization.position[0, 1] //x, y
harrynguyen 26:cd503e08a218 474
harrynguyen 26:cd503e08a218 475 Thread::wait(odometry_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 476 }
harrynguyen 26:cd503e08a218 477 }
harrynguyen 26:cd503e08a218 478
harrynguyen 26:cd503e08a218 479 /**/
harrynguyen 26:cd503e08a218 480 /**
harrynguyen 26:cd503e08a218 481 * motor control loop as an individual thread
harrynguyen 26:cd503e08a218 482 */
harrynguyen 26:cd503e08a218 483 void motorControl_thread(void const *n)
harrynguyen 26:cd503e08a218 484 {
harrynguyen 26:cd503e08a218 485 motorControl_t.start();
harrynguyen 26:cd503e08a218 486
harrynguyen 26:cd503e08a218 487 float pitch_th, pitch_om, yaw_th, yaw_om;
harrynguyen 26:cd503e08a218 488 float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
harrynguyen 26:cd503e08a218 489 float W_l, W_r;
harrynguyen 26:cd503e08a218 490
harrynguyen 26:cd503e08a218 491 while(true) {
harrynguyen 26:cd503e08a218 492 // debugprint.printf("%.2f Running 3 motorControl \n", imuTime);
harrynguyen 26:cd503e08a218 493 //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
harrynguyen 26:cd503e08a218 494 if(imu.imu_stabilized[1] ==1) {
harrynguyen 26:cd503e08a218 495
harrynguyen 26:cd503e08a218 496 pitch_th = DEG_TO_RAD(imu.Pose[0]);
harrynguyen 26:cd503e08a218 497 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 26:cd503e08a218 498 if(pitch_th > M_PI) pitch_th -= 2*M_PI;
harrynguyen 26:cd503e08a218 499 if(pitch_th < -1*M_PI) pitch_th += 2*M_PI;
harrynguyen 26:cd503e08a218 500
harrynguyen 26:cd503e08a218 501
harrynguyen 26:cd503e08a218 502 pitch_om = DEG_TO_RAD(imu.AngVel[0]);
harrynguyen 26:cd503e08a218 503
harrynguyen 26:cd503e08a218 504 yaw_th = DEG_TO_RAD(imu.Pose[2]);
harrynguyen 26:cd503e08a218 505
harrynguyen 26:cd503e08a218 506 yaw_om = DEG_TO_RAD(imu.AngVel[2]);
harrynguyen 26:cd503e08a218 507
harrynguyen 26:cd503e08a218 508 wheelTh_l = odometry.revolutions[0]*(-2*M_PI);
harrynguyen 26:cd503e08a218 509 wheelTh_r = odometry.revolutions[1]*(-2*M_PI);
harrynguyen 26:cd503e08a218 510
harrynguyen 26:cd503e08a218 511 wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI);
harrynguyen 26:cd503e08a218 512 wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI);
harrynguyen 26:cd503e08a218 513
harrynguyen 26:cd503e08a218 514 // Rasp.printf("%.2f \n", purePursuit_velocity);
harrynguyen 26:cd503e08a218 515 ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0);
harrynguyen 26:cd503e08a218 516 ref_gamma = purePursuit_gamma;
harrynguyen 26:cd503e08a218 517 ref_dgamma = purePursuit_omega;
harrynguyen 26:cd503e08a218 518
harrynguyen 26:cd503e08a218 519 attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
harrynguyen 26:cd503e08a218 520 pitch_th, pitch_om, yaw_th , yaw_om,
harrynguyen 26:cd503e08a218 521 wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
harrynguyen 26:cd503e08a218 522 0, ref_dbeta, ref_beta,
harrynguyen 26:cd503e08a218 523 0, ref_dtheta, ref_theta,
harrynguyen 26:cd503e08a218 524 0, ref_dgamma, ref_gamma,
harrynguyen 26:cd503e08a218 525 &u1, &u2);
harrynguyen 26:cd503e08a218 526
harrynguyen 26:cd503e08a218 527
harrynguyen 26:cd503e08a218 528
harrynguyen 26:cd503e08a218 529 // Rasp.printf("Reach %d \n", waypointReached_flag);
harrynguyen 26:cd503e08a218 530 // Rasp.printf("Finsih %d \n", waypointSetFinish_flag);
harrynguyen 26:cd503e08a218 531 // if(waypointSetFinish_flag == 0 && waypointReached_flag == 0 ) {
harrynguyen 26:cd503e08a218 532 // Rasp.printf("Motor enable: %d \n", motor_enable);
harrynguyen 26:cd503e08a218 533 if (motor_enable == 1){
harrynguyen 26:cd503e08a218 534 rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);// Virgo
harrynguyen 26:cd503e08a218 535 rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
harrynguyen 26:cd503e08a218 536 // Rasp.printf("%.2f \n", rpm_cmd[0]);
harrynguyen 26:cd503e08a218 537 // rpm_cmd[0]=W_l*60/(2*M_PI)*(1.0); //Orion
harrynguyen 26:cd503e08a218 538 // rpm_cmd[1]=W_r*60/(2*M_PI)*(1.0);
harrynguyen 26:cd503e08a218 539
harrynguyen 26:cd503e08a218 540 // rpm_cmd[0]=0;
harrynguyen 26:cd503e08a218 541 // rpm_cmd[1]=0;
harrynguyen 26:cd503e08a218 542
harrynguyen 26:cd503e08a218 543 if( (generalFunctions::abs_f(rpm_cmd[0]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 10.0) )
harrynguyen 26:cd503e08a218 544 rpm_cmd[0] = 47.5*generalFunctions::sign_f(rpm_cmd[0]);
harrynguyen 26:cd503e08a218 545 else if(generalFunctions::abs_f(rpm_cmd[0]) <= 10.0)
harrynguyen 26:cd503e08a218 546 rpm_cmd[0] = 0;
harrynguyen 26:cd503e08a218 547
harrynguyen 26:cd503e08a218 548 if( (generalFunctions::abs_f(rpm_cmd[1]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 10.0) )
harrynguyen 26:cd503e08a218 549 rpm_cmd[1] = 47.5*generalFunctions::sign_f(rpm_cmd[1]);
harrynguyen 26:cd503e08a218 550 else if(generalFunctions::abs_f(rpm_cmd[1]) <= 10.0)
harrynguyen 26:cd503e08a218 551 rpm_cmd[1] = 0;
harrynguyen 26:cd503e08a218 552
harrynguyen 26:cd503e08a218 553 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
harrynguyen 26:cd503e08a218 554 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
harrynguyen 26:cd503e08a218 555
harrynguyen 26:cd503e08a218 556
harrynguyen 26:cd503e08a218 557 } else {
harrynguyen 26:cd503e08a218 558 rpm_cmd[0]=0;
harrynguyen 26:cd503e08a218 559 rpm_cmd[1]=0;
harrynguyen 26:cd503e08a218 560
harrynguyen 26:cd503e08a218 561 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read());
harrynguyen 26:cd503e08a218 562 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read());
harrynguyen 26:cd503e08a218 563 }
harrynguyen 26:cd503e08a218 564
harrynguyen 26:cd503e08a218 565 pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read());
harrynguyen 26:cd503e08a218 566 pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read());
harrynguyen 26:cd503e08a218 567
harrynguyen 26:cd503e08a218 568 drive.setPWM_L(pwm_cmd[0]);
harrynguyen 26:cd503e08a218 569 drive.setPWM_R(pwm_cmd[1]);
harrynguyen 26:cd503e08a218 570
harrynguyen 26:cd503e08a218 571 }
harrynguyen 26:cd503e08a218 572
harrynguyen 26:cd503e08a218 573 motorControl_t.reset();
harrynguyen 26:cd503e08a218 574
harrynguyen 26:cd503e08a218 575 Thread::wait(motorControl_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 576 }
harrynguyen 26:cd503e08a218 577 }
harrynguyen 26:cd503e08a218 578
harrynguyen 26:cd503e08a218 579 //*************Raspberry Pi and STM32 Communication
harrynguyen 26:cd503e08a218 580 //****Transmit
harrynguyen 26:cd503e08a218 581 void raspberrytx_thread(void const *n)
harrynguyen 26:cd503e08a218 582 {
harrynguyen 26:cd503e08a218 583 while(1){
harrynguyen 26:cd503e08a218 584 //This part for Communication
harrynguyen 26:cd503e08a218 585 Rasp.printf("%.4f: ",imuTime); //timestamp
harrynguyen 26:cd503e08a218 586 Rasp.printf("%.2f,%.2f,%.2f; ",imu.Pose[0], imu.Pose[1], imu.Pose[2]); //euler x,y,z
harrynguyen 26:cd503e08a218 587 //// Rasp.printf("%.2f,%.2f,%.2f; ",imu.AngVel[0], imu.AngVel[1], imu.AngVel[2]); //ang vel x,y,z
harrynguyen 26:cd503e08a218 588 //// Rasp.printf("%.2f,%.2f,%.2f; ",imu.LinAcc[0], imu.LinAcc[1], imu.LinAcc[2]); //Linear acc
harrynguyen 26:cd503e08a218 589 Rasp.printf("%.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
harrynguyen 26:cd503e08a218 590 Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
harrynguyen 26:cd503e08a218 591 Rasp.printf("%.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
harrynguyen 26:cd503e08a218 592 Rasp.printf("%d,%d,%d; ",trilateration.range_array[0], trilateration.range_array[1], trilateration.range_array[2]); //uwb range data
harrynguyen 26:cd503e08a218 593 // Rasp.printf("%.2f,%.2f; ",odometry.revolutions[0] * 2*M_PI, odometry.revolutions[1] * 2*M_PI); //Wheel Position L,R
harrynguyen 26:cd503e08a218 594 Rasp.printf("%.2f,%.2f; ",odometry.rpm[0] * 2*M_PI / 60, odometry.rpm[1] * 2*M_PI / 60); //Wheel Speed L,R
harrynguyen 26:cd503e08a218 595 Rasp.printf("%.2f,%.2f; ", target_waypoint[0], target_waypoint[1]); //Waypoint heading to
harrynguyen 26:cd503e08a218 596
harrynguyen 26:cd503e08a218 597 Rasp.printf("%d", waypointReached_flag);
harrynguyen 26:cd503e08a218 598 // //Rasp.printf("%.2f,%.2f; ",comm.DataIn.data[13], comm.DataIn.data[14]); //Drivetrain Command L,R
harrynguyen 26:cd503e08a218 599 // Rasp.printf("%.2f,%.2f;",rpm_compensated[0] * 2*M_PI / 60, rpm_compensated[1] * 2*M_PI / 60); //Compensated Command L,R
harrynguyen 26:cd503e08a218 600 Rasp.printf("\n");
harrynguyen 26:cd503e08a218 601
harrynguyen 26:cd503e08a218 602 //This part for Debug:
harrynguyen 26:cd503e08a218 603 //Rasp.printf("time: %.4f: ",imuTime); //timestamp
harrynguyen 26:cd503e08a218 604 //Rasp.printf("imu: %.2f,%.2f,%.2f; ",imu.Pose[0], imu.Pose[1], imu.Pose[2]); //euler x,y,z
harrynguyen 26:cd503e08a218 605 //Rasp.printf("position: %.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
harrynguyen 26:cd503e08a218 606 //Rasp.printf("uwb: %.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
harrynguyen 26:cd503e08a218 607 //Rasp.printf("lidar: %u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
harrynguyen 26:cd503e08a218 608 //Rasp.printf("waypoint data: %d %d %d; ", waypoints_set[1][0],waypoints_set[1][1], waypoints_set[1][2]);
harrynguyen 26:cd503e08a218 609 //Rasp.printf("waypointReached: %d; ", waypointReached_flag);
harrynguyen 26:cd503e08a218 610 //Rasp.printf(";\n");
harrynguyen 26:cd503e08a218 611 Thread::wait(RaspTransmit_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 612 }
harrynguyen 26:cd503e08a218 613 }
harrynguyen 26:cd503e08a218 614
harrynguyen 26:cd503e08a218 615 //****Receive
harrynguyen 26:cd503e08a218 616 //void rxRaspCallback(){
harrynguyen 26:cd503e08a218 617 // NVIC_DisableIRQ(USART1_IRQn);
harrynguyen 26:cd503e08a218 618 // led = !led;;
harrynguyen 26:cd503e08a218 619 // while(Rasp.getc() != '$'){
harrynguyen 26:cd503e08a218 620 // }
harrynguyen 26:cd503e08a218 621 // for(int i = 0 ; i < sizeof(rasp_data); i ++){
harrynguyen 26:cd503e08a218 622 // rasp_data[i] = Rasp.getc();
harrynguyen 26:cd503e08a218 623 // if(rasp_data[i] == '\n'){
harrynguyen 26:cd503e08a218 624 // rasp_data_flag = 1;
harrynguyen 26:cd503e08a218 625 // break;
harrynguyen 26:cd503e08a218 626 // }
harrynguyen 26:cd503e08a218 627 // }
harrynguyen 26:cd503e08a218 628 // NVIC_EnableIRQ(USART1_IRQn);
harrynguyen 26:cd503e08a218 629 //}
harrynguyen 26:cd503e08a218 630
harrynguyen 26:cd503e08a218 631 void rxRaspCallback(MODSERIAL_IRQ_INFO *q) {
harrynguyen 26:cd503e08a218 632 MODSERIAL *serial = q->serial;
harrynguyen 26:cd503e08a218 633 if ( serial->rxGetLastChar() == '\n') {
harrynguyen 26:cd503e08a218 634 rasp_newline_detected = true;
harrynguyen 26:cd503e08a218 635 }
harrynguyen 26:cd503e08a218 636 }
harrynguyen 26:cd503e08a218 637
harrynguyen 26:cd503e08a218 638 void raspberryrx_thread(void const *n)
harrynguyen 26:cd503e08a218 639 {
harrynguyen 26:cd503e08a218 640 Rasp.baud(115200);
harrynguyen 26:cd503e08a218 641 // Rasp.attach(&rxRaspCallback, RawSerial::RxIrq);
harrynguyen 26:cd503e08a218 642 Rasp.attach(&rxRaspCallback, MODSERIAL::RxIrq);
harrynguyen 26:cd503e08a218 643 for(int i = 0 ; i < sizeof(rasp_data); i ++){
harrynguyen 26:cd503e08a218 644 rasp_data[i] = NULL;
harrynguyen 26:cd503e08a218 645 }
harrynguyen 26:cd503e08a218 646
harrynguyen 26:cd503e08a218 647
harrynguyen 26:cd503e08a218 648 while(1){
harrynguyen 26:cd503e08a218 649 // led = 0;
harrynguyen 26:cd503e08a218 650 while (! rasp_newline_detected ) ;//debugprint.printf("detecting the line \n"); //should be: If newline_detected --> Compute
harrynguyen 26:cd503e08a218 651 rasp_newline_detected = false;
harrynguyen 26:cd503e08a218 652 while(Rasp.getc() != '$'){
harrynguyen 26:cd503e08a218 653 }
harrynguyen 26:cd503e08a218 654 for(int i = 0 ; i < sizeof(rasp_data); i ++){
harrynguyen 26:cd503e08a218 655 rasp_data[i] = Rasp.getc();
harrynguyen 26:cd503e08a218 656 if(rasp_data[i] == '\n'){
harrynguyen 26:cd503e08a218 657 rasp_data_flag = 1;
harrynguyen 26:cd503e08a218 658 break;
harrynguyen 26:cd503e08a218 659 }
harrynguyen 26:cd503e08a218 660 }
harrynguyen 26:cd503e08a218 661 //debugprint.printf("Running rasp %s \n",rasp_data);
harrynguyen 26:cd503e08a218 662 Thread::wait(100);
harrynguyen 26:cd503e08a218 663 // if (rasp_data_flag == 1){
harrynguyen 26:cd503e08a218 664 // debugprint.printf(" %.2f Running 5 rasp rx \n", imuTime);
harrynguyen 26:cd503e08a218 665 // memcpy(waypoint_data, rasp_data, sizeof(rasp_data));
harrynguyen 26:cd503e08a218 666 waypoint_parser_fn(rasp_data);
harrynguyen 26:cd503e08a218 667 for(int i = 0 ; i < sizeof(rasp_data); i ++){
harrynguyen 26:cd503e08a218 668 rasp_data[i] = 0;
harrynguyen 26:cd503e08a218 669 }
harrynguyen 26:cd503e08a218 670 rasp_data_flag = 0;
harrynguyen 26:cd503e08a218 671
harrynguyen 26:cd503e08a218 672 Thread::wait(RaspReceive_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 673
harrynguyen 26:cd503e08a218 674 }
harrynguyen 26:cd503e08a218 675 }
harrynguyen 26:cd503e08a218 676
harrynguyen 26:cd503e08a218 677 //******End***********************************************
harrynguyen 26:cd503e08a218 678
harrynguyen 26:cd503e08a218 679 /*****Extract waypoint from rasp_data*/
harrynguyen 26:cd503e08a218 680 //void waypoint_parser(void const *n){
harrynguyen 26:cd503e08a218 681 // char waypoint_x[5];
harrynguyen 26:cd503e08a218 682 // char waypoint_y[5];
harrynguyen 26:cd503e08a218 683 // while(1){
harrynguyen 26:cd503e08a218 684 // //Print the data for debugging
harrynguyen 26:cd503e08a218 685 //// for(int i =0 ; i < sizeof(waypoint_data); i++){
harrynguyen 26:cd503e08a218 686 //// debugprint.putc(waypoint_data[i]);
harrynguyen 26:cd503e08a218 687 //// }
harrynguyen 26:cd503e08a218 688 // //debugprint.printf("Waypoint data %s \n",waypoint_data);
harrynguyen 26:cd503e08a218 689 //
harrynguyen 26:cd503e08a218 690 // memcpy(waypoint_x, &waypoint_data[0], 4);
harrynguyen 26:cd503e08a218 691 // waypoint_x[sizeof(waypoint_x-1)] = '\0';
harrynguyen 26:cd503e08a218 692 //// debugprint.printf("waypoint x data char: %s \n", waypoint_x);
harrynguyen 26:cd503e08a218 693 //
harrynguyen 26:cd503e08a218 694 // memcpy(waypoint_y, &waypoint_data[0]+5, 4);
harrynguyen 26:cd503e08a218 695 // waypoint_y[sizeof(waypoint_y-1)] = '\0';
harrynguyen 26:cd503e08a218 696 //// debugprint.printf("waypoint y data char: %s \n", waypoint_y);
harrynguyen 26:cd503e08a218 697 //
harrynguyen 26:cd503e08a218 698 // waypoints_set[1][0] = strtol(waypoint_x,NULL,10);
harrynguyen 26:cd503e08a218 699 // waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
harrynguyen 26:cd503e08a218 700 // waypoint_ready = 1;
harrynguyen 26:cd503e08a218 701 //// debugprint.printf("waypoint data: %d %d \n", waypoints_set[2][0],waypoints_set[2][1]);
harrynguyen 26:cd503e08a218 702 //
harrynguyen 26:cd503e08a218 703 // Thread::wait(100);
harrynguyen 26:cd503e08a218 704 // }
harrynguyen 26:cd503e08a218 705 //}
harrynguyen 26:cd503e08a218 706
harrynguyen 26:cd503e08a218 707 //Use function to call when necessary
harrynguyen 26:cd503e08a218 708 //void waypoint_parser_fn(char* waypoint_data){
harrynguyen 26:cd503e08a218 709 // char waypoint_x[5];
harrynguyen 26:cd503e08a218 710 // char waypoint_y[5];
harrynguyen 26:cd503e08a218 711 // char target_vel[3];
harrynguyen 26:cd503e08a218 712 // //Print the data for debugging
harrynguyen 26:cd503e08a218 713 //// for(int i =0 ; i < sizeof(waypoint_data); i++){
harrynguyen 26:cd503e08a218 714 //// debugprint.putc(waypoint_data[i]);
harrynguyen 26:cd503e08a218 715 //// }
harrynguyen 26:cd503e08a218 716 // //debugprint.printf("Waypoint data %s \n",waypoint_data);
harrynguyen 26:cd503e08a218 717 //
harrynguyen 26:cd503e08a218 718 // memcpy(waypoint_x, &waypoint_data[0], 4);
harrynguyen 26:cd503e08a218 719 // waypoint_x[sizeof(waypoint_x-1)] = '\0';
harrynguyen 26:cd503e08a218 720 //// debugprint.printf("waypoint x data char: %s \n", waypoint_x);
harrynguyen 26:cd503e08a218 721 //
harrynguyen 26:cd503e08a218 722 // memcpy(waypoint_y, &waypoint_data[0]+5, 4);
harrynguyen 26:cd503e08a218 723 // waypoint_y[sizeof(waypoint_y-1)] = '\0';
harrynguyen 26:cd503e08a218 724 //// debugprint.printf("waypoint y data char: %s \n", waypoint_y);
harrynguyen 26:cd503e08a218 725 // memcpy(target_vel, &waypoint_data[0]+10, 2);
harrynguyen 26:cd503e08a218 726 // target_vel[sizeof(target_vel-1)] = '\0';
harrynguyen 26:cd503e08a218 727 //
harrynguyen 26:cd503e08a218 728 // waypoints_set[1][0] = strtol(waypoint_x,NULL,10); //convert string to int
harrynguyen 26:cd503e08a218 729 // waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
harrynguyen 26:cd503e08a218 730 // waypoints_set[1][2] = strtol(target_vel,NULL,10);
harrynguyen 26:cd503e08a218 731 // waypoint_ready = 1;
harrynguyen 26:cd503e08a218 732 // debugprint.printf("waypoint data: %d %d %d \n", waypoints_set[1][0],waypoints_set[1][1], waypoints_set[1][2]);
harrynguyen 26:cd503e08a218 733 //
harrynguyen 26:cd503e08a218 734 //}
harrynguyen 26:cd503e08a218 735
harrynguyen 26:cd503e08a218 736 void waypoint_parser_fn(char* waypoint_data){
harrynguyen 26:cd503e08a218 737 char header;
harrynguyen 26:cd503e08a218 738 // char waypoint_x[5];
harrynguyen 26:cd503e08a218 739 // char waypoint_y[5];
harrynguyen 26:cd503e08a218 740 // char target_vel[3];
harrynguyen 26:cd503e08a218 741 //Print the data for debugging
harrynguyen 26:cd503e08a218 742 // for(int i =0 ; i < sizeof(waypoint_data); i++){
harrynguyen 26:cd503e08a218 743 // debugprint.putc(waypoint_data[i]);
harrynguyen 26:cd503e08a218 744 // }
harrynguyen 26:cd503e08a218 745 // Rasp.printf("Waypoint data %s \n",waypoint_data);
harrynguyen 26:cd503e08a218 746 header = waypoint_data[0];
harrynguyen 26:cd503e08a218 747 switch (header) {
harrynguyen 26:cd503e08a218 748 case '1': purePursuit_enable = 0; //turn off waypoint controller
harrynguyen 26:cd503e08a218 749 char linear[5],angular[5];
harrynguyen 27:c813451bfb4d 750 int gamma_deg;
harrynguyen 26:cd503e08a218 751 memcpy(linear, &waypoint_data[0]+2, 4);
harrynguyen 26:cd503e08a218 752 linear[sizeof(linear-1)] = '\0';
harrynguyen 26:cd503e08a218 753 //Rasp.printf("%s \n", linear);
harrynguyen 26:cd503e08a218 754
harrynguyen 26:cd503e08a218 755 memcpy(angular, &waypoint_data[0]+7, 4);
harrynguyen 26:cd503e08a218 756 angular[sizeof(angular-1)] = '\0';
harrynguyen 26:cd503e08a218 757 //Rasp.printf("%s \n", angular);
harrynguyen 26:cd503e08a218 758
harrynguyen 26:cd503e08a218 759
harrynguyen 26:cd503e08a218 760 purePursuit_velocity = strtol(linear,NULL,10); //convert string to int
harrynguyen 27:c813451bfb4d 761 gamma_deg = strtol(angular,NULL,10);
harrynguyen 27:c813451bfb4d 762 purePursuit_gamma = DEG_TO_RAD(gamma_deg);
harrynguyen 26:cd503e08a218 763 //Rasp.printf("%.2f \n", purePursuit_velocity);
harrynguyen 26:cd503e08a218 764
harrynguyen 26:cd503e08a218 765 motor_enable = 1;
harrynguyen 26:cd503e08a218 766
harrynguyen 26:cd503e08a218 767 break;
harrynguyen 26:cd503e08a218 768
harrynguyen 26:cd503e08a218 769 case '2': purePursuit_enable = 1;
harrynguyen 26:cd503e08a218 770 char waypoint_x[5], waypoint_y[5], target_vel[3];
harrynguyen 26:cd503e08a218 771 memcpy(waypoint_x, &waypoint_data[0]+2, 4);
harrynguyen 26:cd503e08a218 772 waypoint_x[sizeof(waypoint_x-1)] = '\0';
harrynguyen 26:cd503e08a218 773 // Rasp.printf("%s \n", waypoint_x); //Debug print
harrynguyen 26:cd503e08a218 774
harrynguyen 26:cd503e08a218 775 Rasp.printf("%s \n", waypoint_data); //Debug print
harrynguyen 26:cd503e08a218 776
harrynguyen 26:cd503e08a218 777 memcpy(waypoint_y, &waypoint_data[0]+7, 4);
harrynguyen 26:cd503e08a218 778 waypoint_y[sizeof(waypoint_y-1)] = '\0';
harrynguyen 26:cd503e08a218 779 // Rasp.printf("%s \n", waypoint_y);
harrynguyen 26:cd503e08a218 780
harrynguyen 26:cd503e08a218 781 memcpy(target_vel, &waypoint_data[0]+12, 2);
harrynguyen 26:cd503e08a218 782 target_vel[sizeof(target_vel-1)] = '\0';
harrynguyen 26:cd503e08a218 783 // Rasp.printf("%s \n", target_vel);
harrynguyen 26:cd503e08a218 784
harrynguyen 26:cd503e08a218 785 waypoints_set[1][0] = strtol(waypoint_x,NULL,10); //convert string to int
harrynguyen 26:cd503e08a218 786 waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
harrynguyen 26:cd503e08a218 787 waypoints_set[1][2] = strtol(target_vel,NULL,10);
harrynguyen 26:cd503e08a218 788 waypoint_ready = 1;
harrynguyen 26:cd503e08a218 789 motor_enable = 1;
harrynguyen 26:cd503e08a218 790
harrynguyen 26:cd503e08a218 791 break;
harrynguyen 26:cd503e08a218 792 case '3': motor_enable = 0; //Turn off the motor controller
harrynguyen 26:cd503e08a218 793
harrynguyen 26:cd503e08a218 794 default: break;
harrynguyen 26:cd503e08a218 795
harrynguyen 26:cd503e08a218 796 }
harrynguyen 26:cd503e08a218 797 }
harrynguyen 26:cd503e08a218 798
harrynguyen 26:cd503e08a218 799
harrynguyen 26:cd503e08a218 800 //-*****************************UWB***********************
harrynguyen 26:cd503e08a218 801 void uwb_thread(void const *n)
harrynguyen 26:cd503e08a218 802 {
harrynguyen 26:cd503e08a218 803 // float uwb_time = 0.0;
harrynguyen 26:cd503e08a218 804 // float time_out = 0.0;
harrynguyen 26:cd503e08a218 805 uwb.baud(115200);
harrynguyen 26:cd503e08a218 806 // uwb.attach(&rxUwbCallback, RawSerial::RxIrq);
harrynguyen 26:cd503e08a218 807 uwb.attach(&rxUwbCallback, MODSERIAL::RxIrq);
harrynguyen 26:cd503e08a218 808 for (int j = 0; j< sizeof(uwb_data); j++) {
harrynguyen 26:cd503e08a218 809 uwb_data[j] = NULL;
harrynguyen 26:cd503e08a218 810 }
harrynguyen 26:cd503e08a218 811 while(1){
harrynguyen 26:cd503e08a218 812 // debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
harrynguyen 26:cd503e08a218 813 // debugprint.printf("Running uwb %s \n",uwb_data);
harrynguyen 26:cd503e08a218 814
harrynguyen 26:cd503e08a218 815 //NEW:
harrynguyen 26:cd503e08a218 816 while (! newline_detected ); // debugprint.printf("detecting the line \n"); //should be: If newline_detected --> Compute : No because : if else, the data will be skip.
harrynguyen 26:cd503e08a218 817 newline_detected = false;
harrynguyen 26:cd503e08a218 818 //checking start of the message with letter "m"
harrynguyen 26:cd503e08a218 819 while(uwb.getc() != 'm'){
harrynguyen 26:cd503e08a218 820 }
harrynguyen 26:cd503e08a218 821 for(int i = 0 ; i < sizeof(uwb_data); i ++){
harrynguyen 26:cd503e08a218 822 uwb_data[i] = uwb.getc();
harrynguyen 26:cd503e08a218 823 if(uwb_data[i] == '\n'){
harrynguyen 26:cd503e08a218 824 uwb_data_flag = 1;
harrynguyen 26:cd503e08a218 825 break;
harrynguyen 26:cd503e08a218 826 }
harrynguyen 26:cd503e08a218 827
harrynguyen 26:cd503e08a218 828 // debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
harrynguyen 26:cd503e08a218 829 uwbtriangulation_fn(uwb_data); // Running Triateration (The function nam is misleading)
harrynguyen 26:cd503e08a218 830 // ekf_fn(&ekf); //run ekf
harrynguyen 26:cd503e08a218 831 debugprint.printf("%s",uwb_data);
harrynguyen 26:cd503e08a218 832 for (int j = 0; j< sizeof(uwb_data); j++) {
harrynguyen 26:cd503e08a218 833 uwb_data[j] = 0;
harrynguyen 26:cd503e08a218 834 }
harrynguyen 26:cd503e08a218 835 uwb_data_flag = 0;
harrynguyen 26:cd503e08a218 836 }
harrynguyen 26:cd503e08a218 837
harrynguyen 26:cd503e08a218 838 //OLD
harrynguyen 26:cd503e08a218 839 // if(uwb_data_flag == 1){
harrynguyen 26:cd503e08a218 840 //// memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */); //copy data from buffer
harrynguyen 26:cd503e08a218 841 // uwbtriangulation_fn(uwb_data);
harrynguyen 26:cd503e08a218 842 // for (int j = 0; j< sizeof(uwb_data); j++) {
harrynguyen 26:cd503e08a218 843 // uwb_data[j] = 0;
harrynguyen 26:cd503e08a218 844 // }
harrynguyen 26:cd503e08a218 845 // uwb_data_flag = 0;
harrynguyen 26:cd503e08a218 846 //
harrynguyen 26:cd503e08a218 847 // }
harrynguyen 26:cd503e08a218 848
harrynguyen 26:cd503e08a218 849 // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 850 //debugprint.printf("%s \n",range);
harrynguyen 26:cd503e08a218 851 Thread::wait(200);
harrynguyen 26:cd503e08a218 852 }
harrynguyen 26:cd503e08a218 853
harrynguyen 26:cd503e08a218 854 }
harrynguyen 26:cd503e08a218 855
harrynguyen 26:cd503e08a218 856 //void uwbtriangulation_thread(void const *n)
harrynguyen 26:cd503e08a218 857 //{
harrynguyen 26:cd503e08a218 858 // int anchorheight = 1.8;
harrynguyen 26:cd503e08a218 859 // anchorArray[0].x = 0.0;
harrynguyen 26:cd503e08a218 860 // anchorArray[0].y = 0.0;
harrynguyen 26:cd503e08a218 861 // anchorArray[0].z = anchorheight;
harrynguyen 26:cd503e08a218 862 //
harrynguyen 26:cd503e08a218 863 // anchorArray[1].x = 3.0;
harrynguyen 26:cd503e08a218 864 // anchorArray[1].y = 0.0;
harrynguyen 26:cd503e08a218 865 // anchorArray[1].z = anchorheight;
harrynguyen 26:cd503e08a218 866 //
harrynguyen 26:cd503e08a218 867 // anchorArray[2].x = 0.0;
harrynguyen 26:cd503e08a218 868 // anchorArray[2].y = 4.0;
harrynguyen 26:cd503e08a218 869 // anchorArray[2].z = anchorheight;
harrynguyen 26:cd503e08a218 870 //
harrynguyen 26:cd503e08a218 871 // range_array[0] = 0;
harrynguyen 26:cd503e08a218 872 // range_array[1] = 0;
harrynguyen 26:cd503e08a218 873 // range_array[2] = 0;
harrynguyen 26:cd503e08a218 874 // range_array[3] = 0;
harrynguyen 26:cd503e08a218 875 //
harrynguyen 26:cd503e08a218 876 // while(1){
harrynguyen 26:cd503e08a218 877 //// debugprint.printf("%.2f Running 6 uwb triangulation \n", imuTime);
harrynguyen 26:cd503e08a218 878 // //memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */);
harrynguyen 26:cd503e08a218 879 //
harrynguyen 26:cd503e08a218 880 // memcpy(rangestring_array[0], &uwb_data1[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
harrynguyen 26:cd503e08a218 881 // rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
harrynguyen 26:cd503e08a218 882 // range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
harrynguyen 26:cd503e08a218 883 //
harrynguyen 26:cd503e08a218 884 // memcpy(rangestring_array[1], &uwb_data1[0] + 13 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 885 // rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
harrynguyen 26:cd503e08a218 886 // range_array[1] = strtol(rangestring_array[1],NULL, 16);
harrynguyen 26:cd503e08a218 887 //
harrynguyen 26:cd503e08a218 888 // memcpy(rangestring_array[2], &uwb_data1[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 889 // rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
harrynguyen 26:cd503e08a218 890 // range_array[2] = strtol(rangestring_array[2],NULL, 16);
harrynguyen 26:cd503e08a218 891 //
harrynguyen 26:cd503e08a218 892 // trilateration.GetLocation (&bestsolution, 1, anchorArray, range_array);
harrynguyen 26:cd503e08a218 893 // //memcpy(range[1], &uwb_data1[0] + 13 /* Offset */, sizeof(range[1]) /* Length */);
harrynguyen 26:cd503e08a218 894 // // range_array[1] = strtol(range[1],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 895 // // memcpy(range[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 896 // // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 897 // // range_float[0] = strtol(range[0],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 898 // // range_float[1] = strtol(range[1],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 899 // // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 900 // // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 901 // // debugprint.printf(range[0]);
harrynguyen 26:cd503e08a218 902 //// debugprint.puts(range[1]);
harrynguyen 26:cd503e08a218 903 // // debugprint.puts(range[2]);
harrynguyen 26:cd503e08a218 904 // // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 905 // //debugprint.printf("%s",uwb_data);
harrynguyen 26:cd503e08a218 906 // //debugprint.printf("%s \n",uwb_data[66]);
harrynguyen 26:cd503e08a218 907 // Thread::wait(250);
harrynguyen 26:cd503e08a218 908 // }
harrynguyen 26:cd503e08a218 909 //
harrynguyen 26:cd503e08a218 910 //}
harrynguyen 26:cd503e08a218 911
harrynguyen 26:cd503e08a218 912 void uwbtriangulation_fn(char* uwb_data)
harrynguyen 26:cd503e08a218 913 {
harrynguyen 26:cd503e08a218 914 char rangestring_array[3][10];
harrynguyen 26:cd503e08a218 915 memcpy(rangestring_array[0], &uwb_data[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
harrynguyen 26:cd503e08a218 916 rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
harrynguyen 26:cd503e08a218 917 trilateration.range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
harrynguyen 26:cd503e08a218 918
harrynguyen 26:cd503e08a218 919 memcpy(&rangestring_array[1], &uwb_data[0] + 13 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 920 rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
harrynguyen 26:cd503e08a218 921 trilateration.range_array[1] = strtol(rangestring_array[1],NULL, 16);
harrynguyen 26:cd503e08a218 922
harrynguyen 26:cd503e08a218 923 memcpy(&rangestring_array[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 924 rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
harrynguyen 26:cd503e08a218 925 trilateration.range_array[2] = strtol(rangestring_array[2],NULL, 16);
harrynguyen 26:cd503e08a218 926
harrynguyen 26:cd503e08a218 927 trilateration.GetLocation (&trilateration.robot_pos, 1, trilateration.anchor_pos, trilateration.range_array);
harrynguyen 26:cd503e08a218 928 }
harrynguyen 26:cd503e08a218 929
harrynguyen 26:cd503e08a218 930
harrynguyen 26:cd503e08a218 931 //void rxUwbCallback() {
harrynguyen 26:cd503e08a218 932 // NVIC_DisableIRQ(USART2_IRQn);
harrynguyen 26:cd503e08a218 933 // while(uwb.getc() != 'm'){
harrynguyen 26:cd503e08a218 934 // }
harrynguyen 26:cd503e08a218 935 // for(int i = 0 ; i < sizeof(uwb_data); i ++){
harrynguyen 26:cd503e08a218 936 // uwb_data[i] = uwb.getc();
harrynguyen 26:cd503e08a218 937 // if(uwb_data[i] == '\n'){
harrynguyen 26:cd503e08a218 938 // uwb_data_flag = 1;
harrynguyen 26:cd503e08a218 939 // break;
harrynguyen 26:cd503e08a218 940 // }
harrynguyen 26:cd503e08a218 941 // }
harrynguyen 26:cd503e08a218 942 // NVIC_EnableIRQ(USART2_IRQn);
harrynguyen 26:cd503e08a218 943 //}
harrynguyen 26:cd503e08a218 944
harrynguyen 26:cd503e08a218 945 void rxUwbCallback(MODSERIAL_IRQ_INFO *q) {
harrynguyen 26:cd503e08a218 946 MODSERIAL *serial = q->serial;
harrynguyen 26:cd503e08a218 947 if ( serial->rxGetLastChar() == '\n') {
harrynguyen 26:cd503e08a218 948 newline_detected = true;
harrynguyen 26:cd503e08a218 949 }
harrynguyen 26:cd503e08a218 950 }
harrynguyen 26:cd503e08a218 951
harrynguyen 26:cd503e08a218 952 //*******************************************************
harrynguyen 26:cd503e08a218 953
harrynguyen 26:cd503e08a218 954
harrynguyen 26:cd503e08a218 955
harrynguyen 26:cd503e08a218 956
harrynguyen 26:cd503e08a218 957
harrynguyen 26:cd503e08a218 958 /**
harrynguyen 26:cd503e08a218 959 * purepursuit loop as an individual thread
harrynguyen 26:cd503e08a218 960 */
harrynguyen 26:cd503e08a218 961 void purePursuit_thread(void const *n)
harrynguyen 26:cd503e08a218 962 {
harrynguyen 26:cd503e08a218 963 while(true) {
harrynguyen 26:cd503e08a218 964 // debugprint.printf("%.2f Running 7 purePursuit \n", imuTime);
harrynguyen 26:cd503e08a218 965 if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
harrynguyen 26:cd503e08a218 966 //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 26:cd503e08a218 967 purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 26:cd503e08a218 968
harrynguyen 26:cd503e08a218 969 if(purePursuit.robotFrame_targetDistance <= waypointZone)
harrynguyen 26:cd503e08a218 970 waypointReached_flag = 1;
harrynguyen 26:cd503e08a218 971 else
harrynguyen 26:cd503e08a218 972 waypointReached_flag = 0;
harrynguyen 26:cd503e08a218 973 }
harrynguyen 26:cd503e08a218 974 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 975 }
harrynguyen 26:cd503e08a218 976 }
harrynguyen 26:cd503e08a218 977
harrynguyen 26:cd503e08a218 978 /**
harrynguyen 26:cd503e08a218 979 * waypoint tracking loop as individual thread
harrynguyen 26:cd503e08a218 980 */
harrynguyen 26:cd503e08a218 981 void waypointCmd_thread(void const *n)
harrynguyen 26:cd503e08a218 982 {
harrynguyen 26:cd503e08a218 983 while(true) {
harrynguyen 26:cd503e08a218 984 // debugprint.printf("%.2f waypoint cmd \n", imuTime);
harrynguyen 26:cd503e08a218 985 //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
harrynguyen 26:cd503e08a218 986 if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
harrynguyen 26:cd503e08a218 987
harrynguyen 26:cd503e08a218 988 // if(waypoint_index > totalWaypoints) {
harrynguyen 26:cd503e08a218 989 // target_velocity = 0.0; //stop the robot
harrynguyen 26:cd503e08a218 990 // waypointSetFinish_flag = 1;
harrynguyen 26:cd503e08a218 991 // }
harrynguyen 26:cd503e08a218 992
harrynguyen 26:cd503e08a218 993 if(waypointReached_flag == 1 && waypoint_ready == 0) {
harrynguyen 26:cd503e08a218 994 target_velocity = 0.0; //stop the robot;
harrynguyen 26:cd503e08a218 995 motor_enable = 0;
harrynguyen 26:cd503e08a218 996 }
harrynguyen 26:cd503e08a218 997
harrynguyen 26:cd503e08a218 998 // debugprint.printf("waypointReached_flag = %d, waypoint_ready = %d \n", waypointReached_flag,waypoint_ready);
harrynguyen 26:cd503e08a218 999 // debugprint.printf("target waypoint %.2f %.2f \n", target_waypoint[0], target_waypoint[1]);
harrynguyen 26:cd503e08a218 1000 if(waypointReached_flag == 1 && waypointSetFinish_flag == 0 && waypoint_ready == 1) {
harrynguyen 26:cd503e08a218 1001 waypoint_index = 1;
harrynguyen 26:cd503e08a218 1002 // if(waypointReached_flag == 1 && waypointSetFinish_flag == 0){
harrynguyen 26:cd503e08a218 1003 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 26:cd503e08a218 1004 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 26:cd503e08a218 1005 target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
harrynguyen 26:cd503e08a218 1006 //target_velocity = 90*(driveTrain_maxV/100.0);
harrynguyen 26:cd503e08a218 1007 waypoint_ready = 0;
harrynguyen 26:cd503e08a218 1008 // waypoint_index++;
harrynguyen 26:cd503e08a218 1009 }
harrynguyen 26:cd503e08a218 1010 }
harrynguyen 26:cd503e08a218 1011 Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
harrynguyen 26:cd503e08a218 1012 }
harrynguyen 26:cd503e08a218 1013 }
harrynguyen 26:cd503e08a218 1014
harrynguyen 26:cd503e08a218 1015 ///**
harrynguyen 26:cd503e08a218 1016 ///**
harrynguyen 26:cd503e08a218 1017 // * nRF network communications as an individual thread
harrynguyen 26:cd503e08a218 1018 // */
harrynguyen 26:cd503e08a218 1019 //void comm_thread(void const *n)
harrynguyen 26:cd503e08a218 1020 //{
harrynguyen 26:cd503e08a218 1021 // comm.init(); //initialize communications unit
harrynguyen 26:cd503e08a218 1022 // Thread::wait(1000); //wait for a bit for radio to complete setup
harrynguyen 26:cd503e08a218 1023 // dataSend_flag=0;
harrynguyen 26:cd503e08a218 1024 //
harrynguyen 26:cd503e08a218 1025 // float data[2];
harrynguyen 26:cd503e08a218 1026 // wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0);
harrynguyen 26:cd503e08a218 1027 // //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0);
harrynguyen 26:cd503e08a218 1028 //
harrynguyen 26:cd503e08a218 1029 // while(true) {
harrynguyen 26:cd503e08a218 1030 // dataSend_flag =1;
harrynguyen 26:cd503e08a218 1031 //
harrynguyen 26:cd503e08a218 1032 // if((dataSend_flag == 1) && (comm.tx_ready == 1)) {
harrynguyen 26:cd503e08a218 1033 //
harrynguyen 26:cd503e08a218 1034 // comm.DataOut.addr = 0; //send to node address
harrynguyen 26:cd503e08a218 1035 //
harrynguyen 26:cd503e08a218 1036 // comm.DataOut.parameter[0] = 1; //parameter def 0
harrynguyen 26:cd503e08a218 1037 // comm.DataOut.parameter[1] = 2; //parameter def 1
harrynguyen 26:cd503e08a218 1038 //
harrynguyen 26:cd503e08a218 1039 // comm.DataOut.dataLen = 20; //length of data to be sent
harrynguyen 26:cd503e08a218 1040 //
harrynguyen 26:cd503e08a218 1041 // comm.DataOut.data[0] = imuTime; //timestamp
harrynguyen 26:cd503e08a218 1042 // comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle
harrynguyen 26:cd503e08a218 1043 // comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle
harrynguyen 26:cd503e08a218 1044 // comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle
harrynguyen 26:cd503e08a218 1045 // comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity
harrynguyen 26:cd503e08a218 1046 // comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity
harrynguyen 26:cd503e08a218 1047 // comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity
harrynguyen 26:cd503e08a218 1048 // comm.DataOut.data[7] = imu.LinAcc[0]; //x acc
harrynguyen 26:cd503e08a218 1049 // comm.DataOut.data[8] = imu.LinAcc[1]; //y acc
harrynguyen 26:cd503e08a218 1050 // comm.DataOut.data[9] = imu.LinAcc[2]; //z acc
harrynguyen 26:cd503e08a218 1051 // comm.DataOut.data[10] = localization.position[0]; //localization position x
harrynguyen 26:cd503e08a218 1052 // comm.DataOut.data[11] = localization.position[1]; //localization position y
harrynguyen 26:cd503e08a218 1053 // comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position
harrynguyen 26:cd503e08a218 1054 // comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position
harrynguyen 26:cd503e08a218 1055 // comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity
harrynguyen 26:cd503e08a218 1056 // comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity
harrynguyen 26:cd503e08a218 1057 // comm.DataOut.data[16] = bestsolution.x; // uwb x position //Harry changed here
harrynguyen 26:cd503e08a218 1058 // comm.DataOut.data[17] = bestsolution.y; // uwb y position
harrynguyen 26:cd503e08a218 1059 //// comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM %
harrynguyen 26:cd503e08a218 1060 //// comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM %
harrynguyen 26:cd503e08a218 1061 // comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command
harrynguyen 26:cd503e08a218 1062 // comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command
harrynguyen 26:cd503e08a218 1063 //
harrynguyen 26:cd503e08a218 1064 //
harrynguyen 26:cd503e08a218 1065 // comm_status[2] = comm.send();
harrynguyen 26:cd503e08a218 1066 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1067 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1068 //
harrynguyen 26:cd503e08a218 1069 // if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0
harrynguyen 26:cd503e08a218 1070 // }
harrynguyen 26:cd503e08a218 1071 //
harrynguyen 26:cd503e08a218 1072 // else {
harrynguyen 26:cd503e08a218 1073 // comm_status[2] = comm.update();
harrynguyen 26:cd503e08a218 1074 //
harrynguyen 26:cd503e08a218 1075 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1076 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1077 //
harrynguyen 26:cd503e08a218 1078 // if(comm_status[1] == 1) {
harrynguyen 26:cd503e08a218 1079 // //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen);
harrynguyen 26:cd503e08a218 1080 // if(go_cmd == 0) {
harrynguyen 26:cd503e08a218 1081 // if(comm.DataIn.parameter[1] == 0x10) go_cmd=1;
harrynguyen 26:cd503e08a218 1082 // }
harrynguyen 26:cd503e08a218 1083 // }
harrynguyen 26:cd503e08a218 1084 //
harrynguyen 26:cd503e08a218 1085 //
harrynguyen 26:cd503e08a218 1086 // }
harrynguyen 26:cd503e08a218 1087 //
harrynguyen 26:cd503e08a218 1088 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1089 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1090 //
harrynguyen 26:cd503e08a218 1091 // Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily
harrynguyen 26:cd503e08a218 1092 // }
harrynguyen 26:cd503e08a218 1093 //}
harrynguyen 26:cd503e08a218 1094
harrynguyen 26:cd503e08a218 1095
harrynguyen 26:cd503e08a218 1096
harrynguyen 26:cd503e08a218 1097 /**
harrynguyen 26:cd503e08a218 1098 * debug data print loop as an individual thread
harrynguyen 26:cd503e08a218 1099 */
harrynguyen 26:cd503e08a218 1100 #define print_lines 15 //number of info lines being printed on screen
harrynguyen 26:cd503e08a218 1101 void print_thread(void const *n)
harrynguyen 26:cd503e08a218 1102 {
harrynguyen 26:cd503e08a218 1103 //clear 14 lines while going up, these are the initilization lines printed on screen
harrynguyen 26:cd503e08a218 1104 for(int l=14; l>0; l--) {
harrynguyen 26:cd503e08a218 1105 debugprint.printf("\e[1A"); //go up 1 line
harrynguyen 26:cd503e08a218 1106 debugprint.printf("\e[K"); //clear line
harrynguyen 26:cd503e08a218 1107 }
harrynguyen 26:cd503e08a218 1108
harrynguyen 26:cd503e08a218 1109 debugprint.printf("************ VIRGO v3: Status Monitor *************\n\n");
harrynguyen 26:cd503e08a218 1110 for(int l=print_lines; l>0; l--) debugprint.printf("\n");
harrynguyen 26:cd503e08a218 1111 debugprint.printf("\n===================================================");
harrynguyen 26:cd503e08a218 1112 debugprint.printf("\e[1A"); //go up 1 line
harrynguyen 26:cd503e08a218 1113
harrynguyen 26:cd503e08a218 1114 while(true) {
harrynguyen 26:cd503e08a218 1115 //move cursor up # of lines printed to create a static display and clear the first line
harrynguyen 26:cd503e08a218 1116 for(int l=print_lines; l>0; l--) debugprint.printf("\e[1A");
harrynguyen 26:cd503e08a218 1117 debugprint.printf("\e[K");
harrynguyen 26:cd503e08a218 1118
harrynguyen 26:cd503e08a218 1119 debugprint.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
harrynguyen 26:cd503e08a218 1120 // debugprint.printf("Ranging: %.2f, %u\n\e\n", RangeSensor.theta_idx,RangeSensor.distance);
harrynguyen 26:cd503e08a218 1121 debugprint.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
harrynguyen 26:cd503e08a218 1122 debugprint.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
harrynguyen 26:cd503e08a218 1123 debugprint.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
harrynguyen 26:cd503e08a218 1124 debugprint.printf("Odometry : %f, %f \n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 26:cd503e08a218 1125 // 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]);
harrynguyen 26:cd503e08a218 1126 //
harrynguyen 26:cd503e08a218 1127 // //debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
harrynguyen 26:cd503e08a218 1128 //
harrynguyen 26:cd503e08a218 1129 debugprint.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
harrynguyen 26:cd503e08a218 1130 debugprint.printf("Waypoint Tracking: distanceToWaypoint %.1f, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE));
harrynguyen 26:cd503e08a218 1131 debugprint.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
harrynguyen 26:cd503e08a218 1132 //
harrynguyen 26:cd503e08a218 1133 // debug.printf("SMC: ref_beta %.2f, ref_dbeta %.3f\n\e[K", RAD_TO_DEG(ref_beta), RAD_TO_DEG(ref_dbeta));
harrynguyen 26:cd503e08a218 1134 // debug.printf("SMC: ref_gamma %.2f, ref_dgamma %.3f\n\e[K", RAD_TO_DEG(ref_gamma), RAD_TO_DEG(ref_dgamma));
harrynguyen 26:cd503e08a218 1135 // debug.printf("SMC: ref_theta %.2f, ref_dtheta %.3f\n\e[K", RAD_TO_DEG(ref_theta), RAD_TO_DEG(ref_dtheta));
harrynguyen 26:cd503e08a218 1136 // debug.printf("SMC: u1*tc %.2f rpm, u2*tc %.2f rpm\n\e[K", u1*0.005*60/(2*M_PI), u2*0.005*60/(2*M_PI));
harrynguyen 26:cd503e08a218 1137 //
harrynguyen 26:cd503e08a218 1138 debugprint.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
harrynguyen 26:cd503e08a218 1139 debugprint.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
harrynguyen 26:cd503e08a218 1140 debugprint.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
harrynguyen 26:cd503e08a218 1141 // //debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 26:cd503e08a218 1142 //
harrynguyen 26:cd503e08a218 1143 // //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);
harrynguyen 26:cd503e08a218 1144 // //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);
harrynguyen 26:cd503e08a218 1145 //
harrynguyen 26:cd503e08a218 1146 // 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);
harrynguyen 26:cd503e08a218 1147 // //debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
harrynguyen 26:cd503e08a218 1148 // debugprint.printf("Raspberry waypoint: %s \n\e[K", rasp_data);
harrynguyen 26:cd503e08a218 1149 debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
harrynguyen 26:cd503e08a218 1150 // debugprint.printf("UWB ranging: %s %s %s\n\e[K", rangestring_array[0],rangestring_array[1], rangestring_array[2]);
harrynguyen 26:cd503e08a218 1151 debugprint.printf("UWB ranging: %d %d %d \n\e[K", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
harrynguyen 26:cd503e08a218 1152 debugprint.printf("x : %f , y : %f , z : %f \n\e[K", trilateration.robot_pos.x, trilateration.robot_pos.y, trilateration.robot_pos.z);
harrynguyen 26:cd503e08a218 1153 // debugprint.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);
harrynguyen 26:cd503e08a218 1154
harrynguyen 26:cd503e08a218 1155 Thread::wait(PrintLoop_PeriodMS);
harrynguyen 26:cd503e08a218 1156 }
harrynguyen 26:cd503e08a218 1157 }
harrynguyen 26:cd503e08a218 1158
harrynguyen 26:cd503e08a218 1159