latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V5 by
main.cpp@2:436ed0069b61, 2018-04-02 (annotated)
- Committer:
- 59010050
- Date:
- Mon Apr 02 11:46:08 2018 +0000
- Revision:
- 2:436ed0069b61
- Parent:
- 1:852156b5cca1
mon
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 | |
59010050 | 2:436ed0069b61 | 38 | void servoleft(); |
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 | 2:436ed0069b61 | 44 | int pos_up_end = 1500; |
59010050 | 2:436ed0069b61 | 45 | int edit_up = 1400; |
59010050 | 2:436ed0069b61 | 46 | |
59010050 | 1:852156b5cca1 | 47 | int state_count = 1; |
59010050 | 2:436ed0069b61 | 48 | int state_count2 = 1; |
59010050 | 1:852156b5cca1 | 49 | int round_count = 1; |
59010050 | 2:436ed0069b61 | 50 | int round_count2 = 1; |
Khanchana | 0:3af6fae90816 | 51 | int main() { |
59010050 | 1:852156b5cca1 | 52 | pc.baud(1000000); |
59010050 | 2:436ed0069b61 | 53 | //pc.printf("malin"); |
59010050 | 1:852156b5cca1 | 54 | |
59010050 | 1:852156b5cca1 | 55 | timer1.start(); // start timer counting |
59010050 | 1:852156b5cca1 | 56 | if (pc.getc() == '1') |
59010050 | 1:852156b5cca1 | 57 | { |
59010050 | 1:852156b5cca1 | 58 | thread2.start(move); |
59010050 | 2:436ed0069b61 | 59 | //thread1.start(IMU); |
59010050 | 1:852156b5cca1 | 60 | } |
59010050 | 1:852156b5cca1 | 61 | |
59010050 | 1:852156b5cca1 | 62 | } |
59010050 | 2:436ed0069b61 | 63 | void servoleft(){ |
59010050 | 2:436ed0069b61 | 64 | |
59010050 | 2:436ed0069b61 | 65 | if(state_count2 == 1){ |
59010050 | 2:436ed0069b61 | 66 | Servo1.SetPosition(pos_down); |
59010050 | 2:436ed0069b61 | 67 | //pos_down = pos_down+5; |
59010050 | 2:436ed0069b61 | 68 | wait(0.01); |
59010050 | 2:436ed0069b61 | 69 | pc.printf("left %d\n",pos_down); |
59010050 | 2:436ed0069b61 | 70 | if(pos_down == pos_down_end+5 and pos_up == 1000){ |
59010050 | 2:436ed0069b61 | 71 | state_count2 = 2; |
59010050 | 2:436ed0069b61 | 72 | } |
59010050 | 2:436ed0069b61 | 73 | } |
59010050 | 2:436ed0069b61 | 74 | else if(state_count2 == 2){ |
59010050 | 2:436ed0069b61 | 75 | Servo2.SetPosition(pos_up); |
59010050 | 2:436ed0069b61 | 76 | //pos_up = pos_up+5; |
59010050 | 2:436ed0069b61 | 77 | wait(0.01); |
59010050 | 2:436ed0069b61 | 78 | pc.printf("stage2"); |
59010050 | 2:436ed0069b61 | 79 | if(pos_down == pos_down_end+5 and pos_up == edit_up+5){ |
59010050 | 2:436ed0069b61 | 80 | state_count2 = 3; |
59010050 | 2:436ed0069b61 | 81 | } |
59010050 | 2:436ed0069b61 | 82 | } |
59010050 | 2:436ed0069b61 | 83 | else if(state_count2 == 3){ |
59010050 | 2:436ed0069b61 | 84 | Servo1.SetPosition(pos_down); |
59010050 | 2:436ed0069b61 | 85 | //pos_down = pos_down-5; |
59010050 | 2:436ed0069b61 | 86 | wait(0.01); |
59010050 | 2:436ed0069b61 | 87 | pc.printf("stage3"); |
59010050 | 2:436ed0069b61 | 88 | if(pos_down == 1395 and pos_up == edit_up+5){ |
59010050 | 2:436ed0069b61 | 89 | state_count2 = 4; |
59010050 | 2:436ed0069b61 | 90 | } |
59010050 | 2:436ed0069b61 | 91 | } |
59010050 | 2:436ed0069b61 | 92 | else if(state_count2 == 4){ |
59010050 | 2:436ed0069b61 | 93 | Servo2.SetPosition(pos_up); |
59010050 | 2:436ed0069b61 | 94 | //pos_up = pos_up-5; |
59010050 | 2:436ed0069b61 | 95 | wait(0.01); |
59010050 | 2:436ed0069b61 | 96 | pc.printf("stage4"); |
59010050 | 2:436ed0069b61 | 97 | if(pos_down == 1395 and pos_up == 995){ |
59010050 | 2:436ed0069b61 | 98 | state_count2 = 0; |
59010050 | 2:436ed0069b61 | 99 | } |
59010050 | 2:436ed0069b61 | 100 | } |
59010050 | 2:436ed0069b61 | 101 | else if (state_count2 == 0 and round_count2 < 10){ |
59010050 | 2:436ed0069b61 | 102 | round_count2 = round_count2+1; |
59010050 | 2:436ed0069b61 | 103 | state_count2 = 1; |
59010050 | 2:436ed0069b61 | 104 | pos_down = 1400; |
59010050 | 2:436ed0069b61 | 105 | pos_up = 1000; |
59010050 | 2:436ed0069b61 | 106 | } |
59010050 | 2:436ed0069b61 | 107 | else { |
59010050 | 2:436ed0069b61 | 108 | pc.printf("1stop"); |
59010050 | 2:436ed0069b61 | 109 | //thread1.stop(IMU); |
59010050 | 2:436ed0069b61 | 110 | } |
59010050 | 2:436ed0069b61 | 111 | |
59010050 | 2:436ed0069b61 | 112 | |
59010050 | 2:436ed0069b61 | 113 | } |
59010050 | 1:852156b5cca1 | 114 | void move() { |
59010050 | 1:852156b5cca1 | 115 | attitude_setup(); |
Khanchana | 0:3af6fae90816 | 116 | Servo1.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 117 | Servo2.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 118 | Servo3.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 119 | Servo4.Enable(1000,2000); |
59010050 | 2:436ed0069b61 | 120 | //pc.printf("start"); |
59010050 | 2:436ed0069b61 | 121 | //pc.printf("start"); |
59010050 | 2:436ed0069b61 | 122 | //pc.printf("start"); |
59010050 | 2:436ed0069b61 | 123 | //pc.printf("start"); |
59010050 | 1:852156b5cca1 | 124 | pc.printf("start"); |
Khanchana | 0:3af6fae90816 | 125 | while(1){ |
59010050 | 2:436ed0069b61 | 126 | servoleft(); |
59010050 | 1:852156b5cca1 | 127 | if(state_count == 1){ |
59010050 | 1:852156b5cca1 | 128 | Servo3.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 129 | pos_down = pos_down+5; |
59010050 | 1:852156b5cca1 | 130 | wait(0.01); |
59010050 | 2:436ed0069b61 | 131 | pc.printf("right %d\n",pos_down); |
59010050 | 2:436ed0069b61 | 132 | if(pos_down == pos_down_end+5 and pos_up == 1000){ |
59010050 | 1:852156b5cca1 | 133 | state_count = 2; |
59010050 | 1:852156b5cca1 | 134 | } |
Khanchana | 0:3af6fae90816 | 135 | } |
59010050 | 1:852156b5cca1 | 136 | else if(state_count == 2){ |
59010050 | 1:852156b5cca1 | 137 | Servo4.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 138 | pos_up = pos_up+5; |
59010050 | 1:852156b5cca1 | 139 | wait(0.01); |
59010050 | 2:436ed0069b61 | 140 | if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){ |
59010050 | 1:852156b5cca1 | 141 | state_count = 3; |
59010050 | 1:852156b5cca1 | 142 | } |
Khanchana | 0:3af6fae90816 | 143 | } |
59010050 | 1:852156b5cca1 | 144 | else if(state_count == 3){ |
59010050 | 1:852156b5cca1 | 145 | Servo3.SetPosition(pos_down); |
59010050 | 1:852156b5cca1 | 146 | pos_down = pos_down-5; |
59010050 | 1:852156b5cca1 | 147 | wait(0.01); |
59010050 | 2:436ed0069b61 | 148 | if(pos_down == 1395 and pos_up == pos_up_end+5){ |
59010050 | 1:852156b5cca1 | 149 | state_count = 4; |
59010050 | 1:852156b5cca1 | 150 | } |
Khanchana | 0:3af6fae90816 | 151 | } |
59010050 | 1:852156b5cca1 | 152 | else if(state_count == 4){ |
59010050 | 1:852156b5cca1 | 153 | Servo4.SetPosition(pos_up); |
59010050 | 1:852156b5cca1 | 154 | pos_up = pos_up-5; |
59010050 | 1:852156b5cca1 | 155 | wait(0.01); |
59010050 | 2:436ed0069b61 | 156 | if(pos_down == 1395 and pos_up == 995){ |
59010050 | 1:852156b5cca1 | 157 | state_count = 0; |
59010050 | 1:852156b5cca1 | 158 | } |
59010050 | 1:852156b5cca1 | 159 | } |
59010050 | 1:852156b5cca1 | 160 | else if (state_count == 0 and round_count < 10){ |
59010050 | 1:852156b5cca1 | 161 | round_count = round_count+1; |
59010050 | 1:852156b5cca1 | 162 | state_count = 1; |
59010050 | 1:852156b5cca1 | 163 | pos_down = 1400; |
59010050 | 1:852156b5cca1 | 164 | pos_up = 1000; |
59010050 | 1:852156b5cca1 | 165 | } |
59010050 | 1:852156b5cca1 | 166 | else { |
59010050 | 1:852156b5cca1 | 167 | pc.printf("stop"); |
59010050 | 1:852156b5cca1 | 168 | } |
Khanchana | 0:3af6fae90816 | 169 | } |
59010050 | 1:852156b5cca1 | 170 | } |