Nick van den Berg / Mbed 2 deprecated The_Claw_with_EMG_Control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Nick van den Berg

main.cpp@19:fd67b0c1a246, 2016-11-07 (annotated)

Committer:
Nickname
Date:
Mon Nov 07 12:28:36 2016 +0000
Revision:
19:fd67b0c1a246
Parent:
18:cbe3fe6d89a7 
Final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:048fbd80203e 1 #include "mbed.h"
meikefrok 5:3d88f7506cd9 2 #include "MODSERIAL.h"
meikefrok 5:3d88f7506cd9 3 #define SERIAL_BAUD 115200
meikefrok 3:3a671d01bcb8 4 #include "motordriver.h"
meikefrok 5:3d88f7506cd9 5 #include "QEI.h"
meikefrok 5:3d88f7506cd9 6 #include "Servo.h"
meikefrok 10:31e4c3d71ee6 7 #include "HIDScope.h"
megrootens 0:048fbd80203e 8
meikefrok 5:3d88f7506cd9 9 //======== Serial Communication ================================================
megrootens 0:048fbd80203e 10 MODSERIAL pc(USBTX,USBRX);
megrootens 0:048fbd80203e 11
meikefrok 5:3d88f7506cd9 12 //======== Motor and QEI =======================================================
meikefrok 5:3d88f7506cd9 13 int Brakeable;
meikefrok 5:3d88f7506cd9 14 int sign;
meikefrok 5:3d88f7506cd9 15
meikefrok 5:3d88f7506cd9 16 // motor
Nickname 18:cbe3fe6d89a7 17 Motor Cart(D5, D4, D4, Brakeable); //right motor
Nickname 18:cbe3fe6d89a7 18 Motor Arm(D6,D7, D7, Brakeable); //left motor
meikefrok 5:3d88f7506cd9 19
Nickname 18:cbe3fe6d89a7 20 // QEI
meikefrok 6:23b1ed826b59 21 QEI Encoder_Cart(D10, D11, NC, 6400);
meikefrok 6:23b1ed826b59 22 QEI Encoder_Arm(D12, D13, NC, 6400);
megrootens 0:048fbd80203e 23
meikefrok 5:3d88f7506cd9 24 // servo
meikefrok 5:3d88f7506cd9 25 Servo servo(D9);
megrootens 0:048fbd80203e 26
meikefrok 5:3d88f7506cd9 27 //======== Miscellaneous =======================================================
meikefrok 5:3d88f7506cd9 28 // button
meikefrok 5:3d88f7506cd9 29 InterruptIn btn(SW2);
meikefrok 5:3d88f7506cd9 30 InterruptIn btn2(SW3);
meikefrok 5:3d88f7506cd9 31
meikefrok 5:3d88f7506cd9 32 InterruptIn btn_cart(D1);
meikefrok 5:3d88f7506cd9 33 InterruptIn btn_arm(D2);
meikefrok 5:3d88f7506cd9 34 InterruptIn btn_claw(D3);
meikefrok 5:3d88f7506cd9 35
meikefrok 5:3d88f7506cd9 36 // led
meikefrok 2:ad4b181a6422 37 DigitalOut led_r(LED_RED);
meikefrok 2:ad4b181a6422 38 DigitalOut led_g(LED_GREEN);
meikefrok 2:ad4b181a6422 39 DigitalOut led_b(LED_BLUE);
megrootens 0:048fbd80203e 40
meikefrok 5:3d88f7506cd9 41 // potmeter
meikefrok 5:3d88f7506cd9 42 AnalogIn pot_cart(A2);
meikefrok 5:3d88f7506cd9 43 AnalogIn pot_arm(A3);
megrootens 0:048fbd80203e 44
Nickname 18:cbe3fe6d89a7 45 AnalogIn emgl(A1); //labels are attached to the olimex shields the left tricep
Nickname 18:cbe3fe6d89a7 46 AnalogIn emgr(A0); //should obviously be connected to the shield with an L label
meikefrok 7:9715323b20ac 47
meikefrok 5:3d88f7506cd9 48 // ticker
Nickname 18:cbe3fe6d89a7 49 Ticker tick_part_cart; //ticker to switch to cart part
Nickname 18:cbe3fe6d89a7 50 Ticker tick_part_arm; //ticker to switch to arm part
Nickname 18:cbe3fe6d89a7 51 Ticker tick_part_claw; //ticker to switch to claw part
Nickname 18:cbe3fe6d89a7 52 Ticker sampleTicker; //ticker to sample the EMG signal
Nickname 18:cbe3fe6d89a7 53 Ticker measureTicker; //ticker to read out encoder
Nickname 18:cbe3fe6d89a7 54
Nickname 18:cbe3fe6d89a7 55 HIDScope scope(2); //scope has two ports for the two EMG signals
meikefrok 5:3d88f7506cd9 56
meikefrok 5:3d88f7506cd9 57 //======== Variables ===========================================================
meikefrok 5:3d88f7506cd9 58 // speed
meikefrok 16:3c9a3ff09765 59 double cart_speed = 0.2;
meikefrok 6:23b1ed826b59 60 double arm_speed = 0.1;
megrootens 0:048fbd80203e 61
Nickname 19:fd67b0c1a246 62 // position from encoder
Nickname 19:fd67b0c1a246 63 float factor_cart = 0.05802; //factor to convert impuls to displacement
Nickname 19:fd67b0c1a246 64 float factor_arm = 0.1539; //factor to convert impuls to angular rotation
meikefrok 5:3d88f7506cd9 65 int position_cart;
meikefrok 5:3d88f7506cd9 66 int position_arm;
Nickname 18:cbe3fe6d89a7 67 int position_claw;
meikefrok 5:3d88f7506cd9 68 float ain_cart; //Variable to store the analog input of the cart
meikefrok 5:3d88f7506cd9 69 float ain_arm; //Variable to store the analog input of the arm
megrootens 0:048fbd80203e 70
Nickname 18:cbe3fe6d89a7 71 //calibrate
meikefrok 17:18e9df406502 72 int t = 0;
meikefrok 17:18e9df406502 73 double value_right = 0;
meikefrok 17:18e9df406502 74 double value_left = 0;
meikefrok 17:18e9df406502 75
Nickname 18:cbe3fe6d89a7 76 //P Controller
Nickname 18:cbe3fe6d89a7 77 const double Setpoint_Cart = 0;
Nickname 18:cbe3fe6d89a7 78 const double Setpoint_Arm = 0;
Nickname 18:cbe3fe6d89a7 79 double Arm_ControlSpeed = 0;
Nickname 18:cbe3fe6d89a7 80 double Cart_ControlSpeed = 0;
Nickname 18:cbe3fe6d89a7 81 double SetpointError_Cart = 0;
Nickname 18:cbe3fe6d89a7 82 double SetpointError_Arm = 0;
Nickname 18:cbe3fe6d89a7 83 double setpoint;
Nickname 18:cbe3fe6d89a7 84 //Cart
Nickname 18:cbe3fe6d89a7 85 const double Ts = 0.002; //Ts=1/fs (sample frequency)
Nickname 18:cbe3fe6d89a7 86 const double Cart_Kp = 0.1, Cart_Ki = 0.0, Cart_Kd = 0.0;
Nickname 18:cbe3fe6d89a7 87 double Cart_error = 0;
Nickname 18:cbe3fe6d89a7 88 double Cart_e_prev = 0;
Nickname 18:cbe3fe6d89a7 89 //Arm
Nickname 18:cbe3fe6d89a7 90 const double Arm_Kp = 0.1, Arm_Ki = 0.0, Arm_Kd = 0.0;
Nickname 18:cbe3fe6d89a7 91 double Arm_error = 0;
Nickname 18:cbe3fe6d89a7 92 double Arm_e_prev = 0;
Nickname 18:cbe3fe6d89a7 93
Nickname 18:cbe3fe6d89a7 94
Nickname 18:cbe3fe6d89a7 95 // miscellaneous
Nickname 18:cbe3fe6d89a7 96 const float kTimeToggle = 0.05f; //period with which to toggle the parts
Nickname 18:cbe3fe6d89a7 97 const int LedOn = 0; //LED on if 0
Nickname 18:cbe3fe6d89a7 98 volatile int part_id = 1; //ID of what part should move, begins with cart
Nickname 18:cbe3fe6d89a7 99 volatile int servo_id = 1; //ID to the side the servo moves, begins in center
Nickname 18:cbe3fe6d89a7 100 int waiting_claw = 1000; //Servo delay to make claw control easier
Nickname 18:cbe3fe6d89a7 101
Nickname 18:cbe3fe6d89a7 102 //======== Variables Filter ====================================================
Nickname 18:cbe3fe6d89a7 103 /*coefficients of each filter
Nickname 18:cbe3fe6d89a7 104 lno = left tricep notch filter at 50 Hz
Nickname 18:cbe3fe6d89a7 105 lno2 = left tricep notch filter at 100 Hz
Nickname 18:cbe3fe6d89a7 106 lhf = left tricep high pass filter
Nickname 18:cbe3fe6d89a7 107 llf = left tricep lowpass filter
Nickname 18:cbe3fe6d89a7 108 same goes for rno etc.
Nickname 18:cbe3fe6d89a7 109 */
Nickname 18:cbe3fe6d89a7 110 double lno_b0 = 0.9911; double rno_b0 = 0.9911;
Nickname 18:cbe3fe6d89a7 111 double lno_b1 = -1.6036; double rno_b1 = -1.6036;
Nickname 18:cbe3fe6d89a7 112 double lno_b2 = 0.9911; double rno_b2 = 0.9911;
Nickname 18:cbe3fe6d89a7 113 double lno_a1 = -1.603; double rno_a1 = -1.603;
Nickname 18:cbe3fe6d89a7 114 double lno_a2 = 0.9822; double rno_a2 = 0.9822;
Nickname 18:cbe3fe6d89a7 115
Nickname 18:cbe3fe6d89a7 116 double lno2_b0 = 0.9824; double rno2_b0 = 0.9824;
Nickname 18:cbe3fe6d89a7 117 double lno2_b1 = -0.6071; double rno2_b1 = -0.6071;
Nickname 18:cbe3fe6d89a7 118 double lno2_b2 = 0.9824; double rno2_b2 = 0.9824;
Nickname 18:cbe3fe6d89a7 119 double lno2_a1 = -0.6071; double rno2_a1 = -0.6071;
Nickname 18:cbe3fe6d89a7 120 double lno2_a2 = 0.9647; double rno2_a2 = 0.9647;
Nickname 18:cbe3fe6d89a7 121
Nickname 18:cbe3fe6d89a7 122 double lhf_b0 = 0.9355; double rhf_b0 = 0.9355;
Nickname 18:cbe3fe6d89a7 123 double lhf_b1 = -1.8711; double rhf_b1 = -1.8711;
Nickname 18:cbe3fe6d89a7 124 double lhf_b2 = 0.9355; double rhf_b2 = 0.9355;
Nickname 18:cbe3fe6d89a7 125 double lhf_a1 = -1.8669; double rhf_a1 = -1.8669;
Nickname 18:cbe3fe6d89a7 126 double lhf_a2 = 0.8752; double rhf_a2 = 0.8752;
Nickname 18:cbe3fe6d89a7 127
Nickname 18:cbe3fe6d89a7 128 double llf_b0 = 8.7656e-5; double rlf_b0 = 8.7656e-5;
Nickname 18:cbe3fe6d89a7 129 double llf_b1 = 1.17531e-4; double rlf_b1 = 1.17531e-4;
Nickname 18:cbe3fe6d89a7 130 double llf_b2 = 8.7656e-5; double rlf_b2 = 8.7656e-5;
Nickname 18:cbe3fe6d89a7 131 double llf_a1 = -1.9733; double rlf_a1 = -1.9733;
Nickname 18:cbe3fe6d89a7 132 double llf_a2 = 0.9737; double rlf_a2 = 0.9737;
Nickname 18:cbe3fe6d89a7 133
Nickname 18:cbe3fe6d89a7 134 //starting values of the biquads of the corresponding filters
Nickname 18:cbe3fe6d89a7 135 double lno_v1 = 0, lno_v2 = 0;
Nickname 18:cbe3fe6d89a7 136 double lno2_v1 = 0, lno2_v2 = 0;
Nickname 18:cbe3fe6d89a7 137 double lhf_v1 = 0, lhf_v2 = 0;
Nickname 18:cbe3fe6d89a7 138 double llf_v1 = 0, llf_v2 = 0;
Nickname 18:cbe3fe6d89a7 139
Nickname 18:cbe3fe6d89a7 140 double rno_v1 = 0, rno_v2 = 0;
Nickname 18:cbe3fe6d89a7 141 double rno2_v1 = 0, rno2_v2 = 0;
Nickname 18:cbe3fe6d89a7 142 double rhf_v1 = 0, rhf_v2 = 0;
Nickname 18:cbe3fe6d89a7 143 double rlf_v1 = 0, rlf_v2 = 0;
Nickname 18:cbe3fe6d89a7 144
Nickname 18:cbe3fe6d89a7 145 //declaration of the outputs of each biquad. The output of the previous biquad
Nickname 18:cbe3fe6d89a7 146 //is the input for the next biquad. So lno_y goes into lhf_y etc.
Nickname 18:cbe3fe6d89a7 147 double lno_y; double rno_y;
Nickname 18:cbe3fe6d89a7 148 double lno2_y; double rno2_y;
Nickname 18:cbe3fe6d89a7 149 double lhf_y; double rhf_y;
Nickname 18:cbe3fe6d89a7 150 double llf_y; double rlf_y;
Nickname 18:cbe3fe6d89a7 151 double lrect_y; double rrect_y;
Nickname 18:cbe3fe6d89a7 152
Nickname 18:cbe3fe6d89a7 153 //set the threshold value for the filtered signal
Nickname 18:cbe3fe6d89a7 154 //if the signal exceeds this value the motors will start to rotate
Nickname 19:fd67b0c1a246 155 const double threshold_value = 0.08;
Nickname 18:cbe3fe6d89a7 156
Nickname 18:cbe3fe6d89a7 157 /* declaration of each biquad
Nickname 18:cbe3fe6d89a7 158 The coefficients will be filled in later on in void scopeSend
Nickname 18:cbe3fe6d89a7 159 The input of the first biquad is the raw EMG signal and the output of the last
Nickname 18:cbe3fe6d89a7 160 biquad is the filtered signal. This is done for both left and right so this
Nickname 18:cbe3fe6d89a7 161 makes two chains of 4 biquads */
Nickname 18:cbe3fe6d89a7 162 double biquad_lno(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 163 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 164 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 165 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 166 v2 = v1;
Nickname 18:cbe3fe6d89a7 167 v1 = v;
Nickname 18:cbe3fe6d89a7 168 return y;
Nickname 18:cbe3fe6d89a7 169 }
Nickname 18:cbe3fe6d89a7 170 double biquad_lno2(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 171 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 172 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 173 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 174 v2 = v1;
Nickname 18:cbe3fe6d89a7 175 v1 = v;
Nickname 18:cbe3fe6d89a7 176 return y;
Nickname 18:cbe3fe6d89a7 177 }
Nickname 18:cbe3fe6d89a7 178 double biquad_lhf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 179 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 180 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 181 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 182 v2 = v1;
Nickname 18:cbe3fe6d89a7 183 v1 = v;
Nickname 18:cbe3fe6d89a7 184 return y;
Nickname 18:cbe3fe6d89a7 185 }
Nickname 18:cbe3fe6d89a7 186 double biquad_llf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 187 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 188 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 189 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 190 v2 = v1;
Nickname 18:cbe3fe6d89a7 191 v1 = v;
Nickname 18:cbe3fe6d89a7 192 return y;
Nickname 18:cbe3fe6d89a7 193 }
Nickname 18:cbe3fe6d89a7 194 double biquad_rno(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 195 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 196 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 197 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 198 v2 = v1;
Nickname 18:cbe3fe6d89a7 199 v1 = v;
Nickname 18:cbe3fe6d89a7 200 return y;
Nickname 18:cbe3fe6d89a7 201 }
Nickname 18:cbe3fe6d89a7 202 double biquad_rno2(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 203 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 204 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 205 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 206 v2 = v1;
Nickname 18:cbe3fe6d89a7 207 v1 = v;
Nickname 18:cbe3fe6d89a7 208 return y;
Nickname 18:cbe3fe6d89a7 209 }
Nickname 18:cbe3fe6d89a7 210 double biquad_rhf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 211 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 212 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 213 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 214 v2 = v1;
Nickname 18:cbe3fe6d89a7 215 v1 = v;
Nickname 18:cbe3fe6d89a7 216 return y;
Nickname 18:cbe3fe6d89a7 217 }
Nickname 18:cbe3fe6d89a7 218 double biquad_rlf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 219 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 220 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 221 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 222 v2 = v1;
Nickname 18:cbe3fe6d89a7 223 v1 = v;
Nickname 18:cbe3fe6d89a7 224 return y;
Nickname 18:cbe3fe6d89a7 225 }
Nickname 18:cbe3fe6d89a7 226
Nickname 18:cbe3fe6d89a7 227 //calibration to determine maximum EMG value
meikefrok 17:18e9df406502 228 double calibrate_right(){
meikefrok 17:18e9df406502 229 for(t=0; t<200; t++){
meikefrok 17:18e9df406502 230 if(value_right <= rlf_y){
Nickname 18:cbe3fe6d89a7 231 value_right = rlf_y;
meikefrok 17:18e9df406502 232 }else{
Nickname 18:cbe3fe6d89a7 233 value_right = value_right +0.0;
Nickname 18:cbe3fe6d89a7 234 }}
meikefrok 17:18e9df406502 235 return value_right;
meikefrok 17:18e9df406502 236 }
meikefrok 17:18e9df406502 237 double calibrate_left(){
meikefrok 17:18e9df406502 238 for(t=0; t<200; t++){
meikefrok 17:18e9df406502 239 if(value_left <= llf_y){
meikefrok 17:18e9df406502 240 value_left = llf_y;
meikefrok 17:18e9df406502 241 }else{
Nickname 18:cbe3fe6d89a7 242 value_left = value_left +0.0;
Nickname 18:cbe3fe6d89a7 243 }}
meikefrok 17:18e9df406502 244 return value_left;
meikefrok 17:18e9df406502 245 }
meikefrok 17:18e9df406502 246
Nickname 18:cbe3fe6d89a7 247 //======== P Controller ========================================================
Nickname 18:cbe3fe6d89a7 248 double p_control(double error, const double kp, const double ki, const double kd,
Nickname 18:cbe3fe6d89a7 249 double &e_int, double &e_prev){
Nickname 18:cbe3fe6d89a7 250 double e_der = (error - e_prev) / Ts;
Nickname 18:cbe3fe6d89a7 251 e_prev = error;
Nickname 18:cbe3fe6d89a7 252 e_int = e_int + (Ts * error);
meikefrok 17:18e9df406502 253
Nickname 18:cbe3fe6d89a7 254 return kp*error + ki + e_int + kd + e_der;
meikefrok 17:18e9df406502 255 }
meikefrok 17:18e9df406502 256
meikefrok 7:9715323b20ac 257 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 7:9715323b20ac 258 //======== Functions and main ==============================================================
meikefrok 7:9715323b20ac 259 /* function that calculates the filtered EMG signal from the raw EMG signal.
Nickname 18:cbe3fe6d89a7 260 So 2 chains of 4 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 261 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
Nickname 18:cbe3fe6d89a7 262 The filtered signals (rlf_y and llf_y) are shown in channel 0 and 1 (scope.set)*/
Nickname 18:cbe3fe6d89a7 263 void scopeSend(){
meikefrok 7:9715323b20ac 264 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
meikefrok 7:9715323b20ac 265 lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
meikefrok 7:9715323b20ac 266 lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
meikefrok 7:9715323b20ac 267 lrect_y = fabs(lhf_y);
meikefrok 17:18e9df406502 268 llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/value_left;
meikefrok 17:18e9df406502 269
meikefrok 7:9715323b20ac 270 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
meikefrok 7:9715323b20ac 271 rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
meikefrok 7:9715323b20ac 272 rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
meikefrok 7:9715323b20ac 273 rrect_y = fabs(rhf_y);
meikefrok 17:18e9df406502 274 rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/value_right;
meikefrok 17:18e9df406502 275
meikefrok 16:3c9a3ff09765 276 scope.set(1, llf_y);
meikefrok 16:3c9a3ff09765 277 scope.set(0, rlf_y);
meikefrok 16:3c9a3ff09765 278 scope.send();
meikefrok 7:9715323b20ac 279 }
meikefrok 7:9715323b20ac 280
meikefrok 10:31e4c3d71ee6 281 void Measure(){
Nickname 18:cbe3fe6d89a7 282 // encoder Cart
Nickname 18:cbe3fe6d89a7 283 position_cart = (Encoder_Cart.getPulses()*factor_cart);
meikefrok 10:31e4c3d71ee6 284 ain_cart = pot_cart.read();
meikefrok 12:b31df384b170 285
Nickname 18:cbe3fe6d89a7 286 // encoder Arm
Nickname 18:cbe3fe6d89a7 287 position_arm = (Encoder_Arm.getPulses()*factor_arm);
meikefrok 13:dd7b41766f5f 288 ain_arm = pot_arm.read();
meikefrok 13:dd7b41766f5f 289
meikefrok 16:3c9a3ff09765 290 if (ain_arm == 0){
meikefrok 16:3c9a3ff09765 291 Encoder_Arm.reset();
meikefrok 16:3c9a3ff09765 292 }else {}
meikefrok 10:31e4c3d71ee6 293 }
meikefrok 10:31e4c3d71ee6 294
meikefrok 5:3d88f7506cd9 295 // Switch between Cart, Arm and Claw
meikefrok 8:ac4e5afbdbcd 296 void SwitchCart(){
meikefrok 2:ad4b181a6422 297 switch (part_id) {
Nickname 18:cbe3fe6d89a7 298 //Cart
Nickname 18:cbe3fe6d89a7 299 case 2: {
Nickname 18:cbe3fe6d89a7 300 led_r = LedOn;
Nickname 19:fd67b0c1a246 301 if (rlf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 302 if(position_cart <= -170){
Nickname 18:cbe3fe6d89a7 303 Cart.stop(1)==1; //If the cart is at the right side, it stops
Nickname 18:cbe3fe6d89a7 304
Nickname 18:cbe3fe6d89a7 305 }else if(position_cart >= 170 && position_arm <=-100){
Nickname 18:cbe3fe6d89a7 306 Cart.stop(1) == 1; //If the cart is at the left side and the arm is
Nickname 18:cbe3fe6d89a7 307 //rotated 60 degrees to the left, the cart can't move to the right.
Nickname 18:cbe3fe6d89a7 308
Nickname 18:cbe3fe6d89a7 309 }else if(position_cart >= 170 && position_arm <=-160 && position_claw == -18){
Nickname 18:cbe3fe6d89a7 310 Cart.stop(1) == 1;
Nickname 18:cbe3fe6d89a7 311
Nickname 18:cbe3fe6d89a7 312 }else{
Nickname 18:cbe3fe6d89a7 313 Cart.speed(cart_speed)==cart_speed;
Nickname 18:cbe3fe6d89a7 314 }
Nickname 19:fd67b0c1a246 315 if (llf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 316 Cart.stop(1)==1;
meikefrok 5:3d88f7506cd9 317 }
Nickname 19:fd67b0c1a246 318 }else if (llf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 319 if(position_cart >= 170){
Nickname 18:cbe3fe6d89a7 320 Cart.stop(1)==1; ////If the cart is at the left side, it stops
meikefrok 16:3c9a3ff09765 321
Nickname 18:cbe3fe6d89a7 322 }else if(position_cart <= -170 && position_arm >=100){
Nickname 18:cbe3fe6d89a7 323 Cart.stop(1) == 1; //similar with the left side
Nickname 18:cbe3fe6d89a7 324
Nickname 18:cbe3fe6d89a7 325 }else if(position_cart <= -170 && position_arm >=160 && position_claw == 27){
Nickname 18:cbe3fe6d89a7 326 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 327
Nickname 18:cbe3fe6d89a7 328 }else{
Nickname 18:cbe3fe6d89a7 329 Cart.speed(-cart_speed)==-cart_speed;
meikefrok 5:3d88f7506cd9 330 }
Nickname 19:fd67b0c1a246 331 if (rlf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 332 Cart.stop(1)==1;
meikefrok 12:b31df384b170 333 }
Nickname 18:cbe3fe6d89a7 334 }else {
Nickname 18:cbe3fe6d89a7 335 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 336 }}
Nickname 18:cbe3fe6d89a7 337 if(!btn && !btn2) {
Nickname 18:cbe3fe6d89a7 338 Arm.speed(0) == 0;
Nickname 18:cbe3fe6d89a7 339 if(position_cart<0){
Nickname 18:cbe3fe6d89a7 340 Cart.speed(-0.1)== -0.1;
Nickname 18:cbe3fe6d89a7 341 }else if(position_cart>0){
Nickname 18:cbe3fe6d89a7 342 Cart.speed(0.1)==0.1;
Nickname 18:cbe3fe6d89a7 343 }else{
Nickname 18:cbe3fe6d89a7 344 Cart.stop(0)==0;
Nickname 18:cbe3fe6d89a7 345 }}
Nickname 18:cbe3fe6d89a7 346 //Controller
Nickname 18:cbe3fe6d89a7 347 SetpointError_Cart = setpoint - position_cart;
Nickname 18:cbe3fe6d89a7 348 //set direction
Nickname 18:cbe3fe6d89a7 349 if (SetpointError_Cart < 0) {
meikefrok 17:18e9df406502 350 cart_speed = 0;
Nickname 18:cbe3fe6d89a7 351 }else{}
Nickname 18:cbe3fe6d89a7 352
Nickname 18:cbe3fe6d89a7 353 Cart_ControlSpeed = Ts * fabs( p_control(SetpointError_Cart, Cart_Kp, Cart_Ki, Cart_Kd, Cart_error, Cart_e_prev));
Nickname 18:cbe3fe6d89a7 354 if (fabs(SetpointError_Cart) < fabs(Setpoint_Cart*0.05)) {
meikefrok 17:18e9df406502 355 cart_speed = 0;
Nickname 18:cbe3fe6d89a7 356 }else{
meikefrok 17:18e9df406502 357 cart_speed = Cart_ControlSpeed;
meikefrok 17:18e9df406502 358 }
meikefrok 5:3d88f7506cd9 359 // controle LED
meikefrok 3:3a671d01bcb8 360 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 361 led_b = not LedOn;
meikefrok 12:b31df384b170 362
meikefrok 5:3d88f7506cd9 363 pc.baud(115200);
meikefrok 5:3d88f7506cd9 364 pc.printf("Distance in mm: %i\n", position_cart);
megrootens 0:048fbd80203e 365 break;
Nickname 18:cbe3fe6d89a7 366 }}
meikefrok 8:ac4e5afbdbcd 367
meikefrok 8:ac4e5afbdbcd 368 void SwitchArm(){
meikefrok 8:ac4e5afbdbcd 369 switch (part_id) {
Nickname 18:cbe3fe6d89a7 370 //Cart
Nickname 18:cbe3fe6d89a7 371 case 3: {
Nickname 18:cbe3fe6d89a7 372 led_g = LedOn;
Nickname 19:fd67b0c1a246 373 if (rlf_y > threshold_value) {
Nickname 18:cbe3fe6d89a7 374 if(position_cart > -170 && position_arm >= 90){ //If the cart is not at
Nickname 18:cbe3fe6d89a7 375 Arm.stop(1)==1; //the end, the arm can't move any further than 45 degrees
Nickname 18:cbe3fe6d89a7 376
Nickname 18:cbe3fe6d89a7 377 }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){
meikefrok 13:dd7b41766f5f 378 Arm.stop(1)==1;
meikefrok 16:3c9a3ff09765 379
Nickname 18:cbe3fe6d89a7 380 }else if(position_cart<= -170 && position_arm>=160){ //If the cart is at
Nickname 18:cbe3fe6d89a7 381 Arm.stop(1)==1; //the right end, the arm can't move any further than 70 degrees
Nickname 18:cbe3fe6d89a7 382
Nickname 18:cbe3fe6d89a7 383 }else{
Nickname 18:cbe3fe6d89a7 384 Arm.speed(arm_speed)==arm_speed;
Nickname 18:cbe3fe6d89a7 385 }
Nickname 18:cbe3fe6d89a7 386
Nickname 18:cbe3fe6d89a7 387 if (llf_y > threshold_value){
meikefrok 13:dd7b41766f5f 388 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 389 }
Nickname 18:cbe3fe6d89a7 390
Nickname 19:fd67b0c1a246 391 }else if (llf_y > threshold_value) {
Nickname 18:cbe3fe6d89a7 392 if(position_cart < 170 && position_arm <= -90){
Nickname 18:cbe3fe6d89a7 393 Arm.stop(1)==1; //similar with the right side
Nickname 18:cbe3fe6d89a7 394
Nickname 18:cbe3fe6d89a7 395 }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
Nickname 18:cbe3fe6d89a7 396 Arm.stop(1)==1;
Nickname 18:cbe3fe6d89a7 397
Nickname 18:cbe3fe6d89a7 398 }else if(position_cart>=170 && position_arm<=-160){
Nickname 18:cbe3fe6d89a7 399 Arm.stop(1)==1; //similar with the right side
meikefrok 13:dd7b41766f5f 400
meikefrok 8:ac4e5afbdbcd 401 }else{
Nickname 18:cbe3fe6d89a7 402 Arm.speed(-arm_speed)==-arm_speed;
Nickname 18:cbe3fe6d89a7 403 }
Nickname 18:cbe3fe6d89a7 404 if (rlf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 405 Cart.stop(1)==1;
meikefrok 13:dd7b41766f5f 406 }
Nickname 18:cbe3fe6d89a7 407
Nickname 18:cbe3fe6d89a7 408 }else {
Nickname 18:cbe3fe6d89a7 409 Arm.stop(1)==1;
Nickname 18:cbe3fe6d89a7 410 }
meikefrok 17:18e9df406502 411
Nickname 18:cbe3fe6d89a7 412 if(!btn&&!btn2){
Nickname 18:cbe3fe6d89a7 413 Cart.speed(0) == 0;
Nickname 18:cbe3fe6d89a7 414 if(position_arm>0){
Nickname 18:cbe3fe6d89a7 415 Arm.speed(-0.1)== -0.1;
Nickname 18:cbe3fe6d89a7 416 }else if(position_arm<0){
Nickname 18:cbe3fe6d89a7 417 Arm.speed(0.1)==0.1;
Nickname 18:cbe3fe6d89a7 418 }else{
Nickname 18:cbe3fe6d89a7 419 Arm.stop(0)==0;
Nickname 18:cbe3fe6d89a7 420 }}
Nickname 18:cbe3fe6d89a7 421 //Controller
Nickname 18:cbe3fe6d89a7 422 SetpointError_Arm = setpoint - position_arm;
Nickname 18:cbe3fe6d89a7 423 //set direction
Nickname 18:cbe3fe6d89a7 424 if (SetpointError_Arm > 0) {
meikefrok 17:18e9df406502 425 arm_speed = 0;
Nickname 18:cbe3fe6d89a7 426 }else {}
meikefrok 17:18e9df406502 427
Nickname 18:cbe3fe6d89a7 428 Arm_ControlSpeed = Ts * fabs( p_control(SetpointError_Arm, Arm_Kp, Arm_Ki, Arm_Kd, Arm_error, Arm_e_prev));
Nickname 18:cbe3fe6d89a7 429 if (fabs(SetpointError_Arm) < fabs(Setpoint_Arm*0.05)) {
meikefrok 17:18e9df406502 430 Arm_ControlSpeed = 0;
meikefrok 17:18e9df406502 431 }else{
meikefrok 17:18e9df406502 432 arm_speed = Arm_ControlSpeed;
Nickname 18:cbe3fe6d89a7 433 }}
meikefrok 8:ac4e5afbdbcd 434 // controle LED
meikefrok 8:ac4e5afbdbcd 435 led_r = not LedOn;
meikefrok 8:ac4e5afbdbcd 436 led_b = not LedOn;
meikefrok 8:ac4e5afbdbcd 437
meikefrok 13:dd7b41766f5f 438 pc.baud(115200);
meikefrok 13:dd7b41766f5f 439 pc.printf("Degrees: %i\n", position_arm);
meikefrok 8:ac4e5afbdbcd 440 break;
megrootens 0:048fbd80203e 441 }
meikefrok 8:ac4e5afbdbcd 442 }
Nickname 18:cbe3fe6d89a7 443
meikefrok 8:ac4e5afbdbcd 444
meikefrok 15:caf29b6f5261 445 void SwitchClaw(){
Nickname 18:cbe3fe6d89a7 446 switch (part_id) {
meikefrok 15:caf29b6f5261 447 case 4: {
meikefrok 15:caf29b6f5261 448 led_b = LedOn;
Nickname 19:fd67b0c1a246 449 if(rlf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 450 servo_id ++;
meikefrok 15:caf29b6f5261 451 switch (servo_id) {
Nickname 18:cbe3fe6d89a7 452 case 0: {
Nickname 18:cbe3fe6d89a7 453 led_r = LedOn;
Nickname 18:cbe3fe6d89a7 454 led_b = not LedOn;
Nickname 18:cbe3fe6d89a7 455 led_g = not LedOn;
Nickname 18:cbe3fe6d89a7 456 servo.position(-18);
Nickname 18:cbe3fe6d89a7 457 break;
Nickname 18:cbe3fe6d89a7 458 }
Nickname 18:cbe3fe6d89a7 459 case 1: {
Nickname 18:cbe3fe6d89a7 460 led_b = LedOn;
Nickname 18:cbe3fe6d89a7 461 led_r = not LedOn;
Nickname 18:cbe3fe6d89a7 462 led_g = not LedOn;
Nickname 18:cbe3fe6d89a7 463 servo.position(3);
Nickname 18:cbe3fe6d89a7 464 wait_ms(waiting_claw);
Nickname 18:cbe3fe6d89a7 465 break;
Nickname 18:cbe3fe6d89a7 466 }
Nickname 18:cbe3fe6d89a7 467 case 2: {
Nickname 18:cbe3fe6d89a7 468 led_g = LedOn;
Nickname 18:cbe3fe6d89a7 469 led_r = not LedOn;
Nickname 18:cbe3fe6d89a7 470 led_b = not LedOn;
Nickname 18:cbe3fe6d89a7 471 servo.position(27);
Nickname 18:cbe3fe6d89a7 472 break;
Nickname 18:cbe3fe6d89a7 473 }}
Nickname 19:fd67b0c1a246 474 }else if(llf_y > threshold_value){
meikefrok 15:caf29b6f5261 475 servo_id --;
meikefrok 15:caf29b6f5261 476
meikefrok 15:caf29b6f5261 477 switch (servo_id) {
meikefrok 15:caf29b6f5261 478 case 0: {
meikefrok 15:caf29b6f5261 479 led_r = LedOn;
meikefrok 15:caf29b6f5261 480 led_b = not LedOn;
meikefrok 15:caf29b6f5261 481 led_g = not LedOn;
meikefrok 15:caf29b6f5261 482 servo.position(-18);
meikefrok 15:caf29b6f5261 483 break;
meikefrok 15:caf29b6f5261 484 }
meikefrok 15:caf29b6f5261 485 case 1: {
meikefrok 15:caf29b6f5261 486 led_b = LedOn;
meikefrok 15:caf29b6f5261 487 led_r = not LedOn;
meikefrok 15:caf29b6f5261 488 led_g = not LedOn;
meikefrok 15:caf29b6f5261 489 servo.position(3);
meikefrok 15:caf29b6f5261 490 wait_ms(waiting_claw);
meikefrok 15:caf29b6f5261 491 break;
meikefrok 15:caf29b6f5261 492 }
meikefrok 15:caf29b6f5261 493 case 2: {
meikefrok 15:caf29b6f5261 494 led_g = LedOn;
meikefrok 15:caf29b6f5261 495 led_r = not LedOn;
meikefrok 15:caf29b6f5261 496 led_b = not LedOn;
meikefrok 15:caf29b6f5261 497 servo.position(27);
meikefrok 15:caf29b6f5261 498 break;
meikefrok 15:caf29b6f5261 499 }
meikefrok 15:caf29b6f5261 500 }
meikefrok 15:caf29b6f5261 501 }else{}
Nickname 18:cbe3fe6d89a7 502 }
meikefrok 15:caf29b6f5261 503 led_r = not LedOn;
meikefrok 15:caf29b6f5261 504 led_g = not LedOn;
meikefrok 15:caf29b6f5261 505 position_claw = servo.read();
meikefrok 15:caf29b6f5261 506 break;
meikefrok 15:caf29b6f5261 507 }
meikefrok 15:caf29b6f5261 508 }
megrootens 0:048fbd80203e 509
meikefrok 5:3d88f7506cd9 510 // Switch the part
Nickname 18:cbe3fe6d89a7 511 void SetValue2(){
meikefrok 3:3a671d01bcb8 512 part_id = 2;
meikefrok 6:23b1ed826b59 513 }
Nickname 18:cbe3fe6d89a7 514 void SetValue3(){
meikefrok 3:3a671d01bcb8 515 part_id = 3;
meikefrok 6:23b1ed826b59 516 }
Nickname 18:cbe3fe6d89a7 517 void SetValue4(){
meikefrok 3:3a671d01bcb8 518 part_id = 4;
megrootens 0:048fbd80203e 519 }
megrootens 0:048fbd80203e 520
meikefrok 5:3d88f7506cd9 521 // Main
megrootens 0:048fbd80203e 522 int main()
megrootens 0:048fbd80203e 523 {
meikefrok 3:3a671d01bcb8 524 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 525 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 526 led_b = not LedOn;
megrootens 0:048fbd80203e 527
Nickname 18:cbe3fe6d89a7 528 tick_part_cart.attach(&SwitchCart,kTimeToggle);
meikefrok 15:caf29b6f5261 529 tick_part_arm.attach(&SwitchArm,kTimeToggle);
meikefrok 15:caf29b6f5261 530 tick_part_claw.attach(&SwitchClaw,0.1f);
meikefrok 11:97f824629da5 531 measureTicker.attach(Measure, 0.005);
meikefrok 15:caf29b6f5261 532 sampleTicker.attach(scopeSend,0.01);
meikefrok 15:caf29b6f5261 533
meikefrok 3:3a671d01bcb8 534 btn_cart.fall(&SetValue2);
meikefrok 3:3a671d01bcb8 535 btn_arm.fall(&SetValue3);
Nickname 18:cbe3fe6d89a7 536 btn_claw.fall(&SetValue4);
Nickname 18:cbe3fe6d89a7 537 }