latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
main.cpp@4:6c8b844d291f, 2018-04-04 (annotated)
- Committer:
- 59010050
- Date:
- Wed Apr 04 17:14:22 2018 +0000
- Revision:
- 4:6c8b844d291f
- Parent:
- 3:98ef5105926e
- Child:
- 5:d5c2e8f852fd
lateeeeeest
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 | 4:6c8b844d291f | 49 | float down_degree = 0.00 ; |
59010050 | 4:6c8b844d291f | 50 | float up_degree = 0.00 ; |
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 | 4:6c8b844d291f | 80 | //if (pc.getc() == '1') |
59010050 | 4:6c8b844d291f | 81 | //{ |
59010050 | 3:98ef5105926e | 82 | pc.printf("ma"); |
59010050 | 3:98ef5105926e | 83 | thread2.start(servo); |
59010050 | 4:6c8b844d291f | 84 | //thread1.start(IMU); |
59010050 | 4:6c8b844d291f | 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 | 4:6c8b844d291f | 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 | 4:6c8b844d291f | 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 | 4:6c8b844d291f | 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 | 4:6c8b844d291f | 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 | 3:98ef5105926e | 190 | pc.printf("Finish"); |
59010050 | 3:98ef5105926e | 191 | thread1.terminate(); |
59010050 | 3:98ef5105926e | 192 | break; |
Khanchana | 0:3af6fae90816 | 193 | } |
59010050 | 3:98ef5105926e | 194 | } |
59010050 | 3:98ef5105926e | 195 | } |
59010050 | 3:98ef5105926e | 196 | |
59010050 | 3:98ef5105926e | 197 | void servo_Right() |
59010050 | 3:98ef5105926e | 198 | { |
59010050 | 3:98ef5105926e | 199 | if(state_count_right == 1) { |
59010050 | 3:98ef5105926e | 200 | Servo3.SetPosition(pos_down_right); |
59010050 | 3:98ef5105926e | 201 | wait(waittime); |
59010050 | 3:98ef5105926e | 202 | pos_down_right = pos_down_right + step_down_right; |
59010050 | 3:98ef5105926e | 203 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) { |
59010050 | 3:98ef5105926e | 204 | state_count_right = 2; |
59010050 | 3:98ef5105926e | 205 | } |
59010050 | 3:98ef5105926e | 206 | /*pc.printf("RAD"); |
59010050 | 3:98ef5105926e | 207 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 208 | pc.printf("RAP"); |
59010050 | 3:98ef5105926e | 209 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 210 | } else if(state_count_right == 2) { |
59010050 | 3:98ef5105926e | 211 | Servo4.SetPosition(pos_up_right); |
59010050 | 3:98ef5105926e | 212 | wait(waittime); |
59010050 | 3:98ef5105926e | 213 | pos_up_right = pos_up_right + step_up_right; |
59010050 | 3:98ef5105926e | 214 | 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 | 215 | state_count_right = 3; |
59010050 | 3:98ef5105926e | 216 | } |
59010050 | 3:98ef5105926e | 217 | /*pc.printf("RBD"); |
59010050 | 3:98ef5105926e | 218 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 219 | pc.printf("RBP"); |
59010050 | 3:98ef5105926e | 220 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 221 | } else if(state_count_right == 3) { |
59010050 | 3:98ef5105926e | 222 | Servo3.SetPosition(pos_down_right); |
59010050 | 3:98ef5105926e | 223 | wait(waittime); |
59010050 | 3:98ef5105926e | 224 | pos_down_right = pos_down_right - step_down_right; |
59010050 | 3:98ef5105926e | 225 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { |
59010050 | 3:98ef5105926e | 226 | state_count_right = 4; |
Khanchana | 0:3af6fae90816 | 227 | } |
59010050 | 3:98ef5105926e | 228 | /*pc.printf("RCD"); |
59010050 | 3:98ef5105926e | 229 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 230 | pc.printf("RCP"); |
59010050 | 3:98ef5105926e | 231 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 232 | } else if(state_count_right == 4) { |
59010050 | 3:98ef5105926e | 233 | Servo4.SetPosition(pos_up_right); |
59010050 | 3:98ef5105926e | 234 | wait(waittime); |
59010050 | 3:98ef5105926e | 235 | pos_up_right = pos_up_right - step_up_right; |
59010050 | 3:98ef5105926e | 236 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) { |
59010050 | 3:98ef5105926e | 237 | state_count_right = 0; |
Khanchana | 0:3af6fae90816 | 238 | } |
59010050 | 3:98ef5105926e | 239 | /*pc.printf("RDD"); |
59010050 | 3:98ef5105926e | 240 | pc.printf("%f\n",pos_down_right); |
59010050 | 3:98ef5105926e | 241 | pc.printf("RDP"); |
59010050 | 3:98ef5105926e | 242 | pc.printf("%f\n",pos_up_right);*/ |
59010050 | 3:98ef5105926e | 243 | } else if (state_count_right == 0 and round_count_right < round) { |
59010050 | 3:98ef5105926e | 244 | round_count_right = round_count_right+1; |
59010050 | 3:98ef5105926e | 245 | state_count_right = 1; |
59010050 | 3:98ef5105926e | 246 | pos_down_right = pos_down_start; |
59010050 | 3:98ef5105926e | 247 | pos_up_right = pos_up_start; |
Khanchana | 0:3af6fae90816 | 248 | } |
59010050 | 3:98ef5105926e | 249 | } |
59010050 | 3:98ef5105926e | 250 | |
59010050 | 3:98ef5105926e | 251 | |
59010050 | 3:98ef5105926e | 252 | void servo() { |
59010050 | 3:98ef5105926e | 253 | attitude_setup(); |
59010050 | 4:6c8b844d291f | 254 | getvalue(); |
59010050 | 3:98ef5105926e | 255 | pc.printf("start\n"); |
59010050 | 4:6c8b844d291f | 256 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 257 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 258 | cal_step_down(); |
59010050 | 3:98ef5105926e | 259 | cal_step_up(); |
59010050 | 4:6c8b844d291f | 260 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 261 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 262 | move(); |
59010050 | 3:98ef5105926e | 263 | } |
59010050 | 3:98ef5105926e | 264 | |
59010050 | 3:98ef5105926e | 265 | void getvalue() { |
59010050 | 3:98ef5105926e | 266 | pc.printf("case 1 = 90-15 \n"); |
59010050 | 3:98ef5105926e | 267 | pc.printf("case 2 = 90-30 \n"); |
59010050 | 3:98ef5105926e | 268 | pc.printf("case 3 = 90-45 \n"); |
59010050 | 3:98ef5105926e | 269 | pc.printf("case 4 = 90-60 \n"); |
59010050 | 3:98ef5105926e | 270 | pc.printf("case 5 = 80-45 \n"); |
59010050 | 3:98ef5105926e | 271 | pc.printf("case 6 = 85-45 \n"); |
59010050 | 3:98ef5105926e | 272 | pc.printf("case 7 = 95-45 \n"); |
59010050 | 3:98ef5105926e | 273 | pc.printf("case 8 = 100-45 \n"); |
59010050 | 3:98ef5105926e | 274 | value = pc.getc() ; |
59010050 | 3:98ef5105926e | 275 | switch (value) { |
59010050 | 3:98ef5105926e | 276 | case '1': { |
59010050 | 3:98ef5105926e | 277 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 278 | up_degree = 15.00 ; |
59010050 | 3:98ef5105926e | 279 | break; |
59010050 | 3:98ef5105926e | 280 | } |
59010050 | 3:98ef5105926e | 281 | case '2': { |
59010050 | 3:98ef5105926e | 282 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 283 | up_degree = 30.00 ; |
59010050 | 3:98ef5105926e | 284 | break; |
59010050 | 3:98ef5105926e | 285 | } |
59010050 | 3:98ef5105926e | 286 | case '3': { |
59010050 | 3:98ef5105926e | 287 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 288 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 289 | break; |
59010050 | 3:98ef5105926e | 290 | } |
59010050 | 3:98ef5105926e | 291 | case '4': { |
59010050 | 3:98ef5105926e | 292 | down_degree = 90.00 ; |
59010050 | 3:98ef5105926e | 293 | up_degree = 60.00 ; |
59010050 | 3:98ef5105926e | 294 | break; |
59010050 | 3:98ef5105926e | 295 | } |
59010050 | 3:98ef5105926e | 296 | case '5': { |
59010050 | 3:98ef5105926e | 297 | down_degree = 80.00 ; |
59010050 | 3:98ef5105926e | 298 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 299 | break; |
59010050 | 3:98ef5105926e | 300 | } |
59010050 | 3:98ef5105926e | 301 | case '6': { |
59010050 | 3:98ef5105926e | 302 | down_degree = 85.00 ; |
59010050 | 3:98ef5105926e | 303 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 304 | break; |
59010050 | 3:98ef5105926e | 305 | } |
59010050 | 3:98ef5105926e | 306 | case '7': { |
59010050 | 3:98ef5105926e | 307 | down_degree = 95.00 ; |
59010050 | 3:98ef5105926e | 308 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 309 | break; |
59010050 | 3:98ef5105926e | 310 | } |
59010050 | 3:98ef5105926e | 311 | case '8': { |
59010050 | 3:98ef5105926e | 312 | down_degree = 100.00 ; |
59010050 | 3:98ef5105926e | 313 | up_degree = 45.00 ; |
59010050 | 3:98ef5105926e | 314 | break; |
59010050 | 3:98ef5105926e | 315 | } |
59010050 | 3:98ef5105926e | 316 | break; |
59010050 | 3:98ef5105926e | 317 | } |
59010050 | 4:6c8b844d291f | 318 | //pc.printf("%f \n",down_degree ); |
59010050 | 4:6c8b844d291f | 319 | //pc.printf("%f \n",up_degree ); |
59010050 | 3:98ef5105926e | 320 | |
59010050 | 3:98ef5105926e | 321 | } |