Script for controlling 2 DC-motors and a gripper-servo using buttons
Dependencies: MODSERIAL QEI Servo mbed
Diff: main.cpp
- Revision:
- 5:9b5edadc023b
- Parent:
- 4:84bd5ead83f9
- Child:
- 6:98121d2d76a6
--- a/main.cpp Fri Oct 07 18:29:28 2016 +0000 +++ b/main.cpp Sat Oct 08 15:34:37 2016 +0000 @@ -2,79 +2,43 @@ #include "MODSERIAL.h" #include "Servo.h" -DigitalOut Direcion_M1(D4); -DigitalOut Speed_M1(D5); -DigitalOut Speed_M2(D6); -DigitalOut Direction_M2(D7); - -Servo Gripper_servo(A3); +DigitalOut Direction_M1(D4); //To control the rotation direction of the arm +DigitalOut Speed_M1(D5); //To control the rotation speed of the arm +DigitalOut Speed_M2(D6); //To control the translation direction of the arm +DigitalOut Direction_M2(D7); //To control the translation speed of the arm -InterruptIn Switch_1(D8); //To control the translation of the arm -InterruptIn Switch_2(D9); //To control the rotation to the left -InterruptIn Switch_3(D10); //To control the rotation to the right -InterruptIn Switch_4(D11); //To control the gripper -int counter_translation=1; -int counter_rotation_left=1; -int counter_rotation_right=1; -int counter_gripper=1; - -MODSERIAL pc(USBTX, USBRX); +Servo gripper_servo(D3); //To control the gripper (Note: doesn't work on D8, unknown why) -void translation (){ - switch (counter_translation){ - case 1: - digitalWrite(Direction_M1, 1); //The arm will get longer - analogWrite(Speed_M1, 255); //The motor is turned on - pc.printf("The arm will now get longer"); - wait(0.5f); - break; - case 2: - digitalWrite(Direction_M1, 1); //The arm will get longer - analogWrite(Speed_M1, 0); //The motor is turned off - pc.printf("The arm will now stop"); - wait(0.5f); - break; - case 3: - digitalWrite(Direction_M1, 0); //The arm will get shorter - analogWrite(Speed_M1, 255); //The motor is turned off - pc.printf("The arm will now get shorter"); - wait(0.5f); - break; - case 4: - digitalWrite(Direction_M1, 0); //The arm will get shorter - analogWrite(Speed_M1, 0); //The motor is turned off - pc.printf("The arm will now stop"); - wait(0.5f); - break; - } -} +InterruptIn Switch_1(D9); //To control the rotation to the left +InterruptIn Switch_2(D10); //To control the rotation to the right +InterruptIn Switch_3(D11); //To control the translation of the arm +InterruptIn Switch_4(D12); //To control the gripper -void switch_counter_translation (){ - counter_translation++; - if (counter_translation > 4){ - counter_translation=1; - } - translation(); -} +int counter_rotation_left=0; //To count the number of times the rotation_left switch (switch_1) has been pushed +int counter_rotation_right=0; //To count the number of times the rotation_right switch (switch_2) has been pushed +int counter_translation=0; //To count the number of times the translation switch (switch_3) has been pushed +int counter_gripper=0; //To count the number of times the gripper switch (switch_4) has been pushed + +MODSERIAL pc(USBTX, USBRX); //To make connection with the PC void rotation_left (){ switch (counter_rotation_left){ - case 1: - digitalWrite(Direction_M2, 1); //The arm will rotate to the left - analogWrite(Speed_M2, 255); //The motor is turned on - pc.printf("The arm will now rotate to the left"); + case 1: //For activating the rotation to the left + Direction_M1 = 1; //The arm will rotate to the left + Speed_M1 = 1; //The motor is turned on + pc.printf("The arm will now rotate to the left \n"); wait(0.5f); break; - case 2: - digitalWrite(Direction_M2, 1); //The arm will rotate to the left - analogWrite(Speed_M2, 0); //The motor is turned off - pc.printf("The arm will now stop"); + case 2: //For stopping the rotation to the left + Direction_M1 = 1; //The arm will rotate to the left + Speed_M1 = 0; //The motor is turned off + pc.printf("The arm will now stop rotating to the left \n"); wait(0.5f); break; } } -void switch_counter_rotation_left (){ +void switch_counter_rotation_left (){ //To count the number of times the rotation_left switch (switch_1) has been pushed counter_rotation_left++; if (counter_rotation_left > 2){ counter_rotation_left=1; @@ -84,22 +48,22 @@ void rotation_right (){ switch (counter_rotation_right){ - case 1: - digitalWrite(Direction_M2, 0); //The arm will rotate to the right - analogWrite(Speed_M2, 255); //The motor is turned on - pc.printf("The arm will now rotate to the right"); + case 1: //For activation the rotation to the right + Direction_M1 = 0; //The arm will rotate to the right + Speed_M1 = 1; //The motor is turned on + pc.printf("The arm will now rotate to the right \n"); wait(0.5f); break; - case 2: - digitalWrite(Direction_M2, 0); //The arm will rotate to the right - analogWrite(Speed_M2, 0); //The motor is turned off - pc.printf("The arm will now stop"); + case 2: //For stopping the rotation to the right + Direction_M1 = 0; //The arm will rotate to the right + Speed_M1 = 0; //The motor is turned off + pc.printf("The arm will now stop rotating to the right \n"); wait(0.5f); break; } } -void switch_counter_rotation_right (){ +void switch_counter_rotation_right (){ //To count the number of times the rotation_right switch (switch_2) has been pushed counter_rotation_right++; if (counter_rotation_right> 2){ counter_rotation_right=1; @@ -107,22 +71,59 @@ rotation_right(); } +void translation (){ + switch (counter_translation){ + case 1: //For activating the elongation of the arm + Direction_M2 = 1; //The arm will get longer + Speed_M2 = 1; //The motor is turned on + pc.printf("The arm will now get longer \n"); + wait(0.5f); + break; + case 2: //For stopping the elongation of the arm + Direction_M2 = 1; //The arm will get longer + Speed_M2 = 0; //The motor is turned off + pc.printf("The arm will now stop getting longer \n"); + wait(0.5f); + break; + case 3: //For activating the shortening of the arm + Direction_M2 = 0; //The arm will get shorter + Speed_M2 = 1; //The motor is turned off + pc.printf("The arm will now get shorter \n"); + wait(0.5f); + break; + case 4: //For stopping the shortening of the arm + Direction_M2 = 0; //The arm will get shorter + Speed_M2 = 0; //The motor is turned off + pc.printf("The arm will now stop getting shorter \n"); + wait(0.5f); + break; + } +} + +void switch_counter_translation (){ //To count the number of times the translation switch (switch_3) has been pushed + counter_translation++; + if (counter_translation > 4){ + counter_translation=1; + } + translation(); +} + void gripper (){ switch (counter_gripper){ case 1: - Gripper_servo(0); //The gripper is now closed - pc.printf("The gripper will now close"); + gripper_servo = 0; //The gripper is now closed + pc.printf("The gripper will now close \n"); wait(0.5f); break; case 2: - Gripper_servo(0.5); //The gripper is now closed - pc.printf("The gripper will now open"); + gripper_servo = 1; //The gripper is now open + pc.printf("The gripper will now open \n"); wait(0.5f); break; } } -void switch_counter_gripper (){ +void switch_counter_gripper (){ //To count the number of times the gripper switch (switch_4) has been pushed counter_gripper++; if (counter_gripper> 2){ counter_gripper=1; @@ -134,14 +135,16 @@ pc.baud(115200); pc.printf("RESET \n"); - digitalWrite(Direction_M1, 1); //The arm will initially get longer - analogWrite(Speed_M1, 0); //The motor is initially turned off - digitalWrite(Direction_M1, 1); //The arm will initially get longer - analogWrite(Speed_M1, 0); //The motor is initially turned off - Gripper_servo(0.5); //The gripper is initially open + Direction_M1 = 1; //The arm will initially get longer + Speed_M1 = 0; //The first motor is initially turned off + Direction_M2 = 255; //The arm will initially turn left + Speed_M1 = 0; //The second motor is initially turned off + gripper_servo = 1; //The gripper is initially open - switch_1.rise(&switch_counter_translation); - switch_2.rise(&switch_counter_rotation_left); - switch_3.rise(&switch_counter_rotation_right); - switch_4.rise(&switch_counter_gripper); + Switch_1.rise(&switch_counter_rotation_left); + Switch_2.rise(&switch_counter_rotation_right); + Switch_3.rise(&switch_counter_translation); + Switch_4.rise(&switch_counter_gripper); + + while (true); } \ No newline at end of file