This is for our FYDP project. 2 MPU6050s are used
main.cpp
- Committer:
- majik
- Date:
- 2015-03-21
- Revision:
- 1:815f16490da8
- Parent:
- 0:21019d94ad33
- Child:
- 2:7b3822eacad8
File content as of revision 1:815f16490da8:
/* This is the current working code for the FYDP Autowalk -only IMU1 works. I don't know why IMU2 works. I'll fix that later */ #include "robot.h" #include "bt_shell.h" RtosTimer *MotorStopTimer; RtosTimer *HalfHourTimer; void HalfHourUpdate(void const *args) { long_time += t.read_us(); t.reset(); } /* RtosTimer *OpticalFlowTimer; void update_opt(void const *args) { reckon.updateOpticalFlow(); } void RF_mesh_thread(void const *args) //this thread handles the mesh network { while(1){ Thread::wait(10); } } */ void moveMotors(int Lspeed, int Rspeed, int ms) { *motors.Left = Lspeed; *motors.Right = Rspeed; if(ms > 0) MotorStopTimer->start(ms); } void stopMotors(void const *args) { *motors.Left = 0; *motors.Right = 0; send_flag = 0; } void CURRENT_thread(void const *args) { float current; while(1){ current = current_sensor.get_current(); bt.lock(); bt.printf("\n\rCurrent: %f mA",current); bt.unlock(); Thread::wait(50); } } void optflow_thread(void const *args) { while(true) { reckon.updateOpticalFlow(); Thread::wait(3); } } void IMU2_thread(void const *args) { test_dmp2(); start_dmp2(mpu2); while(1) { update_dmp2(); Thread::yield(); } } void IMU_thread(void const *args) { bt.baud(BT_BAUD_RATE); //you have to do this for some reason test_dmp(); test_dmp2(); bt.baud(BT_BAUD_RATE); //you have to do this for some reason start_dmp(mpu); start_dmp2(mpu2); //calibrate_1(); while(1) { /*if(calibrate_flag) { calibrate_1(); calibrate_flag = 0; }*/ //if(!mpuInterrupt && fifoCount < packetSize); //interrupt not ready //else { //mpu interrupt is ready update_dmp(); update_dmp2(); //mpuInterrupt = false; //this resets the interrupt flag //mpuInterrupt2 = false; //} Thread::yield(); } } void bt_shell(void const *args) { bt_shell_init(); bt.printf("BT shell initialized\r\n"); while(true) { bt_shell_run(); Thread::yield(); } } int main() { initRobot(); MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL); HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL); HalfHourTimer->start(1800000); //doesn't work because the timer takes over (high priority) //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL); //OpticalFlowTimer->start(10); //doesn't work because the timer takes over (high priority) Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL); //if you get a hard fault, increase memory size of this thread (second last value) Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL); // Thread IMU2_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL); // Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL); //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it. //Thread optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL); Thread::wait(osWaitForever); }