Diff: orion_main.cpp

Revision:

21:7cd86bea7f83

Child:

23:6806c3bacf58

--- /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