Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

orion_main.cpp@30:44676e1b38f8, 2018-03-20 (annotated)

Committer:
ahmed_lv
Date:
Tue Mar 20 05:56:22 2018 +0000
Revision:
30:44676e1b38f8
Parent:
29:a6a812ee83ea 
Editted Input Variables to PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrynguyen 21:7cd86bea7f83 1 /**
harrynguyen 21:7cd86bea7f83 2 * @author Akash Vibhute
harrynguyen 21:7cd86bea7f83 3 * < akash . roboticist [at] gmail . com >
harrynguyen 21:7cd86bea7f83 4 *
harrynguyen 21:7cd86bea7f83 5 * @section LICENSE
harrynguyen 21:7cd86bea7f83 6 *
harrynguyen 21:7cd86bea7f83 7 * Copyright (c) 2015 Akash Vibhute
harrynguyen 21:7cd86bea7f83 8 *
harrynguyen 21:7cd86bea7f83 9 * Licensed under the Apache License, Version 2.0 (the "License");
harrynguyen 21:7cd86bea7f83 10 * you may not use this file except in compliance with the License.
harrynguyen 21:7cd86bea7f83 11 * You may obtain a copy of the License at
harrynguyen 21:7cd86bea7f83 12 *
harrynguyen 21:7cd86bea7f83 13 * http://www.apache.org/licenses/LICENSE-2.0
harrynguyen 21:7cd86bea7f83 14 *
harrynguyen 21:7cd86bea7f83 15 * Unless required by applicable law or agreed to in writing, software
harrynguyen 21:7cd86bea7f83 16 * distributed under the License is distributed on an "AS IS" BASIS,
harrynguyen 21:7cd86bea7f83 17 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
harrynguyen 21:7cd86bea7f83 18 * See the License for the specific language governing permissions and
harrynguyen 21:7cd86bea7f83 19 * limitations under the License.
harrynguyen 21:7cd86bea7f83 20 *
harrynguyen 21:7cd86bea7f83 21 * The above copyright notice and this permission notice shall be included in
harrynguyen 21:7cd86bea7f83 22 * all copies or substantial portions of the Software.
harrynguyen 21:7cd86bea7f83 23 *
harrynguyen 21:7cd86bea7f83 24 * @section DESCRIPTION
harrynguyen 21:7cd86bea7f83 25 *
harrynguyen 21:7cd86bea7f83 26 * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015]
harrynguyen 21:7cd86bea7f83 27 * Robot controller software for SUTD Virgo version 3 robot
harrynguyen 21:7cd86bea7f83 28 *
harrynguyen 21:7cd86bea7f83 29 */
harrynguyen 21:7cd86bea7f83 30
harrynguyen 21:7cd86bea7f83 31 /**
harrynguyen 21:7cd86bea7f83 32 * Header file for including all necessary functions for robot and defining any
harrynguyen 21:7cd86bea7f83 33 * custom functions written in the main file.
harrynguyen 21:7cd86bea7f83 34 */
harrynguyen 21:7cd86bea7f83 35 #include "main.h"
harrynguyen 21:7cd86bea7f83 36 #include "globalExterns.h"
ahmed_lv 27:2db168d9fb18 37 #include "VL53L0X.h"
ahmed_lv 27:2db168d9fb18 38 #define W 0.1
harrynguyen 21:7cd86bea7f83 39
harrynguyen 21:7cd86bea7f83 40 /**
harrynguyen 21:7cd86bea7f83 41 * Functions, Threads & General Definitions
harrynguyen 21:7cd86bea7f83 42 */
harrynguyen 21:7cd86bea7f83 43 //*****************************************************************************
ahmed_lv 27:2db168d9fb18 44
ahmed_lv 27:2db168d9fb18 45 //** Ranging **//
ahmed_lv 27:2db168d9fb18 46 I2C ItC_ranging(i2c2_SDA, i2c2_SCL);
ahmed_lv 27:2db168d9fb18 47 Timer time_r;
ahmed_lv 27:2db168d9fb18 48
ahmed_lv 27:2db168d9fb18 49 DigitalOut xshut1(xs1);
ahmed_lv 27:2db168d9fb18 50 DigitalOut xshut2(xs2);
ahmed_lv 27:2db168d9fb18 51 DigitalOut xshut3(xs3);
ahmed_lv 27:2db168d9fb18 52 DigitalOut xshut4(xs4);
ahmed_lv 27:2db168d9fb18 53 DigitalOut xshut5(xs5);
ahmed_lv 27:2db168d9fb18 54
ahmed_lv 27:2db168d9fb18 55 VL53L0X sensor1(&ItC_ranging, &time_r);
ahmed_lv 27:2db168d9fb18 56 VL53L0X sensor2(&ItC_ranging, &time_r);
ahmed_lv 27:2db168d9fb18 57 VL53L0X sensor3(&ItC_ranging, &time_r);
ahmed_lv 27:2db168d9fb18 58 VL53L0X sensor4(&ItC_ranging, &time_r);
ahmed_lv 27:2db168d9fb18 59 VL53L0X sensor5(&ItC_ranging, &time_r);
ahmed_lv 27:2db168d9fb18 60
ahmed_lv 27:2db168d9fb18 61
ahmed_lv 27:2db168d9fb18 62 struct RangeData{
ahmed_lv 27:2db168d9fb18 63 uint16_t exleft, left, front, right, exright;;
ahmed_lv 27:2db168d9fb18 64 };
ahmed_lv 27:2db168d9fb18 65 RangeData RangeSensor;
ahmed_lv 27:2db168d9fb18 66
ahmed_lv 27:2db168d9fb18 67
harrynguyen 21:7cd86bea7f83 68 //** Drivetrain **
harrynguyen 21:7cd86bea7f83 69 motorDriver drive; //motor drive train
harrynguyen 21:7cd86bea7f83 70 odometer odometry; //odometer function
ahmed_lv 26:32eaf3c3ac2e 71 pidBearing PID_B; //PID control for bearing
harrynguyen 21:7cd86bea7f83 72 pidControl PID_L, PID_R; //pidcontroller for left and right motors
harrynguyen 21:7cd86bea7f83 73
ahmed_lv 30:44676e1b38f8 74 //**new PID control**/*#LV*/
ahmed_lv 29:a6a812ee83ea 75 float bearingToWaypoint, robotFrame, prevError, speedChange;
ahmed_lv 29:a6a812ee83ea 76 float k3, cmdL, cmdR, Error;
ahmed_lv 30:44676e1b38f8 77 //**end of new control **/*#LV*/
ahmed_lv 26:32eaf3c3ac2e 78
harrynguyen 21:7cd86bea7f83 79 Timer motorControl_t;
harrynguyen 21:7cd86bea7f83 80 float rpm_cmd[2]; //drive motor rpm command
harrynguyen 21:7cd86bea7f83 81 float rpm_compensated[2]; //rpm command compensated by acc limit
harrynguyen 21:7cd86bea7f83 82 float targetAcceleration = 300.0; //RPM/s acceleration
harrynguyen 21:7cd86bea7f83 83 float pwm_cmd[2]; //drive motor pwm command
harrynguyen 21:7cd86bea7f83 84
harrynguyen 21:7cd86bea7f83 85 /* THREAD */
harrynguyen 21:7cd86bea7f83 86 void odometry_thread(void const *n);
harrynguyen 21:7cd86bea7f83 87 void motorControl_thread(void const *n);
harrynguyen 21:7cd86bea7f83 88 /* ** */
harrynguyen 21:7cd86bea7f83 89 //-------------
harrynguyen 21:7cd86bea7f83 90
harrynguyen 21:7cd86bea7f83 91 //** Localization **
harrynguyen 21:7cd86bea7f83 92 IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class
harrynguyen 21:7cd86bea7f83 93 float imuTime;
harrynguyen 21:7cd86bea7f83 94 Localization localization; //localization function
harrynguyen 21:7cd86bea7f83 95
harrynguyen 21:7cd86bea7f83 96 /* FUNCTION */
harrynguyen 21:7cd86bea7f83 97 bool imuInit_function();
harrynguyen 21:7cd86bea7f83 98 /* ** */
harrynguyen 21:7cd86bea7f83 99
harrynguyen 21:7cd86bea7f83 100 /* THREAD */
harrynguyen 21:7cd86bea7f83 101 void imu_thread(void const *n);
harrynguyen 21:7cd86bea7f83 102
harrynguyen 21:7cd86bea7f83 103 //-------------
harrynguyen 21:7cd86bea7f83 104
harrynguyen 21:7cd86bea7f83 105 //** Power Monitor **
harrynguyen 21:7cd86bea7f83 106 BattGuage battery; //Battery fuel gauge wrapper
harrynguyen 21:7cd86bea7f83 107
harrynguyen 21:7cd86bea7f83 108 /* THREAD */
harrynguyen 21:7cd86bea7f83 109
harrynguyen 21:7cd86bea7f83 110 /* ** */
harrynguyen 21:7cd86bea7f83 111 //-------------
harrynguyen 21:7cd86bea7f83 112
harrynguyen 21:7cd86bea7f83 113 //** Trajectory tracking **
harrynguyen 21:7cd86bea7f83 114 purePursuit purePursuit;
harrynguyen 23:6806c3bacf58 115 //kinematics kinematics;
harrynguyen 21:7cd86bea7f83 116
harrynguyen 21:7cd86bea7f83 117 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
harrynguyen 21:7cd86bea7f83 118 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
harrynguyen 21:7cd86bea7f83 119 uint8_t totalWaypoints = 5;
harrynguyen 21:7cd86bea7f83 120 //kite pattern 200cm long, 100cm wide
harrynguyen 21:7cd86bea7f83 121 int16_t waypoints_set[][4] = { {0,0,90,0},
harrynguyen 21:7cd86bea7f83 122 {100,100,90,0},
harrynguyen 21:7cd86bea7f83 123 {0,200,90,0},
harrynguyen 21:7cd86bea7f83 124 {-100,100,90,0},
harrynguyen 21:7cd86bea7f83 125 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 126 {0,0,90,0},
harrynguyen 21:7cd86bea7f83 127 {0,0,90,0}
harrynguyen 21:7cd86bea7f83 128 };
harrynguyen 21:7cd86bea7f83 129
harrynguyen 21:7cd86bea7f83 130 float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached.
harrynguyen 21:7cd86bea7f83 131 uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached
harrynguyen 21:7cd86bea7f83 132 uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop.
harrynguyen 21:7cd86bea7f83 133 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 21:7cd86bea7f83 134 float target_velocity =0.0; //target velocity in mm/s
harrynguyen 21:7cd86bea7f83 135 float distanceToWaypoint; //distance from robot to waypoint
harrynguyen 21:7cd86bea7f83 136 uint8_t waypoint_index=0;
harrynguyen 21:7cd86bea7f83 137 uint8_t go_cmd=0; //make robot run a waypoint set
harrynguyen 21:7cd86bea7f83 138
harrynguyen 21:7cd86bea7f83 139 /* THREAD */
harrynguyen 21:7cd86bea7f83 140 void purePursuit_thread(void const *n);
harrynguyen 21:7cd86bea7f83 141 void waypointCmd_thread(void const *n);
harrynguyen 21:7cd86bea7f83 142 /* ** */
harrynguyen 21:7cd86bea7f83 143 //-------------
harrynguyen 21:7cd86bea7f83 144
harrynguyen 21:7cd86bea7f83 145 //** Attitude control **
harrynguyen 21:7cd86bea7f83 146 attitudeControl attitudeControl;
harrynguyen 21:7cd86bea7f83 147
harrynguyen 21:7cd86bea7f83 148 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
harrynguyen 21:7cd86bea7f83 149 //** Declarations of misc functions **
harrynguyen 21:7cd86bea7f83 150 Serial Debug(uart_TX, uart_RX); //Debug serial port
harrynguyen 21:7cd86bea7f83 151 DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication
harrynguyen 21:7cd86bea7f83 152
harrynguyen 21:7cd86bea7f83 153 /* THREAD */
harrynguyen 21:7cd86bea7f83 154 void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 155 void print_thread(void const *n); //Debug printing thread
ahmed_lv 27:2db168d9fb18 156 void ranging_thread(void const *n); //ranging VL53L0X
harrynguyen 21:7cd86bea7f83 157 /* ** */
ahmed_lv 27:2db168d9fb18 158
harrynguyen 21:7cd86bea7f83 159 //-----------------------------------------------------------------------------
harrynguyen 21:7cd86bea7f83 160
harrynguyen 21:7cd86bea7f83 161
harrynguyen 21:7cd86bea7f83 162 int main()
harrynguyen 21:7cd86bea7f83 163 {
harrynguyen 21:7cd86bea7f83 164 Debug.baud(PC_BAUDRATE);
harrynguyen 21:7cd86bea7f83 165
harrynguyen 21:7cd86bea7f83 166 debugLED =1;
harrynguyen 21:7cd86bea7f83 167
harrynguyen 21:7cd86bea7f83 168 //wait_ms(5000);
harrynguyen 21:7cd86bea7f83 169
harrynguyen 21:7cd86bea7f83 170 Debug.printf("** Starting Virgo v3 Routines *************\n\n");
harrynguyen 21:7cd86bea7f83 171
harrynguyen 21:7cd86bea7f83 172 //** start Hearbeat loop as a thread **
harrynguyen 21:7cd86bea7f83 173 Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 174 Debug.printf("* Hearbeat loop started *\n");
harrynguyen 21:7cd86bea7f83 175
harrynguyen 21:7cd86bea7f83 176 //** start IMU funtion as Thread **
harrynguyen 21:7cd86bea7f83 177 Thread IMU_function(imu_thread, NULL, osPriorityHigh);
harrynguyen 21:7cd86bea7f83 178 Debug.printf("* IMU routine started *\n");
harrynguyen 21:7cd86bea7f83 179
harrynguyen 21:7cd86bea7f83 180 //** start OdometryUpdate function as Thread **
harrynguyen 21:7cd86bea7f83 181 Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 182 Debug.printf("* Odometry routine started *\n");
harrynguyen 21:7cd86bea7f83 183
harrynguyen 21:7cd86bea7f83 184 //** start MotorControl function as Thread **
harrynguyen 21:7cd86bea7f83 185 Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 186 Debug.printf("* Motor control routine started *\n");
harrynguyen 21:7cd86bea7f83 187
harrynguyen 21:7cd86bea7f83 188 //** start PurePursuit controller as Thread **
harrynguyen 21:7cd86bea7f83 189 Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 190 Debug.printf("* PurePursuit controller routine started *\n");
harrynguyen 21:7cd86bea7f83 191
harrynguyen 21:7cd86bea7f83 192 //** start Waypoint commander function as Thread **
harrynguyen 21:7cd86bea7f83 193 Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
harrynguyen 21:7cd86bea7f83 194 Debug.printf("* Waypoint commander routine started *\n");
harrynguyen 21:7cd86bea7f83 195
ahmed_lv 27:2db168d9fb18 196
ahmed_lv 27:2db168d9fb18 197
ahmed_lv 27:2db168d9fb18 198
ahmed_lv 28:39d694b0e998 199 /* xshut1 = 0;
ahmed_lv 27:2db168d9fb18 200 xshut2 = 0;
ahmed_lv 27:2db168d9fb18 201 xshut3 = 0;
ahmed_lv 27:2db168d9fb18 202 xshut4 = 0;
ahmed_lv 27:2db168d9fb18 203 xshut5 = 0;
ahmed_lv 27:2db168d9fb18 204
ahmed_lv 27:2db168d9fb18 205 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 206
ahmed_lv 27:2db168d9fb18 207 /////////////////////////
ahmed_lv 27:2db168d9fb18 208 xshut1 = 1;
ahmed_lv 27:2db168d9fb18 209 Debug.printf("Sensor 1: \nXSHUT ON\n");
ahmed_lv 27:2db168d9fb18 210 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 211
ahmed_lv 27:2db168d9fb18 212 sensor1.init();
ahmed_lv 27:2db168d9fb18 213 Debug.printf("S1 Initialized...\n");
ahmed_lv 27:2db168d9fb18 214 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 215
ahmed_lv 27:2db168d9fb18 216 sensor1.setAddress((uint8_t)23);
ahmed_lv 27:2db168d9fb18 217 Debug.printf("S1 Address set...\n");
ahmed_lv 27:2db168d9fb18 218 /////////////////////////
ahmed_lv 27:2db168d9fb18 219 xshut2 = 1;
ahmed_lv 27:2db168d9fb18 220 Debug.printf("Sensor 2: \nXSHUT ON\n");
ahmed_lv 27:2db168d9fb18 221 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 222
ahmed_lv 27:2db168d9fb18 223 sensor2.init();
ahmed_lv 27:2db168d9fb18 224 Debug.printf("S2 Initialized...\n");
ahmed_lv 27:2db168d9fb18 225 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 226
ahmed_lv 27:2db168d9fb18 227 sensor2.setAddress((uint8_t)25);
ahmed_lv 27:2db168d9fb18 228 Debug.printf("S2 Address set...\n");
ahmed_lv 27:2db168d9fb18 229 /////////////////////////
ahmed_lv 27:2db168d9fb18 230 xshut3 = 1;
ahmed_lv 27:2db168d9fb18 231 Debug.printf("Sensor 3: \nXSHUT ON\n");
ahmed_lv 27:2db168d9fb18 232 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 233
ahmed_lv 27:2db168d9fb18 234 sensor3.init();
ahmed_lv 27:2db168d9fb18 235 Debug.printf("S3 Initialized...\n");
ahmed_lv 27:2db168d9fb18 236 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 237
ahmed_lv 27:2db168d9fb18 238 sensor3.setAddress((uint8_t)27);
ahmed_lv 27:2db168d9fb18 239 Debug.printf("S3 Address set...\n");
ahmed_lv 27:2db168d9fb18 240 /////////////////////////
ahmed_lv 27:2db168d9fb18 241 xshut4 = 1;
ahmed_lv 27:2db168d9fb18 242 Debug.printf("Sensor 4: \nXSHUT ON\n");
ahmed_lv 27:2db168d9fb18 243 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 244
ahmed_lv 27:2db168d9fb18 245 sensor4.init();
ahmed_lv 27:2db168d9fb18 246 Debug.printf("S4 Initialized...\n");
ahmed_lv 27:2db168d9fb18 247 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 248
ahmed_lv 27:2db168d9fb18 249 sensor4.setAddress((uint8_t)29);
ahmed_lv 27:2db168d9fb18 250 Debug.printf("S4 Address set...\n");
ahmed_lv 27:2db168d9fb18 251 /////////////////////////
ahmed_lv 27:2db168d9fb18 252 xshut5 = 1;
ahmed_lv 27:2db168d9fb18 253 Debug.printf("Sensor 5: \nXSHUT ON\n");
ahmed_lv 27:2db168d9fb18 254 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 255
ahmed_lv 27:2db168d9fb18 256 sensor5.init();
ahmed_lv 27:2db168d9fb18 257 Debug.printf("S5 Initialized...\n");
ahmed_lv 27:2db168d9fb18 258 Thread::wait(W);
ahmed_lv 27:2db168d9fb18 259
ahmed_lv 27:2db168d9fb18 260 sensor5.setAddress((uint8_t)22);
ahmed_lv 27:2db168d9fb18 261 Debug.printf("S5 Address set...\n");
ahmed_lv 27:2db168d9fb18 262
ahmed_lv 27:2db168d9fb18 263 /////////////////////////
ahmed_lv 27:2db168d9fb18 264 sensor1.startContinuous();
ahmed_lv 27:2db168d9fb18 265 sensor2.startContinuous();
ahmed_lv 27:2db168d9fb18 266 sensor3.startContinuous();
ahmed_lv 27:2db168d9fb18 267 sensor4.startContinuous();
ahmed_lv 27:2db168d9fb18 268 sensor5.startContinuous();
ahmed_lv 27:2db168d9fb18 269 //** start Ranging funtion as Thread **
ahmed_lv 27:2db168d9fb18 270 Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
ahmed_lv 28:39d694b0e998 271 Debug.printf("* Ranging routine started *\n"); */
ahmed_lv 27:2db168d9fb18 272
ahmed_lv 27:2db168d9fb18 273
ahmed_lv 27:2db168d9fb18 274
harrynguyen 21:7cd86bea7f83 275 //** start Debug print loop as a thread **
harrynguyen 21:7cd86bea7f83 276 Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
harrynguyen 21:7cd86bea7f83 277 Debug.printf("* Print loop started *\n\n\n");
harrynguyen 21:7cd86bea7f83 278
harrynguyen 21:7cd86bea7f83 279 Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
harrynguyen 21:7cd86bea7f83 280
harrynguyen 21:7cd86bea7f83 281 while(1) {
harrynguyen 21:7cd86bea7f83 282
harrynguyen 21:7cd86bea7f83 283 }
harrynguyen 21:7cd86bea7f83 284 }
harrynguyen 21:7cd86bea7f83 285
harrynguyen 21:7cd86bea7f83 286 /**
harrynguyen 21:7cd86bea7f83 287 * heartbeat loop as an individual thread
harrynguyen 21:7cd86bea7f83 288 */
harrynguyen 21:7cd86bea7f83 289 void heartbeat_thread(void const *n)
harrynguyen 21:7cd86bea7f83 290 {
harrynguyen 21:7cd86bea7f83 291 while(true) {
harrynguyen 21:7cd86bea7f83 292 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 293 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 294 Thread::wait(Hearbeat_RateMS-50);
harrynguyen 21:7cd86bea7f83 295 debugLED = !debugLED;
harrynguyen 21:7cd86bea7f83 296 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 297 } else
harrynguyen 21:7cd86bea7f83 298 debugLED = 1;
harrynguyen 21:7cd86bea7f83 299 }
harrynguyen 21:7cd86bea7f83 300 }
harrynguyen 21:7cd86bea7f83 301
harrynguyen 21:7cd86bea7f83 302 /**
harrynguyen 21:7cd86bea7f83 303 * imu initialization function
harrynguyen 21:7cd86bea7f83 304 */
harrynguyen 21:7cd86bea7f83 305 bool imuInit_function()
harrynguyen 21:7cd86bea7f83 306 {
harrynguyen 21:7cd86bea7f83 307 return (imu.imuInit());
harrynguyen 21:7cd86bea7f83 308 }
harrynguyen 21:7cd86bea7f83 309
harrynguyen 21:7cd86bea7f83 310 /**
harrynguyen 21:7cd86bea7f83 311 * imu update loop as an individual thread
harrynguyen 21:7cd86bea7f83 312 */
harrynguyen 21:7cd86bea7f83 313 void imu_thread(void const *n)
harrynguyen 21:7cd86bea7f83 314 {
harrynguyen 21:7cd86bea7f83 315 bool init_status = imuInit_function();
harrynguyen 21:7cd86bea7f83 316 Thread::wait(100);
harrynguyen 21:7cd86bea7f83 317
harrynguyen 21:7cd86bea7f83 318 while(init_status) {
harrynguyen 21:7cd86bea7f83 319 imu.imuUpdate();
harrynguyen 21:7cd86bea7f83 320
harrynguyen 21:7cd86bea7f83 321 //Usage:
harrynguyen 21:7cd86bea7f83 322 //imu.Pose[0, 1, 2]; //euler x, y, z
harrynguyen 21:7cd86bea7f83 323 //imu.AngVel[0, 1, 2]; //AngVel x, y, z
harrynguyen 21:7cd86bea7f83 324 //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z
harrynguyen 21:7cd86bea7f83 325 //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z
harrynguyen 21:7cd86bea7f83 326
harrynguyen 21:7cd86bea7f83 327 imuTime = imu.time_s;
harrynguyen 21:7cd86bea7f83 328
harrynguyen 21:7cd86bea7f83 329 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 330 }
harrynguyen 21:7cd86bea7f83 331 }
harrynguyen 21:7cd86bea7f83 332
ahmed_lv 27:2db168d9fb18 333
ahmed_lv 27:2db168d9fb18 334 /**
ahmed_lv 27:2db168d9fb18 335 * Ranging sensor loop as an individual thread
ahmed_lv 27:2db168d9fb18 336 */
ahmed_lv 27:2db168d9fb18 337
ahmed_lv 27:2db168d9fb18 338 void ranging_thread(void const *n)
ahmed_lv 27:2db168d9fb18 339 {
ahmed_lv 27:2db168d9fb18 340
ahmed_lv 27:2db168d9fb18 341
ahmed_lv 27:2db168d9fb18 342
ahmed_lv 27:2db168d9fb18 343
ahmed_lv 27:2db168d9fb18 344 while(1)
ahmed_lv 27:2db168d9fb18 345 {
ahmed_lv 27:2db168d9fb18 346 if(imu.imu_stabilized[1] ==1){
ahmed_lv 27:2db168d9fb18 347 RangeSensor.exleft = sensor1.readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 348 RangeSensor.left = sensor2.readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 349 RangeSensor.front = sensor3.readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 350 RangeSensor.right = sensor4.readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 351 RangeSensor.exright = sensor5.readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 352 //debugprint.printf("in ranging %u \n\e[K", RangeSensor.left);
ahmed_lv 27:2db168d9fb18 353 }
ahmed_lv 27:2db168d9fb18 354 Thread::wait(1.0);
ahmed_lv 27:2db168d9fb18 355
ahmed_lv 27:2db168d9fb18 356 }
ahmed_lv 27:2db168d9fb18 357 }
ahmed_lv 27:2db168d9fb18 358
ahmed_lv 27:2db168d9fb18 359
harrynguyen 21:7cd86bea7f83 360 /**
harrynguyen 21:7cd86bea7f83 361 * odometry update loop as an individual thread
harrynguyen 21:7cd86bea7f83 362 */
harrynguyen 21:7cd86bea7f83 363 void odometry_thread(void const *n)
harrynguyen 21:7cd86bea7f83 364 {
harrynguyen 21:7cd86bea7f83 365 odometry.init();
harrynguyen 21:7cd86bea7f83 366 Thread::wait(50);
harrynguyen 21:7cd86bea7f83 367
harrynguyen 21:7cd86bea7f83 368 while(true) {
harrynguyen 21:7cd86bea7f83 369 odometry.update();
harrynguyen 21:7cd86bea7f83 370
harrynguyen 21:7cd86bea7f83 371 //Usage:
harrynguyen 21:7cd86bea7f83 372 //odometer.revolutions[0, 1]; //revolutions left, right
harrynguyen 21:7cd86bea7f83 373 //odometer.rpm[0, 1]; //rpm left, right
harrynguyen 21:7cd86bea7f83 374
harrynguyen 21:7cd86bea7f83 375 localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions);
harrynguyen 21:7cd86bea7f83 376
harrynguyen 21:7cd86bea7f83 377 //Usage:
harrynguyen 21:7cd86bea7f83 378 //localization.position[0, 1] //x, y
harrynguyen 21:7cd86bea7f83 379
harrynguyen 21:7cd86bea7f83 380 Thread::wait(odometry_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 381 }
harrynguyen 21:7cd86bea7f83 382 }
harrynguyen 21:7cd86bea7f83 383
harrynguyen 21:7cd86bea7f83 384 /**/
harrynguyen 21:7cd86bea7f83 385 float rpm_smc = 500;
harrynguyen 21:7cd86bea7f83 386 float ref_dtheta = 0;
harrynguyen 21:7cd86bea7f83 387 float ref_theta = 0;
harrynguyen 21:7cd86bea7f83 388
harrynguyen 21:7cd86bea7f83 389 float ref_dgamma = 0;
harrynguyen 21:7cd86bea7f83 390 float ref_gamma = 0;
harrynguyen 21:7cd86bea7f83 391
harrynguyen 21:7cd86bea7f83 392 float ref_beta = DEG_TO_RAD(0.0);
harrynguyen 21:7cd86bea7f83 393 float ref_dbeta = 0;
harrynguyen 21:7cd86bea7f83 394
harrynguyen 21:7cd86bea7f83 395 float u1, u2;
harrynguyen 21:7cd86bea7f83 396 /**/
harrynguyen 21:7cd86bea7f83 397 /**
harrynguyen 21:7cd86bea7f83 398 * motor control loop as an individual thread
harrynguyen 21:7cd86bea7f83 399 */
harrynguyen 21:7cd86bea7f83 400 void motorControl_thread(void const *n)
harrynguyen 21:7cd86bea7f83 401 {
harrynguyen 21:7cd86bea7f83 402 motorControl_t.start();
harrynguyen 21:7cd86bea7f83 403
harrynguyen 21:7cd86bea7f83 404 float pitch_th, pitch_om, yaw_th, yaw_om;
harrynguyen 21:7cd86bea7f83 405 float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r;
harrynguyen 21:7cd86bea7f83 406 float W_l, W_r;
harrynguyen 21:7cd86bea7f83 407
harrynguyen 21:7cd86bea7f83 408 while(true) {
harrynguyen 21:7cd86bea7f83 409
harrynguyen 21:7cd86bea7f83 410 //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
harrynguyen 21:7cd86bea7f83 411 if(imu.imu_stabilized[1] ==1) {
harrynguyen 21:7cd86bea7f83 412
ahmed_lv 30:44676e1b38f8 413 /*#LV*/
ahmed_lv 29:a6a812ee83ea 414 PID_B.setSpeedChange( &W_l, &W_r, &speedChange, &Error,
ahmed_lv 29:a6a812ee83ea 415 target_waypoint,
ahmed_lv 29:a6a812ee83ea 416 localization.position,
ahmed_lv 29:a6a812ee83ea 417 imu.Pose[2],
ahmed_lv 29:a6a812ee83ea 418 &robotFrame,
ahmed_lv 29:a6a812ee83ea 419 waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1],
ahmed_lv 29:a6a812ee83ea 420 &k3,
ahmed_lv 29:a6a812ee83ea 421 3.5 ); //new controller
ahmed_lv 30:44676e1b38f8 422 /*#LV*/
harrynguyen 21:7cd86bea7f83 423
harrynguyen 21:7cd86bea7f83 424
harrynguyen 21:7cd86bea7f83 425 if(waypointSetFinish_flag == 0) {
harrynguyen 25:3a82f868c101 426 rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);
harrynguyen 25:3a82f868c101 427 rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
harrynguyen 25:3a82f868c101 428 // rpm_cmd[0]=-800;
harrynguyen 25:3a82f868c101 429 // rpm_cmd[1]=-800;
harrynguyen 21:7cd86bea7f83 430
harrynguyen 21:7cd86bea7f83 431 if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) )
harrynguyen 21:7cd86bea7f83 432 rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]);
harrynguyen 21:7cd86bea7f83 433 else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0)
harrynguyen 21:7cd86bea7f83 434 rpm_cmd[0] = 0;
harrynguyen 21:7cd86bea7f83 435
harrynguyen 21:7cd86bea7f83 436 if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) )
harrynguyen 21:7cd86bea7f83 437 rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]);
harrynguyen 21:7cd86bea7f83 438 else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0)
harrynguyen 21:7cd86bea7f83 439 rpm_cmd[1] = 0;
harrynguyen 21:7cd86bea7f83 440
harrynguyen 21:7cd86bea7f83 441 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 442 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 443
harrynguyen 21:7cd86bea7f83 444 //rpm_compensated[0]= rpm_cmd[0];
harrynguyen 21:7cd86bea7f83 445 //rpm_compensated[1]= rpm_cmd[1];
harrynguyen 21:7cd86bea7f83 446
harrynguyen 21:7cd86bea7f83 447 } else {
harrynguyen 21:7cd86bea7f83 448 rpm_cmd[0]=0;
harrynguyen 21:7cd86bea7f83 449 rpm_cmd[1]=0;
harrynguyen 21:7cd86bea7f83 450
harrynguyen 21:7cd86bea7f83 451 rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 452 rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read());
harrynguyen 21:7cd86bea7f83 453 }
harrynguyen 21:7cd86bea7f83 454
harrynguyen 21:7cd86bea7f83 455 pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read());
harrynguyen 21:7cd86bea7f83 456 pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read());
harrynguyen 21:7cd86bea7f83 457
harrynguyen 21:7cd86bea7f83 458 drive.setPWM_L(pwm_cmd[0]);
harrynguyen 21:7cd86bea7f83 459 drive.setPWM_R(pwm_cmd[1]);
ahmed_lv 26:32eaf3c3ac2e 460
harrynguyen 21:7cd86bea7f83 461
harrynguyen 21:7cd86bea7f83 462 }
harrynguyen 21:7cd86bea7f83 463
harrynguyen 21:7cd86bea7f83 464 motorControl_t.reset();
harrynguyen 21:7cd86bea7f83 465
harrynguyen 21:7cd86bea7f83 466 Thread::wait(motorControl_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 467 }
harrynguyen 21:7cd86bea7f83 468 }
harrynguyen 21:7cd86bea7f83 469
harrynguyen 21:7cd86bea7f83 470 /**
harrynguyen 21:7cd86bea7f83 471 * purepursuit loop as an individual thread
harrynguyen 21:7cd86bea7f83 472 */
harrynguyen 21:7cd86bea7f83 473 void purePursuit_thread(void const *n)
harrynguyen 21:7cd86bea7f83 474 {
harrynguyen 21:7cd86bea7f83 475 while(true) {
harrynguyen 21:7cd86bea7f83 476 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 477 //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
ahmed_lv 29:a6a812ee83ea 478 // purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
ahmed_lv 29:a6a812ee83ea 479 //
ahmed_lv 29:a6a812ee83ea 480 // if(purePursuit.robotFrame_targetDistance <= waypointZone)
ahmed_lv 29:a6a812ee83ea 481 // waypointReached_flag = 1;
ahmed_lv 29:a6a812ee83ea 482 // else
ahmed_lv 29:a6a812ee83ea 483 // waypointReached_flag = 0;
harrynguyen 21:7cd86bea7f83 484
ahmed_lv 30:44676e1b38f8 485 /*#LV*/
ahmed_lv 29:a6a812ee83ea 486 PID_B.findRobotFrameDistance(target_waypoint, localization.position, &robotFrame);
ahmed_lv 29:a6a812ee83ea 487
ahmed_lv 29:a6a812ee83ea 488 if(robotFrame <= waypointZone)
harrynguyen 21:7cd86bea7f83 489 waypointReached_flag = 1;
harrynguyen 21:7cd86bea7f83 490 else
ahmed_lv 29:a6a812ee83ea 491 waypointReached_flag = 0;
ahmed_lv 30:44676e1b38f8 492 /*#LV*/
ahmed_lv 29:a6a812ee83ea 493
harrynguyen 21:7cd86bea7f83 494 }
harrynguyen 21:7cd86bea7f83 495 Thread::wait(imu_UpdatePeriodMS);
harrynguyen 21:7cd86bea7f83 496 }
harrynguyen 21:7cd86bea7f83 497 }
harrynguyen 21:7cd86bea7f83 498
harrynguyen 21:7cd86bea7f83 499 /**
harrynguyen 21:7cd86bea7f83 500 * waypoint tracking loop as individual thread
harrynguyen 21:7cd86bea7f83 501 */
harrynguyen 21:7cd86bea7f83 502 void waypointCmd_thread(void const *n)
harrynguyen 21:7cd86bea7f83 503 {
harrynguyen 21:7cd86bea7f83 504 while(true) {
harrynguyen 21:7cd86bea7f83 505 //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
harrynguyen 21:7cd86bea7f83 506 if(imu.imu_stabilized[0] ==1) {
harrynguyen 21:7cd86bea7f83 507 if(waypoint_index > totalWaypoints) {
harrynguyen 21:7cd86bea7f83 508 target_velocity = 0.0; //stop the robot
harrynguyen 21:7cd86bea7f83 509 waypointSetFinish_flag = 1;
harrynguyen 21:7cd86bea7f83 510 }
harrynguyen 21:7cd86bea7f83 511
harrynguyen 21:7cd86bea7f83 512 if(waypointReached_flag == 1 && waypointSetFinish_flag == 0) {
harrynguyen 21:7cd86bea7f83 513 target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 21:7cd86bea7f83 514 target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters
harrynguyen 21:7cd86bea7f83 515 target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s
harrynguyen 21:7cd86bea7f83 516 waypoint_index++;
harrynguyen 21:7cd86bea7f83 517 }
harrynguyen 21:7cd86bea7f83 518 }
harrynguyen 21:7cd86bea7f83 519 Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient
harrynguyen 21:7cd86bea7f83 520 }
harrynguyen 21:7cd86bea7f83 521 }
harrynguyen 21:7cd86bea7f83 522
harrynguyen 21:7cd86bea7f83 523
harrynguyen 21:7cd86bea7f83 524
harrynguyen 21:7cd86bea7f83 525
harrynguyen 21:7cd86bea7f83 526
harrynguyen 21:7cd86bea7f83 527
harrynguyen 21:7cd86bea7f83 528
harrynguyen 21:7cd86bea7f83 529 /**
harrynguyen 21:7cd86bea7f83 530 * Debug data print loop as an individual thread
harrynguyen 21:7cd86bea7f83 531 */
harrynguyen 21:7cd86bea7f83 532 #define print_lines 15 //number of info lines being printed on screen
harrynguyen 21:7cd86bea7f83 533 void print_thread(void const *n)
harrynguyen 21:7cd86bea7f83 534 {
harrynguyen 21:7cd86bea7f83 535 //clear 14 lines while going up, these are the initilization lines printed on screen
harrynguyen 21:7cd86bea7f83 536 for(int l=14; l>0; l--) {
harrynguyen 21:7cd86bea7f83 537 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 538 Debug.printf("\e[K"); //clear line
harrynguyen 21:7cd86bea7f83 539 }
harrynguyen 21:7cd86bea7f83 540
harrynguyen 21:7cd86bea7f83 541 Debug.printf("************ VIRGO v3: Status Monitor *************\n\n");
harrynguyen 21:7cd86bea7f83 542 for(int l=print_lines; l>0; l--) Debug.printf("\n");
harrynguyen 21:7cd86bea7f83 543 Debug.printf("\n===================================================");
harrynguyen 21:7cd86bea7f83 544 Debug.printf("\e[1A"); //go up 1 line
harrynguyen 21:7cd86bea7f83 545
harrynguyen 21:7cd86bea7f83 546 while(true) {
harrynguyen 21:7cd86bea7f83 547 //move cursor up # of lines printed to create a static display and clear the first line
harrynguyen 21:7cd86bea7f83 548 for(int l=print_lines; l>0; l--) Debug.printf("\e[1A");
harrynguyen 21:7cd86bea7f83 549 Debug.printf("\e[K");
harrynguyen 21:7cd86bea7f83 550
harrynguyen 21:7cd86bea7f83 551 Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
harrynguyen 21:7cd86bea7f83 552 Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
harrynguyen 21:7cd86bea7f83 553 Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
harrynguyen 21:7cd86bea7f83 554 Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
harrynguyen 21:7cd86bea7f83 555 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 21:7cd86bea7f83 556
ahmed_lv 28:39d694b0e998 557 // Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
harrynguyen 21:7cd86bea7f83 558 Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
ahmed_lv 26:32eaf3c3ac2e 559 Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
ahmed_lv 29:a6a812ee83ea 560 Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", Error, speedChange, k3);
harrynguyen 21:7cd86bea7f83 561 Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
ahmed_lv 28:39d694b0e998 562 // Debug.printf("Ranging: %u, %u, %u, %u, %u \n\e[K", RangeSensor.exleft, RangeSensor.left, RangeSensor.front, RangeSensor.right, RangeSensor.exright);
ahmed_lv 26:32eaf3c3ac2e 563
harrynguyen 21:7cd86bea7f83 564
harrynguyen 21:7cd86bea7f83 565 Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
harrynguyen 21:7cd86bea7f83 566 //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
harrynguyen 21:7cd86bea7f83 567 Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]);
ahmed_lv 28:39d694b0e998 568 Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]);
harrynguyen 21:7cd86bea7f83 569
harrynguyen 21:7cd86bea7f83 570 //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 21:7cd86bea7f83 571 //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 21:7cd86bea7f83 572
harrynguyen 23:6806c3bacf58 573
harrynguyen 21:7cd86bea7f83 574 //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]);
harrynguyen 21:7cd86bea7f83 575
harrynguyen 21:7cd86bea7f83 576 Thread::wait(PrintLoop_PeriodMS);
harrynguyen 21:7cd86bea7f83 577 }
harrynguyen 21:7cd86bea7f83 578 }