Dave Lu
/
FYDP
IMU and knee angle. No servo yet
Fork of FYDP_Final2 by
Diff: main.cpp
- Revision:
- 0:21019d94ad33
- Child:
- 1:815f16490da8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Mar 21 21:31:29 2015 +0000 @@ -0,0 +1,134 @@ +/* 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 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 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); +} \ No newline at end of file