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@29:43056f5cd0db, 2018-06-01 (annotated)

Committer:
harrynguyen
Date:
Fri Jun 01 01:54:04 2018 +0000
Revision:
29:43056f5cd0db
Parent:
28:edb5e3770ae1
Child:
30:3cfa8d7f84de
Fix lidar bug

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 29:43056f5cd0db 260 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
harrynguyen 29:43056f5cd0db 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 29:43056f5cd0db 760 purePursuit_velocity = strtol(linear,NULL,10); //convert string to int linear in mm
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 28:edb5e3770ae1 827 }
harrynguyen 29:43056f5cd0db 828 // debugprint.printf("Running uwb %s \n",uwb_data);
harrynguyen 28:edb5e3770ae1 829 // debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
harrynguyen 26:cd503e08a218 830 uwbtriangulation_fn(uwb_data); // Running Triateration (The function nam is misleading)
harrynguyen 29:43056f5cd0db 831 // debugprint.printf("UWB ranging: %d %d %d \n", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
harrynguyen 26:cd503e08a218 832 // ekf_fn(&ekf); //run ekf
harrynguyen 28:edb5e3770ae1 833 // debugprint.printf("%s",uwb_data);
harrynguyen 26:cd503e08a218 834 for (int j = 0; j< sizeof(uwb_data); j++) {
harrynguyen 26:cd503e08a218 835 uwb_data[j] = 0;
harrynguyen 26:cd503e08a218 836 }
harrynguyen 26:cd503e08a218 837 uwb_data_flag = 0;
harrynguyen 26:cd503e08a218 838
harrynguyen 26:cd503e08a218 839 //OLD
harrynguyen 26:cd503e08a218 840 // if(uwb_data_flag == 1){
harrynguyen 26:cd503e08a218 841 //// memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */); //copy data from buffer
harrynguyen 26:cd503e08a218 842 // uwbtriangulation_fn(uwb_data);
harrynguyen 26:cd503e08a218 843 // for (int j = 0; j< sizeof(uwb_data); j++) {
harrynguyen 26:cd503e08a218 844 // uwb_data[j] = 0;
harrynguyen 26:cd503e08a218 845 // }
harrynguyen 26:cd503e08a218 846 // uwb_data_flag = 0;
harrynguyen 26:cd503e08a218 847 //
harrynguyen 26:cd503e08a218 848 // }
harrynguyen 26:cd503e08a218 849
harrynguyen 26:cd503e08a218 850 // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 851 //debugprint.printf("%s \n",range);
harrynguyen 26:cd503e08a218 852 Thread::wait(200);
harrynguyen 26:cd503e08a218 853 }
harrynguyen 26:cd503e08a218 854
harrynguyen 26:cd503e08a218 855 }
harrynguyen 26:cd503e08a218 856
harrynguyen 26:cd503e08a218 857 //void uwbtriangulation_thread(void const *n)
harrynguyen 26:cd503e08a218 858 //{
harrynguyen 26:cd503e08a218 859 // int anchorheight = 1.8;
harrynguyen 26:cd503e08a218 860 // anchorArray[0].x = 0.0;
harrynguyen 26:cd503e08a218 861 // anchorArray[0].y = 0.0;
harrynguyen 26:cd503e08a218 862 // anchorArray[0].z = anchorheight;
harrynguyen 26:cd503e08a218 863 //
harrynguyen 26:cd503e08a218 864 // anchorArray[1].x = 3.0;
harrynguyen 26:cd503e08a218 865 // anchorArray[1].y = 0.0;
harrynguyen 26:cd503e08a218 866 // anchorArray[1].z = anchorheight;
harrynguyen 26:cd503e08a218 867 //
harrynguyen 26:cd503e08a218 868 // anchorArray[2].x = 0.0;
harrynguyen 26:cd503e08a218 869 // anchorArray[2].y = 4.0;
harrynguyen 26:cd503e08a218 870 // anchorArray[2].z = anchorheight;
harrynguyen 26:cd503e08a218 871 //
harrynguyen 26:cd503e08a218 872 // range_array[0] = 0;
harrynguyen 26:cd503e08a218 873 // range_array[1] = 0;
harrynguyen 26:cd503e08a218 874 // range_array[2] = 0;
harrynguyen 26:cd503e08a218 875 // range_array[3] = 0;
harrynguyen 26:cd503e08a218 876 //
harrynguyen 26:cd503e08a218 877 // while(1){
harrynguyen 26:cd503e08a218 878 //// debugprint.printf("%.2f Running 6 uwb triangulation \n", imuTime);
harrynguyen 26:cd503e08a218 879 // //memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */);
harrynguyen 26:cd503e08a218 880 //
harrynguyen 26:cd503e08a218 881 // memcpy(rangestring_array[0], &uwb_data1[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
harrynguyen 26:cd503e08a218 882 // rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
harrynguyen 26:cd503e08a218 883 // range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
harrynguyen 26:cd503e08a218 884 //
harrynguyen 26:cd503e08a218 885 // memcpy(rangestring_array[1], &uwb_data1[0] + 13 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 886 // rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
harrynguyen 26:cd503e08a218 887 // range_array[1] = strtol(rangestring_array[1],NULL, 16);
harrynguyen 26:cd503e08a218 888 //
harrynguyen 26:cd503e08a218 889 // memcpy(rangestring_array[2], &uwb_data1[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 890 // rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
harrynguyen 26:cd503e08a218 891 // range_array[2] = strtol(rangestring_array[2],NULL, 16);
harrynguyen 26:cd503e08a218 892 //
harrynguyen 26:cd503e08a218 893 // trilateration.GetLocation (&bestsolution, 1, anchorArray, range_array);
harrynguyen 26:cd503e08a218 894 // //memcpy(range[1], &uwb_data1[0] + 13 /* Offset */, sizeof(range[1]) /* Length */);
harrynguyen 26:cd503e08a218 895 // // range_array[1] = strtol(range[1],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 896 // // memcpy(range[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 897 // // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 898 // // range_float[0] = strtol(range[0],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 899 // // range_float[1] = strtol(range[1],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 900 // // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
harrynguyen 26:cd503e08a218 901 // // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 902 // // debugprint.printf(range[0]);
harrynguyen 26:cd503e08a218 903 //// debugprint.puts(range[1]);
harrynguyen 26:cd503e08a218 904 // // debugprint.puts(range[2]);
harrynguyen 26:cd503e08a218 905 // // debugprint.printf("\n");
harrynguyen 26:cd503e08a218 906 // //debugprint.printf("%s",uwb_data);
harrynguyen 26:cd503e08a218 907 // //debugprint.printf("%s \n",uwb_data[66]);
harrynguyen 26:cd503e08a218 908 // Thread::wait(250);
harrynguyen 26:cd503e08a218 909 // }
harrynguyen 26:cd503e08a218 910 //
harrynguyen 26:cd503e08a218 911 //}
harrynguyen 26:cd503e08a218 912
harrynguyen 26:cd503e08a218 913 void uwbtriangulation_fn(char* uwb_data)
harrynguyen 26:cd503e08a218 914 {
harrynguyen 26:cd503e08a218 915 char rangestring_array[3][10];
harrynguyen 26:cd503e08a218 916 memcpy(rangestring_array[0], &uwb_data[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
harrynguyen 26:cd503e08a218 917 rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
harrynguyen 26:cd503e08a218 918 trilateration.range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
harrynguyen 26:cd503e08a218 919
harrynguyen 26:cd503e08a218 920 memcpy(&rangestring_array[1], &uwb_data[0] + 13 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 921 rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
harrynguyen 26:cd503e08a218 922 trilateration.range_array[1] = strtol(rangestring_array[1],NULL, 16);
harrynguyen 26:cd503e08a218 923
harrynguyen 26:cd503e08a218 924 memcpy(&rangestring_array[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
harrynguyen 26:cd503e08a218 925 rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
harrynguyen 26:cd503e08a218 926 trilateration.range_array[2] = strtol(rangestring_array[2],NULL, 16);
harrynguyen 26:cd503e08a218 927
harrynguyen 26:cd503e08a218 928 trilateration.GetLocation (&trilateration.robot_pos, 1, trilateration.anchor_pos, trilateration.range_array);
harrynguyen 26:cd503e08a218 929 }
harrynguyen 26:cd503e08a218 930
harrynguyen 26:cd503e08a218 931
harrynguyen 26:cd503e08a218 932 //void rxUwbCallback() {
harrynguyen 26:cd503e08a218 933 // NVIC_DisableIRQ(USART2_IRQn);
harrynguyen 26:cd503e08a218 934 // while(uwb.getc() != 'm'){
harrynguyen 26:cd503e08a218 935 // }
harrynguyen 26:cd503e08a218 936 // for(int i = 0 ; i < sizeof(uwb_data); i ++){
harrynguyen 26:cd503e08a218 937 // uwb_data[i] = uwb.getc();
harrynguyen 26:cd503e08a218 938 // if(uwb_data[i] == '\n'){
harrynguyen 26:cd503e08a218 939 // uwb_data_flag = 1;
harrynguyen 26:cd503e08a218 940 // break;
harrynguyen 26:cd503e08a218 941 // }
harrynguyen 26:cd503e08a218 942 // }
harrynguyen 26:cd503e08a218 943 // NVIC_EnableIRQ(USART2_IRQn);
harrynguyen 26:cd503e08a218 944 //}
harrynguyen 26:cd503e08a218 945
harrynguyen 26:cd503e08a218 946 void rxUwbCallback(MODSERIAL_IRQ_INFO *q) {
harrynguyen 26:cd503e08a218 947 MODSERIAL *serial = q->serial;
harrynguyen 26:cd503e08a218 948 if ( serial->rxGetLastChar() == '\n') {
harrynguyen 26:cd503e08a218 949 newline_detected = true;
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 /**
harrynguyen 26:cd503e08a218 960 * purepursuit loop as an individual thread
harrynguyen 26:cd503e08a218 961 */
harrynguyen 26:cd503e08a218 962 void purePursuit_thread(void const *n)
harrynguyen 26:cd503e08a218 963 {
harrynguyen 26:cd503e08a218 964 while(true) {
harrynguyen 26:cd503e08a218 965 // debugprint.printf("%.2f Running 7 purePursuit \n", imuTime);
harrynguyen 26:cd503e08a218 966 if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
harrynguyen 26:cd503e08a218 967 //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 26:cd503e08a218 968 purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
harrynguyen 26:cd503e08a218 969
harrynguyen 26:cd503e08a218 970 if(purePursuit.robotFrame_targetDistance <= waypointZone)
harrynguyen 26:cd503e08a218 971 waypointReached_flag = 1;
harrynguyen 26:cd503e08a218 972 else
harrynguyen 26:cd503e08a218 973 waypointReached_flag = 0;
harrynguyen 26:cd503e08a218 974 }
harrynguyen 26:cd503e08a218 975 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 26:cd503e08a218 976 }
harrynguyen 26:cd503e08a218 977 }
harrynguyen 26:cd503e08a218 978
harrynguyen 26:cd503e08a218 979 /**
harrynguyen 26:cd503e08a218 980 * waypoint tracking loop as individual thread
harrynguyen 26:cd503e08a218 981 */
harrynguyen 26:cd503e08a218 982 void waypointCmd_thread(void const *n)
harrynguyen 26:cd503e08a218 983 {
harrynguyen 26:cd503e08a218 984 while(true) {
harrynguyen 26:cd503e08a218 985 // debugprint.printf("%.2f waypoint cmd \n", imuTime);
harrynguyen 26:cd503e08a218 986 //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
harrynguyen 26:cd503e08a218 987 if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
harrynguyen 26:cd503e08a218 988
harrynguyen 26:cd503e08a218 989 // if(waypoint_index > totalWaypoints) {
harrynguyen 26:cd503e08a218 990 // target_velocity = 0.0; //stop the robot
harrynguyen 26:cd503e08a218 991 // waypointSetFinish_flag = 1;
harrynguyen 26:cd503e08a218 992 // }
harrynguyen 26:cd503e08a218 993
harrynguyen 26:cd503e08a218 994 if(waypointReached_flag == 1 && waypoint_ready == 0) {
harrynguyen 26:cd503e08a218 995 target_velocity = 0.0; //stop the robot;
harrynguyen 26:cd503e08a218 996 motor_enable = 0;
harrynguyen 26:cd503e08a218 997 }
harrynguyen 26:cd503e08a218 998
harrynguyen 26:cd503e08a218 999 // debugprint.printf("waypointReached_flag = %d, waypoint_ready = %d \n", waypointReached_flag,waypoint_ready);
harrynguyen 26:cd503e08a218 1000 // debugprint.printf("target waypoint %.2f %.2f \n", target_waypoint[0], target_waypoint[1]);
harrynguyen 26:cd503e08a218 1001 if(waypointReached_flag == 1 && waypointSetFinish_flag == 0 && waypoint_ready == 1) {
harrynguyen 26:cd503e08a218 1002 waypoint_index = 1;
harrynguyen 26:cd503e08a218 1003 // if(waypointReached_flag == 1 && waypointSetFinish_flag == 0){
harrynguyen 26:cd503e08a218 1004 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 26:cd503e08a218 1005 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 26:cd503e08a218 1006 target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
harrynguyen 26:cd503e08a218 1007 //target_velocity = 90*(driveTrain_maxV/100.0);
harrynguyen 26:cd503e08a218 1008 waypoint_ready = 0;
harrynguyen 26:cd503e08a218 1009 // waypoint_index++;
harrynguyen 26:cd503e08a218 1010 }
harrynguyen 26:cd503e08a218 1011 }
harrynguyen 26:cd503e08a218 1012 Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
harrynguyen 26:cd503e08a218 1013 }
harrynguyen 26:cd503e08a218 1014 }
harrynguyen 26:cd503e08a218 1015
harrynguyen 26:cd503e08a218 1016 ///**
harrynguyen 26:cd503e08a218 1017 ///**
harrynguyen 26:cd503e08a218 1018 // * nRF network communications as an individual thread
harrynguyen 26:cd503e08a218 1019 // */
harrynguyen 26:cd503e08a218 1020 //void comm_thread(void const *n)
harrynguyen 26:cd503e08a218 1021 //{
harrynguyen 26:cd503e08a218 1022 // comm.init(); //initialize communications unit
harrynguyen 26:cd503e08a218 1023 // Thread::wait(1000); //wait for a bit for radio to complete setup
harrynguyen 26:cd503e08a218 1024 // dataSend_flag=0;
harrynguyen 26:cd503e08a218 1025 //
harrynguyen 26:cd503e08a218 1026 // float data[2];
harrynguyen 26:cd503e08a218 1027 // wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0);
harrynguyen 26:cd503e08a218 1028 // //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0);
harrynguyen 26:cd503e08a218 1029 //
harrynguyen 26:cd503e08a218 1030 // while(true) {
harrynguyen 26:cd503e08a218 1031 // dataSend_flag =1;
harrynguyen 26:cd503e08a218 1032 //
harrynguyen 26:cd503e08a218 1033 // if((dataSend_flag == 1) && (comm.tx_ready == 1)) {
harrynguyen 26:cd503e08a218 1034 //
harrynguyen 26:cd503e08a218 1035 // comm.DataOut.addr = 0; //send to node address
harrynguyen 26:cd503e08a218 1036 //
harrynguyen 26:cd503e08a218 1037 // comm.DataOut.parameter[0] = 1; //parameter def 0
harrynguyen 26:cd503e08a218 1038 // comm.DataOut.parameter[1] = 2; //parameter def 1
harrynguyen 26:cd503e08a218 1039 //
harrynguyen 26:cd503e08a218 1040 // comm.DataOut.dataLen = 20; //length of data to be sent
harrynguyen 26:cd503e08a218 1041 //
harrynguyen 26:cd503e08a218 1042 // comm.DataOut.data[0] = imuTime; //timestamp
harrynguyen 26:cd503e08a218 1043 // comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle
harrynguyen 26:cd503e08a218 1044 // comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle
harrynguyen 26:cd503e08a218 1045 // comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle
harrynguyen 26:cd503e08a218 1046 // comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity
harrynguyen 26:cd503e08a218 1047 // comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity
harrynguyen 26:cd503e08a218 1048 // comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity
harrynguyen 26:cd503e08a218 1049 // comm.DataOut.data[7] = imu.LinAcc[0]; //x acc
harrynguyen 26:cd503e08a218 1050 // comm.DataOut.data[8] = imu.LinAcc[1]; //y acc
harrynguyen 26:cd503e08a218 1051 // comm.DataOut.data[9] = imu.LinAcc[2]; //z acc
harrynguyen 26:cd503e08a218 1052 // comm.DataOut.data[10] = localization.position[0]; //localization position x
harrynguyen 26:cd503e08a218 1053 // comm.DataOut.data[11] = localization.position[1]; //localization position y
harrynguyen 26:cd503e08a218 1054 // comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position
harrynguyen 26:cd503e08a218 1055 // comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position
harrynguyen 26:cd503e08a218 1056 // comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity
harrynguyen 26:cd503e08a218 1057 // comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity
harrynguyen 26:cd503e08a218 1058 // comm.DataOut.data[16] = bestsolution.x; // uwb x position //Harry changed here
harrynguyen 26:cd503e08a218 1059 // comm.DataOut.data[17] = bestsolution.y; // uwb y position
harrynguyen 26:cd503e08a218 1060 //// comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM %
harrynguyen 26:cd503e08a218 1061 //// comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM %
harrynguyen 26:cd503e08a218 1062 // comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command
harrynguyen 26:cd503e08a218 1063 // comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command
harrynguyen 26:cd503e08a218 1064 //
harrynguyen 26:cd503e08a218 1065 //
harrynguyen 26:cd503e08a218 1066 // comm_status[2] = comm.send();
harrynguyen 26:cd503e08a218 1067 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1068 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1069 //
harrynguyen 26:cd503e08a218 1070 // if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0
harrynguyen 26:cd503e08a218 1071 // }
harrynguyen 26:cd503e08a218 1072 //
harrynguyen 26:cd503e08a218 1073 // else {
harrynguyen 26:cd503e08a218 1074 // comm_status[2] = comm.update();
harrynguyen 26:cd503e08a218 1075 //
harrynguyen 26:cd503e08a218 1076 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1077 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1078 //
harrynguyen 26:cd503e08a218 1079 // if(comm_status[1] == 1) {
harrynguyen 26:cd503e08a218 1080 // //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen);
harrynguyen 26:cd503e08a218 1081 // if(go_cmd == 0) {
harrynguyen 26:cd503e08a218 1082 // if(comm.DataIn.parameter[1] == 0x10) go_cmd=1;
harrynguyen 26:cd503e08a218 1083 // }
harrynguyen 26:cd503e08a218 1084 // }
harrynguyen 26:cd503e08a218 1085 //
harrynguyen 26:cd503e08a218 1086 //
harrynguyen 26:cd503e08a218 1087 // }
harrynguyen 26:cd503e08a218 1088 //
harrynguyen 26:cd503e08a218 1089 // comm_status[0] = (comm_status[2] & 0b0001);
harrynguyen 26:cd503e08a218 1090 // comm_status[1] = (comm_status[2] & 0b0010) >> 1;
harrynguyen 26:cd503e08a218 1091 //
harrynguyen 26:cd503e08a218 1092 // Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily
harrynguyen 26:cd503e08a218 1093 // }
harrynguyen 26:cd503e08a218 1094 //}
harrynguyen 26:cd503e08a218 1095
harrynguyen 26:cd503e08a218 1096
harrynguyen 26:cd503e08a218 1097
harrynguyen 26:cd503e08a218 1098 /**
harrynguyen 26:cd503e08a218 1099 * debug data print loop as an individual thread
harrynguyen 26:cd503e08a218 1100 */
harrynguyen 26:cd503e08a218 1101 #define print_lines 15 //number of info lines being printed on screen
harrynguyen 26:cd503e08a218 1102 void print_thread(void const *n)
harrynguyen 26:cd503e08a218 1103 {
harrynguyen 26:cd503e08a218 1104 //clear 14 lines while going up, these are the initilization lines printed on screen
harrynguyen 26:cd503e08a218 1105 for(int l=14; l>0; l--) {
harrynguyen 26:cd503e08a218 1106 debugprint.printf("\e[1A"); //go up 1 line
harrynguyen 26:cd503e08a218 1107 debugprint.printf("\e[K"); //clear line
harrynguyen 26:cd503e08a218 1108 }
harrynguyen 26:cd503e08a218 1109
harrynguyen 26:cd503e08a218 1110 debugprint.printf("************ VIRGO v3: Status Monitor *************\n\n");
harrynguyen 26:cd503e08a218 1111 for(int l=print_lines; l>0; l--) debugprint.printf("\n");
harrynguyen 26:cd503e08a218 1112 debugprint.printf("\n===================================================");
harrynguyen 26:cd503e08a218 1113 debugprint.printf("\e[1A"); //go up 1 line
harrynguyen 26:cd503e08a218 1114
harrynguyen 26:cd503e08a218 1115 while(true) {
harrynguyen 26:cd503e08a218 1116 //move cursor up # of lines printed to create a static display and clear the first line
harrynguyen 26:cd503e08a218 1117 for(int l=print_lines; l>0; l--) debugprint.printf("\e[1A");
harrynguyen 26:cd503e08a218 1118 debugprint.printf("\e[K");
harrynguyen 26:cd503e08a218 1119
harrynguyen 26:cd503e08a218 1120 debugprint.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
harrynguyen 26:cd503e08a218 1121 // debugprint.printf("Ranging: %.2f, %u\n\e\n", RangeSensor.theta_idx,RangeSensor.distance);
harrynguyen 26:cd503e08a218 1122 debugprint.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
harrynguyen 26:cd503e08a218 1123 debugprint.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
harrynguyen 26:cd503e08a218 1124 debugprint.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
harrynguyen 26:cd503e08a218 1125 debugprint.printf("Odometry : %f, %f \n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 26:cd503e08a218 1126 // 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 1127 //
harrynguyen 26:cd503e08a218 1128 // //debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
harrynguyen 26:cd503e08a218 1129 //
harrynguyen 26:cd503e08a218 1130 debugprint.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
harrynguyen 26:cd503e08a218 1131 debugprint.printf("Waypoint Tracking: distanceToWaypoint %.1f, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE));
harrynguyen 26:cd503e08a218 1132 debugprint.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
harrynguyen 26:cd503e08a218 1133 //
harrynguyen 26:cd503e08a218 1134 // 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 1135 // 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 1136 // 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 1137 // 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 1138 //
harrynguyen 26:cd503e08a218 1139 debugprint.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
harrynguyen 26:cd503e08a218 1140 debugprint.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
harrynguyen 26:cd503e08a218 1141 debugprint.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
harrynguyen 26:cd503e08a218 1142 // //debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 26:cd503e08a218 1143 //
harrynguyen 26:cd503e08a218 1144 // //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 1145 // //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 1146 //
harrynguyen 26:cd503e08a218 1147 // 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 1148 // //debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
harrynguyen 26:cd503e08a218 1149 // debugprint.printf("Raspberry waypoint: %s \n\e[K", rasp_data);
harrynguyen 26:cd503e08a218 1150 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 1151 // debugprint.printf("UWB ranging: %s %s %s\n\e[K", rangestring_array[0],rangestring_array[1], rangestring_array[2]);
harrynguyen 26:cd503e08a218 1152 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 1153 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 1154 // 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 1155
harrynguyen 26:cd503e08a218 1156 Thread::wait(PrintLoop_PeriodMS);
harrynguyen 26:cd503e08a218 1157 }
harrynguyen 26:cd503e08a218 1158 }
harrynguyen 26:cd503e08a218 1159
harrynguyen 26:cd503e08a218 1160