Script for controlling 2 DC-motors and a gripper-servo using buttons

Dependencies:   MODSERIAL QEI Servo mbed

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?

UserRevisionLine numberNew 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 }