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