![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
Servo code v.1
Diff: main.cpp
- Revision:
- 1:852156b5cca1
- Parent:
- 0:3af6fae90816
--- a/main.cpp Sun Apr 01 14:20:53 2018 +0000 +++ b/main.cpp Mon Apr 02 10:18:44 2018 +0000 @@ -1,65 +1,121 @@ #include "mbed.h" #include "Servo.h" #include "rtos.h" +#include "attitude.h" Serial pc(USBTX, USBRX); - +//Serial bt(A7,A2); +Timer timer1; +Thread thread1; + +Thread thread2; + +void IMU() +{ + while(1) { + if (timer1.read_us() >=1000)// read time in ms + { + attitude_get(); + pc.printf(" %f \t", ax*10 ); + pc.printf(" %f \t", ay*10 ); + pc.printf(" %f \t", az*10 - 10); //cm/s*s + + pc.printf(" %f \t", gx ); + pc.printf(" %f \t", gy ); + pc.printf(" %f \t", gz ); //deg/s */ + pc.printf("%.0f\t %.0f \t \n\r", roll, pitch ); + + timer1.reset(); // reset timer + } + } +} + Servo Servo1(D6); Servo Servo2(D8); Servo Servo3(D9); Servo Servo4(D10); void myservoLeft(); +void move(); int pos_down = 1400; int pos_up = 1000; int pos_down_end = 1700; -int pos_up_end = 1700; +int pos_up_end = ; +int final_posdown = 1700 ; +int final_posup = 1466.66; +int state_count = 1; +int round_count = 1; int main() { - pc.baud(9600); - wait(5); + pc.baud(1000000); + pc.printf("malin"); + + timer1.start(); // start timer counting + if (pc.getc() == '1') + { + thread2.start(move); + thread1.start(IMU); + } + +} +void move() { + attitude_setup(); Servo1.Enable(1000,2000); Servo2.Enable(1000,2000); Servo3.Enable(1000,2000); Servo4.Enable(1000,2000); + pc.printf("start"); + pc.printf("start"); + pc.printf("start"); + pc.printf("start"); + pc.printf("start"); while(1){ - myservoLeft(); - if(pos_down <= pos_down_end and pos_up == 1000){ + + if(state_count == 1){ Servo1.SetPosition(pos_down); - pos_down = pos_down+2; + Servo3.SetPosition(pos_down); + pos_down = pos_down+5; + wait(0.01); + if(pos_down >= pos_down_end+5 and pos_up >= 1000){ + state_count = 2; + } } - else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + else if(state_count == 2){ Servo2.SetPosition(pos_up); - pos_up = pos_up+2; + Servo4.SetPosition(pos_up); + pos_up = pos_up+5; + wait(0.01); + if(pos_down >= pos_down_end+5 and pos_up >= pos_up_end+5){ + state_count = 3; + } } - else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + else if(state_count == 3){ Servo1.SetPosition(pos_down); - pos_down = pos_down-2; + Servo3.SetPosition(pos_down); + pos_down = pos_down-5; + wait(0.01); + if(pos_down <= 1395 and pos_up >= pos_up_end+5){ + state_count = 4; + } } - else if(pos_down == 1398 and pos_up > 1000){ + else if(state_count == 4){ Servo2.SetPosition(pos_up); - pos_up = pos_up-2; - } + Servo4.SetPosition(pos_up); + pos_up = pos_up-5; + wait(0.01); + if(pos_down <= 1395 and pos_up <= 995){ + state_count = 0; + } + } + else if (state_count == 0 and round_count < 10){ + round_count = round_count+1; + state_count = 1; + pos_down = 1400; + pos_up = 1000; + } + else { + pc.printf("stop"); + } } -} - -void myservoLeft() -{ - if(pos_down <= pos_down_end and pos_up == 1000){ - Servo3.SetPosition(pos_down); - pos_down = pos_down+2; - } - else if(pos_down == pos_down_end+2 and pos_up <= 1700){ - Servo4.SetPosition(pos_up); - pos_up = pos_up+2; - } - else if(pos_down >= 1400 and pos_up == pos_up_end+2){ - Servo3.SetPosition(pos_down); - pos_down = pos_down-2; - } - else if(pos_down == 1398 and pos_up > 1000){ - Servo4.SetPosition(pos_up); - pos_up = pos_up-2; - } -} +} \ No newline at end of file