Dave Lu / FYDP_Final2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

main.cpp@5:d2e955a94940, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 09:58:50 2015 +0000
Revision:
5:d2e955a94940
Parent:
4:05484073a641 
only imu output

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:21019d94ad33 1 #include "robot.h"
majik 0:21019d94ad33 2 #include "bt_shell.h"
majik 0:21019d94ad33 3
majik 0:21019d94ad33 4
majik 3:47461d37adfb 5
majik 3:47461d37adfb 6 void motor_t(void const *args)
majik 0:21019d94ad33 7 {
majik 3:47461d37adfb 8 double angle = 1;
majik 3:47461d37adfb 9 while(1){
majik 3:47461d37adfb 10 //move the servo so it reflects the IMU ROLL
majik 3:47461d37adfb 11 if (((t.read()- prev)*1000) > write_rate){
majik 3:47461d37adfb 12 angle = imu_data.ypr[1]/90;
majik 3:47461d37adfb 13 myservo = angle;
majik 3:47461d37adfb 14 //angle = angle - 0.01;
majik 3:47461d37adfb 15 //if (angle < 0)
majik 3:47461d37adfb 16 // angle = 1;
majik 3:47461d37adfb 17 prev = t.read();
majik 3:47461d37adfb 18 }
majik 3:47461d37adfb 19 Thread::wait(10);
majik 3:47461d37adfb 20 }
majik 0:21019d94ad33 21 }
majik 0:21019d94ad33 22
majik 1:815f16490da8 23 void IMU2_thread(void const *args)
majik 1:815f16490da8 24 {
majik 1:815f16490da8 25 test_dmp2();
majik 1:815f16490da8 26 start_dmp2(mpu2);
majik 1:815f16490da8 27 while(1)
majik 1:815f16490da8 28 {
majik 1:815f16490da8 29 update_dmp2();
majik 1:815f16490da8 30 Thread::yield();
majik 1:815f16490da8 31 }
majik 4:05484073a641 32 }
majik 0:21019d94ad33 33 void IMU_thread(void const *args)
majik 0:21019d94ad33 34 {
tntmarket 5:d2e955a94940 35 bt.baud(BT_BAUD_RATE);
tntmarket 5:d2e955a94940 36 test_dmp();
tntmarket 5:d2e955a94940 37 bt.baud(BT_BAUD_RATE);
majik 0:21019d94ad33 38 start_dmp(mpu);
majik 3:47461d37adfb 39
majik 0:21019d94ad33 40 while(1) {
majik 3:47461d37adfb 41 update_dmp();
majik 0:21019d94ad33 42 Thread::yield();
majik 0:21019d94ad33 43 }
majik 0:21019d94ad33 44 }
majik 0:21019d94ad33 45
majik 0:21019d94ad33 46 void bt_shell(void const *args)
majik 0:21019d94ad33 47 {
majik 0:21019d94ad33 48 bt_shell_init();
majik 0:21019d94ad33 49 bt.printf("BT shell initialized\r\n");
majik 0:21019d94ad33 50
majik 0:21019d94ad33 51 while(true) {
majik 0:21019d94ad33 52 bt_shell_run();
majik 2:7b3822eacad8 53 Thread::wait(10);
majik 0:21019d94ad33 54 }
majik 0:21019d94ad33 55 }
majik 0:21019d94ad33 56
majik 0:21019d94ad33 57 int main()
majik 0:21019d94ad33 58 {
majik 0:21019d94ad33 59 initRobot();
majik 0:21019d94ad33 60
tntmarket 5:d2e955a94940 61 Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
majik 0:21019d94ad33 62
majik 0:21019d94ad33 63 Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
majik 0:21019d94ad33 64
tntmarket 5:d2e955a94940 65 Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL);
majik 3:47461d37adfb 66 Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);
majik 0:21019d94ad33 67
majik 0:21019d94ad33 68 Thread::wait(osWaitForever);
majik 0:21019d94ad33 69 }