Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: main.cpp
- Revision:
- 3:47461d37adfb
- Parent:
- 2:7b3822eacad8
- Child:
- 4:05484073a641
--- a/main.cpp Sat Mar 21 22:44:36 2015 +0000 +++ b/main.cpp Sun Mar 22 00:43:42 2015 +0000 @@ -5,7 +5,7 @@ #include "robot.h" #include "bt_shell.h" -RtosTimer *MotorStopTimer; +RtosTimer *MotorCMDTimer; RtosTimer *HalfHourTimer; void HalfHourUpdate(void const *args) @@ -31,13 +31,30 @@ */ -void moveMotors(int Lspeed, int Rspeed, int ms) + +void moveMotors(int i, int j, int k) +{ + //This function does nothing. LEAVE BLANK + //myservo = 1; + Thread::yield(); +} + +void motor_t(void const *args) { - *motors.Left = Lspeed; - *motors.Right = Rspeed; - - if(ms > 0) - MotorStopTimer->start(ms); + + 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) @@ -66,9 +83,10 @@ Thread::wait(3); } } - +/* void IMU2_thread(void const *args) { + //This thread does not work test_dmp2(); start_dmp2(mpu2); while(1) @@ -76,32 +94,23 @@ 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(); + //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); - //calibrate_1(); - + // start_dmp2(mpu2); + while(1) { - /*if(calibrate_flag) - { - calibrate_1(); - calibrate_flag = 0; - }*/ - - //if(!mpuInterrupt && fifoCount < packetSize); //interrupt not ready - //else { //mpu interrupt is ready - // update_dmp2(); - update_dmp(); - - //mpuInterrupt = false; //this resets the interrupt flag - //mpuInterrupt2 = false; - //} + update_dmp(); + //update_dmp2(); + //mpuInterrupt = false; //this resets the interrupt flag + //mpuInterrupt2 = false; //Thread::wait(10); Thread::yield(); } @@ -123,10 +132,10 @@ { initRobot(); - MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL); - HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL); + //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) + //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) @@ -136,11 +145,11 @@ Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL); - // Thread IMU2_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL); + // Thread IMU2_th(IMU_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 optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); + Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);