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

Dependencies:   MODSERIAL QEI Servo mbed

Committer:
huismaja
Date:
Fri Oct 07 17:48:34 2016 +0000
Revision:
3:0a4bfcb3f339
Parent:
2:b20570f160c6
Child:
4:84bd5ead83f9
Control of 2 DC-motors for extension and for rotation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
huismaja 0:6c8444d06e97 1 #include "mbed.h"
huismaja 0:6c8444d06e97 2 #include "MODSERIAL.h'
huismaja 0:6c8444d06e97 3
huismaja 1:0d55a4bf2269 4 DigitalOut Direcion_M1(D4);
huismaja 1:0d55a4bf2269 5 DigitalOut Speed_M1(D5);
huismaja 1:0d55a4bf2269 6 //DigitalOut Speed_M2(D6);
huismaja 1:0d55a4bf2269 7 //DigitalOut Direction_M2(D7);
huismaja 1:0d55a4bf2269 8
huismaja 2:b20570f160c6 9 InterruptIn Switch_1(D8);
huismaja 3:0a4bfcb3f339 10 InterruptIn Switch_2(D9);
huismaja 3:0a4bfcb3f339 11 InterruptIn Switch_3(D10);
huismaja 2:b20570f160c6 12 int counter_extension=1;
huismaja 3:0a4bfcb3f339 13 int counter_rotation_left=1;
huismaja 3:0a4bfcb3f339 14 int counter_rotation_right=1;
huismaja 2:b20570f160c6 15
huismaja 2:b20570f160c6 16 MODSERIAL pc(USBTX, USBRX);
huismaja 0:6c8444d06e97 17
huismaja 2:b20570f160c6 18 void extension (){
huismaja 2:b20570f160c6 19 switch (counter_extension){
huismaja 2:b20570f160c6 20 case 1:
huismaja 2:b20570f160c6 21 digitalWrite(Direction_M1, 1); //The arm will get longer
huismaja 2:b20570f160c6 22 analogWrite(Speed_M1, 255); //The motor is turned on
huismaja 2:b20570f160c6 23 pc.printf("The arm will now get longer");
huismaja 2:b20570f160c6 24 wait(0.5f);
huismaja 2:b20570f160c6 25 break;
huismaja 2:b20570f160c6 26 case 2:
huismaja 2:b20570f160c6 27 digitalWrite(Direction_M1, 1); //The arm will get longer
huismaja 2:b20570f160c6 28 analogWrite(Speed_M1, 0); //The motor is turned off
huismaja 2:b20570f160c6 29 pc.printf("The arm will now stop");
huismaja 2:b20570f160c6 30 wait(0.5f);
huismaja 2:b20570f160c6 31 break;
huismaja 2:b20570f160c6 32 case 3:
huismaja 2:b20570f160c6 33 digitalWrite(Direction_M1, 0); //The arm will get shorter
huismaja 2:b20570f160c6 34 analogWrite(Speed_M1, 255); //The motor is turned off
huismaja 2:b20570f160c6 35 pc.printf("The arm will now get shorter");
huismaja 2:b20570f160c6 36 wait(0.5f);
huismaja 2:b20570f160c6 37 break;
huismaja 2:b20570f160c6 38 case 4:
huismaja 2:b20570f160c6 39 digitalWrite(Direction_M1, 0); //The arm will get shorter
huismaja 2:b20570f160c6 40 analogWrite(Speed_M1, 0); //The motor is turned off
huismaja 2:b20570f160c6 41 pc.printf("The arm will now stop");
huismaja 2:b20570f160c6 42 wait(0.5f);
huismaja 2:b20570f160c6 43 break;
huismaja 2:b20570f160c6 44 }
huismaja 2:b20570f160c6 45 }
huismaja 2:b20570f160c6 46
huismaja 2:b20570f160c6 47 void switch_counter_extension (){
huismaja 2:b20570f160c6 48 counter_extension++;
huismaja 2:b20570f160c6 49 if (counter_extension > 4){
huismaja 2:b20570f160c6 50 counter_extension=1;
huismaja 2:b20570f160c6 51 }
huismaja 2:b20570f160c6 52 extension();
huismaja 2:b20570f160c6 53 }
huismaja 0:6c8444d06e97 54
huismaja 3:0a4bfcb3f339 55 void rotation_left (){
huismaja 3:0a4bfcb3f339 56 switch (counter_rotation_left){
huismaja 3:0a4bfcb3f339 57 case 1:
huismaja 3:0a4bfcb3f339 58 digitalWrite(Direction_M1, 1); //The arm will rotate to the left
huismaja 3:0a4bfcb3f339 59 analogWrite(Speed_M1, 255); //The motor is turned on
huismaja 3:0a4bfcb3f339 60 pc.printf("The arm will now rotate to the left");
huismaja 3:0a4bfcb3f339 61 wait(0.5f);
huismaja 3:0a4bfcb3f339 62 break;
huismaja 3:0a4bfcb3f339 63 case 2:
huismaja 3:0a4bfcb3f339 64 digitalWrite(Direction_M1, 1); //The arm will rotate to the left
huismaja 3:0a4bfcb3f339 65 analogWrite(Speed_M1, 0); //The motor is turned off
huismaja 3:0a4bfcb3f339 66 pc.printf("The arm will now stop");
huismaja 3:0a4bfcb3f339 67 wait(0.5f);
huismaja 3:0a4bfcb3f339 68 break;
huismaja 3:0a4bfcb3f339 69 }
huismaja 3:0a4bfcb3f339 70 }
huismaja 3:0a4bfcb3f339 71
huismaja 3:0a4bfcb3f339 72 void switch_counter_rotation_left (){
huismaja 3:0a4bfcb3f339 73 counter_rotation_left++;
huismaja 3:0a4bfcb3f339 74 if (counter_rotation_left > 2){
huismaja 3:0a4bfcb3f339 75 counter_rotation_left=1;
huismaja 3:0a4bfcb3f339 76 }
huismaja 3:0a4bfcb3f339 77 rotation_left();
huismaja 3:0a4bfcb3f339 78 }
huismaja 3:0a4bfcb3f339 79
huismaja 3:0a4bfcb3f339 80 void rotation_right (){
huismaja 3:0a4bfcb3f339 81 switch (counter_rotation_right){
huismaja 3:0a4bfcb3f339 82 case 1:
huismaja 3:0a4bfcb3f339 83 digitalWrite(Direction_M1, 0); //The arm will rotate to the right
huismaja 3:0a4bfcb3f339 84 analogWrite(Speed_M1, 255); //The motor is turned on
huismaja 3:0a4bfcb3f339 85 pc.printf("The arm will now rotate to the right");
huismaja 3:0a4bfcb3f339 86 wait(0.5f);
huismaja 3:0a4bfcb3f339 87 break;
huismaja 3:0a4bfcb3f339 88 case 2:
huismaja 3:0a4bfcb3f339 89 digitalWrite(Direction_M1, 0); //The arm will rotate to the right
huismaja 3:0a4bfcb3f339 90 analogWrite(Speed_M1, 0); //The motor is turned off
huismaja 3:0a4bfcb3f339 91 pc.printf("The arm will now stop");
huismaja 3:0a4bfcb3f339 92 wait(0.5f);
huismaja 3:0a4bfcb3f339 93 break;
huismaja 3:0a4bfcb3f339 94 }
huismaja 3:0a4bfcb3f339 95 }
huismaja 3:0a4bfcb3f339 96
huismaja 3:0a4bfcb3f339 97 void switch_counter_rotation_right (){
huismaja 3:0a4bfcb3f339 98 counter_rotation_right++;
huismaja 3:0a4bfcb3f339 99 if (counter_rotation_right> 2){
huismaja 3:0a4bfcb3f339 100 counter_rotation_right=1;
huismaja 3:0a4bfcb3f339 101 }
huismaja 3:0a4bfcb3f339 102 rotation_right();
huismaja 3:0a4bfcb3f339 103 }
huismaja 3:0a4bfcb3f339 104
huismaja 0:6c8444d06e97 105 int main(){
huismaja 0:6c8444d06e97 106 pc.baud(115200);
huismaja 0:6c8444d06e97 107 pc.printf("RESET \n");
huismaja 1:0d55a4bf2269 108
huismaja 1:0d55a4bf2269 109 digitalWrite(Direction_M1, 1); //The arm will initially get longer
huismaja 1:0d55a4bf2269 110 analogWrite(Speed_M1, 0); //The motor is initially turned off
huismaja 3:0a4bfcb3f339 111 digitalWrite(Direction_M1, 1); //The arm will initially get longer
huismaja 3:0a4bfcb3f339 112 analogWrite(Speed_M1, 0); //The motor is initially turned off
huismaja 1:0d55a4bf2269 113
huismaja 2:b20570f160c6 114 switch_1.rise(&switch_counter_extension);
huismaja 3:0a4bfcb3f339 115 switch_2.rise(&switch_counter_rotation_left);
huismaja 3:0a4bfcb3f339 116 switch_3.rise(&switch_counter_rotation_right);
huismaja 0:6c8444d06e97 117 }