Janet Huisman / Mbed 2 deprecated Motor_Control_buttons

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

Dependencies:   MODSERIAL QEI Servo mbed

main.cpp@8:9c58ca13076e, 2016-10-10 (annotated)

Committer:
huismaja
Date:
Mon Oct 10 12:09:49 2016 +0000
Revision:
8:9c58ca13076e
Parent:
7:9a3809a9ab3e
Child:
9:cca4d4084775
Working script with variable speed for the DC-motors

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 8:9c58ca13076e 5 DigitalIn Encoder_M1A(D9);
huismaja 8:9c58ca13076e 6 DigitalIn Encoder_M1B(D10);
huismaja 8:9c58ca13076e 7 DigitalIn Encoder_M2A(D11);
huismaja 8:9c58ca13076e 8 DigitalIn Encoder_M2B(D12);
huismaja 6:98121d2d76a6 9
huismaja 5:9b5edadc023b 10 DigitalOut Direction_M1(D4); //To control the rotation direction of the arm
huismaja 6:98121d2d76a6 11 PwmOut Speed_M1(D5); //To control the rotation speed of the arm
huismaja 6:98121d2d76a6 12 PwmOut Speed_M2(D6); //To control the translation direction of the arm
huismaja 5:9b5edadc023b 13 DigitalOut Direction_M2(D7); //To control the translation speed of the arm
huismaja 1:0d55a4bf2269 14
huismaja 7:9a3809a9ab3e 15 Servo gripper_servo(D13); //To control the gripper (Note: D8=PTC12)
huismaja 0:6c8444d06e97 16
huismaja 7:9a3809a9ab3e 17 InterruptIn Switch_1(SW2); //To control the rotation to the left
huismaja 7:9a3809a9ab3e 18 InterruptIn Switch_2(SW3); //To control the rotation to the right
huismaja 7:9a3809a9ab3e 19 InterruptIn Switch_3(D2); //To control the translation of the arm
huismaja 7:9a3809a9ab3e 20 InterruptIn Switch_4(D3); //To control the gripper
huismaja 2:b20570f160c6 21
huismaja 5:9b5edadc023b 22 int counter_rotation_left=0; //To count the number of times the rotation_left switch (switch_1) has been pushed
huismaja 5:9b5edadc023b 23 int counter_rotation_right=0; //To count the number of times the rotation_right switch (switch_2) has been pushed
huismaja 5:9b5edadc023b 24 int counter_translation=0; //To count the number of times the translation switch (switch_3) has been pushed
huismaja 5:9b5edadc023b 25 int counter_gripper=0; //To count the number of times the gripper switch (switch_4) has been pushed
huismaja 5:9b5edadc023b 26
huismaja 5:9b5edadc023b 27 MODSERIAL pc(USBTX, USBRX); //To make connection with the PC
huismaja 0:6c8444d06e97 28
huismaja 8:9c58ca13076e 29 const double pi = 3.1415926535897;
huismaja 8:9c58ca13076e 30 double speed_rotation=pi/5; //in rad/sec
huismaja 8:9c58ca13076e 31 double speed_translation=pi/5; //in rad/sec
huismaja 8:9c58ca13076e 32 double speedM1=speed_rotation/8.4;
huismaja 8:9c58ca13076e 33 double speedM2=speed_translation/8.4;
huismaja 6:98121d2d76a6 34
huismaja 3:0a4bfcb3f339 35 void rotation_left (){
huismaja 3:0a4bfcb3f339 36 switch (counter_rotation_left){
huismaja 5:9b5edadc023b 37 case 1: //For activating the rotation to the left
huismaja 5:9b5edadc023b 38 Direction_M1 = 1; //The arm will rotate to the left
huismaja 8:9c58ca13076e 39 Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
huismaja 8:9c58ca13076e 40 pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1);
huismaja 3:0a4bfcb3f339 41 wait(0.5f);
huismaja 3:0a4bfcb3f339 42 break;
huismaja 5:9b5edadc023b 43 case 2: //For stopping the rotation to the left
huismaja 5:9b5edadc023b 44 Direction_M1 = 1; //The arm will rotate to the left
huismaja 5:9b5edadc023b 45 Speed_M1 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 46 pc.printf("The arm will now stop rotating to the left \n");
huismaja 3:0a4bfcb3f339 47 wait(0.5f);
huismaja 3:0a4bfcb3f339 48 break;
huismaja 3:0a4bfcb3f339 49 }
huismaja 3:0a4bfcb3f339 50 }
huismaja 3:0a4bfcb3f339 51
huismaja 5:9b5edadc023b 52 void switch_counter_rotation_left (){ //To count the number of times the rotation_left switch (switch_1) has been pushed
huismaja 3:0a4bfcb3f339 53 counter_rotation_left++;
huismaja 3:0a4bfcb3f339 54 if (counter_rotation_left > 2){
huismaja 3:0a4bfcb3f339 55 counter_rotation_left=1;
huismaja 3:0a4bfcb3f339 56 }
huismaja 3:0a4bfcb3f339 57 rotation_left();
huismaja 3:0a4bfcb3f339 58 }
huismaja 3:0a4bfcb3f339 59
huismaja 3:0a4bfcb3f339 60 void rotation_right (){
huismaja 3:0a4bfcb3f339 61 switch (counter_rotation_right){
huismaja 5:9b5edadc023b 62 case 1: //For activation the rotation to the right
huismaja 5:9b5edadc023b 63 Direction_M1 = 0; //The arm will rotate to the right
huismaja 8:9c58ca13076e 64 Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
huismaja 8:9c58ca13076e 65 pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1);
huismaja 3:0a4bfcb3f339 66 wait(0.5f);
huismaja 3:0a4bfcb3f339 67 break;
huismaja 5:9b5edadc023b 68 case 2: //For stopping the rotation to the right
huismaja 5:9b5edadc023b 69 Direction_M1 = 0; //The arm will rotate to the right
huismaja 5:9b5edadc023b 70 Speed_M1 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 71 pc.printf("The arm will now stop rotating to the right \n");
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 5:9b5edadc023b 77 void switch_counter_rotation_right (){ //To count the number of times the rotation_right switch (switch_2) has been pushed
huismaja 3:0a4bfcb3f339 78 counter_rotation_right++;
huismaja 3:0a4bfcb3f339 79 if (counter_rotation_right> 2){
huismaja 3:0a4bfcb3f339 80 counter_rotation_right=1;
huismaja 3:0a4bfcb3f339 81 }
huismaja 3:0a4bfcb3f339 82 rotation_right();
huismaja 3:0a4bfcb3f339 83 }
huismaja 3:0a4bfcb3f339 84
huismaja 5:9b5edadc023b 85 void translation (){
huismaja 5:9b5edadc023b 86 switch (counter_translation){
huismaja 8:9c58ca13076e 87 case 1: //For activating the elongation of the arm
huismaja 8:9c58ca13076e 88 Direction_M2 = 1; //The arm will get longer
huismaja 8:9c58ca13076e 89 Speed_M2 = speedM2; //The motor is turned on at speed_rotation rad/sec
huismaja 5:9b5edadc023b 90 pc.printf("The arm will now get longer \n");
huismaja 5:9b5edadc023b 91 wait(0.5f);
huismaja 5:9b5edadc023b 92 break;
huismaja 8:9c58ca13076e 93 case 2: //For stopping the elongation of the arm
huismaja 8:9c58ca13076e 94 Direction_M2 = 1; //The arm will get longer
huismaja 8:9c58ca13076e 95 Speed_M2 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 96 pc.printf("The arm will now stop getting longer \n");
huismaja 5:9b5edadc023b 97 wait(0.5f);
huismaja 5:9b5edadc023b 98 break;
huismaja 8:9c58ca13076e 99 case 3: //For activating the shortening of the arm
huismaja 8:9c58ca13076e 100 Direction_M2 = 0; //The arm will get shorter
huismaja 8:9c58ca13076e 101 Speed_M2 = speedM2; //The motor is turned on at speed_rotation rad/sec
huismaja 5:9b5edadc023b 102 pc.printf("The arm will now get shorter \n");
huismaja 5:9b5edadc023b 103 wait(0.5f);
huismaja 5:9b5edadc023b 104 break;
huismaja 8:9c58ca13076e 105 case 4: //For stopping the shortening of the arm
huismaja 8:9c58ca13076e 106 Direction_M2 = 0; //The arm will get shorter
huismaja 8:9c58ca13076e 107 Speed_M2 = 0; //The motor is turned off
huismaja 5:9b5edadc023b 108 pc.printf("The arm will now stop getting shorter \n");
huismaja 5:9b5edadc023b 109 wait(0.5f);
huismaja 5:9b5edadc023b 110 break;
huismaja 5:9b5edadc023b 111 }
huismaja 5:9b5edadc023b 112 }
huismaja 5:9b5edadc023b 113
huismaja 5:9b5edadc023b 114 void switch_counter_translation (){ //To count the number of times the translation switch (switch_3) has been pushed
huismaja 5:9b5edadc023b 115 counter_translation++;
huismaja 5:9b5edadc023b 116 if (counter_translation > 4){
huismaja 5:9b5edadc023b 117 counter_translation=1;
huismaja 5:9b5edadc023b 118 }
huismaja 5:9b5edadc023b 119 translation();
huismaja 5:9b5edadc023b 120 }
huismaja 5:9b5edadc023b 121
huismaja 4:84bd5ead83f9 122 void gripper (){
huismaja 4:84bd5ead83f9 123 switch (counter_gripper){
huismaja 4:84bd5ead83f9 124 case 1:
huismaja 5:9b5edadc023b 125 gripper_servo = 0; //The gripper is now closed
huismaja 5:9b5edadc023b 126 pc.printf("The gripper will now close \n");
huismaja 4:84bd5ead83f9 127 wait(0.5f);
huismaja 4:84bd5ead83f9 128 break;
huismaja 4:84bd5ead83f9 129 case 2:
huismaja 5:9b5edadc023b 130 gripper_servo = 1; //The gripper is now open
huismaja 5:9b5edadc023b 131 pc.printf("The gripper will now open \n");
huismaja 4:84bd5ead83f9 132 wait(0.5f);
huismaja 4:84bd5ead83f9 133 break;
huismaja 4:84bd5ead83f9 134 }
huismaja 4:84bd5ead83f9 135 }
huismaja 4:84bd5ead83f9 136
huismaja 5:9b5edadc023b 137 void switch_counter_gripper (){ //To count the number of times the gripper switch (switch_4) has been pushed
huismaja 4:84bd5ead83f9 138 counter_gripper++;
huismaja 4:84bd5ead83f9 139 if (counter_gripper> 2){
huismaja 4:84bd5ead83f9 140 counter_gripper=1;
huismaja 4:84bd5ead83f9 141 }
huismaja 4:84bd5ead83f9 142 gripper();
huismaja 4:84bd5ead83f9 143 }
huismaja 4:84bd5ead83f9 144
huismaja 0:6c8444d06e97 145 int main(){
huismaja 0:6c8444d06e97 146 pc.baud(115200);
huismaja 0:6c8444d06e97 147 pc.printf("RESET \n");
huismaja 1:0d55a4bf2269 148
huismaja 5:9b5edadc023b 149 Direction_M1 = 1; //The arm will initially get longer
huismaja 5:9b5edadc023b 150 Speed_M1 = 0; //The first motor is initially turned off
huismaja 5:9b5edadc023b 151 Direction_M2 = 255; //The arm will initially turn left
huismaja 6:98121d2d76a6 152 Speed_M2 = 0; //The second motor is initially turned off
huismaja 5:9b5edadc023b 153 gripper_servo = 1; //The gripper is initially open
huismaja 1:0d55a4bf2269 154
huismaja 5:9b5edadc023b 155 Switch_1.rise(&switch_counter_rotation_left);
huismaja 5:9b5edadc023b 156 Switch_2.rise(&switch_counter_rotation_right);
huismaja 5:9b5edadc023b 157 Switch_3.rise(&switch_counter_translation);
huismaja 5:9b5edadc023b 158 Switch_4.rise(&switch_counter_gripper);
huismaja 5:9b5edadc023b 159
huismaja 5:9b5edadc023b 160 while (true);
huismaja 0:6c8444d06e97 161 }