File content as of revision 30:3cfa8d7f84de:

#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 sensor1(&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;
    
    uint16_t range[5];
    float sensorValues[5][5];    
    };
RangeData RangeSensor;

/*LV*/

pidBearing PID_B; //PID control for bearing
motionPlanning MP;
//void ranging_thread(void const *n); 
void motionPlanning_thread(void const *n);

/*LV*/



//** 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 = 350.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 Motion Planning funtion as Thread **
    Thread MotionPlanning_function(motionPlanning_thread, NULL, osPriorityNormal);
    debugprint.printf("* Motion Planning 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);

sensor1.init();
debugprint.printf("S1 Initialized...\n");
Thread::wait(W);

sensor1.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");

//////////////////////////////////////////////////

sensor1.startContinuous();
sensor2.startContinuous();
sensor3.startContinuous();

sensor4.startContinuous();
sensor5.startContinuous();
debugprint.printf("S5 Address set... %u \n",sensor1.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);
//      }   



    while(1)
    {
      //if(imu.imu_stabilized[1] ==1){
                        
         for (int n=0; n<5; n++){//stores 5 values from each sensor       
            RangeSensor.sensorValues[0][n] = sensor1.readRangeContinuousMillimeters();//extreme right
            RangeSensor.sensorValues[1][n] = sensor2.readRangeContinuousMillimeters();            
            RangeSensor.sensorValues[2][n] = sensor3.readRangeContinuousMillimeters();
            RangeSensor.sensorValues[3][n] = sensor4.readRangeContinuousMillimeters(); 
            RangeSensor.sensorValues[4][n] = sensor5.readRangeContinuousMillimeters();}//extreme left   
        
        
        for (int i=0; i<5; i++){ 
            RangeSensor.range[i] = generalFunctions::moving_window(RangeSensor.sensorValues[i], 5);}  
            
      //}      
            Thread::wait(1.0);  
              
    }
            
}
 
/**/
/**
 * motion planning loop as an individual thread
 */
 
 void motionPlanning_thread(void const *n){
        
        while(true) {
            
            if(imu.imu_stabilized[1] ==1){
                            
                            MP.planTrack(RangeSensor.range, 
                                target_waypoint,
                                localization.position,
                                imu.Pose[2],
                                PID_B.robotFrame, PID_B.Error, imuTime);
                }
            Thread::wait(4.0);
            
            }
     }
/**/

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 W_l, W_r;
    


    while(true) {
        

        //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
        if(imu.imu_stabilized[1] ==1) {
            
        PID_B.setSpeedChange(   &W_l, &W_r,
                                    MP.plan,
                                    localization.position,
                                    imu.Pose[2], 
                                    &MP.linearSpeed, target_velocity, MP.kp, MP.kd, MP.GTGTrue); //new controller              

            if(motor_enable == 1) {
                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]=700;
//                rpm_cmd[1]=700;

//                if( (generalFunctions::abs_f(rpm_cmd[0]) < 500.0) && (generalFunctions::abs_f(rpm_cmd[0]) > 100.0) ) //was 500 and 475
//                    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);
    }
}

//*************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.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);
                
                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; ",PID_B.robotFrame); //LV RF
               
//                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];
                                int gamma_deg;
                                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 linear in mm
                                gamma_deg = strtol(angular,NULL,10);
                                purePursuit_gamma = DEG_TO_RAD(gamma_deg);
                                //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("Running uwb %s \n",uwb_data);
//    debugprint.printf("%.2f Running 5 uwb update \n",imuTime);
    uwbtriangulation_fn(uwb_data); // Running Triateration (The function nam is misleading)
//    debugprint.printf("UWB ranging: %d %d %d \n", trilateration.range_array[0],trilateration.range_array[1],trilateration.range_array[2]);
//    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;

/*#LV*/
        PID_B.findRobotFrameDistance(target_waypoint, localization.position);   
                  
            if(PID_B.robotFrame <= waypointZone)
                waypointReached_flag = 1;
            else
                waypointReached_flag = 0;  
/*#LV*/ 

        }
        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 = 1.0;
                //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.range[4], RangeSensor.range[3], RangeSensor.range[2], RangeSensor.range[1], RangeSensor.range[0]);

//        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);
    }
}