![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
Changing about IMU
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V1 by
main.cpp
- Committer:
- worasuchad
- Date:
- 2018-03-23
- Revision:
- 3:5bfa7291c639
- Parent:
- 2:d4bd9ff10e8e
File content as of revision 3:5bfa7291c639:
////////////////////////////////////////////////////////////////// // project: TurtleBot Project // // code v.: 1.0 // // board : NUCLEO-F303KB // // date : 20/2/2018 // // code by: Coding on Earth by Humans // ////////////////////////////////////////////////////////////////// ///////////////////////// init //////////////////////////////// ////////////////////////////////////////////////////////////////// #include "mbed.h" #include "rtos.h" Serial pc(USBTX, USBRX); Thread thread1; //control servo left Thread thread2; //control servo right Thread thread3; //read data from IMU ///////////////////////// IMU //////////////////////////////// ////////////////////////////////////////////////////////////////// #include "attitude.h" /* #include "MPU9250.h" float sum = 0; uint32_t sumCount = 0; char buffer[14]; float origin = 0; MPU9250 mpu9250; Timer t; */ ///////////////////////// Servo //////////////////////////////// ////////////////////////////////////////////////////////////////// #include "Servo.h" Servo Servo1(D10); Servo Servo2(D6); Servo Servo3(D8); Servo Servo4(D9); /* int pos_up_start; int pos_up_end; int pos_down_start; int pos_down_end;*/ int pos_down_start = 1400; int pos_down_end = 1600; int pos_up_start = 1000; int pos_up_end = 1600; ///////////////////////// prototype func /////////////////////// ////////////////////////////////////////////////////////////////// void myservoLeft(); void myservoRight(); void IMU(); ///////////////////////// main //////////////////////////// ////////////////////////////////////////////////////////////////// int main() { IMU(); thread1.start(myservoLeft); thread2.start(myservoRight); /* while(1) { printf("Hello World! Turtlebot is READY\n"); printf("case 1-5\n"); switch(pc.getc()) { case '1': pos_down_start = 1400; pos_down_end = 1700; pos_up_start = 1000; pos_up_end = 1700; break; case '2': pos_down_start = 1400; pos_down_end = 1600; pos_up_start = 1000; pos_up_end = 1600; break; case '3': pos_down_start = 1400; pos_down_end = 1650; pos_up_start = 1000; pos_up_end = 1500; break; case '4': pos_down_start = 1400; pos_down_end = 1700; pos_up_start = 1000; pos_up_end = 1650; break; case '5': pos_down_start = 1400; pos_down_end = 1600; pos_up_start = 1000; pos_up_end = 1550; break; } printf("position down motor start = %d\n", pos_down_start); printf("position down motor end = %d\n", pos_down_end); printf("position up motor start = %d\n", pos_up_start); printf("position up motor end = %d\n", pos_up_end); thread1.start(myservoLeft); thread2.start(myservoRight); thread3.start(IMU); } */ } ///////////////////////// myservoLeft ///////////////////////// ////////////////////////////////////////////////////////////////// void myservoLeft() { for(int n = 0; n <= 5; n += 1) { Servo1.Enable(1000,20000); Servo2.Disable(); for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { Servo1.SetPosition(pos); wait(0.01); } Servo2.Enable(1000,20000); Servo1.Disable(); for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { Servo2.SetPosition(pos); wait(0.01); } Servo1.Enable(1000,20000); Servo2.Disable(); for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { Servo1.SetPosition(pos); wait(0.01); } Servo2.Enable(1000,20000); Servo1.Disable(); for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { Servo2.SetPosition(pos); wait(0.01); } } } ///////////////////////// myservoRight /////////////////////// ////////////////////////////////////////////////////////////////// void myservoRight() { for(int n = 0; n <= 5; n += 1) { Servo3.Enable(1000,20000); Servo4.Disable(); for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { Servo3.SetPosition(pos); wait(0.01); } Servo4.Enable(1000,20000); Servo3.Disable(); for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { Servo4.SetPosition(pos); wait(0.01); } Servo3.Enable(1000,20000); Servo4.Disable(); for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { Servo3.SetPosition(pos); wait(0.01); } Servo4.Enable(1000,20000); Servo3.Disable(); for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { Servo4.SetPosition(pos); wait(0.01); } } } ///////////////////////// IMU /////////////////////// ////////////////////////////////////////////////////////////////// void IMU() { attitude_setup(); while(1) { attitude_get(); pc.printf("%.0f %.0f %.0f \n\r", roll, pitch, yaw ); } }