Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
Diff: orion_main.cpp
- Revision:
- 23:6806c3bacf58
- Parent:
- 21:7cd86bea7f83
- Child:
- 25:3a82f868c101
--- a/orion_main.cpp Thu Feb 08 06:28:37 2018 +0000 +++ b/orion_main.cpp Tue Mar 13 10:07:17 2018 +0000 @@ -71,14 +71,13 @@ //------------- //** Communications ** -nRF24NetworkHandler comm; //nRF24 radio and network layer handler function -uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted -uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, +//nRF24NetworkHandler comm; //nRF24 radio and network layer handler function +//uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted +//uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, -cmdParser wirelessCmd; +//cmdParser wirelessCmd; -/* THREAD */ -void comm_thread(void const *n); + /* ** */ //------------- @@ -92,7 +91,7 @@ //** Trajectory tracking ** purePursuit purePursuit; -kinematics kinematics; +//kinematics kinematics; float purePursuit_velocity, purePursuit_omega, purePursuit_gamma; //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace @@ -124,8 +123,8 @@ //** Attitude control ** attitudeControl attitudeControl; -pidAttitudeControl pidPitchControl; -pidAttitudeControl pidRollControl; +//pidAttitudeControl pidPitchControl; +//pidAttitudeControl pidRollControl; float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control @@ -134,18 +133,13 @@ /* ** */ //------------- -//** Camera ** -camera camera; //camera driver -uint8_t cameraFlag; //flag to enable camera: 0 - off, 1 - frequency controlled, 2 - permanantly on -/* THREAD */ -void cameraControl_thread(void const *n); /* ** */ //------------- -//** Robot data recorder ** -RDR virgoRDR; -uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled +////** Robot data recorder ** +//RDR virgoRDR; +//uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled /* THREAD */ @@ -198,18 +192,6 @@ Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal); Debug.printf("* Waypoint commander routine started *\n"); - //** start comm loop as a thread ** - Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024); - Debug.printf("* Communications loop started *\n"); - - //** start camera control loop as a thread ** - Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal); - Debug.printf("* Camera control loop started *\n"); - - //** Start data recorder as Thread ** - //Thread dataRecorder_function(dataRecorder_thread, NULL, osPriorityNormal); - //Debug.printf("* Data Recorder routine started *\n"); - //** start Debug print loop as a thread ** Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024); Debug.printf("* Print loop started *\n\n\n"); @@ -445,145 +427,11 @@ } } -/** - * nRF network communications as an individual thread - */ -void comm_thread(void const *n) -{ - comm.init(); //initialize communications unit - Thread::wait(1000); //wait for a bit for radio to complete setup - dataSend_flag=0; - float data[2]; - wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0); - //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0); - - while(true) { - dataSend_flag =1; - - if((dataSend_flag == 1) && (comm.tx_ready == 1)) { - - comm.DataOut.addr = 0; //send to node address - - comm.DataOut.parameter[0] = 1; //parameter def 0 - comm.DataOut.parameter[1] = 2; //parameter def 1 - - comm.DataOut.dataLen = 20; //length of data to be sent - - comm.DataOut.data[0] = imuTime; //timestamp - comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle - comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle - comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle - comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity - comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity - comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity - comm.DataOut.data[7] = imu.LinAcc[0]; //x acc - comm.DataOut.data[8] = imu.LinAcc[1]; //y acc - comm.DataOut.data[9] = imu.LinAcc[2]; //z acc - comm.DataOut.data[10] = localization.position[0]; //localization position x - comm.DataOut.data[11] = localization.position[1]; //localization position y - comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position - comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position - comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity - comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity - comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM % - comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM % - comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command - comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command - - - comm_status[2] = comm.send(); - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0 - } - - else { - comm_status[2] = comm.update(); - - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - if(comm_status[1] == 1) { - //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen); - if(go_cmd == 0) { - if(comm.DataIn.parameter[1] == 0x10) go_cmd=1; - } - } - } - - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily - } -} -/** - * CameraControl loop as an individual thread - */ -void cameraControl_thread(void const *n) -{ - cameraFlag=2; - camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0); - - while(true) { - if(cameraFlag == 0) { - camera.setState(0); - } - - if(cameraFlag == 1) { - camera.updateState(); - camera.setState(camera.camFlag); - } - - if(cameraFlag == 2) { - camera.setState(1); - } - - Thread::wait(100); //proocess thread every 100ms - } -} - -/** - * Data recorder loop as an individual thread - */ -void dataRecorder_thread(void const *n) -{ - virgoRDR.set_size(2); - - while(0) { - if(recordFlag == 1) { - virgoRDR.record(imuTime, 0); - virgoRDR.record(imu.Pose[0], 1); - virgoRDR.record(imu.Pose[1], 2); - virgoRDR.record(imu.Pose[2], 3); - virgoRDR.record(imu.AngVel[0], 4); - virgoRDR.record(imu.AngVel[1], 5); - virgoRDR.record(imu.AngVel[2], 6); - virgoRDR.record(localization.position[0], 7); - virgoRDR.record(localization.position[1], 8); - virgoRDR.record(odometry.revolutions[0] * 2*M_PI, 9); - virgoRDR.record(odometry.revolutions[1] * 2*M_PI, 10); - virgoRDR.record(odometry.rpm[0], 11); - virgoRDR.record(odometry.rpm[1], 12); - virgoRDR.record(rpm_cmd[0], 13); - virgoRDR.record(rpm_cmd[1], 14); - - virgoRDR.increment_row(); - - //virgoRDR.size(); //to find number of rows in data recorder - //virgoRDR.current_row(); //current row being recorded to - //data[row][col]; //access stored data - } - - Thread::wait(DataRecorder_PeriodMS); //proocess thread every 100ms - } -} /** * Debug data print loop as an individual thread @@ -632,7 +480,7 @@ //Debug.printf("PID_L: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_L.PIDFf_terms[0], PID_L.PIDFf_terms[1], PID_L.PIDFf_terms[2], PID_L.PIDFf_terms[3], PID_L.Summ_term); //Debug.printf("PID_R: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_R.PIDFf_terms[0], PID_R.PIDFf_terms[1], PID_R.PIDFf_terms[2], PID_R.PIDFf_terms[3], PID_R.Summ_term); - Debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready); + //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]); Thread::wait(PrintLoop_PeriodMS);