Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
Diff: orion_main.cpp
- Revision:
- 21:7cd86bea7f83
- Child:
- 23:6806c3bacf58
diff -r 2720a283d1c5 -r 7cd86bea7f83 orion_main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/orion_main.cpp Thu Feb 08 02:13:47 2018 +0000 @@ -0,0 +1,640 @@ +/** + * @author Akash Vibhute + * < akash . roboticist [at] gmail . com > + * + * @section LICENSE + * + * Copyright (c) 2015 Akash Vibhute + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * @section DESCRIPTION + * + * Virgo_v3 robot controller v1.0 with Virgo_v3 PCB [AV22032015] + * Robot controller software for SUTD Virgo version 3 robot + * + */ + +/** + * Header file for including all necessary functions for robot and defining any + * custom functions written in the main file. + */ +#include "main.h" +#include "globalExterns.h" + +/** + * Functions, Threads & General Definitions + */ +//***************************************************************************** +//** Drivetrain ** +motorDriver drive; //motor drive train +odometer odometry; //odometer function +pidControl PID_L, PID_R; //pidcontroller for left and right motors + +Timer motorControl_t; +float rpm_cmd[2]; //drive motor rpm command +float rpm_compensated[2]; //rpm command compensated by acc limit +float targetAcceleration = 300.0; //RPM/s acceleration +float pwm_cmd[2]; //drive motor pwm command + +/* THREAD */ +void odometry_thread(void const *n); +void motorControl_thread(void const *n); +/* ** */ +//------------- + +//** Localization ** +IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class +float imuTime; +Localization localization; //localization function + +/* FUNCTION */ +bool imuInit_function(); +/* ** */ + +/* THREAD */ +void imu_thread(void const *n); +/* ** */ +//------------- + +//** Communications ** +nRF24NetworkHandler comm; //nRF24 radio and network layer handler function +uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted +uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, + +cmdParser wirelessCmd; + +/* THREAD */ +void comm_thread(void const *n); +/* ** */ +//------------- + +//** Power Monitor ** +BattGuage battery; //Battery fuel gauge wrapper + +/* THREAD */ + +/* ** */ +//------------- + +//** Trajectory tracking ** +purePursuit purePursuit; +kinematics kinematics; + +float purePursuit_velocity, purePursuit_omega, purePursuit_gamma; +//waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace +uint8_t totalWaypoints = 5; +//kite pattern 200cm long, 100cm wide +int16_t waypoints_set[][4] = { {0,0,90,0}, + {100,100,90,0}, + {0,200,90,0}, + {-100,100,90,0}, + {0,0,90,0}, + {0,0,90,0}, + {0,0,90,0} +}; + +float waypointZone = 300.0; //diameter around desired waypoint, if robot reaches within that zone then waypoint is reached. +uint8_t waypointReached_flag = 0; //indicates if the desired waypoint has been reached +uint8_t waypointSetFinish_flag = 0; //indicates if the desired waypoint set is over and the robot needs to stop. +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 +float target_velocity =0.0; //target velocity in mm/s +float distanceToWaypoint; //distance from robot to waypoint +uint8_t waypoint_index=0; +uint8_t go_cmd=0; //make robot run a waypoint set + +/* THREAD */ +void purePursuit_thread(void const *n); +void waypointCmd_thread(void const *n); +/* ** */ +//------------- + +//** Attitude control ** +attitudeControl attitudeControl; +pidAttitudeControl pidPitchControl; +pidAttitudeControl pidRollControl; + +float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control + +/* THREAD */ + +/* ** */ +//------------- + +//** Camera ** +camera camera; //camera driver +uint8_t cameraFlag; //flag to enable camera: 0 - off, 1 - frequency controlled, 2 - permanantly on + +/* THREAD */ +void cameraControl_thread(void const *n); +/* ** */ +//------------- + +//** Robot data recorder ** +RDR virgoRDR; +uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled + +/* THREAD */ + +/* ** */ +//------------- + +//** Declarations of misc functions ** +Serial Debug(uart_TX, uart_RX); //Debug serial port +DigitalOut debugLED(debug_LED); //led for Debugging and heartbeat indication + +/* THREAD */ +void heartbeat_thread(void const *n); //heartbeat loop as an individual thread +void print_thread(void const *n); //Debug printing thread +/* ** */ +//------------- +//----------------------------------------------------------------------------- + + +int main() +{ + Debug.baud(PC_BAUDRATE); + + debugLED =1; + + //wait_ms(5000); + + Debug.printf("** Starting Virgo v3 Routines *************\n\n"); + + //** start Hearbeat loop as a thread ** + Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal); + Debug.printf("* Hearbeat loop started *\n"); + + //** start IMU funtion as Thread ** + Thread IMU_function(imu_thread, NULL, osPriorityHigh); + Debug.printf("* IMU routine started *\n"); + + //** start OdometryUpdate function as Thread ** + Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024); + Debug.printf("* Odometry routine started *\n"); + + //** start MotorControl function as Thread ** + Thread MotorControl_function(motorControl_thread, NULL, osPriorityNormal); + Debug.printf("* Motor control routine started *\n"); + + //** start PurePursuit controller as Thread ** + Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal); + Debug.printf("* PurePursuit controller routine started *\n"); + + //** start Waypoint commander function as Thread ** + Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal); + Debug.printf("* Waypoint commander routine started *\n"); + + //** start comm loop as a thread ** + Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024); + Debug.printf("* Communications loop started *\n"); + + //** start camera control loop as a thread ** + Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal); + Debug.printf("* Camera control loop started *\n"); + + //** Start data recorder as Thread ** + //Thread dataRecorder_function(dataRecorder_thread, NULL, osPriorityNormal); + //Debug.printf("* Data Recorder routine started *\n"); + + //** start Debug print loop as a thread ** + Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024); + Debug.printf("* Print loop started *\n\n\n"); + + Debug.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n"); + + while(1) { + + } +} + +/** + * heartbeat loop as an individual thread + */ +void heartbeat_thread(void const *n) +{ + while(true) { + if(imu.imu_stabilized[0] ==1) { + debugLED = !debugLED; + Thread::wait(Hearbeat_RateMS-50); + debugLED = !debugLED; + Thread::wait(50); + } else + debugLED = 1; + } +} + +/** + * imu initialization function + */ +bool imuInit_function() +{ + return (imu.imuInit()); +} + +/** + * imu update loop as an individual thread + */ +void imu_thread(void const *n) +{ + bool init_status = imuInit_function(); + Thread::wait(100); + + while(init_status) { + imu.imuUpdate(); + + //Usage: + //imu.Pose[0, 1, 2]; //euler x, y, z + //imu.AngVel[0, 1, 2]; //AngVel x, y, z + //imu.LinAcc[0, 1, 2]; //LinAcc x, y, z + //imu.Quat[0, 1, 2, 3]; //Quaternion w, x, y, z + + imuTime = imu.time_s; + + Thread::wait(imu_UpdatePeriodMS); + } +} + +/** + * odometry update loop as an individual thread + */ +void odometry_thread(void const *n) +{ + odometry.init(); + Thread::wait(50); + + while(true) { + odometry.update(); + + //Usage: + //odometer.revolutions[0, 1]; //revolutions left, right + //odometer.rpm[0, 1]; //rpm left, right + + localization.updatePosition(DEG_TO_RAD(imu.Pose[2]), odometry.revolutions); + + //Usage: + //localization.position[0, 1] //x, y + + Thread::wait(odometry_UpdatePeriodMS); + } +} + +/**/ +float rpm_smc = 500; +float ref_dtheta = 0; +float ref_theta = 0; + +float ref_dgamma = 0; +float ref_gamma = 0; + +float ref_beta = DEG_TO_RAD(0.0); +float ref_dbeta = 0; + +float u1, u2; +/**/ +/** + * motor control loop as an individual thread + */ +void motorControl_thread(void const *n) +{ + motorControl_t.start(); + + float pitch_th, pitch_om, yaw_th, yaw_om; + float wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r; + float W_l, W_r; + + while(true) { + + //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) { + if(imu.imu_stabilized[1] ==1) { + + pitch_th = DEG_TO_RAD(imu.Pose[0]); + if(pitch_th < -1*M_PI) pitch_th += 2*M_PI; + if(pitch_th > M_PI) pitch_th -= 2*M_PI; + if(pitch_th < -1*M_PI) pitch_th += 2*M_PI; + + + pitch_om = DEG_TO_RAD(imu.AngVel[0]); + + yaw_th = DEG_TO_RAD(imu.Pose[2]); + + yaw_om = DEG_TO_RAD(imu.AngVel[2]); + + wheelTh_l = odometry.revolutions[0]*(-2*M_PI); + wheelTh_r = odometry.revolutions[1]*(-2*M_PI); + + wheelOm_l = (odometry.rpm[0]/60)*(-2*M_PI); + wheelOm_r = (odometry.rpm[1]/60)*(-2*M_PI); + + //ref_dtheta = rpm_smc*(-2*M_PI/60.0); //*generalFunctions::abs_f(sin(2*M_PI*imuTime/(15*2))); + //ref_theta = ref_dtheta * imuTime; + //ref_gamma = DEG_TO_RAD(30.0) ;//* sin(2*M_PI*imuTime/(30*2)); + + ref_dtheta = (purePursuit_velocity * (60.0 / (M_PI * wheel_dia)))*(-2*M_PI/60.0); + ref_gamma = purePursuit_gamma; + ref_dgamma = purePursuit_omega; + + attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(), + pitch_th, pitch_om, yaw_th , yaw_om, + wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r, + 0, ref_dbeta, ref_beta, + 0, ref_dtheta, ref_theta, + 0, ref_dgamma, ref_gamma, + &u1, &u2); + + + + if(waypointSetFinish_flag == 0) { +// rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0); +// rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0); + rpm_cmd[0]=-800; + rpm_cmd[1]=-800; + + if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) ) + rpm_cmd[0] = 475.0*generalFunctions::sign_f(rpm_cmd[0]); + else if(generalFunctions::abs_f(rpm_cmd[0]) <= 100.0) + rpm_cmd[0] = 0; + + if( (generalFunctions::abs_f(rpm_cmd[1]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 100.0) ) + rpm_cmd[1] = 475.0*generalFunctions::sign_f(rpm_cmd[1]); + else if(generalFunctions::abs_f(rpm_cmd[1]) <= 100.0) + rpm_cmd[1] = 0; + + rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], targetAcceleration, motorControl_t.read()); + rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], targetAcceleration, motorControl_t.read()); + + //rpm_compensated[0]= rpm_cmd[0]; + //rpm_compensated[1]= rpm_cmd[1]; + + } else { + rpm_cmd[0]=0; + rpm_cmd[1]=0; + + rpm_compensated[0]=PID_L.processAcc(rpm_cmd[0], 225.0, motorControl_t.read()); + rpm_compensated[1]=PID_R.processAcc(rpm_cmd[1], 225.0, motorControl_t.read()); + } + + pwm_cmd[0]=PID_L.calcOutput(rpm_compensated[0], odometry.rpm[0], motorControl_t.read()); + pwm_cmd[1]=PID_R.calcOutput(rpm_compensated[1], odometry.rpm[1], motorControl_t.read()); + + drive.setPWM_L(pwm_cmd[0]); + drive.setPWM_R(pwm_cmd[1]); + + + } + + motorControl_t.reset(); + + Thread::wait(motorControl_UpdatePeriodMS); + } +} + +/** + * purepursuit loop as an individual thread + */ +void purePursuit_thread(void const *n) +{ + while(true) { + if(imu.imu_stabilized[0] ==1) { + //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2])); + purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2])); + + if(purePursuit.robotFrame_targetDistance <= waypointZone) + waypointReached_flag = 1; + else + waypointReached_flag = 0; + } + Thread::wait(imu_UpdatePeriodMS); + } +} + +/** + * waypoint tracking loop as individual thread + */ +void waypointCmd_thread(void const *n) +{ + while(true) { + //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) { + if(imu.imu_stabilized[0] ==1) { + if(waypoint_index > totalWaypoints) { + target_velocity = 0.0; //stop the robot + waypointSetFinish_flag = 1; + } + + if(waypointReached_flag == 1 && waypointSetFinish_flag == 0) { + target_waypoint[0] = waypoints_set[waypoint_index][0] * 10.0; //convert coordinate from centimeters to millimeters + target_waypoint[1] = waypoints_set[waypoint_index][1] * 10.0; //convert coordinate from centimeters to millimeters + target_velocity = waypoints_set[waypoint_index][2] * (driveTrain_maxV/100.0); //convert speed from percentage to mm/s + waypoint_index++; + } + } + Thread::wait(100); //waypoint update doesnt need to be very fast, 10Hz is more than sufficient + } +} + +/** + * nRF network communications as an individual thread + */ +void comm_thread(void const *n) +{ + comm.init(); //initialize communications unit + Thread::wait(1000); //wait for a bit for radio to complete setup + dataSend_flag=0; + + float data[2]; + wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0); + //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0); + + while(true) { + dataSend_flag =1; + + if((dataSend_flag == 1) && (comm.tx_ready == 1)) { + + comm.DataOut.addr = 0; //send to node address + + comm.DataOut.parameter[0] = 1; //parameter def 0 + comm.DataOut.parameter[1] = 2; //parameter def 1 + + comm.DataOut.dataLen = 20; //length of data to be sent + + comm.DataOut.data[0] = imuTime; //timestamp + comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle + comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle + comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle + comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity + comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity + comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity + comm.DataOut.data[7] = imu.LinAcc[0]; //x acc + comm.DataOut.data[8] = imu.LinAcc[1]; //y acc + comm.DataOut.data[9] = imu.LinAcc[2]; //z acc + comm.DataOut.data[10] = localization.position[0]; //localization position x + comm.DataOut.data[11] = localization.position[1]; //localization position y + comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position + comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position + comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity + comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity + comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM % + comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM % + comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command + comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command + + + comm_status[2] = comm.send(); + comm_status[0] = (comm_status[2] & 0b0001); + comm_status[1] = (comm_status[2] & 0b0010) >> 1; + + if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0 + } + + else { + comm_status[2] = comm.update(); + + comm_status[0] = (comm_status[2] & 0b0001); + comm_status[1] = (comm_status[2] & 0b0010) >> 1; + + if(comm_status[1] == 1) { + //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen); + if(go_cmd == 0) { + if(comm.DataIn.parameter[1] == 0x10) go_cmd=1; + } + } + + + } + + comm_status[0] = (comm_status[2] & 0b0001); + comm_status[1] = (comm_status[2] & 0b0010) >> 1; + + Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily + } +} + + +/** + * CameraControl loop as an individual thread + */ +void cameraControl_thread(void const *n) +{ + cameraFlag=2; + camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0); + + while(true) { + if(cameraFlag == 0) { + camera.setState(0); + } + + if(cameraFlag == 1) { + camera.updateState(); + camera.setState(camera.camFlag); + } + + if(cameraFlag == 2) { + camera.setState(1); + } + + Thread::wait(100); //proocess thread every 100ms + } +} + +/** + * Data recorder loop as an individual thread + */ +void dataRecorder_thread(void const *n) +{ + virgoRDR.set_size(2); + + while(0) { + if(recordFlag == 1) { + virgoRDR.record(imuTime, 0); + virgoRDR.record(imu.Pose[0], 1); + virgoRDR.record(imu.Pose[1], 2); + virgoRDR.record(imu.Pose[2], 3); + virgoRDR.record(imu.AngVel[0], 4); + virgoRDR.record(imu.AngVel[1], 5); + virgoRDR.record(imu.AngVel[2], 6); + virgoRDR.record(localization.position[0], 7); + virgoRDR.record(localization.position[1], 8); + virgoRDR.record(odometry.revolutions[0] * 2*M_PI, 9); + virgoRDR.record(odometry.revolutions[1] * 2*M_PI, 10); + virgoRDR.record(odometry.rpm[0], 11); + virgoRDR.record(odometry.rpm[1], 12); + virgoRDR.record(rpm_cmd[0], 13); + virgoRDR.record(rpm_cmd[1], 14); + + virgoRDR.increment_row(); + + //virgoRDR.size(); //to find number of rows in data recorder + //virgoRDR.current_row(); //current row being recorded to + //data[row][col]; //access stored data + } + + Thread::wait(DataRecorder_PeriodMS); //proocess thread every 100ms + } +} + +/** + * Debug data print loop as an individual thread + */ +#define print_lines 15 //number of info lines being printed on screen +void print_thread(void const *n) +{ + //clear 14 lines while going up, these are the initilization lines printed on screen + for(int l=14; l>0; l--) { + Debug.printf("\e[1A"); //go up 1 line + Debug.printf("\e[K"); //clear line + } + + Debug.printf("************ VIRGO v3: Status Monitor *************\n\n"); + for(int l=print_lines; l>0; l--) Debug.printf("\n"); + Debug.printf("\n==================================================="); + Debug.printf("\e[1A"); //go up 1 line + + while(true) { + //move cursor up # of lines printed to create a static display and clear the first line + for(int l=print_lines; l>0; l--) Debug.printf("\e[1A"); + Debug.printf("\e[K"); + + Debug.printf("Elapsed time: %.2f s\n\e[K", imuTime); // + Debug.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); // + Debug.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]); + Debug.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]); + 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]); + + //Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell()); + + Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index); + Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE)); + Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]); + + Debug.printf("SMC: ref_beta %.2f, ref_dbeta %.3f\n\e[K", RAD_TO_DEG(ref_beta), RAD_TO_DEG(ref_dbeta)); + Debug.printf("SMC: ref_gamma %.2f, ref_dgamma %.3f\n\e[K", RAD_TO_DEG(ref_gamma), RAD_TO_DEG(ref_dgamma)); + Debug.printf("SMC: ref_theta %.2f, ref_dtheta %.3f\n\e[K", RAD_TO_DEG(ref_theta), RAD_TO_DEG(ref_dtheta)); + 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)); + + Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]); + //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0); + Debug.printf("Measured RPM (L,R): %.1f, %.1f\n\e[K", odometry.rpm[0], odometry.rpm[1]); + //Debug.printf("Measured Revolutions (L,R): %.1f, %.1f\n\e[K", odometry.revolutions[0], odometry.revolutions[1]); + + //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); + //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); + + Debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready); + //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]); + + Thread::wait(PrintLoop_PeriodMS); + } +} \ No newline at end of file