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
Revision 23:6806c3bacf58, committed 2018-03-13
- Comitter:
- harrynguyen
- Date:
- Tue Mar 13 10:07:17 2018 +0000
- Parent:
- 22:37bd71204097
- Child:
- 24:2b5e0ad907f4
- Commit message:
- Remove NRF, Data Recorder, And Camera control of Virgo
Changed in this revision
--- a/00_ControllerCore/globalExterns.h Thu Feb 08 06:28:37 2018 +0000 +++ b/00_ControllerCore/globalExterns.h Tue Mar 13 10:07:17 2018 +0000 @@ -38,9 +38,6 @@ extern Localization localization; //localization function //------------- -//** Communications ** -extern nRF24NetworkHandler comm; //nRF24 radio and network layer handler function -extern uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted //------------- //** Power Monitor **
--- a/00_ControllerCore/main.h Thu Feb 08 06:28:37 2018 +0000 +++ b/00_ControllerCore/main.h Tue Mar 13 10:07:17 2018 +0000 @@ -10,7 +10,7 @@ #include "mbed.h" #include "rtos.h" -#include "virgo3_pinmapping.h" +#include "orion_pinmapping.h" #include "config.h" #include "generalFunctions.h" @@ -19,15 +19,11 @@ #include "pidControl.h" #include "imuHandler.h" #include "localization.h" -#include "kinematics.h" -#include "nRF24_NetworkHandler.h" -#include "cmdParser.h" +//#include "kinematics.h" #include "batt_guage.h" #include "purePursuit.h" #include "attitudeControl.h" -#include "pidAttitudeControl.h" -#include "camera.h" -#include "rdr.h" +//#include "pidAttitudeControl.h" //#include "USBSerial.h"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/00_ControllerCore/orion_pinmapping.h Tue Mar 13 10:07:17 2018 +0000 @@ -0,0 +1,45 @@ +/* + * + * Pinmapping for Virgo v3 PCB AV22032015 + * + */ + +#ifndef VIRGO3_PINMAPPING_H +#define VIRGO3_PINMAPPING_H + + +#define imu_INT PA_8 +#define i2c_SDA PB_9 +#define i2c_SCL PB_8 + +//Motor1 +//#define enc_LA PB_5 +//#define enc_LB PA_4 +//#define enc_RA PB_7 +//#define enc_RB PB_6 +// +//#define mot_LP PC_9 +//#define mot_LM PC_8 +//#define mot_RP PB_15 +//#define mot_RM PB_14 + + +//Motor2 + +#define enc_LA PC_0 +#define enc_LB PC_12 +#define enc_RA PC_11 +#define enc_RB PC_10 +//// +#define mot_LP PB_10 +#define mot_LM PB_13 +#define mot_RP PA_0 +#define mot_RM PA_1 + + +#define batt_STAT PB_13 +#define uart_RX PC_7 +#define uart_TX PC_6 +#define debug_LED PC_1 + +#endif
--- a/00_ControllerCore/virgo3_pinmapping.h Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,57 +0,0 @@ -/* - * - * Pinmapping for Virgo v3 PCB AV22032015 - * - */ - -#ifndef VIRGO3_PINMAPPING_H -#define VIRGO3_PINMAPPING_H - - -#define imu_INT PA_8 -#define i2c_SDA PB_9 -#define i2c_SCL PB_8 - -//Motor1 -//#define enc_LA PB_5 -//#define enc_LB PA_4 -//#define enc_RA PB_7 -//#define enc_RB PB_6 -// -//#define mot_LP PC_9 -//#define mot_LM PC_8 -//#define mot_RP PB_15 -//#define mot_RM PB_14 - - -//Motor2 - -#define enc_LA PC_0 -#define enc_LB PC_12 -#define enc_RA PC_11 -#define enc_RB PC_10 -//// -#define mot_LP PB_10 -#define mot_LM PB_13 -#define mot_RP PA_0 -#define mot_RM PA_1 - - - -#define nrf_CE PB_1 -#define nrf_CSN PA_4 -#define nrf_IRQ PB_0 -#define spi_SCK PA_5 -#define spi_MOSI PA_7 -#define spi_MISO PA_6 -#define cam_EN PC_13 -#define batt_STAT PB_13 -#define uart_RX PC_7 -#define uart_TX PC_6 -#define usb_VBUS PA_9 -#define usb_DP PA_12 -#define usb_DM PA_11 -#define usb_ID PA_10 -#define debug_LED PC_1 - -#endif
--- a/01_DriveTrain/motorDriver.h Thu Feb 08 06:28:37 2018 +0000 +++ b/01_DriveTrain/motorDriver.h Tue Mar 13 10:07:17 2018 +0000 @@ -3,7 +3,7 @@ #include "generalFunctions.h" #include "mbed.h" -#include "virgo3_pinmapping.h" +#include "orion_pinmapping.h" #include "config.h" class motorDriver
--- a/01_DriveTrain/odometer.h Thu Feb 08 06:28:37 2018 +0000 +++ b/01_DriveTrain/odometer.h Tue Mar 13 10:07:17 2018 +0000 @@ -5,7 +5,7 @@ #include "generalFunctions.h" #include "QEI.h" #include "config.h" -#include "virgo3_pinmapping.h" +#include "orion_pinmapping.h" class odometer
--- a/02_Localization/virgo3_imuHandler.lib Thu Feb 08 06:28:37 2018 +0000 +++ b/02_Localization/virgo3_imuHandler.lib Tue Mar 13 10:07:17 2018 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/akashvibhute/code/virgo3_imuHandler/#47b6a8530868 +https://developer.mbed.org/users/akashvibhute/code/virgo3_imuHandler/#8facf92305c2
--- a/03_Communication/RF24.lib Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/akashvibhute/code/RF24/#5cc7136648d1
--- a/03_Communication/RF24Network.lib Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/akashvibhute/code/RF24Network/#b1110d26a900
--- a/03_Communication/commandParser.lib Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/users/akashvibhute/code/commandParser/#6790c74f804a
--- a/03_Communication/debugComm.cpp Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,2 +0,0 @@ -#include "debugComm.h" -
--- a/03_Communication/debugComm.h Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,12 +0,0 @@ -#ifndef debugComm_H -#define debugComm_H - -#include "mbed.h" -#include "config.h" -#include "generalFunctions.h" -#include "virgo3_pinmapping.h" - - - - -#endif \ No newline at end of file
--- a/03_Communication/nRF24_NetworkHandler.lib Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/users/akashvibhute/code/nRF24_NetworkHandler/#b49cd997ec1a
--- a/04_PowerMonitor/batt_guage.h Thu Feb 08 06:28:37 2018 +0000 +++ b/04_PowerMonitor/batt_guage.h Tue Mar 13 10:07:17 2018 +0000 @@ -4,7 +4,7 @@ #include "mbed.h" #include "config.h" #include "generalFunctions.h" -#include "virgo3_pinmapping.h" +#include "orion_pinmapping.h" #include "MAX17048.h"
--- a/07_Camera/camera.cpp Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,39 +0,0 @@ -#include "camera.h" -camera::camera():cameraSw(cam_EN) -{ - cam_state=0; - camFlag=0; - - powerOn_s = 1.0 * camera_OnS(30); //default on value - powerOff_s = 30.0*60.0 - powerOn_s; //default off time - - camera_timer.start(); -} - -void camera::setState(bool val) -{ - if(val == true) cam_state = 1; - if(val == false) cam_state = 0; - - cameraSw = cam_state; -} - -void camera::setFrequency(float capture_time_s, float cycle_time_s) -{ - powerOn_s = camera_OnS(capture_time_s); - powerOff_s = cycle_time_s - powerOn_s; - - camera_timer.reset(); -} - -void camera::updateState() -{ - if(camera_timer.read() <= powerOn_s) - camFlag=1; - - if( (camera_timer.read() > powerOn_s) && (camera_timer.read() <= (powerOn_s + powerOff_s)) ) - camFlag=0; - - if(camera_timer.read() > (powerOn_s + powerOff_s) ) - camera_timer.reset(); -} \ No newline at end of file
--- a/07_Camera/camera.h Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,30 +0,0 @@ -#ifndef camera_H -#define camera_H - -#include "mbed.h" -#include "generalFunctions.h" -#include "virgo3_pinmapping.h" - -#define camera_OnS(x) (x + 40) - -class camera -{ -public: - camera(); - void setState(bool val); - void setFrequency(float capture_time_s, float cycle_time_s); - void updateState(); - - bool camFlag; - float powerOn_s, powerOff_s; - -private: - DigitalOut cameraSw; - uint8_t cam_state; - - Timer camera_timer; - -}; - - -#endif \ No newline at end of file
--- a/09_DataRecorder/rdr.cpp Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,47 +0,0 @@ -#include "rdr.h" - -RDR::RDR() -{ - - -} - -void RDR::set_size(uint8_t coloumns) -{ - if(coloumns <= max_coloumns) - coloumn_size = coloumns; - else - coloumn_size = max_coloumns; - - row_size = max_RAM / coloumn_size; - - row_index = 0; - - for(int i=0; i < row_size; i++) { - for(int j=0; j < coloumn_size; j++) - data[i][j] = 0.0; - } -} - -uint16_t RDR::size() //returns the number of rows in recorder -{ - return(row_size); -} - -void RDR::record(float value, uint8_t coloumn_number) -{ - data[row_index][coloumn_number] = value; -} - -void RDR::increment_row() -{ - row_index += 1; - if(row_index >= row_size) - row_index = 0; -} - -uint16_t RDR::current_row() -{ - return(row_index); -} -
--- a/09_DataRecorder/rdr.h Thu Feb 08 06:28:37 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,29 +0,0 @@ -#ifndef RDR_H -#define RDR_H - -#include "mbed.h" -#include "config.h" - -class RDR -{ -public: - RDR(); - - void set_size(uint8_t coloumns); - - void record(float value, uint8_t coloumn_number); - void increment_row(); - - uint16_t current_row(); - - uint16_t size(); //returns the number of rows in recorder - - float data[max_rows][max_coloumns]; - -private: - uint16_t row_size, coloumn_size; //holds the max approved row and coloumn size to limit ram usage to 64kB - uint16_t row_index; -}; - - -#endif \ No newline at end of file
--- a/orion_main.cpp Thu Feb 08 06:28:37 2018 +0000 +++ b/orion_main.cpp Tue Mar 13 10:07:17 2018 +0000 @@ -71,14 +71,13 @@ //------------- //** Communications ** -nRF24NetworkHandler comm; //nRF24 radio and network layer handler function -uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted -uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, +//nRF24NetworkHandler comm; //nRF24 radio and network layer handler function +//uint8_t dataSend_flag; //flag to indicate data is ready to be transmitted +//uint8_t comm_status[3]; //[2] comm status, [0] decoded tx status, [1] rx status, -cmdParser wirelessCmd; +//cmdParser wirelessCmd; -/* THREAD */ -void comm_thread(void const *n); + /* ** */ //------------- @@ -92,7 +91,7 @@ //** Trajectory tracking ** purePursuit purePursuit; -kinematics kinematics; +//kinematics kinematics; float purePursuit_velocity, purePursuit_omega, purePursuit_gamma; //waypoints tored in format: x_coordinate,y_coordinate,speed_%,heading_toFace @@ -124,8 +123,8 @@ //** Attitude control ** attitudeControl attitudeControl; -pidAttitudeControl pidPitchControl; -pidAttitudeControl pidRollControl; +//pidAttitudeControl pidPitchControl; +//pidAttitudeControl pidRollControl; float pidAttCntrl_correction[2]; //0-pitch control , 1- roll control @@ -134,18 +133,13 @@ /* ** */ //------------- -//** Camera ** -camera camera; //camera driver -uint8_t cameraFlag; //flag to enable camera: 0 - off, 1 - frequency controlled, 2 - permanantly on -/* THREAD */ -void cameraControl_thread(void const *n); /* ** */ //------------- -//** Robot data recorder ** -RDR virgoRDR; -uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled +////** Robot data recorder ** +//RDR virgoRDR; +//uint8_t recordFlag; //flag to enable / disable recording: 0 - off, 1 - frequency controlled /* THREAD */ @@ -198,18 +192,6 @@ Thread WaypointCmdUpdate_function(waypointCmd_thread, NULL, osPriorityNormal); Debug.printf("* Waypoint commander routine started *\n"); - //** start comm loop as a thread ** - Thread Comm_function(comm_thread, NULL, osPriorityNormal, 1024); - Debug.printf("* Communications loop started *\n"); - - //** start camera control loop as a thread ** - Thread CameraControl_function(cameraControl_thread, NULL, osPriorityNormal); - Debug.printf("* Camera control loop started *\n"); - - //** Start data recorder as Thread ** - //Thread dataRecorder_function(dataRecorder_thread, NULL, osPriorityNormal); - //Debug.printf("* Data Recorder routine started *\n"); - //** start Debug print loop as a thread ** Thread PrintLoop_function(print_thread, NULL, osPriorityNormal, 1024); Debug.printf("* Print loop started *\n\n\n"); @@ -445,145 +427,11 @@ } } -/** - * nRF network communications as an individual thread - */ -void comm_thread(void const *n) -{ - comm.init(); //initialize communications unit - Thread::wait(1000); //wait for a bit for radio to complete setup - dataSend_flag=0; - float data[2]; - wirelessCmd.sendData(0x00, RE_CurrentPose, 0, 0); - //wirelessCmd.sendCmd(0x00, getCurrentPosition, 0); - - while(true) { - dataSend_flag =1; - - if((dataSend_flag == 1) && (comm.tx_ready == 1)) { - - comm.DataOut.addr = 0; //send to node address - - comm.DataOut.parameter[0] = 1; //parameter def 0 - comm.DataOut.parameter[1] = 2; //parameter def 1 - - comm.DataOut.dataLen = 20; //length of data to be sent - - comm.DataOut.data[0] = imuTime; //timestamp - comm.DataOut.data[1] = imu.Pose[0]; //euler x / pitch angle - comm.DataOut.data[2] = imu.Pose[1]; //euler x / roll angle - comm.DataOut.data[3] = imu.Pose[2]; //euler z / yaw angle - comm.DataOut.data[4] = imu.AngVel[0]; //euler x / pitch velocity - comm.DataOut.data[5] = imu.AngVel[1]; //euler y / roll velocity - comm.DataOut.data[6] = imu.AngVel[2]; //euler z / yaw velocity - comm.DataOut.data[7] = imu.LinAcc[0]; //x acc - comm.DataOut.data[8] = imu.LinAcc[1]; //y acc - comm.DataOut.data[9] = imu.LinAcc[2]; //z acc - comm.DataOut.data[10] = localization.position[0]; //localization position x - comm.DataOut.data[11] = localization.position[1]; //localization position y - comm.DataOut.data[12] = odometry.revolutions[0] * 2*M_PI; //left wheel position - comm.DataOut.data[13] = odometry.revolutions[1] * 2*M_PI; //right wheel position - comm.DataOut.data[14] = odometry.rpm[0] * 2*M_PI / 60; //left wheel velocity - comm.DataOut.data[15] = odometry.rpm[1] * 2*M_PI / 60; //right wheel velocity - comm.DataOut.data[16] = pwm_cmd[0] * 100.0; //left wheel PWM % - comm.DataOut.data[17] = pwm_cmd[1] * 100.0; //right wheel PWM % - comm.DataOut.data[18] = rpm_compensated[0] * 2*M_PI / 60; //compensated left wheel velocity command - comm.DataOut.data[19] = rpm_compensated[1] * 2*M_PI / 60; //compensated right wheel velocity command - - - comm_status[2] = comm.send(); - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - if(comm_status[0] == 1) dataSend_flag = 0; //if send succeeded, set dataSend_flag to 0 - } - - else { - comm_status[2] = comm.update(); - - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - if(comm_status[1] == 1) { - //wirelessCmd.parseCmd(comm.DataIn.addr, comm.DataIn.parameter, comm.DataIn.data, comm.DataIn.dataLen); - if(go_cmd == 0) { - if(comm.DataIn.parameter[1] == 0x10) go_cmd=1; - } - } - } - - comm_status[0] = (comm_status[2] & 0b0001); - comm_status[1] = (comm_status[2] & 0b0010) >> 1; - - Thread::wait(1); //slow down loop a bit so that CPU usage doesnt shoot up unnecessarily - } -} -/** - * CameraControl loop as an individual thread - */ -void cameraControl_thread(void const *n) -{ - cameraFlag=2; - camera.setFrequency(camera_CaptureTimeS*1.0, camera_cycleMinutes*60.0); - - while(true) { - if(cameraFlag == 0) { - camera.setState(0); - } - - if(cameraFlag == 1) { - camera.updateState(); - camera.setState(camera.camFlag); - } - - if(cameraFlag == 2) { - camera.setState(1); - } - - Thread::wait(100); //proocess thread every 100ms - } -} - -/** - * Data recorder loop as an individual thread - */ -void dataRecorder_thread(void const *n) -{ - virgoRDR.set_size(2); - - while(0) { - if(recordFlag == 1) { - virgoRDR.record(imuTime, 0); - virgoRDR.record(imu.Pose[0], 1); - virgoRDR.record(imu.Pose[1], 2); - virgoRDR.record(imu.Pose[2], 3); - virgoRDR.record(imu.AngVel[0], 4); - virgoRDR.record(imu.AngVel[1], 5); - virgoRDR.record(imu.AngVel[2], 6); - virgoRDR.record(localization.position[0], 7); - virgoRDR.record(localization.position[1], 8); - virgoRDR.record(odometry.revolutions[0] * 2*M_PI, 9); - virgoRDR.record(odometry.revolutions[1] * 2*M_PI, 10); - virgoRDR.record(odometry.rpm[0], 11); - virgoRDR.record(odometry.rpm[1], 12); - virgoRDR.record(rpm_cmd[0], 13); - virgoRDR.record(rpm_cmd[1], 14); - - virgoRDR.increment_row(); - - //virgoRDR.size(); //to find number of rows in data recorder - //virgoRDR.current_row(); //current row being recorded to - //data[row][col]; //access stored data - } - - Thread::wait(DataRecorder_PeriodMS); //proocess thread every 100ms - } -} /** * Debug data print loop as an individual thread @@ -632,7 +480,7 @@ //Debug.printf("PID_L: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_L.PIDFf_terms[0], PID_L.PIDFf_terms[1], PID_L.PIDFf_terms[2], PID_L.PIDFf_terms[3], PID_L.Summ_term); //Debug.printf("PID_R: P %0.3f, I %0.3f, D %0.3f, Ff %0.3f, Summ %0.3f\n\e[K", PID_R.PIDFf_terms[0], PID_R.PIDFf_terms[1], PID_R.PIDFf_terms[2], PID_R.PIDFf_terms[3], PID_R.Summ_term); - Debug.printf("Comm Status: Tx %d, Rx %d, Overall %d, comm.tx_ready %d\n\e[K", comm_status[0], comm_status[1], comm_status[2], comm.tx_ready); + //Debug.printf("Comm Status: %d\n\e[K", comm_status[0]); Thread::wait(PrintLoop_PeriodMS);