Script for controlling 2 DC-motors and a gripper-servo using buttons
Dependencies: MODSERIAL QEI Servo mbed
Diff: main.cpp
- Revision:
- 4:84bd5ead83f9
- Parent:
- 3:0a4bfcb3f339
- Child:
- 5:9b5edadc023b
--- a/main.cpp Fri Oct 07 17:48:34 2016 +0000 +++ b/main.cpp Fri Oct 07 18:29:28 2016 +0000 @@ -1,22 +1,27 @@ #include "mbed.h" -#include "MODSERIAL.h' +#include "MODSERIAL.h" +#include "Servo.h" DigitalOut Direcion_M1(D4); DigitalOut Speed_M1(D5); -//DigitalOut Speed_M2(D6); -//DigitalOut Direction_M2(D7); +DigitalOut Speed_M2(D6); +DigitalOut Direction_M2(D7); + +Servo Gripper_servo(A3); -InterruptIn Switch_1(D8); -InterruptIn Switch_2(D9); -InterruptIn Switch_3(D10); -int counter_extension=1; +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); -void extension (){ - switch (counter_extension){ +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 @@ -44,25 +49,25 @@ } } -void switch_counter_extension (){ - counter_extension++; - if (counter_extension > 4){ - counter_extension=1; +void switch_counter_translation (){ + counter_translation++; + if (counter_translation > 4){ + counter_translation=1; } - extension(); + translation(); } void rotation_left (){ switch (counter_rotation_left){ case 1: - digitalWrite(Direction_M1, 1); //The arm will rotate to the left - analogWrite(Speed_M1, 255); //The motor is turned on + 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"); wait(0.5f); break; case 2: - digitalWrite(Direction_M1, 1); //The arm will rotate to the left - analogWrite(Speed_M1, 0); //The motor is turned off + 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"); wait(0.5f); break; @@ -80,14 +85,14 @@ void rotation_right (){ switch (counter_rotation_right){ case 1: - digitalWrite(Direction_M1, 0); //The arm will rotate to the right - analogWrite(Speed_M1, 255); //The motor is turned on + 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"); wait(0.5f); break; case 2: - digitalWrite(Direction_M1, 0); //The arm will rotate to the right - analogWrite(Speed_M1, 0); //The motor is turned off + 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"); wait(0.5f); break; @@ -102,6 +107,29 @@ rotation_right(); } +void gripper (){ + switch (counter_gripper){ + case 1: + Gripper_servo(0); //The gripper is now closed + pc.printf("The gripper will now close"); + wait(0.5f); + break; + case 2: + Gripper_servo(0.5); //The gripper is now closed + pc.printf("The gripper will now open"); + wait(0.5f); + break; + } +} + +void switch_counter_gripper (){ + counter_gripper++; + if (counter_gripper> 2){ + counter_gripper=1; + } + gripper(); +} + int main(){ pc.baud(115200); pc.printf("RESET \n"); @@ -110,8 +138,10 @@ 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 - switch_1.rise(&switch_counter_extension); + 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); } \ No newline at end of file