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

Files at this revision

API Documentation at this revision

Comitter:
harrynguyen
Date:
Tue Mar 13 10:07:17 2018 +0000
Parent:
22:37bd71204097
Child:
24:2b5e0ad907f4
Commit message:
Remove NRF, Data Recorder, And Camera control of Virgo

Changed in this revision

00_ControllerCore/globalExterns.h Show annotated file Show diff for this revision Revisions of this file
00_ControllerCore/main.h Show annotated file Show diff for this revision Revisions of this file
00_ControllerCore/orion_pinmapping.h Show annotated file Show diff for this revision Revisions of this file
00_ControllerCore/virgo3_pinmapping.h Show diff for this revision Revisions of this file
01_DriveTrain/motorDriver.h Show annotated file Show diff for this revision Revisions of this file
01_DriveTrain/odometer.h Show annotated file Show diff for this revision Revisions of this file
02_Localization/virgo3_imuHandler.lib Show annotated file Show diff for this revision Revisions of this file
03_Communication/RF24.lib Show diff for this revision Revisions of this file
03_Communication/RF24Network.lib Show diff for this revision Revisions of this file
03_Communication/commandParser.lib Show diff for this revision Revisions of this file
03_Communication/debugComm.cpp Show diff for this revision Revisions of this file
03_Communication/debugComm.h Show diff for this revision Revisions of this file
03_Communication/nRF24_NetworkHandler.lib Show diff for this revision Revisions of this file
04_PowerMonitor/batt_guage.h Show annotated file Show diff for this revision Revisions of this file
07_Camera/camera.cpp Show diff for this revision Revisions of this file
07_Camera/camera.h Show diff for this revision Revisions of this file
09_DataRecorder/flash_storage.cpp Show diff for this revision Revisions of this file
09_DataRecorder/flash_storage.h Show diff for this revision Revisions of this file
09_DataRecorder/rdr.cpp Show diff for this revision Revisions of this file
09_DataRecorder/rdr.h Show diff for this revision Revisions of this file
orion_main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/00_ControllerCore/globalExterns.h	Thu Feb 08 06:28:37 2018 +0000
+++ b/00_ControllerCore/globalExterns.h	Tue Mar 13 10:07:17 2018 +0000
@@ -38,9 +38,6 @@
 extern Localization localization;  //localization function
 //-------------
 
-//** Communications **
-extern nRF24NetworkHandler comm;  //nRF24 radio and network layer handler function
-extern uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted
 //-------------
 
 //** Power Monitor **
--- a/00_ControllerCore/main.h	Thu Feb 08 06:28:37 2018 +0000
+++ b/00_ControllerCore/main.h	Tue Mar 13 10:07:17 2018 +0000
@@ -10,7 +10,7 @@
 
 #include "mbed.h"
 #include "rtos.h"
-#include "virgo3_pinmapping.h"
+#include "orion_pinmapping.h"
 #include "config.h"
 #include "generalFunctions.h"
 
@@ -19,15 +19,11 @@
 #include "pidControl.h"
 #include "imuHandler.h"
 #include "localization.h"
-#include "kinematics.h"
-#include "nRF24_NetworkHandler.h"
-#include "cmdParser.h"
+//#include "kinematics.h"
 #include "batt_guage.h"
 #include "purePursuit.h"
 #include "attitudeControl.h"
