Nick van den Berg / Mbed 2 deprecated The_Claw_with_EMG_Control_PID

Cleaner version

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Meike Froklage

main.cpp@18:cbe3fe6d89a7, 2016-11-07 (annotated)

Committer:
Nickname
Date:
Mon Nov 07 11:40:27 2016 +0000
Revision:
18:cbe3fe6d89a7
Parent:
17:18e9df406502 
Clean

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
meikefrok 5:3d88f7506cd9 62 // position
meikefrok 12:b31df384b170 63 float factor_cart = 0.05802;
meikefrok 5:3d88f7506cd9 64 float factor_arm = 0.1539;
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 18:cbe3fe6d89a7 155 const double threshold_value = 0.1;
Nickname 18:cbe3fe6d89a7 156 const double threshold_value_claw = 0.08;
Nickname 18:cbe3fe6d89a7 157 const double threshold_value_cart = 0.08;
Nickname 18:cbe3fe6d89a7 158
Nickname 18:cbe3fe6d89a7 159 /* declaration of each biquad
Nickname 18:cbe3fe6d89a7 160 The coefficients will be filled in later on in void scopeSend
Nickname 18:cbe3fe6d89a7 161 The input of the first biquad is the raw EMG signal and the output of the last
Nickname 18:cbe3fe6d89a7 162 biquad is the filtered signal. This is done for both left and right so this
Nickname 18:cbe3fe6d89a7 163 makes two chains of 4 biquads */
Nickname 18:cbe3fe6d89a7 164 double biquad_lno(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 165 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 166 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 167 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 168 v2 = v1;
Nickname 18:cbe3fe6d89a7 169 v1 = v;
Nickname 18:cbe3fe6d89a7 170 return y;
Nickname 18:cbe3fe6d89a7 171 }
Nickname 18:cbe3fe6d89a7 172 double biquad_lno2(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 173 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 174 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 175 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 176 v2 = v1;
Nickname 18:cbe3fe6d89a7 177 v1 = v;
Nickname 18:cbe3fe6d89a7 178 return y;
Nickname 18:cbe3fe6d89a7 179 }
Nickname 18:cbe3fe6d89a7 180 double biquad_lhf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 181 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 182 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 183 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 184 v2 = v1;
Nickname 18:cbe3fe6d89a7 185 v1 = v;
Nickname 18:cbe3fe6d89a7 186 return y;
Nickname 18:cbe3fe6d89a7 187 }
Nickname 18:cbe3fe6d89a7 188 double biquad_llf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 189 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 190 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 191 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 192 v2 = v1;
Nickname 18:cbe3fe6d89a7 193 v1 = v;
Nickname 18:cbe3fe6d89a7 194 return y;
Nickname 18:cbe3fe6d89a7 195 }
Nickname 18:cbe3fe6d89a7 196 double biquad_rno(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 197 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 198 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 199 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 200 v2 = v1;
Nickname 18:cbe3fe6d89a7 201 v1 = v;
Nickname 18:cbe3fe6d89a7 202 return y;
Nickname 18:cbe3fe6d89a7 203 }
Nickname 18:cbe3fe6d89a7 204 double biquad_rno2(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 205 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 206 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 207 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 208 v2 = v1;
Nickname 18:cbe3fe6d89a7 209 v1 = v;
Nickname 18:cbe3fe6d89a7 210 return y;
Nickname 18:cbe3fe6d89a7 211 }
Nickname 18:cbe3fe6d89a7 212 double biquad_rhf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 213 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 214 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 215 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 216 v2 = v1;
Nickname 18:cbe3fe6d89a7 217 v1 = v;
Nickname 18:cbe3fe6d89a7 218 return y;
Nickname 18:cbe3fe6d89a7 219 }
Nickname 18:cbe3fe6d89a7 220 double biquad_rlf(double u, double&v1, double&v2, const double a1, const double a2,
Nickname 18:cbe3fe6d89a7 221 const double b0, const double b1, const double b2){
Nickname 18:cbe3fe6d89a7 222 double v = u - a1*v1 - a2*v2;
Nickname 18:cbe3fe6d89a7 223 double y = b0*v + b1*v1 + b2*v2;
Nickname 18:cbe3fe6d89a7 224 v2 = v1;
Nickname 18:cbe3fe6d89a7 225 v1 = v;
Nickname 18:cbe3fe6d89a7 226 return y;
Nickname 18:cbe3fe6d89a7 227 }
Nickname 18:cbe3fe6d89a7 228
Nickname 18:cbe3fe6d89a7 229 //calibration to determine maximum EMG value
meikefrok 17:18e9df406502 230 double calibrate_right(){
meikefrok 17:18e9df406502 231 for(t=0; t<200; t++){
meikefrok 17:18e9df406502 232 if(value_right <= rlf_y){
Nickname 18:cbe3fe6d89a7 233 value_right = rlf_y;
meikefrok 17:18e9df406502 234 }else{
Nickname 18:cbe3fe6d89a7 235 value_right = value_right +0.0;
Nickname 18:cbe3fe6d89a7 236 }}
meikefrok 17:18e9df406502 237 return value_right;
meikefrok 17:18e9df406502 238 }
meikefrok 17:18e9df406502 239 double calibrate_left(){
meikefrok 17:18e9df406502 240 for(t=0; t<200; t++){
meikefrok 17:18e9df406502 241 if(value_left <= llf_y){
meikefrok 17:18e9df406502 242 value_left = llf_y;
meikefrok 17:18e9df406502 243 }else{
Nickname 18:cbe3fe6d89a7 244 value_left = value_left +0.0;
Nickname 18:cbe3fe6d89a7 245 }}
meikefrok 17:18e9df406502 246 return value_left;
meikefrok 17:18e9df406502 247 }
meikefrok 17:18e9df406502 248
Nickname 18:cbe3fe6d89a7 249 //======== P Controller ========================================================
Nickname 18:cbe3fe6d89a7 250 double p_control(double error, const double kp, const double ki, const double kd,
Nickname 18:cbe3fe6d89a7 251 double &e_int, double &e_prev){
Nickname 18:cbe3fe6d89a7 252 double e_der = (error - e_prev) / Ts;
Nickname 18:cbe3fe6d89a7 253 e_prev = error;
Nickname 18:cbe3fe6d89a7 254 e_int = e_int + (Ts * error);
meikefrok 17:18e9df406502 255
Nickname 18:cbe3fe6d89a7 256 return kp*error + ki + e_int + kd + e_der;
meikefrok 17:18e9df406502 257 }
meikefrok 17:18e9df406502 258
meikefrok 7:9715323b20ac 259 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
meikefrok 7:9715323b20ac 260 //======== Functions and main ==============================================================
meikefrok 7:9715323b20ac 261 /* function that calculates the filtered EMG signal from the raw EMG signal.
Nickname 18:cbe3fe6d89a7 262 So 2 chains of 4 biquads each are calculating the left and the right filtered EMG signal.
meikefrok 7:9715323b20ac 263 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
Nickname 18:cbe3fe6d89a7 264 The filtered signals (rlf_y and llf_y) are shown in channel 0 and 1 (scope.set)*/
Nickname 18:cbe3fe6d89a7 265 void scopeSend(){
meikefrok 7:9715323b20ac 266 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
meikefrok 7:9715323b20ac 267 lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
meikefrok 7:9715323b20ac 268 lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
meikefrok 7:9715323b20ac 269 lrect_y = fabs(lhf_y);
meikefrok 17:18e9df406502 270 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 271
meikefrok 7:9715323b20ac 272 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
meikefrok 7:9715323b20ac 273 rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
meikefrok 7:9715323b20ac 274 rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
meikefrok 7:9715323b20ac 275 rrect_y = fabs(rhf_y);
meikefrok 17:18e9df406502 276 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 277
meikefrok 16:3c9a3ff09765 278 scope.set(1, llf_y);
meikefrok 16:3c9a3ff09765 279 scope.set(0, rlf_y);
meikefrok 16:3c9a3ff09765 280 scope.send();
meikefrok 7:9715323b20ac 281 }
meikefrok 7:9715323b20ac 282
meikefrok 10:31e4c3d71ee6 283 void Measure(){
Nickname 18:cbe3fe6d89a7 284 // encoder Cart
Nickname 18:cbe3fe6d89a7 285 position_cart = (Encoder_Cart.getPulses()*factor_cart);
meikefrok 10:31e4c3d71ee6 286 ain_cart = pot_cart.read();
meikefrok 12:b31df384b170 287
Nickname 18:cbe3fe6d89a7 288 // encoder Arm
Nickname 18:cbe3fe6d89a7 289 position_arm = (Encoder_Arm.getPulses()*factor_arm);
meikefrok 13:dd7b41766f5f 290 ain_arm = pot_arm.read();
meikefrok 13:dd7b41766f5f 291
meikefrok 16:3c9a3ff09765 292 if (ain_arm == 0){
meikefrok 16:3c9a3ff09765 293 Encoder_Arm.reset();
meikefrok 16:3c9a3ff09765 294 }else {}
meikefrok 10:31e4c3d71ee6 295 }
meikefrok 10:31e4c3d71ee6 296
meikefrok 5:3d88f7506cd9 297 // Switch between Cart, Arm and Claw
meikefrok 8:ac4e5afbdbcd 298 void SwitchCart(){
meikefrok 2:ad4b181a6422 299 switch (part_id) {
Nickname 18:cbe3fe6d89a7 300 //Cart
Nickname 18:cbe3fe6d89a7 301 case 2: {
Nickname 18:cbe3fe6d89a7 302 led_r = LedOn;
Nickname 18:cbe3fe6d89a7 303 if (rlf_y > threshold_value_cart){
Nickname 18:cbe3fe6d89a7 304 if(position_cart <= -170){
Nickname 18:cbe3fe6d89a7 305 Cart.stop(1)==1; //If the cart is at the right side, it stops
Nickname 18:cbe3fe6d89a7 306
Nickname 18:cbe3fe6d89a7 307 }else if(position_cart >= 170 && position_arm <=-100){
Nickname 18:cbe3fe6d89a7 308 Cart.stop(1) == 1; //If the cart is at the left side and the arm is
Nickname 18:cbe3fe6d89a7 309 //rotated 60 degrees to the left, the cart can't move to the right.
Nickname 18:cbe3fe6d89a7 310
Nickname 18:cbe3fe6d89a7 311 }else if(position_cart >= 170 && position_arm <=-160 && position_claw == -18){
Nickname 18:cbe3fe6d89a7 312 Cart.stop(1) == 1;
Nickname 18:cbe3fe6d89a7 313
Nickname 18:cbe3fe6d89a7 314 }else{
Nickname 18:cbe3fe6d89a7 315 Cart.speed(cart_speed)==cart_speed;
Nickname 18:cbe3fe6d89a7 316 }
Nickname 18:cbe3fe6d89a7 317 if (llf_y > threshold_value_cart){
Nickname 18:cbe3fe6d89a7 318 Cart.stop(1)==1;
meikefrok 5:3d88f7506cd9 319 }
Nickname 18:cbe3fe6d89a7 320 }else if (llf_y > threshold_value_cart){
Nickname 18:cbe3fe6d89a7 321 if(position_cart >= 170){
Nickname 18:cbe3fe6d89a7 322 Cart.stop(1)==1; ////If the cart is at the left side, it stops
meikefrok 16:3c9a3ff09765 323
Nickname 18:cbe3fe6d89a7 324 }else if(position_cart <= -170 && position_arm >=100){
Nickname 18:cbe3fe6d89a7 325 Cart.stop(1) == 1; //similar with the left side
Nickname 18:cbe3fe6d89a7 326
Nickname 18:cbe3fe6d89a7 327 }else if(position_cart <= -170 && position_arm >=160 && position_claw == 27){
Nickname 18:cbe3fe6d89a7 328 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 329
Nickname 18:cbe3fe6d89a7 330 }else{
Nickname 18:cbe3fe6d89a7 331 Cart.speed(-cart_speed)==-cart_speed;
meikefrok 5:3d88f7506cd9 332 }
Nickname 18:cbe3fe6d89a7 333 if (rlf_y > threshold_value_cart){
Nickname 18:cbe3fe6d89a7 334 Cart.stop(1)==1;
meikefrok 12:b31df384b170 335 }
Nickname 18:cbe3fe6d89a7 336 }else {
Nickname 18:cbe3fe6d89a7 337 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 338 }}
Nickname 18:cbe3fe6d89a7 339 if(!btn && !btn2) {
Nickname 18:cbe3fe6d89a7 340 Arm.speed(0) == 0;
Nickname 18:cbe3fe6d89a7 341 if(position_cart<0){
Nickname 18:cbe3fe6d89a7 342 Cart.speed(-0.1)== -0.1;
Nickname 18:cbe3fe6d89a7 343 }else if(position_cart>0){
Nickname 18:cbe3fe6d89a7 344 Cart.speed(0.1)==0.1;
Nickname 18:cbe3fe6d89a7 345 }else{
Nickname 18:cbe3fe6d89a7 346 Cart.stop(0)==0;
Nickname 18:cbe3fe6d89a7 347 }}
Nickname 18:cbe3fe6d89a7 348 //Controller
Nickname 18:cbe3fe6d89a7 349 SetpointError_Cart = setpoint - position_cart;
Nickname 18:cbe3fe6d89a7 350 //set direction
Nickname 18:cbe3fe6d89a7 351 if (SetpointError_Cart < 0) {
meikefrok 17:18e9df406502 352 cart_speed = 0;
Nickname 18:cbe3fe6d89a7 353 }else{}
Nickname 18:cbe3fe6d89a7 354
Nickname 18:cbe3fe6d89a7 355 Cart_ControlSpeed = Ts * fabs( p_control(SetpointError_Cart, Cart_Kp, Cart_Ki, Cart_Kd, Cart_error, Cart_e_prev));
Nickname 18:cbe3fe6d89a7 356 if (fabs(SetpointError_Cart) < fabs(Setpoint_Cart*0.05)) {
meikefrok 17:18e9df406502 357 cart_speed = 0;
Nickname 18:cbe3fe6d89a7 358 }else{
meikefrok 17:18e9df406502 359 cart_speed = Cart_ControlSpeed;
meikefrok 17:18e9df406502 360 }
meikefrok 5:3d88f7506cd9 361 // controle LED
meikefrok 3:3a671d01bcb8 362 led_g = not LedOn;
meikefrok 5:3d88f7506cd9 363 led_b = not LedOn;
meikefrok 12:b31df384b170 364
meikefrok 5:3d88f7506cd9 365 pc.baud(115200);
meikefrok 5:3d88f7506cd9 366 pc.printf("Distance in mm: %i\n", position_cart);
megrootens 0:048fbd80203e 367 break;
Nickname 18:cbe3fe6d89a7 368 }}
meikefrok 8:ac4e5afbdbcd 369
meikefrok 8:ac4e5afbdbcd 370 void SwitchArm(){
meikefrok 8:ac4e5afbdbcd 371 switch (part_id) {
Nickname 18:cbe3fe6d89a7 372 //Cart
Nickname 18:cbe3fe6d89a7 373 case 3: {
Nickname 18:cbe3fe6d89a7 374 led_g = LedOn;
Nickname 18:cbe3fe6d89a7 375 if (rlf_y > threshold_value_claw) {
Nickname 18:cbe3fe6d89a7 376 if(position_cart > -170 && position_arm >= 90){ //If the cart is not at
Nickname 18:cbe3fe6d89a7 377 Arm.stop(1)==1; //the end, the arm can't move any further than 45 degrees
Nickname 18:cbe3fe6d89a7 378
Nickname 18:cbe3fe6d89a7 379 }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){
meikefrok 13:dd7b41766f5f 380 Arm.stop(1)==1;
meikefrok 16:3c9a3ff09765 381
Nickname 18:cbe3fe6d89a7 382 }else if(position_cart<= -170 && position_arm>=160){ //If the cart is at
Nickname 18:cbe3fe6d89a7 383 Arm.stop(1)==1; //the right end, the arm can't move any further than 70 degrees
Nickname 18:cbe3fe6d89a7 384
Nickname 18:cbe3fe6d89a7 385 }else{
Nickname 18:cbe3fe6d89a7 386 Arm.speed(arm_speed)==arm_speed;
Nickname 18:cbe3fe6d89a7 387 }
Nickname 18:cbe3fe6d89a7 388
Nickname 18:cbe3fe6d89a7 389 if (llf_y > threshold_value){
meikefrok 13:dd7b41766f5f 390 Cart.stop(1)==1;
Nickname 18:cbe3fe6d89a7 391 }
Nickname 18:cbe3fe6d89a7 392
Nickname 18:cbe3fe6d89a7 393 }else if (llf_y > threshold_value_claw) {
Nickname 18:cbe3fe6d89a7 394 if(position_cart < 170 && position_arm <= -90){
Nickname 18:cbe3fe6d89a7 395 Arm.stop(1)==1; //similar with the right side
Nickname 18:cbe3fe6d89a7 396
Nickname 18:cbe3fe6d89a7 397 }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
Nickname 18:cbe3fe6d89a7 398 Arm.stop(1)==1;
Nickname 18:cbe3fe6d89a7 399
Nickname 18:cbe3fe6d89a7 400 }else if(position_cart>=170 && position_arm<=-160){
Nickname 18:cbe3fe6d89a7 401 Arm.stop(1)==1; //similar with the right side
meikefrok 13:dd7b41766f5f 402
meikefrok 8:ac4e5afbdbcd 403 }else{
Nickname 18:cbe3fe6d89a7 404 Arm.speed(-arm_speed)==-arm_speed;
Nickname 18:cbe3fe6d89a7 405 }
Nickname 18:cbe3fe6d89a7 406 if (rlf_y > threshold_value){
Nickname 18:cbe3fe6d89a7 407 Cart.stop(1)==1;
meikefrok 13:dd7b41766f5f 408 }
Nickname 18:cbe3fe6d89a7 409
Nickname 18:cbe3fe6d89a7 410 }else {
Nickname 18:cbe3fe6d89a7 411 Arm.stop(1)==1;
Nickname 18:cbe3fe6d89a7 412 }
meikefrok 17:18e9df406502 413
Nickname 18:cbe3fe6d89a7 414 if(!btn&&!btn2){
Nickname 18:cbe3fe6d89a7 415 Cart.speed(0) == 0;
Nickname 18:cbe3fe6d89a7 416 if(position_arm>0){
Nickname 18:cbe3fe6d89a7 417 Arm.speed(-0.1)== -0.1;
Nickname 18:cbe3fe6d89a7 418 }else if(position_arm<0){
Nickname 18:cbe3fe6d89a7 419 Arm.speed(0.1)==0.1;
Nickname 18:cbe3fe6d89a7 420 }else{
Nickname 18:cbe3fe6d89a7 421 Arm.stop(0)==0;
Nickname 18:cbe3fe6d89a7 422 }}
Nickname 18:cbe3fe6d89a7 423 //Controller
Nickname 18:cbe3fe6d89a7 424 SetpointError_Arm = setpoint - position_arm;
Nickname 18:cbe3fe6d89a7 425 //set direction
Nickname 18:cbe3fe6d89a7 426 if (SetpointError_Arm > 0) {
meikefrok 17:18e9df406502 427 arm_speed = 0;
Nickname 18:cbe3fe6d89a7 428 }else {}
meikefrok 17:18e9df406502 429
Nickname 18:cbe3fe6d89a7 430 Arm_ControlSpeed = Ts * fabs( p_control(SetpointError_Arm, Arm_Kp, Arm_Ki, Arm_Kd, Arm_error, Arm_e_prev));
Nickname 18:cbe3fe6d89a7 431 if (fabs(SetpointError_Arm) < fabs(Setpoint_Arm*0.05)) {
meikefrok 17:18e9df406502 432 Arm_ControlSpeed = 0;
meikefrok 17:18e9df406502 433 }else{
meikefrok 17:18e9df406502 434 arm_speed = Arm_ControlSpeed;
Nickname 18:cbe3fe6d89a7 435 }}
meikefrok 8:ac4e5afbdbcd 436 // controle LED
meikefrok 8:ac4e5afbdbcd 437 led_r = not LedOn;
meikefrok 8:ac4e5afbdbcd 438 led_b = not LedOn;
meikefrok 8:ac4e5afbdbcd 439
meikefrok 13:dd7b41766f5f 440 pc.baud(115200);
meikefrok 13:dd7b41766f5f 441 pc.printf("Degrees: %i\n", position_arm);
meikefrok 8:ac4e5afbdbcd 442 break;
megrootens 0:048fbd80203e 443 }
meikefrok 8:ac4e5afbdbcd 444 }
Nickname 18:cbe3fe6d89a7 445
meikefrok 8:ac4e5afbdbcd 446
meikefrok 15:caf29b6f5261 447 void SwitchClaw(){
Nickname 18:cbe3fe6d89a7 448 switch (part_id) {
meikefrok 15:caf29b6f5261 449 case 4: {
meikefrok 15:caf29b6f5261 450 led_b = LedOn;
Nickname 18:cbe3fe6d89a7 451 if(rlf_y > threshold_value_claw){
Nickname 18:cbe3fe6d89a7 452 servo_id ++;
meikefrok 15:caf29b6f5261 453 switch (servo_id) {
Nickname 18:cbe3fe6d89a7 454 case 0: {
Nickname 18:cbe3fe6d89a7 455 led_r = LedOn;
Nickname 18:cbe3fe6d89a7 456 led_b = not LedOn;
Nickname 18:cbe3fe6d89a7 457 led_g = not LedOn;
Nickname 18:cbe3fe6d89a7 458 servo.position(-18);
Nickname 18:cbe3fe6d89a7 459 break;
Nickname 18:cbe3fe6d89a7 460 }
Nickname 18:cbe3fe6d89a7 461 case 1: {
Nickname 18:cbe3fe6d89a7 462 led_b = LedOn;
Nickname 18:cbe3fe6d89a7 463 led_r = not LedOn;
Nickname 18:cbe3fe6d89a7 464 led_g = not LedOn;
Nickname 18:cbe3fe6d89a7 465 servo.position(3);
Nickname 18:cbe3fe6d89a7 466 wait_ms(waiting_claw);
Nickname 18:cbe3fe6d89a7 467 break;
Nickname 18:cbe3fe6d89a7 468 }
Nickname 18:cbe3fe6d89a7 469 case 2: {
Nickname 18:cbe3fe6d89a7 470 led_g = LedOn;
Nickname 18:cbe3fe6d89a7 471 led_r = not LedOn;
Nickname 18:cbe3fe6d89a7 472 led_b = not LedOn;
Nickname 18:cbe3fe6d89a7 473 servo.position(27);
Nickname 18:cbe3fe6d89a7 474 break;
Nickname 18:cbe3fe6d89a7 475 }}
meikefrok 15:caf29b6f5261 476 }else if(llf_y > threshold_value_claw){
meikefrok 15:caf29b6f5261 477 servo_id --;
meikefrok 15:caf29b6f5261 478
meikefrok 15:caf29b6f5261 479 switch (servo_id) {
meikefrok 15:caf29b6f5261 480 case 0: {
meikefrok 15:caf29b6f5261 481 led_r = LedOn;
meikefrok 15:caf29b6f5261 482 led_b = not LedOn;
meikefrok 15:caf29b6f5261 483 led_g = not LedOn;
meikefrok 15:caf29b6f5261 484 servo.position(-18);
meikefrok 15:caf29b6f5261 485 break;
meikefrok 15:caf29b6f5261 486 }
meikefrok 15:caf29b6f5261 487 case 1: {
meikefrok 15:caf29b6f5261 488 led_b = LedOn;
meikefrok 15:caf29b6f5261 489 led_r = not LedOn;
meikefrok 15:caf29b6f5261 490 led_g = not LedOn;
meikefrok 15:caf29b6f5261 491 servo.position(3);
meikefrok 15:caf29b6f5261 492 wait_ms(waiting_claw);
meikefrok 15:caf29b6f5261 493 break;
meikefrok 15:caf29b6f5261 494 }
meikefrok 15:caf29b6f5261 495 case 2: {
meikefrok 15:caf29b6f5261 496 led_g = LedOn;
meikefrok 15:caf29b6f5261 497 led_r = not LedOn;
meikefrok 15:caf29b6f5261 498 led_b = not LedOn;
meikefrok 15:caf29b6f5261 499 servo.position(27);
meikefrok 15:caf29b6f5261 500 break;
meikefrok 15:caf29b6f5261 501 }
meikefrok 15:caf29b6f5261 502 }
meikefrok 15:caf29b6f5261 503 }else{}
Nickname 18:cbe3fe6d89a7 504 }
meikefrok 15:caf29b6f5261 505 led_r = not LedOn;
meikefrok 15:caf29b6f5261 506 led_g = not LedOn;
meikefrok 15:caf29b6f5261 507 position_claw = servo.read();
meikefrok 15:caf29b6f5261 508 break;
meikefrok 15:caf29b6f5261 509 }
meikefrok 15:caf29b6f5261 510 }
megrootens 0:048fbd80203e 511
meikefrok 5:3d88f7506cd9 512 // Switch the part
Nickname 18:cbe3fe6d89a7 513 void SetValue2(){
meikefrok 3:3a671d01bcb8 514 part_id = 2;
meikefrok 6:23b1ed826b59 515 }
Nickname 18:cbe3fe6d89a7 516 void SetValue3(){
meikefrok 3:3a671d01bcb8 517 part_id = 3;
meikefrok 6:23b1ed826b59 518 }
Nickname 18:cbe3fe6d89a7 519 void SetValue4(){
meikefrok 3:3a671d01bcb8 520 part_id = 4;
megrootens 0:048fbd80203e 521 }
megrootens 0:048fbd80203e 522
meikefrok 5:3d88f7506cd9 523 // Main
megrootens 0:048fbd80203e 524 int main()
megrootens 0:048fbd80203e 525 {
meikefrok 3:3a671d01bcb8 526 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 527 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 528 led_b = not LedOn;
megrootens 0:048fbd80203e 529
Nickname 18:cbe3fe6d89a7 530 tick_part_cart.attach(&SwitchCart,kTimeToggle);
meikefrok 15:caf29b6f5261 531 tick_part_arm.attach(&SwitchArm,kTimeToggle);
meikefrok 15:caf29b6f5261 532 tick_part_claw.attach(&SwitchClaw,0.1f);
meikefrok 11:97f824629da5 533 measureTicker.attach(Measure, 0.005);
meikefrok 15:caf29b6f5261 534 sampleTicker.attach(scopeSend,0.01);
meikefrok 15:caf29b6f5261 535
meikefrok 3:3a671d01bcb8 536 btn_cart.fall(&SetValue2);
meikefrok 3:3a671d01bcb8 537 btn_arm.fall(&SetValue3);
Nickname 18:cbe3fe6d89a7 538 btn_claw.fall(&SetValue4);
Nickname 18:cbe3fe6d89a7 539 }