Alvee Ahmed
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
Diff: main.cpp
- Revision:
- 26:cd503e08a218
- Child:
- 27:c813451bfb4d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 04 12:54:10 2018 +0000
@@ -0,0 +1,1158 @@
+#include "mbed.h"
+#include "imuHandler.h"
+#include "main.h"
+
+#include "VL53L0X.h"
+#include "Servo.h"
+
+#define W 0.1 //time to initiate the lidar
+
+//TODO
+//1,Rewrite the controller core for the robot - currently using Virgo controller
+//2,Add time-out for UWB
+
+
+
+DigitalOut led(NC);
+
+
+//**Ranging Module**
+/* THREAD */
+void static_ranging_thread(void const *n);
+void ranging_thread(void const *n);
+/* VARIABLE */
+I2C ItC_ranging(ranging_i2c_SDA, ranging_i2c_SCL);
+Timer time_r;
+
+
+DigitalOut xshut1(XS1);
+DigitalOut xshut2(XS2);
+DigitalOut xshut3(XS3);
+
+DigitalOut xshut4(XS4);
+DigitalOut xshut5(XS5);
+
+VL53L0X sensor(&ItC_ranging, &time_r);
+VL53L0X sensor2(&ItC_ranging, &time_r);
+VL53L0X sensor3(&ItC_ranging, &time_r);
+
+VL53L0X sensor4(&ItC_ranging, &time_r);
+VL53L0X sensor5(&ItC_ranging, &time_r);
+
+uint16_t data;
+struct RangeData{
+ uint16_t fwd,right,right_d,left_d,left;
+ double theta_idx;
+ };
+RangeData RangeSensor;
+
+//** IMU **
+/* THREAD */
+void imu_thread(void const *n);
+
+float imuTime;
+IMU_BNO055 imu; //Bosch BNO055 IMU wrapper class. For Invensense IMU use IMU_MPU6050 imu; //MPU9150 / MPU6050 wrapper class
+DigitalOut imu_reset(imu_RST);
+
+/* FUNCTION */
+void imuReset();
+bool imuInit_function();
+
+//** ODOMETRY **
+/* THREAD */
+void odometry_thread(void const *n);
+
+odometer odometry; //odometer function
+Localization localization; //localization function
+
+motorDriver drive; //motor drive train
+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 = 150.0; //RPM/s acceleration
+float pwm_cmd[2]; //drive motor pwm command
+bool motor_enable = 0;
+
+//** Trajectory tracking **
+/* THREAD */
+void purePursuit_thread(void const *n);
+void waypointCmd_thread(void const *n);
+
+purePursuit purePursuit;
+bool purePursuit_enable = 1;
+float purePursuit_velocity, purePursuit_omega, purePursuit_gamma;
+//waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace
+uint8_t totalWaypoints = 6;
+//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}
+//};
+
+int16_t waypoints_set[][4] = { {0,0,0,0},
+ {0,0,0,0},
+};
+
+float waypointZone = 150.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
+
+
+
+/*Motor Control*/
+/* THREAD */
+void motorControl_thread(void const *n);
+attitudeControl attitudeControl;
+
+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;
+
+
+//void waypoint_parser(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);
+/* ** */
+//-------------
+//-----------------------------------------------------------------------------
+
+////** UWB **
+/* THREAD */
+void uwb_thread(void const *n);
+void uwbtriangulation_thread(void const *n);
+/* VARIABLE */
+MODSERIAL uwb(uwb_TX, uwb_RX);
+//RawSerial uwb(uwb_TX, uwb_RX);
+char uwb_data[67];
+char uwb_data1[67];
+volatile bool newline_detected = false;
+//char rangestring_array[3][10];
+//int range_array[4];
+//char range[9];
+//****Trilateration configuration
+Trilateration trilateration;
+bool uwb_data_flag = 0;
+void rxUwbCallback(MODSERIAL_IRQ_INFO *q);
+//void rxUwbCallback();
+//vec3d bestsolution;
+//int distanceArray[4];
+//vec3d anchorArray[3];
+/*FUNCTION*/
+void uwbtriangulation_fn(char* uwb_data);
+////** End UWB **
+
+//***Raspberry Pi Communication***
+/* THREAD */
+void raspberryrx_thread(void const *n);
+void raspberrytx_thread(void const *n);
+/* VARIALE */
+//RawSerial Rasp(PA_9,PA_10);
+MODSERIAL Rasp(rasp_TX, rasp_RX);
+volatile bool rasp_newline_detected = false;
+bool rasp_data_flag = 0;
+char letter[15];
+char rasp_data[30];
+char waypoint_data[30];
+bool waypoint_ready =0;
+/* FUNCTION */
+void waypoint_parser_fn(char* waypoint_data);
+void rxRaspCallback(MODSERIAL_IRQ_INFO *q);
+//void rxRaspCallback();
+//******Debug******
+DigitalOut debugLED(debug_LED);
+Serial debugprint(uart_TX,uart_RX); //debug serial port
+/* THREAD */
+void heartbeat_thread(void const *n); //heartbeat loop as an individual thread
+void print_thread(void const *n); //debug printing thread
+
+
+
+int main() {
+ debugprint.baud(PC_BAUDRATE);
+ Rasp.baud(115200);
+
+ debugLED =1;
+
+ //wait_ms(5000);
+
+ debugprint.printf("** Starting Virgo v3 Routines *************\n\n");
+
+// //** start Hearbeat loop as a thread **
+ Thread Heartbeat_function(heartbeat_thread, NULL, osPriorityNormal);
+ debugprint.printf("* Hearbeat loop started *\n");
+
+ //** start IMU funtion as Thread **
+ Thread IMU_function(imu_thread, NULL, osPriorityHigh);
+ debugprint.printf("* IMU routine started *\n");
+
+// //** start Ranging funtion as Thread **
+// Thread Ranging_function(ranging_thread, NULL, osPriorityNormal);
+// debugprint.printf("* Ranging routine started *\n");
+
+// //** start UwbUpdate function as Thread **
+ Thread UwbUpdate_function(uwb_thread, NULL, osPriorityNormal);
+ debugprint.printf("* Uwb Update routine started *\n");
+
+// //** start Uwb Triangulation function as Thread **
+// Thread UwbTriangulation_function(uwbtriangulation_thread, NULL, osPriorityNormal);
+// debugprint.printf("* Uwb Triangulation routine started *\n");
+
+// //** start Raspberrypi receive function as Thread **
+ Thread Raspberryrx_function(raspberryrx_thread, NULL, osPriorityNormal,1024);
+ debugprint.printf("* Raspberrypi routine started *\n");
+
+ // //** start Raspberrypi transmit function as Thread **
+ Thread Raspberrytx_function(raspberrytx_thread, NULL, osPriorityNormal,1024);
+ debugprint.printf("* Raspberrypi routine started *\n");
+
+// //** start OdometryUpdate function as Thread **
+ Thread Odometry_function(odometry_thread, NULL, osPriorityNormal, 1024);
+ debugprint.printf("* Odometry routine started *\n");
+//
+// //** start MotorControl function as Thread **
+ Thread MotorControl_function(motorControl_thread, NULL, osPriorityHigh); // should be osPriorityHigh
+ debugprint.printf("* Motor control routine started *\n");
+
+// //** start PurePursuit controller as Thread **
+ Thread PurePursuitUpdate_function(purePursuit_thread, NULL, osPriorityNormal);
+ debugprint.printf("* PurePursuit controller routine started *\n");
+
+//* //** start Waypoint commander function as Thread **
+ Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal);
+ debugprint.printf("* Waypoint commander routine started *\n");
+
+//* //** start Waypoint parser function as Thread **
+// Thread WaypointParser_function(waypoint_parser, NULL, osPriorityNormal);
+// debugprint.printf("* Waypoint commander routine started *\n");
+
+//* //** start comm loop as a thread **
+// Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024);
+// debugprint.printf("* Communications loop started *\n");
+
+//* ** start debug print loop as a thread **
+// Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024);
+// debugprint.printf("* Print loop started *\n\n\n");
+
+// Thread GetLoop_function(get_thread,NULL,osPriorityNormal, 1024);
+// debugprint.printf("* Get loop started *\n\n\n");
+ debugprint.printf("Start\n");
+// pinMode(5, OUTPUT);
+// pinMode(4, OUTPUT);
+xshut1 = 0;
+xshut2 = 0;
+xshut3 = 0;
+
+xshut4 = 0;
+xshut5 = 0;
+
+debugprint.printf("\n======== Orion v1: Multiple Range Monitor ========\n");
+debugprint.printf("\nXSHUT OFF\n");
+
+
+Thread::wait(W);
+
+
+
+xshut1 = 1;
+debugprint.printf("Sensor 1: \nXSHUT ON\n");
+Thread::wait(W);
+
+sensor.init();
+debugprint.printf("S1 Initialized...\n");
+Thread::wait(W);
+
+sensor.setAddress((uint8_t)22);
+debugprint.printf("S1 Address set...\n");
+
+
+xshut2 = 1;
+debugprint.printf("\nSensor 2: \nXSHUT ON\n");
+Thread::wait(W);
+
+sensor2.init();
+debugprint.printf("S2 Initialized...\n");
+Thread::wait(W);
+
+sensor2.setAddress((uint8_t)23);
+debugprint.printf("S2 Address set...\n");
+
+xshut3 = 1;
+debugprint.printf("\nSensor 3: \nXSHUT ON\n");
+Thread::wait(W);
+
+sensor3.init();
+debugprint.printf("S3 Initialized...\n");
+Thread::wait(W);
+
+sensor3.setAddress((uint8_t)25);
+debugprint.printf("S3 Address set...\n");
+
+//////////////////////////////////////////////////
+xshut4 = 1;
+debugprint.printf("\nSensor 4: \nXSHUT ON\n");
+Thread::wait(W);
+
+sensor4.init();
+debugprint.printf("S4 Initialized...\n");
+Thread::wait(W);
+
+sensor4.setAddress((uint8_t)27);
+debugprint.printf("S4 Address set...\n");
+
+//////////////////////////////////////////////////
+xshut5 = 1;
+debugprint.printf("\nSensor 5: \nXSHUT ON\n");
+Thread::wait(W);
+
+sensor5.init();
+debugprint.printf("S5 Initialized...\n");
+Thread::wait(W);
+
+sensor5.setAddress((uint8_t)31);
+debugprint.printf("S5 Address set...\n");
+
+//////////////////////////////////////////////////
+
+sensor.startContinuous();
+sensor2.startContinuous();
+sensor3.startContinuous();
+
+sensor4.startContinuous();
+sensor5.startContinuous();
+debugprint.printf("S5 Address set... %u \n",sensor.readRangeContinuousMillimeters());
+debugprint.printf("S5 Address set... %u \n",sensor2.readRangeContinuousMillimeters());
+debugprint.printf("S5 Address set... %u \n",sensor3.readRangeContinuousMillimeters());
+
+debugprint.printf("S5 Address set... %u \n",sensor4.readRangeContinuousMillimeters());
+debugprint.printf("S5 Address set... %u \n",sensor5.readRangeContinuousMillimeters());
+// //** start Ranging funtion as Thread **
+Thread Static_Ranging_function(static_ranging_thread, NULL, osPriorityNormal);
+debugprint.printf("* Ranging routine started *\n");
+ debugprint.printf(" ***** \e[5mWAIT UNTIL IMU IS STABILIZED\e[0;m *****\n");
+
+ while(1) {
+
+ }
+
+
+
+}
+
+//****END MAIN*****************************
+//****THREAD AND FUNCTION******************
+
+/**
+ * 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 loop as an individual thread
+ */
+
+void imu_thread(void const *n)
+{
+ bool init_status = imuInit_function();
+ Thread::wait(100);
+
+ while(init_status) {
+// debugprint.printf("%.2f Running 1 imu \n", imuTime);
+ 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);
+ }
+}
+
+bool imuInit_function()
+{
+ //Set Reset pin low and then high to reset the imu
+ imuReset(); //Physical reset
+ return (imu.imuInit());
+}
+
+void imuReset() //Physical reset
+{
+ imu_reset = 0;
+ wait_ms(50);
+ imu_reset = 1;
+ return;
+}
+
+
+
+
+
+/**
+ * Ranging sensor loop as an individual thread
+ */
+ /**
+ * Ranging sensor loop as an individual thread
+ */
+void static_ranging_thread(void const *n)
+{
+ while(true)
+ {
+// if(imu.imu_stabilized[1] ==1){
+ RangeSensor.right = sensor.readRangeContinuousMillimeters();
+ RangeSensor.right_d = sensor2.readRangeContinuousMillimeters();
+ RangeSensor.fwd = sensor3.readRangeContinuousMillimeters();
+
+ RangeSensor.left_d = sensor4.readRangeContinuousMillimeters();
+ RangeSensor.left = sensor5.readRangeContinuousMillimeters();
+// if (sensor.timeoutOccurred() || sensor2.timeoutOccurred() || sensor3.timeoutOccurred())
+// debugprint.printf(" TIMEOUT\r\n");
+// }
+ Thread::wait(10);
+ }
+}
+
+
+
+void odometry_thread(void const *n)
+{
+ odometry.init();
+ Thread::wait(50);
+
+ while(true) {
+// debugprint.printf("%.2f Running 2 odometry \n", imuTime);
+ 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);
+ }
+}
+
+/**/
+/**
+ * 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) {
+// debugprint.printf("%.2f Running 3 motorControl \n", imuTime);
+ //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);
+
+// Rasp.printf("%.2f \n", purePursuit_velocity);
+ 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);
+
+
+
+// Rasp.printf("Reach %d \n", waypointReached_flag);
+// Rasp.printf("Finsih %d \n", waypointSetFinish_flag);
+// if(waypointSetFinish_flag == 0 && waypointReached_flag == 0 ) {
+// Rasp.printf("Motor enable: %d \n", motor_enable);
+ if (motor_enable == 1){
+ rpm_cmd[0]=W_l*60/(2*M_PI)*(-1.0);// Virgo
+ rpm_cmd[1]=W_r*60/(2*M_PI)*(-1.0);
+// Rasp.printf("%.2f \n", rpm_cmd[0]);
+// rpm_cmd[0]=W_l*60/(2*M_PI)*(1.0); //Orion
+// rpm_cmd[1]=W_r*60/(2*M_PI)*(1.0);
+
+// rpm_cmd[0]=0;
+// rpm_cmd[1]=0;
+
+ if( (generalFunctions::abs_f(rpm_cmd[0]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 10.0) )
+ rpm_cmd[0] = 47.5*generalFunctions::sign_f(rpm_cmd[0]);
+ else if(generalFunctions::abs_f(rpm_cmd[0]) <= 10.0)
+ rpm_cmd[0] = 0;
+
+ if( (generalFunctions::abs_f(rpm_cmd[1]) < 50.0) && (generalFunctions::abs_f(rpm_cmd[1]) > 10.0) )
+ rpm_cmd[1] = 47.5*generalFunctions::sign_f(rpm_cmd[1]);
+ else if(generalFunctions::abs_f(rpm_cmd[1]) <= 10.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());
+
+
+ } 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);
+ }
+}
+
+//*************Raspberry Pi and STM32 Communication
+//****Transmit
+void raspberrytx_thread(void const *n)
+{
+ while(1){
+//This part for Communication
+ Rasp.printf("%.4f: ",imuTime); //timestamp
+ Rasp.printf("%.2f,%.2f,%.2f; ",imu.Pose[0], imu.Pose[1], imu.Pose[2]); //euler x,y,z
+//// Rasp.printf("%.2f,%.2f,%.2f; ",imu.AngVel[0], imu.AngVel[1], imu.AngVel[2]); //ang vel x,y,z
+//// Rasp.printf("%.2f,%.2f,%.2f; ",imu.LinAcc[0], imu.LinAcc[1], imu.LinAcc[2]); //Linear acc
+ Rasp.printf("%.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
+ Rasp.printf("%u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+ Rasp.printf("%.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
+ Rasp.printf("%d,%d,%d; ",trilateration.range_array[0], trilateration.range_array[1], trilateration.range_array[2]); //uwb range data
+// Rasp.printf("%.2f,%.2f; ",odometry.revolutions[0] * 2*M_PI, odometry.revolutions[1] * 2*M_PI); //Wheel Position L,R
+ Rasp.printf("%.2f,%.2f; ",odometry.rpm[0] * 2*M_PI / 60, odometry.rpm[1] * 2*M_PI / 60); //Wheel Speed L,R
+ Rasp.printf("%.2f,%.2f; ", target_waypoint[0], target_waypoint[1]); //Waypoint heading to
+
+ Rasp.printf("%d", waypointReached_flag);
+// //Rasp.printf("%.2f,%.2f; ",comm.DataIn.data[13], comm.DataIn.data[14]); //Drivetrain Command L,R
+// Rasp.printf("%.2f,%.2f;",rpm_compensated[0] * 2*M_PI / 60, rpm_compensated[1] * 2*M_PI / 60); //Compensated Command L,R
+ Rasp.printf("\n");
+
+//This part for Debug:
+//Rasp.printf("time: %.4f: ",imuTime); //timestamp
+//Rasp.printf("imu: %.2f,%.2f,%.2f; ",imu.Pose[0], imu.Pose[1], imu.Pose[2]); //euler x,y,z
+//Rasp.printf("position: %.2f,%.2f; ",localization.position[0], localization.position[1]); //Localization X,Y
+//Rasp.printf("uwb: %.2f,%.2f; ",trilateration.robot_pos.x, trilateration.robot_pos.y); //uwb position
+//Rasp.printf("lidar: %u,%u,%u,%u,%u; ", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+//Rasp.printf("waypoint data: %d %d %d; ", waypoints_set[1][0],waypoints_set[1][1], waypoints_set[1][2]);
+//Rasp.printf("waypointReached: %d; ", waypointReached_flag);
+//Rasp.printf(";\n");
+ Thread::wait(RaspTransmit_UpdatePeriodMS);
+ }
+}
+
+//****Receive
+//void rxRaspCallback(){
+// NVIC_DisableIRQ(USART1_IRQn);
+// led = !led;;
+// while(Rasp.getc() != '$'){
+// }
+// for(int i = 0 ; i < sizeof(rasp_data); i ++){
+// rasp_data[i] = Rasp.getc();
+// if(rasp_data[i] == '\n'){
+// rasp_data_flag = 1;
+// break;
+// }
+// }
+// NVIC_EnableIRQ(USART1_IRQn);
+//}
+
+void rxRaspCallback(MODSERIAL_IRQ_INFO *q) {
+ MODSERIAL *serial = q->serial;
+ if ( serial->rxGetLastChar() == '\n') {
+ rasp_newline_detected = true;
+ }
+}
+
+void raspberryrx_thread(void const *n)
+{
+ Rasp.baud(115200);
+// Rasp.attach(&rxRaspCallback, RawSerial::RxIrq);
+ Rasp.attach(&rxRaspCallback, MODSERIAL::RxIrq);
+ for(int i = 0 ; i < sizeof(rasp_data); i ++){
+ rasp_data[i] = NULL;
+ }
+
+
+ while(1){
+// led = 0;
+ while (! rasp_newline_detected ) ;//debugprint.printf("detecting the line \n"); //should be: If newline_detected --> Compute
+ rasp_newline_detected = false;
+ while(Rasp.getc() != '$'){
+ }
+ for(int i = 0 ; i < sizeof(rasp_data); i ++){
+ rasp_data[i] = Rasp.getc();
+ if(rasp_data[i] == '\n'){
+ rasp_data_flag = 1;
+ break;
+ }
+ }
+ //debugprint.printf("Running rasp %s \n",rasp_data);
+ Thread::wait(100);
+// if (rasp_data_flag == 1){
+// debugprint.printf(" %.2f Running 5 rasp rx \n", imuTime);
+// memcpy(waypoint_data, rasp_data, sizeof(rasp_data));
+ waypoint_parser_fn(rasp_data);
+ for(int i = 0 ; i < sizeof(rasp_data); i ++){
+ rasp_data[i] = 0;
+ }
+ rasp_data_flag = 0;
+
+ Thread::wait(RaspReceive_UpdatePeriodMS);
+
+ }
+}
+
+//******End***********************************************
+
+/*****Extract waypoint from rasp_data*/
+//void waypoint_parser(void const *n){
+// char waypoint_x[5];
+// char waypoint_y[5];
+// while(1){
+// //Print the data for debugging
+//// for(int i =0 ; i < sizeof(waypoint_data); i++){
+//// debugprint.putc(waypoint_data[i]);
+//// }
+// //debugprint.printf("Waypoint data %s \n",waypoint_data);
+//
+// memcpy(waypoint_x, &waypoint_data[0], 4);
+// waypoint_x[sizeof(waypoint_x-1)] = '\0';
+//// debugprint.printf("waypoint x data char: %s \n", waypoint_x);
+//
+// memcpy(waypoint_y, &waypoint_data[0]+5, 4);
+// waypoint_y[sizeof(waypoint_y-1)] = '\0';
+//// debugprint.printf("waypoint y data char: %s \n", waypoint_y);
+//
+// waypoints_set[1][0] = strtol(waypoint_x,NULL,10);
+// waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
+// waypoint_ready = 1;
+//// debugprint.printf("waypoint data: %d %d \n", waypoints_set[2][0],waypoints_set[2][1]);
+//
+// Thread::wait(100);
+// }
+//}
+
+//Use function to call when necessary
+//void waypoint_parser_fn(char* waypoint_data){
+// char waypoint_x[5];
+// char waypoint_y[5];
+// char target_vel[3];
+// //Print the data for debugging
+//// for(int i =0 ; i < sizeof(waypoint_data); i++){
+//// debugprint.putc(waypoint_data[i]);
+//// }
+// //debugprint.printf("Waypoint data %s \n",waypoint_data);
+//
+// memcpy(waypoint_x, &waypoint_data[0], 4);
+// waypoint_x[sizeof(waypoint_x-1)] = '\0';
+//// debugprint.printf("waypoint x data char: %s \n", waypoint_x);
+//
+// memcpy(waypoint_y, &waypoint_data[0]+5, 4);
+// waypoint_y[sizeof(waypoint_y-1)] = '\0';
+//// debugprint.printf("waypoint y data char: %s \n", waypoint_y);
+// memcpy(target_vel, &waypoint_data[0]+10, 2);
+// target_vel[sizeof(target_vel-1)] = '\0';
+//
+// waypoints_set[1][0] = strtol(waypoint_x,NULL,10); //convert string to int
+// waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
+// waypoints_set[1][2] = strtol(target_vel,NULL,10);
+// waypoint_ready = 1;
+// debugprint.printf("waypoint data: %d %d %d \n", waypoints_set[1][0],waypoints_set[1][1], waypoints_set[1][2]);
+//
+//}
+
+void waypoint_parser_fn(char* waypoint_data){
+ char header;
+// char waypoint_x[5];
+// char waypoint_y[5];
+// char target_vel[3];
+ //Print the data for debugging
+// for(int i =0 ; i < sizeof(waypoint_data); i++){
+// debugprint.putc(waypoint_data[i]);
+// }
+// Rasp.printf("Waypoint data %s \n",waypoint_data);
+ header = waypoint_data[0];
+ switch (header) {
+ case '1': purePursuit_enable = 0; //turn off waypoint controller
+ char linear[5],angular[5];
+ memcpy(linear, &waypoint_data[0]+2, 4);
+ linear[sizeof(linear-1)] = '\0';
+ //Rasp.printf("%s \n", linear);
+
+ memcpy(angular, &waypoint_data[0]+7, 4);
+ angular[sizeof(angular-1)] = '\0';
+ //Rasp.printf("%s \n", angular);
+
+
+ purePursuit_velocity = strtol(linear,NULL,10); //convert string to int
+ purePursuit_gamma = strtol(angular,NULL,10);
+ //Rasp.printf("%.2f \n", purePursuit_velocity);
+
+ motor_enable = 1;
+
+ break;
+
+ case '2': purePursuit_enable = 1;
+ char waypoint_x[5], waypoint_y[5], target_vel[3];
+ memcpy(waypoint_x, &waypoint_data[0]+2, 4);
+ waypoint_x[sizeof(waypoint_x-1)] = '\0';
+ // Rasp.printf("%s \n", waypoint_x); //Debug print
+
+ Rasp.printf("%s \n", waypoint_data); //Debug print
+
+ memcpy(waypoint_y, &waypoint_data[0]+7, 4);
+ waypoint_y[sizeof(waypoint_y-1)] = '\0';
+// Rasp.printf("%s \n", waypoint_y);
+
+ memcpy(target_vel, &waypoint_data[0]+12, 2);
+ target_vel[sizeof(target_vel-1)] = '\0';
+ // Rasp.printf("%s \n", target_vel);
+
+ waypoints_set[1][0] = strtol(waypoint_x,NULL,10); //convert string to int
+ waypoints_set[1][1] = strtol(waypoint_y,NULL,10);
+ waypoints_set[1][2] = strtol(target_vel,NULL,10);
+ waypoint_ready = 1;
+ motor_enable = 1;
+
+ break;
+ case '3': motor_enable = 0; //Turn off the motor controller
+
+ default: break;
+
+ }
+}
+
+
+//-*****************************UWB***********************
+void uwb_thread(void const *n)
+{
+// float uwb_time = 0.0;
+// float time_out = 0.0;
+ uwb.baud(115200);
+// uwb.attach(&rxUwbCallback, RawSerial::RxIrq);
+ uwb.attach(&rxUwbCallback, MODSERIAL::RxIrq);
+ for (int j = 0; j< sizeof(uwb_data); j++) {
+ uwb_data[j] = NULL;
+ }
+ while(1){
+// debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
+// debugprint.printf("Running uwb %s \n",uwb_data);
+
+//NEW:
+ while (! newline_detected ); // debugprint.printf("detecting the line \n"); //should be: If newline_detected --> Compute : No because : if else, the data will be skip.
+ newline_detected = false;
+ //checking start of the message with letter "m"
+ while(uwb.getc() != 'm'){
+ }
+ for(int i = 0 ; i < sizeof(uwb_data); i ++){
+ uwb_data[i] = uwb.getc();
+ if(uwb_data[i] == '\n'){
+ uwb_data_flag = 1;
+ break;
+ }
+
+// debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
+ uwbtriangulation_fn(uwb_data); // Running Triateration (The function nam is misleading)
+// ekf_fn(&ekf); //run ekf
+ debugprint.printf("%s",uwb_data);
+ for (int j = 0; j< sizeof(uwb_data); j++) {
+ uwb_data[j] = 0;
+ }
+ uwb_data_flag = 0;
+ }
+
+//OLD
+// if(uwb_data_flag == 1){
+//// memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */); //copy data from buffer
+// uwbtriangulation_fn(uwb_data);
+// for (int j = 0; j< sizeof(uwb_data); j++) {
+// uwb_data[j] = 0;
+// }
+// uwb_data_flag = 0;
+//
+// }
+
+ // debugprint.printf("\n");
+ //debugprint.printf("%s \n",range);
+ Thread::wait(200);
+ }
+
+}
+
+//void uwbtriangulation_thread(void const *n)
+//{
+// int anchorheight = 1.8;
+// anchorArray[0].x = 0.0;
+// anchorArray[0].y = 0.0;
+// anchorArray[0].z = anchorheight;
+//
+// anchorArray[1].x = 3.0;
+// anchorArray[1].y = 0.0;
+// anchorArray[1].z = anchorheight;
+//
+// anchorArray[2].x = 0.0;
+// anchorArray[2].y = 4.0;
+// anchorArray[2].z = anchorheight;
+//
+// range_array[0] = 0;
+// range_array[1] = 0;
+// range_array[2] = 0;
+// range_array[3] = 0;
+//
+// while(1){
+//// debugprint.printf("%.2f Running 6 uwb triangulation \n", imuTime);
+// //memcpy(uwb_data1, uwb_data /* Offset */, 67 /* Length */);
+//
+// memcpy(rangestring_array[0], &uwb_data1[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
+// rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
+// range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
+//
+// memcpy(rangestring_array[1], &uwb_data1[0] + 13 /* Offset */, 9 /* Length */);
+// rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
+// range_array[1] = strtol(rangestring_array[1],NULL, 16);
+//
+// memcpy(rangestring_array[2], &uwb_data1[0] + 23 /* Offset */, 9 /* Length */);
+// rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
+// range_array[2] = strtol(rangestring_array[2],NULL, 16);
+//
+// trilateration.GetLocation (&bestsolution, 1, anchorArray, range_array);
+// //memcpy(range[1], &uwb_data1[0] + 13 /* Offset */, sizeof(range[1]) /* Length */);
+// // range_array[1] = strtol(range[1],NULL, 16)/1000.0;
+// // memcpy(range[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
+// // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
+// // range_float[0] = strtol(range[0],NULL, 16)/1000.0;
+// // range_float[1] = strtol(range[1],NULL, 16)/1000.0;
+// // range_float[2] = strtol(range[2],NULL, 16)/1000.0;
+// // debugprint.printf("\n");
+// // debugprint.printf(range[0]);
+//// debugprint.puts(range[1]);
+// // debugprint.puts(range[2]);
+// // debugprint.printf("\n");
+// //debugprint.printf("%s",uwb_data);
+// //debugprint.printf("%s \n",uwb_data[66]);
+// Thread::wait(250);
+// }
+//
+//}
+
+void uwbtriangulation_fn(char* uwb_data)
+{
+ char rangestring_array[3][10];
+ memcpy(rangestring_array[0], &uwb_data[0] + 3 /* Offset */, 9 /* Length */); //copy substring from a ranging string
+ rangestring_array[0][sizeof(rangestring_array[0])-1] = '\0'; //add NULL terminal (memcpy requires)
+ trilateration.range_array[0] = strtol(rangestring_array[0],NULL, 16); //Convert a ranging string to float
+
+ memcpy(&rangestring_array[1], &uwb_data[0] + 13 /* Offset */, 9 /* Length */);
+ rangestring_array[1][sizeof(rangestring_array[1])-1] = '\0';
+ trilateration.range_array[1] = strtol(rangestring_array[1],NULL, 16);
+
+ memcpy(&rangestring_array[2], &uwb_data[0] + 23 /* Offset */, 9 /* Length */);
+ rangestring_array[2][sizeof(rangestring_array[2])-1] = '\0';
+ trilateration.range_array[2] = strtol(rangestring_array[2],NULL, 16);
+
+ trilateration.GetLocation (&trilateration.robot_pos, 1, trilateration.anchor_pos, trilateration.range_array);
+}
+
+
+//void rxUwbCallback() {
+// NVIC_DisableIRQ(USART2_IRQn);
+// while(uwb.getc() != 'm'){
+// }
+// for(int i = 0 ; i < sizeof(uwb_data); i ++){
+// uwb_data[i] = uwb.getc();
+// if(uwb_data[i] == '\n'){
+// uwb_data_flag = 1;
+// break;
+// }
+// }
+// NVIC_EnableIRQ(USART2_IRQn);
+//}
+
+void rxUwbCallback(MODSERIAL_IRQ_INFO *q) {
+ MODSERIAL *serial = q->serial;
+ if ( serial->rxGetLastChar() == '\n') {
+ newline_detected = true;
+ }
+}
+
+//*******************************************************
+
+
+
+
+
+/**
+/**
+ * purepursuit loop as an individual thread
+ */
+void purePursuit_thread(void const *n)
+{
+ while(true) {
+// debugprint.printf("%.2f Running 7 purePursuit \n", imuTime);
+ if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 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) {
+// debugprint.printf("%.2f waypoint cmd \n", imuTime);
+ //if((imu.imu_stabilized[0] ==1) && (go_cmd == 1)) {
+ if(imu.imu_stabilized[0] ==1 && purePursuit_enable == 1) {
+
+// if(waypoint_index > totalWaypoints) {
+// target_velocity = 0.0; //stop the robot
+// waypointSetFinish_flag = 1;
+// }
+
+ if(waypointReached_flag == 1 && waypoint_ready == 0) {
+ target_velocity = 0.0; //stop the robot;
+ motor_enable = 0;
+ }
+
+// debugprint.printf("waypointReached_flag = %d, waypoint_ready = %d \n", waypointReached_flag,waypoint_ready);
+// debugprint.printf("target waypoint %.2f %.2f \n", target_waypoint[0], target_waypoint[1]);
+ if(waypointReached_flag == 1 && waypointSetFinish_flag == 0 && waypoint_ready == 1) {
+ waypoint_index = 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
+ //target_velocity = 90*(driveTrain_maxV/100.0);
+ waypoint_ready = 0;
+// 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] = bestsolution.x; // uwb x position //Harry changed here
+// comm.DataOut.data[17] = bestsolution.y; // uwb y position
+//// 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
+// }
+//}
+
+
+
+ /**
+ * 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--) {
+ debugprint.printf("\e[1A"); //go up 1 line
+ debugprint.printf("\e[K"); //clear line
+ }
+
+ debugprint.printf("************ VIRGO v3: Status Monitor *************\n\n");
+ for(int l=print_lines; l>0; l--) debugprint.printf("\n");
+ debugprint.printf("\n===================================================");
+ debugprint.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--) debugprint.printf("\e[1A");
+ debugprint.printf("\e[K");
+
+ debugprint.printf("Elapsed time: %.2f s\n\e[K", imuTime); //
+// debugprint.printf("Ranging: %.2f, %u\n\e\n", RangeSensor.theta_idx,RangeSensor.distance);
+ debugprint.printf("Position: %.2f , %.2f\n\e[K", localization.position[0], localization.position[1]); //
+ debugprint.printf("Orientation (X-Y-Z): (%.2f , %.2f , %.2f)\n\e[K", imu.Pose[0], imu.Pose[1], imu.Pose[2]);
+ debugprint.printf("Calib Status : %d, %d \n\e[K", imu.imu_stabilized[0], imu.imu_stabilized[1]);
+ debugprint.printf("Odometry : %f, %f \n\e[K", odometry.revolutions[0], odometry.revolutions[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());
+//
+ debugprint.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
+ debugprint.printf("Waypoint Tracking: distanceToWaypoint %.1f, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE));
+ debugprint.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));
+//
+ debugprint.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
+ debugprint.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);
+ debugprint.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]);
+// debugprint.printf("Raspberry waypoint: %s \n\e[K", rasp_data);
+ debugprint.printf("Ranging: %u, %u, %u, %u, %u\n\e\K", RangeSensor.right,RangeSensor.right_d,RangeSensor.fwd, RangeSensor.left_d, RangeSensor.left);
+// debugprint.printf("UWB ranging: %s %s %s\n\e[K", rangestring_array[0],rangestring_array[1], rangestring_array[2]);
+ debugprint.printf("UWB ranging: %d %d %d \n\e[K", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
+ debugprint.printf("x : %f , y : %f , z : %f \n\e[K", trilateration.robot_pos.x, trilateration.robot_pos.y, trilateration.robot_pos.z);
+ // debugprint.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready);
+
+ Thread::wait(PrintLoop_PeriodMS);
+ }
+}
+
+