latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
main.cpp@5:d5c2e8f852fd, 2018-04-06 (annotated)
- Committer:
- 59010050
- Date:
- Fri Apr 06 08:33:48 2018 +0000
- Revision:
- 5:d5c2e8f852fd
- Parent:
- 4:6c8b844d291f
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 | 5:d5c2e8f852fd | 9 | Timer timerwalk; |
59010050 | 1:852156b5cca1 | 10 | Thread thread1; |
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 | 3:98ef5105926e | 19 | //pc.printf("IMU \n"); |
59010050 | 1:852156b5cca1 | 20 | pc.printf(" %f \t", ax*10 ); |
59010050 | 1:852156b5cca1 | 21 | pc.printf(" %f \t", ay*10 ); |
59010050 | 3:98ef5105926e | 22 | pc.printf(" %f \t", az*10 -10); //cm/s*s |
59010050 | 1:852156b5cca1 | 23 | |
59010050 | 1:852156b5cca1 | 24 | pc.printf(" %f \t", gx ); |
59010050 | 1:852156b5cca1 | 25 | pc.printf(" %f \t", gy ); |
59010050 | 1:852156b5cca1 | 26 | pc.printf(" %f \t", gz ); //deg/s */ |
59010050 | 3:98ef5105926e | 27 | pc.printf("%.0f\t %.0f \t %.0f \n\r", roll, pitch, yaw); |
59010050 | 1:852156b5cca1 | 28 | |
59010050 | 1:852156b5cca1 | 29 | timer1.reset(); // reset timer |
59010050 | 1:852156b5cca1 | 30 | } |
59010050 | 1:852156b5cca1 | 31 | } |
59010050 | 1:852156b5cca1 | 32 | } |
59010050 | 1:852156b5cca1 | 33 | |
Khanchana | 0:3af6fae90816 | 34 | Servo Servo1(D6); |
Khanchana | 0:3af6fae90816 | 35 | Servo Servo2(D8); |
Khanchana | 0:3af6fae90816 | 36 | Servo Servo3(D9); |
Khanchana | 0:3af6fae90816 | 37 | Servo Servo4(D10); |
Khanchana | 0:3af6fae90816 | 38 | |
59010050 | 3:98ef5105926e | 39 | int value; |
59010050 | 5:d5c2e8f852fd | 40 | int walking_time; |
Khanchana | 0:3af6fae90816 | 41 | |
59010050 | 3:98ef5105926e | 42 | void servo_Right(); |
59010050 | 3:98ef5105926e | 43 | void move(); |
59010050 | 3:98ef5105926e | 44 | void cal_step_down(); |
59010050 | 3:98ef5105926e | 45 | void cal_step_up(); |
59010050 | 3:98ef5105926e | 46 | void servo(); |
59010050 | 3:98ef5105926e | 47 | void getvalue(); |
59010050 | 3:98ef5105926e | 48 | |
59010050 | 3:98ef5105926e | 49 | float pos_down_start = 1400.00; |
59010050 | 3:98ef5105926e | 50 | float pos_up_start = 1000.00; |
59010050 | 4:6c8b844d291f | 51 | float down_degree = 0.00 ; |
59010050 | 4:6c8b844d291f | 52 | float up_degree = 0.00 ; |
59010050 | 3:98ef5105926e | 53 | float stepmin = 1; |
59010050 | 5:d5c2e8f852fd | 54 | float round = 5; |
59010050 | 3:98ef5105926e | 55 | float waittime = 0.001 ; |
59010050 | 3:98ef5105926e | 56 | |
59010050 | 3:98ef5105926e | 57 | float pos_down_left = 1400.00; |
59010050 | 3:98ef5105926e | 58 | float pos_up_left = 1000.00; |
59010050 | 3:98ef5105926e | 59 | float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down //90 ใน80 นอก(45)+7 ใน85 นอก+5 |
59010050 | 3:98ef5105926e | 60 | float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5 |
59010050 | 3:98ef5105926e | 61 | float state_count_left = 1; |
59010050 | 3:98ef5105926e | 62 | float round_count_left = 1; |
59010050 | 3:98ef5105926e | 63 | float step_down_left; |
59010050 | 3:98ef5105926e | 64 | float step_up_left; |
59010050 | 3:98ef5105926e | 65 | |
59010050 | 3:98ef5105926e | 66 | float pos_down_right = 1400.00; |
59010050 | 3:98ef5105926e | 67 | float pos_up_right = 1000.00; |
59010050 | 3:98ef5105926e | 68 | float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; //right down //99 |
59010050 | 3:98ef5105926e | 69 | float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5 |
59010050 | 3:98ef5105926e | 70 | float state_count_right = 1; |
59010050 | 3:98ef5105926e | 71 | float round_count_right = 1; |
59010050 | 3:98ef5105926e | 72 | float step_up_right; |
59010050 | 3:98ef5105926e | 73 | float step_down_right; |
59010050 | 3:98ef5105926e | 74 | |
59010050 | 3:98ef5105926e | 75 | |
59010050 | 3:98ef5105926e | 76 | |
Khanchana | 0:3af6fae90816 | 77 | int main() { |
59010050 | 1:852156b5cca1 | 78 | pc.baud(1000000); |
59010050 | 2:436ed0069b61 | 79 | //pc.printf("malin"); |
59010050 | 1:852156b5cca1 | 80 | timer1.start(); // start timer counting |
59010050 | 4:6c8b844d291f | 81 | //if (pc.getc() == '1') |
59010050 | 5:d5c2e8f852fd | 82 | //{ |
59010050 | 3:98ef5105926e | 83 | thread2.start(servo); |
59010050 | 4:6c8b844d291f | 84 | //thread1.start(IMU); |
59010050 | 5:d5c2e8f852fd | 85 | //} |
59010050 | 1:852156b5cca1 | 86 | } |
59010050 | 3:98ef5105926e | 87 | void cal_step_down(){ |
59010050 | 4:6c8b844d291f | 88 | pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); |
59010050 | 4:6c8b844d291f | 89 | pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; |
59010050 | 3:98ef5105926e | 90 | if (pos_down_end_right > pos_down_end_left){ |
59010050 | 3:98ef5105926e | 91 | step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); |
59010050 | 3:98ef5105926e | 92 | step_down_left = stepmin; |
59010050 | 3:98ef5105926e | 93 | } else if (pos_down_end_right < pos_down_end_left){ |
59010050 | 3:98ef5105926e | 94 | step_down_right = stepmin; |
59010050 | 3:98ef5105926e | 95 | step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); |
59010050 | 3:98ef5105926e | 96 | } else{ |
59010050 | 3:98ef5105926e | 97 | step_down_right = stepmin; |
59010050 | 3:98ef5105926e | 98 | step_down_left = stepmin; |
59010050 | 2:436ed0069b61 | 99 | } |
59010050 | 5:d5c2e8f852fd | 100 | /*pc.printf("pos_down_right"); |
59010050 | 3:98ef5105926e | 101 | pc.printf("%f\n",pos_down_end_right); |
59010050 | 3:98ef5105926e | 102 | pc.printf("pos_down_left"); |
59010050 | 3:98ef5105926e | 103 | pc.printf("%f\n",pos_down_end_left); |
59010050 | 3:98ef5105926e | 104 | pc.printf("step_down_right"); |
59010050 | 3:98ef5105926e | 105 | pc.printf("%f\n",step_down_right); |
59010050 | 3:98ef5105926e | 106 | pc.printf("step_down_left"); |
59010050 | 5:d5c2e8f852fd | 107 | pc.printf("%f\n",step_down_left); */ |
59010050 | 3:98ef5105926e | 108 | } |
59010050 | 3:98ef5105926e | 109 | |
59010050 | 3:98ef5105926e | 110 | void cal_step_up(){ |
59010050 | 4:6c8b844d291f | 111 | pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); |
59010050 | 4:6c8b844d291f | 112 | pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); |
59010050 | 3:98ef5105926e | 113 | if (pos_up_end_right > pos_up_end_left){ |
59010050 | 3:98ef5105926e | 114 | step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); |
59010050 | 3:98ef5105926e | 115 | step_up_left = stepmin; |
59010050 | 3:98ef5105926e | 116 | } else if (pos_up_end_right < pos_up_end_left){ |
59010050 | 3:98ef5105926e | 117 | step_up_right = stepmin; |
59010050 | 3:98ef5105926e | 118 | step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); |
59010050 | 3:98ef5105926e | 119 | } else{ |
59010050 | 3:98ef5105926e | 120 | step_up_right = stepmin; |
59010050 | 3:98ef5105926e | 121 | step_up_left = stepmin; |
59010050 | 3:98ef5105926e | 122 | } |
59010050 | 5:d5c2e8f852fd | 123 | /*pc.printf("pos_up_right"); |
59010050 | 3:98ef5105926e | 124 | pc.printf("%f\n",pos_up_end_right); |
59010050 | 3:98ef5105926e | 125 | pc.printf("pos_up_left"); |
59010050 | 3:98ef5105926e | 126 | pc.printf("%f\n",pos_up_end_left); |
59010050 | 3:98ef5105926e | 127 | pc.printf("step_up_right"); |
59010050 | 3:98ef5105926e | 128 | pc.printf("%f\n",step_up_right);; |
59010050 | 3:98ef5105926e | 129 | pc.printf("step_up_left"); |
59010050 | 5:d5c2e8f852fd | 130 | pc.printf("%f\n",step_up_left); */ |
59010050 | 3:98ef5105926e | 131 | } |
59010050 | 3:98ef5105926e | 132 | |
59010050 | 3:98ef5105926e | 133 | void move(){ |
Khanchana | 0:3af6fae90816 | 134 | Servo1.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 135 | Servo2.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 136 | Servo3.Enable(1000,2000); |
Khanchana | 0:3af6fae90816 | 137 | Servo4.Enable(1000,2000); |
59010050 | 3:98ef5105926e | 138 | while(1) { |
59010050 | 3:98ef5105926e | 139 | servo_Right(); |
59010050 | 3:98ef5105926e | 140 | if(state_count_left == 1) { |
59010050 | 3:98ef5105926e | 141 | Servo1.SetPosition(pos_down_left); |
59010050 | 3:98ef5105926e | 142 | wait(waittime); |
59010050 | 3:98ef5105926e | 143 | pos_down_left = pos_down_left + step_down_left; |
59010050 | 3:98ef5105926e | 144 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) { |
59010050 | 3:98ef5105926e | 145 | state_count_left = 2; |
59010050 | 3:98ef5105926e | 146 | } |
59010050 | 3:98ef5105926e | 147 | /*pc.printf("LAD"); |
59010050 | 3:98ef5105926e | 148 | pc.printf("%f\n",pos_down_left); |
59010050 | 3:98ef5105926e | 149 | pc.printf("LAP"); |
59010050 | 3:98ef5105926e | 150 | pc.printf("%f\n",pos_up_left);*/ |
59010050 | 3:98ef5105926e | 151 | } else if(state_count_left == 2) { |
59010050 | 3:98ef5105926e | 152 | Servo2.SetPosition(pos_up_left); |
59010050 | 3:98ef5105926e | 153 | wait(waittime); |
59010050 | 3:98ef5105926e | 154 | pos_up_left = pos_up_left + step_up_left; |
59010050 | 3:98ef5105926e | 155 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { |
59010050 | 3:98ef5105926e | 156 | state_count_left = 3; |
59010050 | 3:98ef5105926e | 157 | } |
59010050 | 3:98ef5105926e | 158 | /*pc.printf("LBD"); |
59010050 | 3:98ef5105926e | 159 | pc.printf("%f\n",pos_down_left); |
59010050 | 3:98ef5105926e | 160 | pc.printf("LBP"); |
59010050 | 3:98ef5105926e | 161 | pc.printf("%f\n",pos_up_left);*/ |
59010050 | 3:98ef5105926e | 162 | } else if(state_count_left == 3) { |
59010050 | 3:98ef5105926e | 163 | Servo1.SetPosition(pos_down_left); |
59010050 | 3:98ef5105926e | 164 | wait(waittime); |
59010050 | 3:98ef5105926e | 165 | pos_down_left = pos_down_left - step_down_left; |
59010050 | 3:98ef5105926e | 166 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { |
59010050 | 3:98ef5105926e | 167 | state_count_left = 4; |
59010050 | 3:98ef5105926e | 168 | } |
59010050 | 3:98ef5105926e | 169 | /*pc.printf("LCD"); |
59010050 | 3:98ef5105926e | 170 | pc.printf("%f\n",pos_down_left); |
59010050 | 3:98ef5105926e | 171 | pc.printf("LCP"); |
59010050 | 3:98ef5105926e | 172 | pc.printf("%f\n",pos_up_left);*/ |
59010050 | 3:98ef5105926e | 173 | } else if(state_count_left == 4) { |
59010050 | 3:98ef5105926e | 174 | Servo2.SetPosition(pos_up_left); |
59010050 | 3:98ef5105926e | 175 | wait(waittime); |
59010050 | 3:98ef5105926e | 176 | pos_up_left = pos_up_left - step_up_left; |
59010050 | 3:98ef5105926e | 177 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) { |
59010050 | 3:98ef5105926e | 178 | state_count_left = 0; |
59010050 | 1:852156b5cca1 | 179 | } |
59010050 | 3:98ef5105926e | 180 | /*pc.printf("LDD"); |
59010050 | 3:98ef5105926e | 181 | pc.printf("%f\n",pos_down_left); |
59010050 | 3:98ef5105926e | 182 | pc.printf("LDP"); |
59010050 | 3:98ef5105926e | 183 | pc.printf("%f\n",pos_up_left);*/ |
59010050 | 3:98ef5105926e | 184 | } else if (state_count_left == 0 and round_count_left < round) { |
59010050 | 3:98ef5105926e | 185 | round_count_left = round_count_left+1; |
59010050 | 3:98ef5105926e | 186 | state_count_left = 1; |
59010050 | 3:98ef5105926e | 187 | pos_down_left = pos_down_start; |
59010050 | 3:98ef5105926e | 188 | pos_up_left = pos_up_start; |
59010050 | 3:98ef5105926e | 189 | } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){ |
59010050 | 5:d5c2e8f852fd | 190 | pc.printf("Finish \n"); |
59010050 | 5:d5c2e8f852fd | 191 | walking_time = timerwalk.read_ms(); |
59010050 | 3:98ef5105926e | 192 | thread1.terminate(); |
59010050 | 5:d5c2e8f852fd | 193 | pc.printf("Walking time = %d \n", walking_time); |
59010050 | 3:98ef5105926e | 194 | break; |
Khanchana | 0:3af6fae90816 | 195 | } |
59010050 | 3:98ef5105926e | 196 | } |
59010050 | 3:98ef5105926e | 197 | } |
59010050 | 3:98ef5105926e | 198 | |
59010050 | 3:98ef5105926e | 199 | void servo_Right() |
59010050 | 3:98ef5105926e | 200 | { |
59010050 | 3:98ef5105926e | 201 | if(state_count_right == 1) { |
59010050 | 3:98ef5105926e | 202 | Servo3.SetPosition(pos_down_right); |
59010050 | 3:98ef5105926e | 203 | wait(waittime); |
59010050 | 3:98ef5105926e | 204 | pos_down_right = pos_down_right + step_down_right; |
59010050 | 3:98ef5105926e | 205 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) { |
59010050 | 3:98ef5105926e | 206 | state_count_right = 2; |
59010050 | 3:98ef5105926e | 207 | } |
59010050 | 3:98ef5105926e | 208 | /*pc.printf("RAD"); |
59010050 | 3:98ef5105926e | 209 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 210 | pc.printf("RAP"); |
59010050 | 3:98ef5105926e | 211 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 212 | } else if(state_count_right == 2) { |
59010050 | 3:98ef5105926e | 213 | Servo4.SetPosition(pos_up_right); |
59010050 | 3:98ef5105926e | 214 | wait(waittime); |
59010050 | 3:98ef5105926e | 215 | pos_up_right = pos_up_right + step_up_right; |
59010050 | 3:98ef5105926e | 216 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { |
59010050 | 3:98ef5105926e | 217 | state_count_right = 3; |
59010050 | 3:98ef5105926e | 218 | } |
59010050 | 3:98ef5105926e | 219 | /*pc.printf("RBD"); |
59010050 | 3:98ef5105926e | 220 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 221 | pc.printf("RBP"); |
59010050 | 3:98ef5105926e | 222 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 223 | } else if(state_count_right == 3) { |
59010050 | 3:98ef5105926e | 224 | Servo3.SetPosition(pos_down_right); |
59010050 | 3:98ef5105926e | 225 | wait(waittime); |
59010050 | 3:98ef5105926e | 226 | pos_down_right = pos_down_right - step_down_right; |
59010050 | 3:98ef5105926e | 227 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { |
59010050 | 3:98ef5105926e | 228 | state_count_right = 4; |
Khanchana | 0:3af6fae90816 | 229 | } |
59010050 | 3:98ef5105926e | 230 | /*pc.printf("RCD"); |
59010050 | 3:98ef5105926e | 231 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 232 | pc.printf("RCP"); |
59010050 | 3:98ef5105926e | 233 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 234 | } else if(state_count_right == 4) { |
59010050 | 3:98ef5105926e | 235 | Servo4.SetPosition(pos_up_right); |
59010050 | 3:98ef5105926e | 236 | wait(waittime); |
59010050 | 3:98ef5105926e | 237 | pos_up_right = pos_up_right - step_up_right; |
59010050 | 3:98ef5105926e | 238 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) { |
59010050 | 3:98ef5105926e | 239 | state_count_right = 0; |
Khanchana | 0:3af6fae90816 | 240 | } |
59010050 | 3:98ef5105926e | 241 | /*pc.printf("RDD"); |
59010050 | 3:98ef5105926e | 242 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 243 | pc.printf("RDP"); |
59010050 | 3:98ef5105926e | 244 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 245 | } else if (state_count_right == 0 and round_count_right < round) { |
59010050 | 3:98ef5105926e | 246 | round_count_right = round_count_right+1; |
59010050 | 3:98ef5105926e | 247 | state_count_right = 1; |
59010050 | 3:98ef5105926e | 248 | pos_down_right = pos_down_start; |
59010050 | 3:98ef5105926e | 249 | pos_up_right = pos_up_start; |
Khanchana | 0:3af6fae90816 | 250 | } |
59010050 | 3:98ef5105926e | 251 | } |
59010050 | 3:98ef5105926e | 252 | |
59010050 | 3:98ef5105926e | 253 | |
59010050 | 3:98ef5105926e | 254 | void servo() { |
59010050 | 3:98ef5105926e | 255 | attitude_setup(); |
59010050 | 4:6c8b844d291f | 256 | getvalue(); |
59010050 | 3:98ef5105926e | 257 | pc.printf("start\n"); |
59010050 | 5:d5c2e8f852fd | 258 | thread1.start(IMU); |
59010050 | 5:d5c2e8f852fd | 259 | timerwalk.start(); // start timer counting |
59010050 | 4:6c8b844d291f | 260 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 261 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 262 | cal_step_down(); |
59010050 | 3:98ef5105926e | 263 | cal_step_up(); |
59010050 | 4:6c8b844d291f | 264 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 265 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 266 | move(); |
59010050 | 3:98ef5105926e | 267 | } |
59010050 | 3:98ef5105926e | 268 | |
59010050 | 3:98ef5105926e | 269 | void getvalue() { |
59010050 | 3:98ef5105926e | 270 | pc.printf("case 1 = 90-15 \n"); |
59010050 | 3:98ef5105926e | 271 | pc.printf("case 2 = 90-30 \n"); |
59010050 | 3:98ef5105926e | 272 | pc.printf("case 3 = 90-45 \n"); |
59010050 | 3:98ef5105926e | 273 | pc.printf("case 4 = 90-60 \n"); |
59010050 | 5:d5c2e8f852fd | 274 | pc.printf("case 5 = 90-75 \n"); |
59010050 | 5:d5c2e8f852fd | 275 | pc.printf("case 6 = 80-45 \n"); |
59010050 | 5:d5c2e8f852fd | 276 | pc.printf("case 7 = 85-45 \n"); |
59010050 | 5:d5c2e8f852fd | 277 | pc.printf("case 8 = 95-45 \n"); |
59010050 | 5:d5c2e8f852fd | 278 | pc.printf("case 9 = 100-45 \n"); |
59010050 | 3:98ef5105926e | 279 | value = pc.getc() ; |
59010050 | 3:98ef5105926e | 280 | switch (value) { |
59010050 | 3:98ef5105926e | 281 | case '1': { |
59010050 | 3:98ef5105926e | 282 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 283 | up_degree = 15.00 ; |
59010050 | 3:98ef5105926e | 284 | break; |
59010050 | 3:98ef5105926e | 285 | } |
59010050 | 3:98ef5105926e | 286 | case '2': { |
59010050 | 3:98ef5105926e | 287 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 288 | up_degree = 30.00 ; |
59010050 | 3:98ef5105926e | 289 | break; |
59010050 | 3:98ef5105926e | 290 | } |
59010050 | 3:98ef5105926e | 291 | case '3': { |
59010050 | 3:98ef5105926e | 292 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 293 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 294 | break; |
59010050 | 3:98ef5105926e | 295 | } |
59010050 | 3:98ef5105926e | 296 | case '4': { |
59010050 | 3:98ef5105926e | 297 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 298 | up_degree = 60.00 ; |
59010050 | 3:98ef5105926e | 299 | break; |
59010050 | 3:98ef5105926e | 300 | } |
59010050 | 3:98ef5105926e | 301 | case '5': { |
59010050 | 5:d5c2e8f852fd | 302 | down_degree = 90.00 ; |
59010050 | 5:d5c2e8f852fd | 303 | up_degree = 75.00 ; |
59010050 | 5:d5c2e8f852fd | 304 | break; |
59010050 | 5:d5c2e8f852fd | 305 | } |
59010050 | 5:d5c2e8f852fd | 306 | case '6': { |
59010050 | 3:98ef5105926e | 307 | down_degree = 80.00 ; |
59010050 | 3:98ef5105926e | 308 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 309 | break; |
59010050 | 3:98ef5105926e | 310 | } |
59010050 | 5:d5c2e8f852fd | 311 | case '7': { |
59010050 | 3:98ef5105926e | 312 | down_degree = 85.00 ; |
59010050 | 3:98ef5105926e | 313 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 314 | break; |
59010050 | 3:98ef5105926e | 315 | } |
59010050 | 5:d5c2e8f852fd | 316 | case '8': { |
59010050 | 3:98ef5105926e | 317 | down_degree = 95.00 ; |
59010050 | 3:98ef5105926e | 318 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 319 | break; |
59010050 | 3:98ef5105926e | 320 | } |
59010050 | 5:d5c2e8f852fd | 321 | case '9': { |
59010050 | 3:98ef5105926e | 322 | down_degree = 100.00 ; |
59010050 | 3:98ef5105926e | 323 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 324 | break; |
59010050 | 3:98ef5105926e | 325 | } |
59010050 | 3:98ef5105926e | 326 | break; |
59010050 | 3:98ef5105926e | 327 | } |
59010050 | 4:6c8b844d291f | 328 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 329 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 330 | |
59010050 | 3:98ef5105926e | 331 | } |