Only imu output
Fork of FYDP_Final2 by
Diff: main.cpp
- Revision:
- 5:d2e955a94940
- Parent:
- 4:05484073a641
diff -r 05484073a641 -r d2e955a94940 main.cpp --- a/main.cpp Sun Mar 22 06:34:30 2015 +0000 +++ b/main.cpp Wed Mar 25 09:58:50 2015 +0000 @@ -1,47 +1,10 @@ -/* 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 @@ -57,36 +20,8 @@ } } -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) @@ -97,21 +32,13 @@ } 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 + bt.baud(BT_BAUD_RATE); + test_dmp(); + bt.baud(BT_BAUD_RATE); 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(); } } @@ -124,34 +51,19 @@ 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 bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL); 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 IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); - //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL); - Thread::wait(osWaitForever); } \ No newline at end of file