Motor_Control_EMG
Dependencies: HIDScope MODSERIAL QEI Servo biquadFilter mbed
Fork of Motor_Control_buttons by
main.cpp@9:cca4d4084775, 2016-10-11 (annotated)
- Committer:
- huismaja
- Date:
- Tue Oct 11 11:00:18 2016 +0000
- Revision:
- 9:cca4d4084775
- Parent:
- 8:9c58ca13076e
- Child:
- 10:cf579c3eaf01
Working encoders for 2 DC-motors --> pulses and angles
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 | 9:cca4d4084775 | 4 | #include "QEI.h" |
huismaja | 0:6c8444d06e97 | 5 | |
huismaja | 9:cca4d4084775 | 6 | QEI encoder_M1 (D9, D10, NC, 8400); |
huismaja | 9:cca4d4084775 | 7 | QEI encoder_M2 (D11, D12, NC, 8400); |
huismaja | 9:cca4d4084775 | 8 | |
huismaja | 9:cca4d4084775 | 9 | Ticker encoder_M1_ticker; |
huismaja | 9:cca4d4084775 | 10 | Ticker encoder_M2_ticker; |
huismaja | 6:98121d2d76a6 | 11 | |
huismaja | 5:9b5edadc023b | 12 | DigitalOut Direction_M1(D4); //To control the rotation direction of the arm |
huismaja | 6:98121d2d76a6 | 13 | PwmOut Speed_M1(D5); //To control the rotation speed of the arm |
huismaja | 6:98121d2d76a6 | 14 | PwmOut Speed_M2(D6); //To control the translation direction of the arm |
huismaja | 5:9b5edadc023b | 15 | DigitalOut Direction_M2(D7); //To control the translation speed of the arm |
huismaja | 1:0d55a4bf2269 | 16 | |
huismaja | 7:9a3809a9ab3e | 17 | Servo gripper_servo(D13); //To control the gripper (Note: D8=PTC12) |
huismaja | 0:6c8444d06e97 | 18 | |
huismaja | 9:cca4d4084775 | 19 | InterruptIn Switch_1(SW2); //To control the rotation to the left |
huismaja | 9:cca4d4084775 | 20 | InterruptIn Switch_2(SW3); //To control the rotation to the right |
huismaja | 7:9a3809a9ab3e | 21 | InterruptIn Switch_3(D2); //To control the translation of the arm |
huismaja | 7:9a3809a9ab3e | 22 | InterruptIn Switch_4(D3); //To control the gripper |
huismaja | 2:b20570f160c6 | 23 | |
huismaja | 5:9b5edadc023b | 24 | int counter_rotation_left=0; //To count the number of times the rotation_left switch (switch_1) has been pushed |
huismaja | 5:9b5edadc023b | 25 | int counter_rotation_right=0; //To count the number of times the rotation_right switch (switch_2) has been pushed |
huismaja | 5:9b5edadc023b | 26 | int counter_translation=0; //To count the number of times the translation switch (switch_3) has been pushed |
huismaja | 5:9b5edadc023b | 27 | int counter_gripper=0; //To count the number of times the gripper switch (switch_4) has been pushed |
huismaja | 5:9b5edadc023b | 28 | |
huismaja | 5:9b5edadc023b | 29 | MODSERIAL pc(USBTX, USBRX); //To make connection with the PC |
huismaja | 0:6c8444d06e97 | 30 | |
huismaja | 8:9c58ca13076e | 31 | const double pi = 3.1415926535897; |
huismaja | 9:cca4d4084775 | 32 | |
huismaja | 8:9c58ca13076e | 33 | double speed_rotation=pi/5; //in rad/sec |
huismaja | 8:9c58ca13076e | 34 | double speed_translation=pi/5; //in rad/sec |
huismaja | 8:9c58ca13076e | 35 | double speedM1=speed_rotation/8.4; |
huismaja | 8:9c58ca13076e | 36 | double speedM2=speed_translation/8.4; |
huismaja | 6:98121d2d76a6 | 37 | |
huismaja | 9:cca4d4084775 | 38 | void read_position_M1(){ |
huismaja | 9:cca4d4084775 | 39 | int pulses_M1 = encoder_M1.getPulses(); |
huismaja | 9:cca4d4084775 | 40 | float angle_M1 = float(pulses_M1)/4200*2.0*pi; |
huismaja | 9:cca4d4084775 | 41 | pc.printf("%i \t%f \t", pulses_M1, angle_M1); |
huismaja | 9:cca4d4084775 | 42 | } |
huismaja | 9:cca4d4084775 | 43 | |
huismaja | 9:cca4d4084775 | 44 | void read_position_M2(){ |
huismaja | 9:cca4d4084775 | 45 | int pulses_M2 = encoder_M2.getPulses(); |
huismaja | 9:cca4d4084775 | 46 | float angle_M2 = float(pulses_M2)/4200*2.0*pi; |
huismaja | 9:cca4d4084775 | 47 | pc.printf("%i \t%f \n", pulses_M2, angle_M2); |
huismaja | 9:cca4d4084775 | 48 | } |
huismaja | 9:cca4d4084775 | 49 | |
huismaja | 3:0a4bfcb3f339 | 50 | void rotation_left (){ |
huismaja | 3:0a4bfcb3f339 | 51 | switch (counter_rotation_left){ |
huismaja | 5:9b5edadc023b | 52 | case 1: //For activating the rotation to the left |
huismaja | 5:9b5edadc023b | 53 | Direction_M1 = 1; //The arm will rotate to the left |
huismaja | 8:9c58ca13076e | 54 | Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec |
huismaja | 8:9c58ca13076e | 55 | pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1); |
huismaja | 3:0a4bfcb3f339 | 56 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 57 | break; |
huismaja | 5:9b5edadc023b | 58 | case 2: //For stopping the rotation to the left |
huismaja | 5:9b5edadc023b | 59 | Direction_M1 = 1; //The arm will rotate to the left |
huismaja | 5:9b5edadc023b | 60 | Speed_M1 = 0; //The motor is turned off |
huismaja | 5:9b5edadc023b | 61 | pc.printf("The arm will now stop rotating to the left \n"); |
huismaja | 3:0a4bfcb3f339 | 62 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 63 | break; |
huismaja | 3:0a4bfcb3f339 | 64 | } |
huismaja | 3:0a4bfcb3f339 | 65 | } |
huismaja | 3:0a4bfcb3f339 | 66 | |
huismaja | 5:9b5edadc023b | 67 | void switch_counter_rotation_left (){ //To count the number of times the rotation_left switch (switch_1) has been pushed |
huismaja | 3:0a4bfcb3f339 | 68 | counter_rotation_left++; |
huismaja | 3:0a4bfcb3f339 | 69 | if (counter_rotation_left > 2){ |
huismaja | 3:0a4bfcb3f339 | 70 | counter_rotation_left=1; |
huismaja | 3:0a4bfcb3f339 | 71 | } |
huismaja | 3:0a4bfcb3f339 | 72 | rotation_left(); |
huismaja | 3:0a4bfcb3f339 | 73 | } |
huismaja | 3:0a4bfcb3f339 | 74 | |
huismaja | 3:0a4bfcb3f339 | 75 | void rotation_right (){ |
huismaja | 3:0a4bfcb3f339 | 76 | switch (counter_rotation_right){ |
huismaja | 5:9b5edadc023b | 77 | case 1: //For activation the rotation to the right |
huismaja | 5:9b5edadc023b | 78 | Direction_M1 = 0; //The arm will rotate to the right |
huismaja | 8:9c58ca13076e | 79 | Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec |
huismaja | 8:9c58ca13076e | 80 | pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1); |
huismaja | 3:0a4bfcb3f339 | 81 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 82 | break; |
huismaja | 5:9b5edadc023b | 83 | case 2: //For stopping the rotation to the right |
huismaja | 5:9b5edadc023b | 84 | Direction_M1 = 0; //The arm will rotate to the right |
huismaja | 5:9b5edadc023b | 85 | Speed_M1 = 0; //The motor is turned off |
huismaja | 5:9b5edadc023b | 86 | pc.printf("The arm will now stop rotating to the right \n"); |
huismaja | 3:0a4bfcb3f339 | 87 | wait(0.5f); |
huismaja | 3:0a4bfcb3f339 | 88 | break; |
huismaja | 3:0a4bfcb3f339 | 89 | } |
huismaja | 3:0a4bfcb3f339 | 90 | } |
huismaja | 3:0a4bfcb3f339 | 91 | |
huismaja | 5:9b5edadc023b | 92 | void switch_counter_rotation_right (){ //To count the number of times the rotation_right switch (switch_2) has been pushed |
huismaja | 3:0a4bfcb3f339 | 93 | counter_rotation_right++; |
huismaja | 3:0a4bfcb3f339 | 94 | if (counter_rotation_right> 2){ |
huismaja | 3:0a4bfcb3f339 | 95 | counter_rotation_right=1; |
huismaja | 3:0a4bfcb3f339 | 96 | } |
huismaja | 3:0a4bfcb3f339 | 97 | rotation_right(); |
huismaja | 3:0a4bfcb3f339 | 98 | } |
huismaja | 3:0a4bfcb3f339 | 99 | |
huismaja | 5:9b5edadc023b | 100 | void translation (){ |
huismaja | 5:9b5edadc023b | 101 | switch (counter_translation){ |
huismaja | 8:9c58ca13076e | 102 | case 1: //For activating the elongation of the arm |
huismaja | 8:9c58ca13076e | 103 | Direction_M2 = 1; //The arm will get longer |
huismaja | 8:9c58ca13076e | 104 | Speed_M2 = speedM2; //The motor is turned on at speed_rotation rad/sec |
huismaja | 5:9b5edadc023b | 105 | pc.printf("The arm will now get longer \n"); |
huismaja | 5:9b5edadc023b | 106 | wait(0.5f); |
huismaja | 5:9b5edadc023b | 107 | break; |
huismaja | 8:9c58ca13076e | 108 | case 2: //For stopping the elongation of the arm |
huismaja | 8:9c58ca13076e | 109 | Direction_M2 = 1; //The arm will get longer |
huismaja | 8:9c58ca13076e | 110 | Speed_M2 = 0; //The motor is turned off |
huismaja | 5:9b5edadc023b | 111 | pc.printf("The arm will now stop getting longer \n"); |
huismaja | 5:9b5edadc023b | 112 | wait(0.5f); |
huismaja | 5:9b5edadc023b | 113 | break; |
huismaja | 8:9c58ca13076e | 114 | case 3: //For activating the shortening of the arm |
huismaja | 8:9c58ca13076e | 115 | Direction_M2 = 0; //The arm will get shorter |
huismaja | 8:9c58ca13076e | 116 | Speed_M2 = speedM2; //The motor is turned on at speed_rotation rad/sec |
huismaja | 5:9b5edadc023b | 117 | pc.printf("The arm will now get shorter \n"); |
huismaja | 5:9b5edadc023b | 118 | wait(0.5f); |
huismaja | 5:9b5edadc023b | 119 | break; |
huismaja | 8:9c58ca13076e | 120 | case 4: //For stopping the shortening of the arm |
huismaja | 8:9c58ca13076e | 121 | Direction_M2 = 0; //The arm will get shorter |
huismaja | 8:9c58ca13076e | 122 | Speed_M2 = 0; //The motor is turned off |
huismaja | 5:9b5edadc023b | 123 | pc.printf("The arm will now stop getting shorter \n"); |
huismaja | 5:9b5edadc023b | 124 | wait(0.5f); |
huismaja | 5:9b5edadc023b | 125 | break; |
huismaja | 5:9b5edadc023b | 126 | } |
huismaja | 5:9b5edadc023b | 127 | } |
huismaja | 5:9b5edadc023b | 128 | |
huismaja | 5:9b5edadc023b | 129 | void switch_counter_translation (){ //To count the number of times the translation switch (switch_3) has been pushed |
huismaja | 5:9b5edadc023b | 130 | counter_translation++; |
huismaja | 5:9b5edadc023b | 131 | if (counter_translation > 4){ |
huismaja | 5:9b5edadc023b | 132 | counter_translation=1; |
huismaja | 5:9b5edadc023b | 133 | } |
huismaja | 5:9b5edadc023b | 134 | translation(); |
huismaja | 5:9b5edadc023b | 135 | } |
huismaja | 5:9b5edadc023b | 136 | |
huismaja | 4:84bd5ead83f9 | 137 | void gripper (){ |
huismaja | 4:84bd5ead83f9 | 138 | switch (counter_gripper){ |
huismaja | 4:84bd5ead83f9 | 139 | case 1: |
huismaja | 5:9b5edadc023b | 140 | gripper_servo = 0; //The gripper is now closed |
huismaja | 5:9b5edadc023b | 141 | pc.printf("The gripper will now close \n"); |
huismaja | 4:84bd5ead83f9 | 142 | wait(0.5f); |
huismaja | 4:84bd5ead83f9 | 143 | break; |
huismaja | 4:84bd5ead83f9 | 144 | case 2: |
huismaja | 5:9b5edadc023b | 145 | gripper_servo = 1; //The gripper is now open |
huismaja | 5:9b5edadc023b | 146 | pc.printf("The gripper will now open \n"); |
huismaja | 4:84bd5ead83f9 | 147 | wait(0.5f); |
huismaja | 4:84bd5ead83f9 | 148 | break; |
huismaja | 4:84bd5ead83f9 | 149 | } |
huismaja | 4:84bd5ead83f9 | 150 | } |
huismaja | 4:84bd5ead83f9 | 151 | |
huismaja | 5:9b5edadc023b | 152 | void switch_counter_gripper (){ //To count the number of times the gripper switch (switch_4) has been pushed |
huismaja | 4:84bd5ead83f9 | 153 | counter_gripper++; |
huismaja | 4:84bd5ead83f9 | 154 | if (counter_gripper> 2){ |
huismaja | 4:84bd5ead83f9 | 155 | counter_gripper=1; |
huismaja | 4:84bd5ead83f9 | 156 | } |
huismaja | 4:84bd5ead83f9 | 157 | gripper(); |
huismaja | 4:84bd5ead83f9 | 158 | } |
huismaja | 4:84bd5ead83f9 | 159 | |
huismaja | 0:6c8444d06e97 | 160 | int main(){ |
huismaja | 0:6c8444d06e97 | 161 | pc.baud(115200); |
huismaja | 0:6c8444d06e97 | 162 | pc.printf("RESET \n"); |
huismaja | 1:0d55a4bf2269 | 163 | |
huismaja | 5:9b5edadc023b | 164 | Direction_M1 = 1; //The arm will initially get longer |
huismaja | 5:9b5edadc023b | 165 | Speed_M1 = 0; //The first motor is initially turned off |
huismaja | 5:9b5edadc023b | 166 | Direction_M2 = 255; //The arm will initially turn left |
huismaja | 6:98121d2d76a6 | 167 | Speed_M2 = 0; //The second motor is initially turned off |
huismaja | 5:9b5edadc023b | 168 | gripper_servo = 1; //The gripper is initially open |
huismaja | 9:cca4d4084775 | 169 | encoder_M1.reset(); |
huismaja | 9:cca4d4084775 | 170 | encoder_M2.reset(); |
huismaja | 9:cca4d4084775 | 171 | encoder_M1_ticker.attach(&read_position_M1,0.5); |
huismaja | 9:cca4d4084775 | 172 | encoder_M2_ticker.attach(&read_position_M2,0.5); |
huismaja | 1:0d55a4bf2269 | 173 | |
huismaja | 5:9b5edadc023b | 174 | Switch_1.rise(&switch_counter_rotation_left); |
huismaja | 5:9b5edadc023b | 175 | Switch_2.rise(&switch_counter_rotation_right); |
huismaja | 5:9b5edadc023b | 176 | Switch_3.rise(&switch_counter_translation); |
huismaja | 5:9b5edadc023b | 177 | Switch_4.rise(&switch_counter_gripper); |
huismaja | 5:9b5edadc023b | 178 | |
huismaja | 5:9b5edadc023b | 179 | while (true); |
huismaja | 0:6c8444d06e97 | 180 | } |