Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
main.cpp
- Committer:
- majik
- Date:
- 2015-03-22
- Revision:
- 4:05484073a641
- Parent:
- 3:47461d37adfb
- Child:
- 5:d2e955a94940
File content as of revision 4:05484073a641:
/* 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 *MotorCMDTimer; 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 i, int j, int k) { //This function does nothing. LEAVE BLANK //myservo = 1; Thread::yield(); } void motor_t(void const *args) { double angle = 1; while(1){ //move the servo so it reflects the IMU ROLL if (((t.read()- prev)*1000) > write_rate){ angle = imu_data.ypr[1]/90; myservo = angle; //angle = angle - 0.01; //if (angle < 0) // angle = 1; prev = t.read(); } Thread::wait(10); } } 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) { //This thread does not work test_dmp2(); start_dmp2(mpu2); while(1) { update_dmp2(); Thread::yield(); } } void IMU_thread(void const *args) { //For some reason, using 2 IMUs causes one of them to be laggy and //the connection fails after a while (buffer overflow??) bt.baud(BT_BAUD_RATE); //you have to do this for some reason test_dmp(); //test IMU1 //test_dmp2(); //test IMU2 bt.baud(BT_BAUD_RATE); //you have to do this for some reason start_dmp(mpu); // start_dmp2(mpu2); while(1) { update_dmp(); //update_dmp2(); //mpuInterrupt = false; //this resets the interrupt flag //mpuInterrupt2 = false; //Thread::wait(10); Thread::yield(); } } void bt_shell(void const *args) { bt_shell_init(); bt.printf("BT shell initialized\r\n"); while(true) { bt_shell_run(); Thread::wait(10); //Thread::yield(); } } int main() { initRobot(); //MotorCMDTimer = 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(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); //Putting IMU2 in a separate thread does not work // Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL); //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it. Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL); Thread::wait(osWaitForever); }