-#include "pidAttitudeControl.h"
-#include "camera.h"
-#include "rdr.h"
+//#include "pidAttitudeControl.h"
 
 //#include "USBSerial.h"
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/00_ControllerCore/orion_pinmapping.h	Tue Mar 13 10:07:17 2018 +0000
@@ -0,0 +1,45 @@
+/*
+ *
+ *  Pinmapping for Virgo v3 PCB AV22032015
+ *
+ */
+
+#ifndef VIRGO3_PINMAPPING_H
+#define VIRGO3_PINMAPPING_H
+
+
+#define imu_INT     PA_8
+#define i2c_SDA     PB_9
+#define i2c_SCL     PB_8
+
+//Motor1
+//#define enc_LA      PB_5 
+//#define enc_LB      PA_4
+//#define enc_RA      PB_7
+//#define enc_RB      PB_6
+//
+//#define mot_LP      PC_9 
+//#define mot_LM      PC_8
+//#define mot_RP      PB_15
+//#define mot_RM      PB_14
+
+
+//Motor2
+
+#define enc_LA      PC_0 
+#define enc_LB      PC_12
+#define enc_RA      PC_11
+#define enc_RB      PC_10
+////
+#define mot_LP      PB_10 
+#define mot_LM      PB_13
+#define mot_RP      PA_0
+#define mot_RM      PA_1
+
+
+#define batt_STAT   PB_13
+#define uart_RX     PC_7
+#define uart_TX     PC_6
+#define debug_LED   PC_1
+
+#endif
--- a/00_ControllerCore/virgo3_pinmapping.h	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-/*
- *
- *  Pinmapping for Virgo v3 PCB AV22032015
- *
- */
-
-#ifndef VIRGO3_PINMAPPING_H
-#define VIRGO3_PINMAPPING_H
-
-
-#define imu_INT     PA_8
-#define i2c_SDA     PB_9
-#define i2c_SCL     PB_8
-
-//Motor1
-//#define enc_LA      PB_5 
-//#define enc_LB      PA_4
-//#define enc_RA      PB_7
-//#define enc_RB      PB_6
-//
-//#define mot_LP      PC_9 
-//#define mot_LM      PC_8
-//#define mot_RP      PB_15
-//#define mot_RM      PB_14
-
-
-//Motor2
-
-#define enc_LA      PC_0 
-#define enc_LB      PC_12
-#define enc_RA      PC_11
-#define enc_RB      PC_10
-////
-#define mot_LP      PB_10 
-#define mot_LM      PB_13
-#define mot_RP      PA_0
-#define mot_RM      PA_1
-
-
-
-#define nrf_CE      PB_1
-#define nrf_CSN     PA_4
-#define nrf_IRQ     PB_0
-#define spi_SCK     PA_5
-#define spi_MOSI    PA_7
-#define spi_MISO    PA_6
-#define cam_EN      PC_13
-#define batt_STAT   PB_13
-#define uart_RX     PC_7
-#define uart_TX     PC_6
-#define usb_VBUS    PA_9
-#define usb_DP      PA_12
-#define usb_DM      PA_11
-#define usb_ID      PA_10
-#define debug_LED   PC_1
-
-#endif
--- a/01_DriveTrain/motorDriver.h	Thu Feb 08 06:28:37 2018 +0000
+++ b/01_DriveTrain/motorDriver.h	Tue Mar 13 10:07:17 2018 +0000
@@ -3,7 +3,7 @@
 
 #include "generalFunctions.h"
 #include "mbed.h"
-#include "virgo3_pinmapping.h"
+#include "orion_pinmapping.h"
 #include "config.h"
 
 class motorDriver
--- a/01_DriveTrain/odometer.h	Thu Feb 08 06:28:37 2018 +0000
+++ b/01_DriveTrain/odometer.h	Tue Mar 13 10:07:17 2018 +0000
@@ -5,7 +5,7 @@
 #include "generalFunctions.h"
 #include "QEI.h"
 #include "config.h"
-#include "virgo3_pinmapping.h"
+#include "orion_pinmapping.h"
 
 
 class odometer
--- a/02_Localization/virgo3_imuHandler.lib	Thu Feb 08 06:28:37 2018 +0000
+++ b/02_Localization/virgo3_imuHandler.lib	Tue Mar 13 10:07:17 2018 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/akashvibhute/code/virgo3_imuHandler/#47b6a8530868
+https://developer.mbed.org/users/akashvibhute/code/virgo3_imuHandler/#8facf92305c2
--- a/03_Communication/RF24.lib	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/akashvibhute/code/RF24/#5cc7136648d1
--- a/03_Communication/RF24Network.lib	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/akashvibhute/code/RF24Network/#b1110d26a900
--- a/03_Communication/commandParser.lib	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/akashvibhute/code/commandParser/#6790c74f804a
--- a/03_Communication/debugComm.cpp	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,2 +0,0 @@
-#include "debugComm.h"
-
--- a/03_Communication/debugComm.h	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,12 +0,0 @@
-#ifndef debugComm_H
-#define debugComm_H
-
-#include "mbed.h"
-#include "config.h"
-#include "generalFunctions.h"
-#include "virgo3_pinmapping.h"
-
-
-
-
-#endif
\ No newline at end of file
--- a/03_Communication/nRF24_NetworkHandler.lib	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/akashvibhute/code/nRF24_NetworkHandler/#b49cd997ec1a
--- a/04_PowerMonitor/batt_guage.h	Thu Feb 08 06:28:37 2018 +0000
+++ b/04_PowerMonitor/batt_guage.h	Tue Mar 13 10:07:17 2018 +0000
@@ -4,7 +4,7 @@
 #include "mbed.h"
 #include "config.h"
 #include "generalFunctions.h"
-#include "virgo3_pinmapping.h"
+#include "orion_pinmapping.h"
 #include "MAX17048.h"
 
 
