latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
main.cpp@1:852156b5cca1, 2018-04-02 (annotated)
- Committer:
- 59010050
- Date:
- Mon Apr 02 10:18:44 2018 +0000
- Revision:
- 1:852156b5cca1
- Parent:
- 0:3af6fae90816
- Child:
- 2:436ed0069b61
latest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Khanchana | 0:3af6fae90816 | 1 | #include "mbed.h" |
Khanchana | 0:3af6fae90816 | 2 | #include "Servo.h" |
Khanchana | 0:3af6fae90816 | 3 | #include "rtos.h" |
59010050 | 1:852156b5cca1 | 4 | #include "attitude.h" |
Khanchana | 0:3af6fae90816 | 5 | |
Khanchana | 0:3af6fae90816 | 6 | Serial pc(USBTX, USBRX); |
59010050 | 1:852156b5cca1 | 7 | //Serial bt(A7,A2); |
59010050 | 1:852156b5cca1 | 8 | Timer timer1; |
59010050 | 1:852156b5cca1 | 9 | Thread thread1; |
59010050 | 1:852156b5cca1 | 10 | |
59010050 | 1:852156b5cca1 | 11 | Thread thread2; |
59010050 | 1:852156b5cca1 | 12 | |
59010050 | 1:852156b5cca1 | 13 | void IMU() |
59010050 | 1:852156b5cca1 | 14 | { |
59010050 | 1:852156b5cca1 | 15 | while(1) { |
59010050 | 1:852156b5cca1 | 16 | if (timer1.read_us() >=1000)// read time in ms |
59010050 | 1:852156b5cca1 | 17 | { |
59010050 | 1:852156b5cca1 | 18 | attitude_get(); |
59010050 | 1:852156b5cca1 | 19 | pc.printf(" %f \t", ax*10 ); |
59010050 | 1:852156b5cca1 | 20 | pc.printf(" %f \t", ay*10 ); |
59010050 | 1:852156b5cca1 | 21 | pc.printf(" %f \t", az*10 - 10); //cm/s*s |
59010050 | 1:852156b5cca1 | 22 | |
59010050 | 1:852156b5cca1 | 23 | pc.printf(" %f \t", gx ); |
59010050 | 1:852156b5cca1 | 24 | pc.printf(" %f \t", gy ); |
59010050 | 1:852156b5cca1 | 25 | pc.printf(" %f \t", gz ); //deg/s */ |
59010050 | 1:852156b5cca1 | 26 | pc.printf("%.0f\t %.0f \t \n\r", roll, pitch ); |
59010050 | 1:852156b5cca1 | 27 | |
59010050 | 1:852156b5cca1 | 28 | timer1.reset(); // reset timer |
59010050 | 1:852156b5cca1 | 29 | } |
59010050 | 1:852156b5cca1 | 30 | } |
59010050 | 1:852156b5cca1 | 31 | } |
59010050 | 1:852156b5cca1 | 32 | |
Khanchana | 0:3af6fae90816 | 33 | Servo Servo1(D6); |
Khanchana | 0:3af6fae90816 | 34 | Servo Servo2(D8); |
Khanchana | 0:3af6fae90816 | 35 | Servo Servo3(D9); |
Khanchana | 0:3af6fae90816 | 36 | Servo Servo4(D10); |
Khanchana | 0:3af6fae90816 | 37 | |
Khanchana | 0:3af6fae90816 | 38 | void myservoLeft(); |
59010050 | 1:852156b5cca1 | 39 | void move(); |
Khanchana | 0:3af6fae90816 | 40 | |
Khanchana | 0:3af6fae90816 | 41 | int pos_down = 1400; |
Khanchana | 0:3af6fae90816 | 42 | int pos_up = 1000; |
Khanchana | 0:3af6fae90816 | 43 | int pos_down_end = 1700; |
59010050 | 1:852156b5cca1 | 44 | int pos_up_end = ; |
59010050 | 1:852156b5cca1 | 45 | int final_posdown = 1700 ; |
59010050 | 1:852156b5cca1 | 46 | int final_posup = 1466.66; |
59010050 | 1:852156b5cca1 | 47 | int state_count = 1; |
59010050 | 1:852156b5cca1 | 48 | int round_count = 1; |
Khanchana | 0:3af6fae90816 | 49 | |
Khanchana | 0:3af6fae90816 | 50 | int main() { |
59010050 | 1:852156b5cca1 | 51 | pc.baud(1000000); |
59010050 | 1:852156b5cca1 | 52 | pc.printf("malin"); |
59010050 | 1:852156b5cca1 | 53 | |
59010050 | 1:852156b5cca1 | 54 | timer1.start(); // start timer counting |
59010050 | 1:852156b5cca1 | 55 | if (pc.getc() == '1') |
59010050 | 1:852156b5cca1 | 56 | { |
59010050 | 1:852156b5cca1 | 57 | thread2.start(move); |
59010050 | 1:852156b5cca1 | 58 | thread1.start(IMU); |
59010050 | 1:852156b5cca1 | 59 | } |
59010050 | 1:852156b5cca1 | 60 | |
59010050 | 1:852156b5cca1 | 61 | } |
59010050 | 1:852156b5cca1 | 62 | void move() { |
59010050 | 1:852156b5cca1 | 63 | attitude_setup(); |
Khanchana | 0:3af6fae90816 | 64 | Servo1.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 65 | Servo2.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 66 | Servo3.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 67 | Servo4.Enable(1000,2000); |
59010050 | 1:852156b5cca1 | 68 | pc.printf("start"); |
59010050 | 1:852156b5cca1 | 69 | pc.printf("start"); |
59010050 | 1:852156b5cca1 | 70 | pc.printf("start"); |
59010050 | 1:852156b5cca1 | 71 | pc.printf("start"); |
59010050 | 1:852156b5cca1 | 72 | pc.printf("start"); |
Khanchana | 0:3af6fae90816 | 73 | while(1){ |
59010050 | 1:852156b5cca1 | 74 | |
59010050 | 1:852156b5cca1 | 75 | if(state_count == 1){ |
Khanchana | 0:3af6fae90816 | 76 | Servo1.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 77 | Servo3.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 78 | pos_down = pos_down+5; |
59010050 | 1:852156b5cca1 | 79 | wait(0.01); |
59010050 | 1:852156b5cca1 | 80 | if(pos_down >= pos_down_end+5 and pos_up >= 1000){ |
59010050 | 1:852156b5cca1 | 81 | state_count = 2; |
59010050 | 1:852156b5cca1 | 82 | } |
Khanchana | 0:3af6fae90816 | 83 | } |
59010050 | 1:852156b5cca1 | 84 | else if(state_count == 2){ |
Khanchana | 0:3af6fae90816 | 85 | Servo2.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 86 | Servo4.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 87 | pos_up = pos_up+5; |
59010050 | 1:852156b5cca1 | 88 | wait(0.01); |
59010050 | 1:852156b5cca1 | 89 | if(pos_down >= pos_down_end+5 and pos_up >= pos_up_end+5){ |
59010050 | 1:852156b5cca1 | 90 | state_count = 3; |
59010050 | 1:852156b5cca1 | 91 | } |
Khanchana | 0:3af6fae90816 | 92 | } |
59010050 | 1:852156b5cca1 | 93 | else if(state_count == 3){ |
Khanchana | 0:3af6fae90816 | 94 | Servo1.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 95 | Servo3.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 96 | pos_down = pos_down-5; |
59010050 | 1:852156b5cca1 | 97 | wait(0.01); |
59010050 | 1:852156b5cca1 | 98 | if(pos_down <= 1395 and pos_up >= pos_up_end+5){ |
59010050 | 1:852156b5cca1 | 99 | state_count = 4; |
59010050 | 1:852156b5cca1 | 100 | } |
Khanchana | 0:3af6fae90816 | 101 | } |
59010050 | 1:852156b5cca1 | 102 | else if(state_count == 4){ |
Khanchana | 0:3af6fae90816 | 103 | Servo2.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 104 | Servo4.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 105 | pos_up = pos_up-5; |
59010050 | 1:852156b5cca1 | 106 | wait(0.01); |
59010050 | 1:852156b5cca1 | 107 | if(pos_down <= 1395 and pos_up <= 995){ |
59010050 | 1:852156b5cca1 | 108 | state_count = 0; |
59010050 | 1:852156b5cca1 | 109 | } |
59010050 | 1:852156b5cca1 | 110 | } |
59010050 | 1:852156b5cca1 | 111 | else if (state_count == 0 and round_count < 10){ |
59010050 | 1:852156b5cca1 | 112 | round_count = round_count+1; |
59010050 | 1:852156b5cca1 | 113 | state_count = 1; |
59010050 | 1:852156b5cca1 | 114 | pos_down = 1400; |
59010050 | 1:852156b5cca1 | 115 | pos_up = 1000; |
59010050 | 1:852156b5cca1 | 116 | } |
59010050 | 1:852156b5cca1 | 117 | else { |
59010050 | 1:852156b5cca1 | 118 | pc.printf("stop"); |
59010050 | 1:852156b5cca1 | 119 | } |
Khanchana | 0:3af6fae90816 | 120 | } |
59010050 | 1:852156b5cca1 | 121 | } |