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:
- 7:9715323b20ac
- Parent:
- 6:23b1ed826b59
- Child:
- 8:ac4e5afbdbcd
--- a/main.cpp Mon Oct 31 13:59:48 2016 +0000
+++ b/main.cpp Wed Nov 02 15:22:16 2016 +0000
@@ -41,8 +41,12 @@
AnalogIn pot_cart(A2);
AnalogIn pot_arm(A3);
+AnalogIn emgl(A1); // labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
+AnalogIn emgr(A0);
+
// ticker
-Ticker tick_part; // ticker to switch parts
+Ticker tick_part; // ticker to switch parts
+Ticker sampleTicker;
//======== Variables ===========================================================
// counters
@@ -72,16 +76,215 @@
// miscellaneous
const float kTimeToggle = 0.25f; // period with which to toggle the parts
const int LedOn = 0; // LED on if 0
-volatile int part_id = 2; // ID of what part should move, begins with the cart
+volatile int part_id = 1; // ID of what part should move, begins with the cart
volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
+//======== Variables Filter ====================================================
+/*coefficients of each filter
+ lno = left tricep notch filter
+ lhf = left tricep high pass filter
+ llf = left tricep lowpass filter
+ same goes for rno etc.
+ */
+double lno_b0 = 0.9911;
+double lno_b1 = -1.6036;
+double lno_b2 = 0.9911;
+double lno_a1 = -1.603;
+double lno_a2 = 0.9822;
+
+double rno_b0 = 0.9911;
+double rno_b1 = -1.6036;
+double rno_b2 = 0.9911;
+double rno_a1 = -1.603;
+double rno_a2 = 0.9822;
+
+double lno2_b0 = 0.9824;
+double lno2_b1 = -0.6071;
+double lno2_b2 = 0.9824;
+double lno2_a1 = -0.6071;
+double lno2_a2 = 0.9647;
+
+double rno2_b0 = 0.9824;
+double rno2_b1 = -0.6071;
+double rno2_b2 = 0.9824;
+double rno2_a1 = -0.6071;
+double rno2_a2 = 0.9647;
+
+double lhf_b0 = 0.9355;
+double lhf_b1 = -1.8711;
+double lhf_b2 = 0.9355;
+double lhf_a1 = -1.8669;
+double lhf_a2 = 0.8752;
+
+double rhf_b0 = 0.9355;
+double rhf_b1 = -1.8711;
+double rhf_b2 = 0.9355;
+double rhf_a1 = -1.8669;
+double rhf_a2 = 0.8752;
+
+
+double llf_b0 = 8.7656e-5;
+double llf_b1 = 1.17531e-4;
+double llf_b2 = 8.7656e-5;
+double llf_a1 = -1.9733;
+double llf_a2 = 0.9737;
+
+double rlf_b0 = 8.7656e-5;
+double rlf_b1 = 1.17531e-4;
+double rlf_b2 = 8.7656e-5;
+double rlf_a1 = -1.9733;
+double rlf_a2 = 0.9737;
+
+
+//starting values of the biquads of the corresponding filters
+double lno_v1 = 0, lno_v2 = 0;
+double lno2_v1 = 0, lno2_v2 = 0;
+double lhf_v1 = 0, lhf_v2 = 0;
+double llf_v1 = 0, llf_v2 = 0;
+
+double rno_v1 = 0, rno_v2 = 0;
+double rno2_v1 = 0, rno2_v2 = 0;
+double rhf_v1 = 0, rhf_v2 = 0;
+double rlf_v1 = 0, rlf_v2 = 0;
+
+/* declaration of the outputs of each biquad.
+the output of the previous biquad is the input for the next biquad.
+so lno_y goes into lhf_y etc.
+*/
+double lno_y;
+double lno2_y;
+double lhf_y;
+double llf_y;
+double lrect_y;
+double rno_y;
+double rno2_y;
+double rhf_y;
+double rlf_y;
+double rrect_y;
+
+// set the threshold value for the filtered signal
+//if the signal exceeds this value the motors will start to rotate
+const double threshold_value_left= 0.08;
+const double threshold_value= 0.08;
+
+
+/* declaration of each biquad
+The coefficients will be filled in later on in void scopeSend
+As said before the input of each biquad is the output of the previous one
+The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
+This is done for both left and right so this makes two chains of 3 biquads */
+
+double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+ const double b1 , const double b2 )
+{
+ double v = u - a1*v1 - a2*v2;
+ double y = b0*v + b1*v1 + b2*v2;
+ v2 = v1;
+ v1 = v;
+ return y;
+}
+
+/* function that calculates the filtered EMG signal from the raw EMG signal.
+So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
+After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
+The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
+
+
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+//======== Functions and main ==============================================================
+/* function that calculates the filtered EMG signal from the raw EMG signal.
+So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
+After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
+The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
+void scopeSend(void){
+ lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
+ lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
+ lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
+ lrect_y = fabs(lhf_y);
+ llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
+ rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
+ rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
+ rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
+ rrect_y = fabs(rhf_y);
+ rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
+ }
+
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//======== Functions and main ==============================================================
// Switch between Cart, Arm and Claw
-void SwitchPart()
-{
+
+void SwitchPart(){
switch (part_id) {
//Cart
case 2: {
@@ -147,196 +350,14 @@
position_cart = (Encoder_Cart.getPulses()*factor_cart) ;
ain_cart = pot_cart.read();
- if (ain_cart == 0){
- Encoder_Cart.reset();
- }else {}
-
wait(0.1);
pc.baud(115200);
pc.printf("Distance in mm: %i\n", position_cart);
break;
}
-
- //Arm
- case 3: {
- led_g = LedOn;
- if(led_g == LedOn){
- num_turned_on_1++;
-
- if(btn && btn2) {
- Arm.speed(0) == 0;
- Cart.speed(0) == 0;
- }
-
- else if (btn && !btn2) {
- 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.stop(1)==1;
-
- }else{
- Arm.speed(arm_speed)==arm_speed;
- }
-
- }else if (!btn && btn2) {
- 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.stop(1)==1;
-
- }else{
- Arm.speed(-arm_speed)==-arm_speed;
- }
-
- }else {
- 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
- led_r = not LedOn;
- led_b = not LedOn;
-
- // encoder
- position_arm = (Encoder_Arm.getPulses()*factor_arm) ;
- ain_arm = pot_arm.read();
-
- if (ain_arm == 0){
- Encoder_Arm.reset();
- }else {}
-
- wait(0.1);
- pc.baud(115200);
- pc.printf("Degrees: %i\n", position_arm);
-
- break;
}
-
- //Claw
- case 4: {
- led_b = LedOn;
- if(led_b == LedOn){
- num_turned_on_2++;
-
- if(btn && btn2){
-
- }else if(btn && !btn2){
- servo_id ++;
-
- switch (servo_id) {
- case 0: {
- led_r = LedOn;
- if (led_r == LedOn) {
- num_claw_turned_on_0++;
- }
-
- led_b = not LedOn;
- led_g = not LedOn;
-
- servo.position(-18);
- pc.printf("Servo position is: left \r\n");
- break;
- }
- case 1: {
- led_b = LedOn;
- if (led_b == LedOn) {
- num_claw_turned_on_1++;
- }
-
- led_r = not LedOn;
- led_g = not LedOn;
-
- servo.position(3);
- pc.printf("Servo position is: center \r\n");
- break;
- }
- case 2: {
- led_g = LedOn;
- if (led_g == LedOn) {
- num_claw_turned_on_2++;
- }
-
- led_r = not LedOn;
- led_b = not LedOn;
-
- servo.position(27);
- pc.printf("Servo position is: right \r\n");
- break;
- }
- }
-
-
- }else if(!btn && btn2){
- servo_id --;
-
- switch (servo_id) {
- case 0: {
- led_r = LedOn;
- if (led_r == LedOn) {
- num_claw_turned_on_0++;
- }
-
- led_b = not LedOn;
- led_g = not LedOn;
-
- servo.position(-18);
- pc.printf("Servo position is: left \r\n");
- break;
- }
- case 1: {
- led_b = LedOn;
- if (led_b == LedOn) {
- num_claw_turned_on_1++;
- }
-
- led_r = not LedOn;
- led_g = not LedOn;
-
- servo.position(3);
- pc.printf("Servo position is: center \r\n");
- break;
- }
- case 2: {
- led_g = LedOn;
- if (led_g == LedOn) {
- num_claw_turned_on_2++;
- }
-
- led_r = not LedOn;
- led_b = not LedOn;
-
- servo.position(27);
- pc.printf("Servo position is: right \r\n");
- break;
- }
- }
-
- }else{}
- }
- led_r = not LedOn;
- led_g = not LedOn;
-
- position_claw = servo.read();
-
- break;
}
- }
-}
// Switch the part
@@ -362,6 +383,7 @@
led_b = not LedOn;
tick_part.attach(&SwitchPart,kTimeToggle);
+ sampleTicker.attach(scopeSend,0.01);
btn_cart.fall(&SetValue2);
btn_arm.fall(&SetValue3);