--- a/07_Camera/camera.cpp	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,39 +0,0 @@
-#include "camera.h"
-camera::camera():cameraSw(cam_EN)
-{
-    cam_state=0;
-    camFlag=0;
-    
-    powerOn_s = 1.0 * camera_OnS(30); //default on value
-    powerOff_s = 30.0*60.0 - powerOn_s; //default off time
-    
-    camera_timer.start();
-}
-
-void camera::setState(bool val)
-{
-    if(val == true) cam_state = 1;
-    if(val == false) cam_state = 0;
-    
-    cameraSw = cam_state;
-}
-
-void camera::setFrequency(float capture_time_s, float cycle_time_s)
-{
-    powerOn_s = camera_OnS(capture_time_s);
-    powerOff_s = cycle_time_s - powerOn_s;
-
-    camera_timer.reset();
-}
-
-void camera::updateState()
-{
-    if(camera_timer.read() <= powerOn_s)
-        camFlag=1;
-
-    if( (camera_timer.read() > powerOn_s) && (camera_timer.read() <= (powerOn_s + powerOff_s)) )
-        camFlag=0;
-
-    if(camera_timer.read() > (powerOn_s + powerOff_s) )
-        camera_timer.reset();
-}
\ No newline at end of file
--- a/07_Camera/camera.h	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-#ifndef camera_H
-#define camera_H
-
-#include "mbed.h"
-#include "generalFunctions.h"
-#include "virgo3_pinmapping.h"
-
-#define camera_OnS(x) (x + 40)
-
-class camera
-{
-public:
-    camera();
-    void setState(bool val);
-    void setFrequency(float capture_time_s, float cycle_time_s);
-    void updateState();
-    
-    bool camFlag;
-    float powerOn_s, powerOff_s;
-
-private:
-    DigitalOut cameraSw;
-    uint8_t cam_state;
-    
-    Timer camera_timer;
-    
-};
-
-
-#endif
\ No newline at end of file
--- a/09_DataRecorder/rdr.cpp	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,47 +0,0 @@
-#include "rdr.h"
-
-RDR::RDR()
-{
-
-
-}
-
-void RDR::set_size(uint8_t coloumns)
-{
-    if(coloumns <= max_coloumns)
-        coloumn_size = coloumns;
-    else
-        coloumn_size = max_coloumns;
-
-    row_size = max_RAM / coloumn_size;
-
-    row_index = 0;
-
-    for(int i=0; i < row_size; i++) {
-        for(int j=0; j < coloumn_size; j++)
-            data[i][j] = 0.0;
-    }
-}
-
-uint16_t RDR::size() //returns the number of rows in recorder
-{
-    return(row_size);
-}
-
-void RDR::record(float value, uint8_t coloumn_number)
-{
-    data[row_index][coloumn_number] = value;
-}
-
-void RDR::increment_row()
-{
-    row_index += 1;
-    if(row_index >= row_size)
-        row_index = 0;
-}
-
-uint16_t RDR::current_row()
-{
-    return(row_index);
-}
-
--- a/09_DataRecorder/rdr.h	Thu Feb 08 06:28:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#ifndef RDR_H
-#define RDR_H
-
-#include "mbed.h"
-#include "config.h"
-
-class RDR
-{
-public:
-    RDR();
-    
-    void set_size(uint8_t coloumns);
-    
-    void record(float value, uint8_t coloumn_number);
-    void increment_row();
-    
-    uint16_t current_row();
-    
-    uint16_t size(); //returns the number of rows in recorder
-    
-    float data[max_rows][max_coloumns];
-
-private:
-    uint16_t row_size, coloumn_size; //holds the max approved row and coloumn size to limit ram usage to 64kB
-    uint16_t row_index;
-};
-
-
-#endif
\ No newline at end of file
--- a/orion_main.cpp	Thu Feb 08 06:28:37 2018 +0000
+++ b/orion_main.cpp	Tue Mar 13 10:07:17 2018 +0000
@@ -71,14 +71,13 @@
 //-------------
 
 //** 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,
+//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;
+//cmdParser wirelessCmd;
 
-/* THREAD */
-void comm_thread(void const *n);
+
 /*   **   */
 //-------------
 
@@ -92,7 +91,7 @@
 
 //** Trajectory tracking **
 purePursuit purePursuit;
-kinematics kinematics;
+//kinematics kinematics;
 
 float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
 //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
@@ -124,8 +123,8 @@
 
 //** Attitude control **
 attitudeControl attitudeControl;
-pidAttitudeControl pidPitchControl;
-pidAttitudeControl pidRollControl;
+//pidAttitudeControl pidPitchControl;
+//pidAttitudeControl pidRollControl;
 
 float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control
 
@@ -134,18 +133,13 @@
 /*   **   */
 //-------------
 
-//** 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
+////** Robot data recorder **
+//RDR virgoRDR;
+//uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled
 
 /* THREAD */
 
@@ -198,18 +192,6 @@
     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");
@@ -445,145 +427,11 @@
     }
 }
 
-/**
- * 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
@@ -632,7 +480,7 @@
         //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);