Script for controlling 2 DC-motors and a gripper-servo using buttons
Dependencies: MODSERIAL QEI Servo mbed
main.cpp@4:84bd5ead83f9, 2016-10-07 (annotated)
- Committer:
- huismaja
- Date:
- Fri Oct 07 18:29:28 2016 +0000
- Revision:
- 4:84bd5ead83f9
- Parent:
- 3:0a4bfcb3f339
- Child:
- 5:9b5edadc023b
Control of 2 DC-motors for translation and rotation and 1 Servo-motor for the gripper
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
huismaja | 0:6c8444d06e97 | 1 | #include "mbed.h" |
huismaja | 4:84bd5ead83f9 | 2 | #include "MODSERIAL.h" |
huismaja | 4:84bd5ead83f9 | 3 | #include "Servo.h" |
huismaja | 0:6c8444d06e97 | 4 | |
huismaja | 1:0d55a4bf2269 | 5 | DigitalOut Direcion_M1(D4); |
huismaja | 1:0d55a4bf2269 | 6 | DigitalOut Speed_M1(D5); |
huismaja | 4:84bd5ead83f9 | 7 | DigitalOut Speed_M2(D6); |
huismaja | 4:84bd5ead83f9 | 8 | DigitalOut Direction_M2(D7); |
huismaja | 4:84bd5ead83f9 | 9 | |
huismaja | 4:84bd5ead83f9 | 10 | Servo Gripper_servo(A3); |
huismaja | 1:0d55a4bf2269 | 11 | |
huismaja | 4:84bd5ead83f9 | 12 | InterruptIn Switch_1(D8); //To control the translation of the arm |
huismaja | 4:84bd5ead83f9 | 13 | InterruptIn Switch_2(D9); //To control the rotation to the left |
huismaja | 4:84bd5ead83f9 | 14 | InterruptIn Switch_3(D10); //To control the rotation to the right |
huismaja | 4:84bd5ead83f9 | 15 | InterruptIn Switch_4(D11); //To control the gripper |
huismaja | 4:84bd5ead83f9 | 16 | int counter_translation=1; |
huismaja | 3:0a4bfcb3f339 | 17 | int counter_rotation_left=1; |
huismaja | 3:0a4bfcb3f339 | 18 | int counter_rotation_right=1; |
huismaja | 4:84bd5ead83f9 | 19 | int counter_gripper=1; |
huismaja | 2:b20570f160c6 | 20 | |
huismaja | 2:b20570f160c6 | 21 | MODSERIAL pc(USBTX, USBRX); |
huismaja | 0:6c8444d06e97 | 22 | |
huismaja | 4:84bd5ead83f9 | 23 | void translation (){ |
huismaja | 4:84bd5ead83f9 | 24 | switch (counter_translation){ |
huismaja | 2:b20570f160c6 | 25 | case 1: |
huismaja | 2:b20570f160c6 | 26 | digitalWrite(Direction_M1, 1); //The arm will get longer |
huismaja | 2:b20570f160c6 | 27 | analogWrite(Speed_M1, 255); //The motor is turned on |
huismaja | 2:b20570f160c6 | 28 | pc.printf("The arm will now get longer"); |
huismaja | 2:b20570f160c6 | 29 | wait(0.5f); |
huismaja | 2:b20570f160c6 | 30 | break; |
huismaja | 2:b20570f160c6 | 31 | case 2: |
huismaja | 2:b20570f160c6 | 32 | digitalWrite(Direction_M1, 1); //The arm will get longer |
huismaja | 2:b20570f160c6 | 33 | analogWrite(Speed_M1, 0); //The motor is turned off |
huismaja | 2:b20570f160c6 | 34 | pc.printf("The arm will now stop"); |
huismaja | 2:b20570f160c6 | 35 | wait(0.5f); |
huismaja | 2:b20570f160c6 | 36 | break; |
huismaja | 2:b20570f160c6 | 37 | case 3: |
huismaja | 2:b20570f160c6 | 38 | digitalWrite(Direction_M1, 0); //The arm will get shorter |
huismaja | 2:b20570f160c6 | 39 | analogWrite(Speed_M1, 255); //The motor is turned off |
huismaja | 2:b20570f160c6 | 40 | pc.printf("The arm will now get shorter"); |
huismaja | 2:b20570f160c6 | 41 | wait(0.5f); |
huismaja | 2:b20570f160c6 | 42 | break; |
huismaja | 2:b20570f160c6 | 43 | case 4: |
huismaja | 2:b20570f160c6 | 44 | digitalWrite(Direction_M1, 0); //The arm will get shorter |
huismaja | 2:b20570f160c6 | 45 | analogWrite(Speed_M1, 0); //The motor is turned off |
huismaja | 2:b20570f160c6 | 46 | pc.printf("The arm will now stop"); |
huismaja | 2:b20570f160c6 | 47 | wait(0.5f); |
huismaja | 2:b20570f160c6 | 48 | break; |
huismaja | 2:b20570f160c6 | 49 | } |
huismaja | 2:b20570f160c6 | 50 | } |
huismaja | 2:b20570f160c6 | 51 | |
huismaja | 4:84bd5ead83f9 | 52 | void switch_counter_translation (){ |
huismaja | 4:84bd5ead83f9 | 53 | counter_translation++; |
huismaja | 4:84bd5ead83f9 | 54 | if (counter_translation > 4){ |
huismaja | 4:84bd5ead83f9 | 55 | counter_translation=1; |
huismaja | 2:b20570f160c6 | 56 | } |
huismaja | 4:84bd5ead83f9 | 57 | translation(); |
huismaja | 2:b20570f160c6 | 58 | } |
huismaja | 0:6c8444d06e97 | 59 | |
huismaja | 3:0a4bfcb3f339 | 60 | void rotation_left (){ |
huismaja | 3:0a4bfcb3f339 | 61 | switch (counter_rotation_left){ |
huismaja | 3:0a4bfcb3f339 | 62 | case 1: |
huismaja | 4:84bd5ead83f9 | 63 | digitalWrite(Direction_M2, 1); //The arm will rotate to the left |
huismaja | 4:84bd5ead83f9 | 64 | analogWrite(Speed_M2, 255); //The motor is turned on |
huismaja | 3:0a4bfcb3f339 | 65 | pc.printf("The arm will now rotate to the left"); |
huismaja | 3:0a4bfcb3f339 | 66 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 67 | break; |
huismaja | 3:0a4bfcb3f339 | 68 | case 2: |
huismaja | 4:84bd5ead83f9 | 69 | digitalWrite(Direction_M2, 1); //The arm will rotate to the left |
huismaja | 4:84bd5ead83f9 | 70 | analogWrite(Speed_M2, 0); //The motor is turned off |
huismaja | 3:0a4bfcb3f339 | 71 | pc.printf("The arm will now stop"); |
huismaja | 3:0a4bfcb3f339 | 72 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 73 | break; |
huismaja | 3:0a4bfcb3f339 | 74 | } |
huismaja | 3:0a4bfcb3f339 | 75 | } |
huismaja | 3:0a4bfcb3f339 | 76 | |
huismaja | 3:0a4bfcb3f339 | 77 | void switch_counter_rotation_left (){ |
huismaja | 3:0a4bfcb3f339 | 78 | counter_rotation_left++; |
huismaja | 3:0a4bfcb3f339 | 79 | if (counter_rotation_left > 2){ |
huismaja | 3:0a4bfcb3f339 | 80 | counter_rotation_left=1; |
huismaja | 3:0a4bfcb3f339 | 81 | } |
huismaja | 3:0a4bfcb3f339 | 82 | rotation_left(); |
huismaja | 3:0a4bfcb3f339 | 83 | } |
huismaja | 3:0a4bfcb3f339 | 84 | |
huismaja | 3:0a4bfcb3f339 | 85 | void rotation_right (){ |
huismaja | 3:0a4bfcb3f339 | 86 | switch (counter_rotation_right){ |
huismaja | 3:0a4bfcb3f339 | 87 | case 1: |
huismaja | 4:84bd5ead83f9 | 88 | digitalWrite(Direction_M2, 0); //The arm will rotate to the right |
huismaja | 4:84bd5ead83f9 | 89 | analogWrite(Speed_M2, 255); //The motor is turned on |
huismaja | 3:0a4bfcb3f339 | 90 | pc.printf("The arm will now rotate to the right"); |
huismaja | 3:0a4bfcb3f339 | 91 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 92 | break; |
huismaja | 3:0a4bfcb3f339 | 93 | case 2: |
huismaja | 4:84bd5ead83f9 | 94 | digitalWrite(Direction_M2, 0); //The arm will rotate to the right |
huismaja | 4:84bd5ead83f9 | 95 | analogWrite(Speed_M2, 0); //The motor is turned off |
huismaja | 3:0a4bfcb3f339 | 96 | pc.printf("The arm will now stop"); |
huismaja | 3:0a4bfcb3f339 | 97 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 98 | break; |
huismaja | 3:0a4bfcb3f339 | 99 | } |
huismaja | 3:0a4bfcb3f339 | 100 | } |
huismaja | 3:0a4bfcb3f339 | 101 | |
huismaja | 3:0a4bfcb3f339 | 102 | void switch_counter_rotation_right (){ |
huismaja | 3:0a4bfcb3f339 | 103 | counter_rotation_right++; |
huismaja | 3:0a4bfcb3f339 | 104 | if (counter_rotation_right> 2){ |
huismaja | 3:0a4bfcb3f339 | 105 | counter_rotation_right=1; |
huismaja | 3:0a4bfcb3f339 | 106 | } |
huismaja | 3:0a4bfcb3f339 | 107 | rotation_right(); |
huismaja | 3:0a4bfcb3f339 | 108 | } |
huismaja | 3:0a4bfcb3f339 | 109 | |
huismaja | 4:84bd5ead83f9 | 110 | void gripper (){ |
huismaja | 4:84bd5ead83f9 | 111 | switch (counter_gripper){ |
huismaja | 4:84bd5ead83f9 | 112 | case 1: |
huismaja | 4:84bd5ead83f9 | 113 | Gripper_servo(0); //The gripper is now closed |
huismaja | 4:84bd5ead83f9 | 114 | pc.printf("The gripper will now close"); |
huismaja | 4:84bd5ead83f9 | 115 | wait(0.5f); |
huismaja | 4:84bd5ead83f9 | 116 | break; |
huismaja | 4:84bd5ead83f9 | 117 | case 2: |
huismaja | 4:84bd5ead83f9 | 118 | Gripper_servo(0.5); //The gripper is now closed |
huismaja | 4:84bd5ead83f9 | 119 | pc.printf("The gripper will now open"); |
huismaja | 4:84bd5ead83f9 | 120 | wait(0.5f); |
huismaja | 4:84bd5ead83f9 | 121 | break; |
huismaja | 4:84bd5ead83f9 | 122 | } |
huismaja | 4:84bd5ead83f9 | 123 | } |
huismaja | 4:84bd5ead83f9 | 124 | |
huismaja | 4:84bd5ead83f9 | 125 | void switch_counter_gripper (){ |
huismaja | 4:84bd5ead83f9 | 126 | counter_gripper++; |
huismaja | 4:84bd5ead83f9 | 127 | if (counter_gripper> 2){ |
huismaja | 4:84bd5ead83f9 | 128 | counter_gripper=1; |
huismaja | 4:84bd5ead83f9 | 129 | } |
huismaja | 4:84bd5ead83f9 | 130 | gripper(); |
huismaja | 4:84bd5ead83f9 | 131 | } |
huismaja | 4:84bd5ead83f9 | 132 | |
huismaja | 0:6c8444d06e97 | 133 | int main(){ |
huismaja | 0:6c8444d06e97 | 134 | pc.baud(115200); |
huismaja | 0:6c8444d06e97 | 135 | pc.printf("RESET \n"); |
huismaja | 1:0d55a4bf2269 | 136 | |
huismaja | 1:0d55a4bf2269 | 137 | digitalWrite(Direction_M1, 1); //The arm will initially get longer |
huismaja | 1:0d55a4bf2269 | 138 | analogWrite(Speed_M1, 0); //The motor is initially turned off |
huismaja | 3:0a4bfcb3f339 | 139 | digitalWrite(Direction_M1, 1); //The arm will initially get longer |
huismaja | 3:0a4bfcb3f339 | 140 | analogWrite(Speed_M1, 0); //The motor is initially turned off |
huismaja | 4:84bd5ead83f9 | 141 | Gripper_servo(0.5); //The gripper is initially open |
huismaja | 1:0d55a4bf2269 | 142 | |
huismaja | 4:84bd5ead83f9 | 143 | switch_1.rise(&switch_counter_translation); |
huismaja | 3:0a4bfcb3f339 | 144 | switch_2.rise(&switch_counter_rotation_left); |
huismaja | 3:0a4bfcb3f339 | 145 | switch_3.rise(&switch_counter_rotation_right); |
huismaja | 4:84bd5ead83f9 | 146 | switch_4.rise(&switch_counter_gripper); |
huismaja | 0:6c8444d06e97 | 147 | } |