Meike Froklage
/
The_Claw_Back-up_Buttons
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MODSERIAL Motordriver QEI Servo mbed
Fork of
The_Claw
by 
Meike Froklage
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MODSERIAL.h" 00003 #define SERIAL_BAUD 115200 00004 #include "motordriver.h" 00005 #include "QEI.h" 00006 #include "Servo.h" 00007 00008 //======== Serial Communication ================================================ 00009 MODSERIAL pc(USBTX,USBRX); 00010 00011 //======== Motor and QEI ======================================================= 00012 int Brakeable; 00013 int sign; 00014 00015 // motor 00016 Motor Cart(D5, D4, D4, Brakeable); // right motor 00017 Motor Arm(D6,D7, D7, Brakeable); // left motor 00018 00019 // qei 00020 QEI Encoder_Cart(D10, D11, NC, 6400); 00021 QEI Encoder_Arm(D12, D13, NC, 6400); 00022 00023 // servo 00024 Servo servo(D9); 00025 00026 //======== Miscellaneous ======================================================= 00027 // button 00028 InterruptIn btn(SW2); 00029 InterruptIn btn2(SW3); 00030 00031 InterruptIn btn_cart(D1); 00032 InterruptIn btn_arm(D2); 00033 InterruptIn btn_claw(D3); 00034 00035 // led 00036 DigitalOut led_r(LED_RED); 00037 DigitalOut led_g(LED_GREEN); 00038 DigitalOut led_b(LED_BLUE); 00039 00040 // potmeter 00041 AnalogIn pot_cart(A2); 00042 AnalogIn pot_arm(A3); 00043 00044 // ticker 00045 Ticker tick_part; // ticker to switch parts 00046 00047 //======== Variables =========================================================== 00048 // counters 00049 int num_turned_on_0 = 0; // count number of times red LED turned on 00050 int num_turned_on_1 = 0; // count number of times green LED turned on 00051 int num_turned_on_2 = 0; // count number of times blue LED turned on 00052 00053 int num_claw_turned_on_0 = 0; // count number of times red LED turned on 00054 int num_claw_turned_on_1 = 0; // count number of times green LED turned on 00055 int num_claw_turned_on_2 = 0; // count number of times blue LED turned on 00056 00057 // speed 00058 double cart_speed = 0.3; 00059 double arm_speed = 0.1; 00060 00061 // position 00062 float factor_cart = 0.05890; 00063 float factor_arm = 0.1539; 00064 int position_cart; 00065 int position_arm; 00066 float ain_cart; //Variable to store the analog input of the cart 00067 float ain_arm; //Variable to store the analog input of the arm 00068 00069 int position_claw; 00070 00071 00072 // miscellaneous 00073 const float kTimeToggle = 0.25f; // period with which to toggle the parts 00074 const int LedOn = 0; // LED on if 0 00075 volatile int part_id = 2; // ID of what part should move, begins with the cart 00076 volatile int servo_id = 1; // ID to the side the servo should move, begins in center position 00077 00078 00079 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 00080 //======== Functions and main ============================================================== 00081 00082 // Switch between Cart, Arm and Claw 00083 void SwitchPart() 00084 { 00085 switch (part_id) { 00086 //Cart 00087 case 2: { 00088 led_r = LedOn; 00089 if(led_r == LedOn){ 00090 num_turned_on_0++; 00091 00092 if(btn && btn2) { 00093 Arm.speed(0) == 0; 00094 Cart.speed(0) == 0; 00095 00096 }else if (btn && !btn2) { 00097 if(position_cart <= -115){ //If the cart is at the right side, it stops 00098 Cart.stop(1)==1; 00099 00100 }else if(position_cart <= -90){ //If the cart is at 90 mm at the right, it slows down 00101 Cart.speed(0.2)==0.2; 00102 00103 }else if(position_cart >= 115 && position_arm <=-45){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right. 00104 Cart.stop(1) == 1; 00105 00106 }else if(position_cart >= 115 && position_arm <=-35 && position_claw == -18){ 00107 Cart.stop(1) == 1; 00108 00109 }else{ 00110 Cart.speed(cart_speed)==cart_speed; 00111 } 00112 00113 }else if (!btn && btn2) { 00114 if(position_cart >= 115){ //If the cart is at the left side, it stops 00115 Cart.stop(1)==1; 00116 00117 }else if(position_cart >= 90){ //If the cart is at the right side, it stops 00118 Cart.speed(-0.2)==-0.2; 00119 00120 }else if(position_cart <= -115 && position_arm >=45){ //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left. 00121 Cart.stop(1)==1; 00122 00123 }else if(position_cart <= -115 && position_arm >=35 && position_claw == 27){ 00124 Cart.stop(1)==1; 00125 00126 }else{ 00127 Cart.speed(-cart_speed)==-cart_speed; 00128 } 00129 00130 }else { 00131 Arm.speed(0) == 0; 00132 if(position_cart<0){ 00133 Cart.speed(-0.1)== -0.1; 00134 }else if(position_cart>0){ 00135 Cart.speed(0.1)==0.1; 00136 }else{ 00137 Cart.stop(0)==0; 00138 } 00139 00140 } 00141 } 00142 // controle LED 00143 led_g = not LedOn; 00144 led_b = not LedOn; 00145 00146 // encoder 00147 position_cart = (Encoder_Cart.getPulses()*factor_cart) ; 00148 ain_cart = pot_cart.read(); 00149 00150 if (ain_cart == 0){ 00151 Encoder_Cart.reset(); 00152 }else {} 00153 00154 wait(0.1); 00155 pc.baud(115200); 00156 pc.printf("Distance in mm: %i\n", position_cart); 00157 00158 break; 00159 } 00160 00161 //Arm 00162 case 3: { 00163 led_g = LedOn; 00164 if(led_g == LedOn){ 00165 num_turned_on_1++; 00166 00167 if(btn && btn2) { 00168 Arm.speed(0) == 0; 00169 Cart.speed(0) == 0; 00170 } 00171 00172 else if (btn && !btn2) { 00173 if(position_cart > -105 && position_arm >= 45){ //If the cart is not at the end, the arm can't move any further than 45 degrees 00174 Arm.stop(1)==1; 00175 00176 }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){ 00177 Arm.stop(1)==1; 00178 00179 }else if(position_cart<= -105 && position_arm>=80){ //If the cart is at the right end, the arm can't move any further than 70 degrees 00180 Arm.stop(1)==1; 00181 00182 }else{ 00183 Arm.speed(arm_speed)==arm_speed; 00184 } 00185 00186 }else if (!btn && btn2) { 00187 if(position_cart < 105 && position_arm <= -45){ //If the cart is not at the end, the arm can't move any further than 45 degrees 00188 Arm.stop(1)==1; 00189 00190 }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){ 00191 Arm.stop(1)==1; 00192 00193 }else if(position_cart>=105 && position_arm<=-80){ //If the cart is at the left end, the arm can't move any further than 70 degrees 00194 Arm.stop(1)==1; 00195 00196 }else{ 00197 Arm.speed(-arm_speed)==-arm_speed; 00198 } 00199 00200 }else { 00201 Cart.speed(0) == 0; 00202 if(position_arm>0){ 00203 Arm.speed(-0.1)== -0.1; 00204 }else if(position_arm<0){ 00205 Arm.speed(0.1)==0.1; 00206 }else{ 00207 Arm.stop(0)==0; 00208 } 00209 } 00210 } 00211 // controle LED 00212 led_r = not LedOn; 00213 led_b = not LedOn; 00214 00215 // encoder 00216 position_arm = (Encoder_Arm.getPulses()*factor_arm) ; 00217 ain_arm = pot_arm.read(); 00218 00219 if (ain_arm == 0){ 00220 Encoder_Arm.reset(); 00221 }else {} 00222 00223 wait(0.1); 00224 pc.baud(115200); 00225 pc.printf("Degrees: %i\n", position_arm); 00226 00227 break; 00228 } 00229 00230 //Claw 00231 case 4: { 00232 led_b = LedOn; 00233 if(led_b == LedOn){ 00234 num_turned_on_2++; 00235 00236 if(btn && btn2){ 00237 00238 }else if(btn && !btn2){ 00239 servo_id ++; 00240 00241 switch (servo_id) { 00242 case 0: { 00243 led_r = LedOn; 00244 if (led_r == LedOn) { 00245 num_claw_turned_on_0++; 00246 } 00247 00248 led_b = not LedOn; 00249 led_g = not LedOn; 00250 00251 servo.position(-18); 00252 pc.printf("Servo position is: left \r\n"); 00253 break; 00254 } 00255 case 1: { 00256 led_b = LedOn; 00257 if (led_b == LedOn) { 00258 num_claw_turned_on_1++; 00259 } 00260 00261 led_r = not LedOn; 00262 led_g = not LedOn; 00263 00264 servo.position(3); 00265 pc.printf("Servo position is: center \r\n"); 00266 break; 00267 } 00268 case 2: { 00269 led_g = LedOn; 00270 if (led_g == LedOn) { 00271 num_claw_turned_on_2++; 00272 } 00273 00274 led_r = not LedOn; 00275 led_b = not LedOn; 00276 00277 servo.position(27); 00278 pc.printf("Servo position is: right \r\n"); 00279 break; 00280 } 00281 } 00282 00283 00284 }else if(!btn && btn2){ 00285 servo_id --; 00286 00287 switch (servo_id) { 00288 case 0: { 00289 led_r = LedOn; 00290 if (led_r == LedOn) { 00291 num_claw_turned_on_0++; 00292 } 00293 00294 led_b = not LedOn; 00295 led_g = not LedOn; 00296 00297 servo.position(-18); 00298 pc.printf("Servo position is: left \r\n"); 00299 break; 00300 } 00301 case 1: { 00302 led_b = LedOn; 00303 if (led_b == LedOn) { 00304 num_claw_turned_on_1++; 00305 } 00306 00307 led_r = not LedOn; 00308 led_g = not LedOn; 00309 00310 servo.position(3); 00311 pc.printf("Servo position is: center \r\n"); 00312 break; 00313 } 00314 case 2: { 00315 led_g = LedOn; 00316 if (led_g == LedOn) { 00317 num_claw_turned_on_2++; 00318 } 00319 00320 led_r = not LedOn; 00321 led_b = not LedOn; 00322 00323 servo.position(27); 00324 pc.printf("Servo position is: right \r\n"); 00325 break; 00326 } 00327 } 00328 00329 }else{} 00330 } 00331 led_r = not LedOn; 00332 led_g = not LedOn; 00333 00334 position_claw = servo.read(); 00335 00336 break; 00337 } 00338 } 00339 } 00340 00341 00342 // Switch the part 00343 void SetValue2() { 00344 part_id = 2; 00345 } 00346 00347 void SetValue3() { 00348 part_id = 3; 00349 } 00350 00351 00352 void SetValue4() { 00353 part_id = 4; 00354 } 00355 00356 00357 // Main 00358 int main() 00359 { 00360 led_r = not LedOn; 00361 led_g = not LedOn; 00362 led_b = not LedOn; 00363 00364 tick_part.attach(&SwitchPart,kTimeToggle); 00365 00366 btn_cart.fall(&SetValue2); 00367 btn_arm.fall(&SetValue3); 00368 btn_claw.fall(&SetValue4); 00369 00370 while (true); 00371 } 00372 00373 00374
Generated on Fri Aug 5 2022 19:08:56 by
1.7.2