Meike Froklage
Robot
Dependencies: HIDScope MODSERIAL Motordriver QEI Servo mbed
Fork of
The_Claw_with_EMG_Control_PID
by 
Meike Froklage
Diff: main.cpp
- Revision:
- 6:23b1ed826b59
- Parent:
- 5:3d88f7506cd9
- Child:
- 7:9715323b20ac
--- a/main.cpp Fri Oct 28 09:01:41 2016 +0000
+++ b/main.cpp Mon Oct 31 13:59:48 2016 +0000
@@ -17,8 +17,8 @@
Motor Arm(D6,D7, D7, Brakeable); // left motor
// qei
-QEI Encoder_Cart(D10, D11, NC, 64);
-QEI Encoder_Arm(D12, D13, NC, 64);
+QEI Encoder_Cart(D10, D11, NC, 6400);
+QEI Encoder_Arm(D12, D13, NC, 6400);
// servo
Servo servo(D9);
@@ -55,10 +55,8 @@
int num_claw_turned_on_2 = 0; // count number of times blue LED turned on
// speed
-double cart_speed = 0.5;
-double cart_stop = 0.2;
-double arm_speed = 0.3;
-double arm_stop = 0.1;
+double cart_speed = 0.3;
+double arm_speed = 0.1;
// position
float factor_cart = 0.06559;
@@ -68,6 +66,9 @@
float ain_cart; //Variable to store the analog input of the cart
float ain_arm; //Variable to store the analog input of the arm
+int position_claw;
+
+
// miscellaneous
const float kTimeToggle = 0.25f; // period with which to toggle the parts
const int LedOn = 0; // LED on if 0
@@ -75,7 +76,6 @@
volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
-
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//======== Functions and main ==============================================================
@@ -95,10 +95,16 @@
}else if (btn && !btn2) {
if(position_cart <= -105){ //If the cart is at the right side, it stops
- Cart.speed(-cart_stop)==-cart_stop;
+ Cart.stop(1)==1;
+
+ }else if(position_cart <= -90){ //If the cart is at 90 mm at the right, it slows down
+ Cart.speed(0.2)==0.2;
- }else if(position_cart >= 105 && position_arm <=-60){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
- Cart.speed(cart_stop) == cart_stop;
+ }else if(position_cart >= 105 && position_arm <=-45){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
+ Cart.stop(1) == 1;
+
+ }else if(position_cart >= 105 && position_arm <=-35 && position_claw == -18){
+ Cart.stop(1) == 1;
}else{
Cart.speed(cart_speed)==cart_speed;
@@ -106,18 +112,31 @@
}else if (!btn && btn2) {
if(position_cart >= 105){ //If the cart is at the left side, it stops
- Cart.speed(cart_stop)==cart_stop;
+ Cart.stop(1)==1;
+
+ }else if(position_cart >= 90){ //If the cart is at the right side, it stops
+ Cart.speed(-0.2)==-0.2;
- }else if(position_cart <= -105 && position_arm >=60){ //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
- Cart.speed(-cart_stop) == -cart_stop;
+ }else if(position_cart <= -105 && position_arm >=45){ //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
+ Cart.stop(1)==1;
+ }else if(position_cart <= -105 && position_arm >=35 && position_claw == 27){
+ Cart.stop(1)==1;
+
}else{
Cart.speed(-cart_speed)==-cart_speed;
}
}else {
Arm.speed(0) == 0;
- Cart.speed(0) == 0;
+ if(position_cart<0){
+ Cart.speed(-0.1)== -0.1;
+ }else if(position_cart>0){
+ Cart.speed(0.1)==0.1;
+ }else{
+ Cart.stop(0)==0;
+ }
+
}
}
// controle LED
@@ -136,7 +155,6 @@
pc.baud(115200);
pc.printf("Distance in mm: %i\n", position_cart);
-
break;
}
@@ -152,30 +170,42 @@
}
else if (btn && !btn2) {
- if(position_cart > -105 && position_arm >= 30){ //If the cart is not at the end, the arm can't move any further than 30 degrees
- Arm.speed(-arm_stop)==-arm_stop;
+ if(position_cart > -105 && position_arm >= 45){ //If the cart is not at the end, the arm can't move any further than 45 degrees
+ Arm.stop(1)==1;
+
+ }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
+ Arm.stop(1)==1;
}else if(position_cart<= -105 && position_arm>=80){ //If the cart is at the right end, the arm can't move any further than 70 degrees
- Arm.speed(-arm_stop)==-arm_stop;
+ Arm.stop(1)==1;
}else{
Arm.speed(arm_speed)==arm_speed;
}
}else if (!btn && btn2) {
- if(position_cart < 105 && position_arm <= -30){ //If the cart is not at the end, the arm can't move any further than 30 degrees
- Arm.speed(arm_stop)==arm_stop;
+ if(position_cart < 105 && position_arm <= -45){ //If the cart is not at the end, the arm can't move any further than 45 degrees
+ Arm.stop(1)==1;
+
+ }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
+ Arm.stop(1)==1;
}else if(position_cart>=105 && position_arm<=-80){ //If the cart is at the left end, the arm can't move any further than 70 degrees
- Arm.speed(arm_stop)==arm_stop;
+ Arm.stop(1)==1;
}else{
Arm.speed(-arm_speed)==-arm_speed;
}
}else {
- Arm.speed(0) == 0;
- Cart.speed(0) == 0;
+ Cart.speed(0) == 0;
+ if(position_arm>0){
+ Arm.speed(-0.1)== -0.1;
+ }else if(position_arm<0){
+ Arm.speed(0.1)==0.1;
+ }else{
+ Arm.stop(0)==0;
+ }
}
}
// controle LED
@@ -194,7 +224,6 @@
pc.baud(115200);
pc.printf("Degrees: %i\n", position_arm);
-
break;
}
@@ -219,7 +248,7 @@
led_b = not LedOn;
led_g = not LedOn;
- servo.position(27);
+ servo.position(-18);
pc.printf("Servo position is: left \r\n");
break;
}
@@ -245,7 +274,7 @@
led_r = not LedOn;
led_b = not LedOn;
- servo.position(-18);
+ servo.position(27);
pc.printf("Servo position is: right \r\n");
break;
}
@@ -265,7 +294,7 @@
led_b = not LedOn;
led_g = not LedOn;
- servo.position(27);
+ servo.position(-18);
pc.printf("Servo position is: left \r\n");
break;
}
@@ -291,7 +320,7 @@
led_r = not LedOn;
led_b = not LedOn;
- servo.position(-18);
+ servo.position(27);
pc.printf("Servo position is: right \r\n");
break;
}
@@ -300,9 +329,10 @@
}else{}
}
led_r = not LedOn;
- led_g = not LedOn;
+ led_g = not LedOn;
-
+ position_claw = servo.read();
+
break;
}
}
@@ -312,11 +342,11 @@
// Switch the part
void SetValue2() {
part_id = 2;
- }
+}
void SetValue3() {
part_id = 3;
- }
+}
void SetValue4() {