Janet Huisman
Script for controlling 2 DC-motors and a gripper-servo using buttons
Dependencies: MODSERIAL QEI Servo mbed
Diff: main.cpp
- Revision:
- 12:35a81d6c6505
- Parent:
- 11:b1ad5267a6bd
- Child:
- 13:ea065d364277
--- a/main.cpp Mon Oct 17 13:01:00 2016 +0000
+++ b/main.cpp Wed Oct 19 11:54:55 2016 +0000
@@ -9,10 +9,10 @@
Ticker encoder_M1_ticker; //Create a ticker for reading the encoder data of encoder_M1
Ticker encoder_M2_ticker; //Create a ticker for reading the encoder data of encoder_M2
-DigitalOut Direction_M1(D4); //To control the rotation direction of the arm
-PwmOut Speed_M1(D5); //To control the rotation speed of the arm
-PwmOut Speed_M2(D6); //To control the translation direction of the arm
-DigitalOut Direction_M2(D7); //To control the translation speed of the arm
+DigitalOut Direction_M2(D4); //To control the rotation direction of the arm
+PwmOut Speed_M2(D5); //To control the rotation speed of the arm
+PwmOut Speed_M1(D6); //To control the translation direction of the arm
+DigitalOut Direction_M1(D7); //To control the translation speed of the arm
Servo gripper_servo(D13); //To control the gripper
InterruptIn Switch_1(SW2); //Switch 1 to control the rotation to the left
@@ -39,16 +39,19 @@
double speedM1=speed_rotation/8.4; //Map the rotation speed from (0-8.4) to (0-1) by dividing by 8.4
double speedM2=speed_translation/8.4; //Map the translation speed from (0-8.4) to (0-1) by dividing by 8.4
+float angle_M1=0;
+float angle_M2=0;
+
void read_position_M1(){ //Function to read the position of motor 1
- int pulses_M1 = encoder_M1.getPulses(); //Read the encoder data and store it in pulses_M1
- float angle_M1 = float(pulses_M1)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
-// pc.printf("%i \t%f \t", pulses_M1, angle_M1);
+ int pulses_M1 = -encoder_M1.getPulses(); //Read the encoder data and store it in pulses_M1
+ angle_M1 = float(pulses_M1)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
+ pc.printf("%i \t%f \t", pulses_M1, angle_M1);
}
void read_position_M2(){ //Function to read the position of motor 2
- int pulses_M2 = encoder_M2.getPulses(); //Read the encoder data and store it in pulses_M2
- float angle_M2 = float(pulses_M2)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
-// pc.printf("%i \t%f \n", pulses_M2, angle_M2);
+ int pulses_M2 = -encoder_M2.getPulses(); //Read the encoder data and store it in pulses_M2
+ angle_M2 = float(pulses_M2)/4200*2.0*pi; //Calculate the angle that corresponds with the measured encoder pulses
+ pc.printf("%i \t%f \n", pulses_M2, angle_M2);
}
void activate_rotation_left (){ //To activate the rotation_left
@@ -62,16 +65,16 @@
void rotation_left (){ //Function to control the rotation to the left
switch (counter_rotation_left){ //Create a switch statement
case 1: //For activating the rotation to the left
- Direction_M1 = 1; //The arm will rotate to the left
+ Direction_M1 = 0; //The arm will rotate to the left
Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1);
- wait(0.5f);
+ wait(0.1f);
break;
case 2: //For stopping the rotation to the left
- Direction_M1 = 1; //The arm will rotate to the left
+ Direction_M1 = 0; //The arm will rotate to the left
Speed_M1 = 0; //The motor is turned off
pc.printf("The arm will now stop rotating to the left \n");
- wait(0.5f);
+ wait(0.1f);
break;
}
}
@@ -87,16 +90,16 @@
void rotation_right (){ //Function to control the rotation to the left
switch (counter_rotation_right){ //Create a switch statement
case 1: //For activation the rotation to the right
- Direction_M1 = 0; //The arm will rotate to the right
+ Direction_M1 = 1; //The arm will rotate to the right
Speed_M1 = speedM1; //The motor is turned on at speed_rotation rad/sec
pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1);
- wait(0.5f);
+ wait(0.1f);
break;
case 2: //For stopping the rotation to the right
- Direction_M1 = 0; //The arm will rotate to the right
+ Direction_M1 = 1; //The arm will rotate to the right
Speed_M1 = 0; //The motor is turned off
pc.printf("The arm will now stop rotating to the right \n");
- wait(0.5f);
+ wait(0.1f);
break;
}
}
@@ -115,25 +118,25 @@
Direction_M2 = 1; //The arm will get longer
Speed_M2 = speedM2; //The motor is turned on at speed_translation rad/sec
pc.printf("The arm will now get longer \n");
- wait(0.5f);
+ wait(0.1f);
break;
case 2: //For stopping the elongation of the arm
Direction_M2 = 1; //The arm will get longer
Speed_M2 = 0; //The motor is turned off
pc.printf("The arm will now stop getting longer \n");
- wait(0.5f);
+ wait(0.1f);
break;
case 3: //For activating the shortening of the arm
Direction_M2 = 0; //The arm will get shorter
Speed_M2 = speedM2; //The motor is turned on at speed_translation rad/sec
pc.printf("The arm will now get shorter \n");
- wait(0.5f);
+ wait(0.1f);
break;
case 4: //For stopping the shortening of the arm
Direction_M2 = 0; //The arm will get shorter
Speed_M2 = 0; //The motor is turned off
pc.printf("The arm will now stop getting shorter \n");
- wait(0.5f);
+ wait(0.1f);
break;
}
}
@@ -151,12 +154,12 @@
case 1: //For closing the gripper
gripper_servo = 0; //The gripper is now closed
pc.printf("The gripper will now close \n");
- wait(0.5f);
+ wait(0.1f);
break;
case 2: //For opening the gripper
gripper_servo = 1; //The gripper is now open
pc.printf("The gripper will now open \n");
- wait(0.5f);
+ wait(0.1f);
break;
}
}
@@ -182,21 +185,21 @@
Switch_4.rise(&activate_gripper); //Use switch_4 to activate the counter_gripper go-flag
while (true){
- if (rotation_left_go == true){ //If the rotation_left go-flag is true
- rotation_left_go = false; //Set the rotation_left go-flag to false
- rotation_left(); //Execute the rotation_left function
+ if (rotation_left_go == true) { //If the rotation_left go-flag is true
+ rotation_left_go = false; //Set the rotation_left go-flag to false
+ rotation_left(); //Execute the rotation_left function
}
- if (rotation_right_go == true){ //If the rotation_right go-flag is true
- rotation_right_go = false; //Set the rotation_right go-flag to false
- rotation_right(); //Execute the rotation_right function
+ if (rotation_right_go == true) { //If the rotation_right go-flag is true
+ rotation_right_go = false; //Set the rotation_right go-flag to false
+ rotation_right(); //Execute the rotation_right function
}
- if (translation_go == true){ //If the translation go-flag is true
- translation_go = false; //Set the translation go-flag to false
- translation(); //Execute the translation function
+ if (translation_go == true) { //If the translation go-flag is true
+ translation_go = false; //Set the translation go-flag to false
+ translation(); //Execute the translation function
}
- if (gripper_go == true){ //If the gripper go-flag is true
- gripper_go = false; //Set the gripper go-flag to false
- gripper(); //Execute the gripper function
+ if (gripper_go == true) { //If the gripper go-flag is true
+ gripper_go = false; //Set the gripper go-flag to false
+ gripper(); //Execute the gripper function
}
}
}
\ No newline at end of file