Nick van den Berg
Cleaner version
Dependencies: HIDScope MODSERIAL Motordriver QEI Servo mbed
Fork of
The_Claw_with_EMG_Control_PID
by 
Meike Froklage
main.cpp@17:18e9df406502, 2016-11-07 (annotated)
- Committer:
- meikefrok
- Date:
- Mon Nov 07 09:38:21 2016 +0000
- Revision:
- 17:18e9df406502
- Parent:
- 16:3c9a3ff09765
- Child:
- 18:cbe3fe6d89a7
Met PID en calibrate
Who changed what in which revision?
| User | Revision | Line number | New 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 |
| meikefrok | 5:3d88f7506cd9 | 17 | Motor Cart(D5, D4, D4, Brakeable); // right motor |
| meikefrok | 5:3d88f7506cd9 | 18 | Motor Arm(D6,D7, D7, Brakeable); // left motor |
| meikefrok | 5:3d88f7506cd9 | 19 | |
| meikefrok | 5:3d88f7506cd9 | 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 | 10:31e4c3d71ee6 | 27 | HIDScope scope(2); //scope has two ports for the two EMG signals |
| meikefrok | 10:31e4c3d71ee6 | 28 | |
| meikefrok | 5:3d88f7506cd9 | 29 | //======== Miscellaneous ======================================================= |
| meikefrok | 5:3d88f7506cd9 | 30 | // button |
| meikefrok | 5:3d88f7506cd9 | 31 | InterruptIn btn(SW2); |
| meikefrok | 5:3d88f7506cd9 | 32 | InterruptIn btn2(SW3); |
| meikefrok | 5:3d88f7506cd9 | 33 | |
| meikefrok | 5:3d88f7506cd9 | 34 | InterruptIn btn_cart(D1); |
| meikefrok | 5:3d88f7506cd9 | 35 | InterruptIn btn_arm(D2); |
| meikefrok | 5:3d88f7506cd9 | 36 | InterruptIn btn_claw(D3); |
| meikefrok | 5:3d88f7506cd9 | 37 | |
| meikefrok | 5:3d88f7506cd9 | 38 | // led |
| meikefrok | 2:ad4b181a6422 | 39 | DigitalOut led_r(LED_RED); |
| meikefrok | 2:ad4b181a6422 | 40 | DigitalOut led_g(LED_GREEN); |
| meikefrok | 2:ad4b181a6422 | 41 | DigitalOut led_b(LED_BLUE); |
| megrootens | 0:048fbd80203e | 42 | |
| meikefrok | 5:3d88f7506cd9 | 43 | // potmeter |
| meikefrok | 5:3d88f7506cd9 | 44 | AnalogIn pot_cart(A2); |
| meikefrok | 5:3d88f7506cd9 | 45 | AnalogIn pot_arm(A3); |
| megrootens | 0:048fbd80203e | 46 | |
| meikefrok | 7:9715323b20ac | 47 | 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 |
| meikefrok | 7:9715323b20ac | 48 | AnalogIn emgr(A0); |
| meikefrok | 7:9715323b20ac | 49 | |
| meikefrok | 5:3d88f7506cd9 | 50 | // ticker |
| meikefrok | 8:ac4e5afbdbcd | 51 | Ticker tick_part; // ticker to switch parts |
| meikefrok | 15:caf29b6f5261 | 52 | Ticker tick_part_arm; |
| meikefrok | 15:caf29b6f5261 | 53 | Ticker tick_part_claw; |
| meikefrok | 10:31e4c3d71ee6 | 54 | Ticker sampleTicker; |
| meikefrok | 15:caf29b6f5261 | 55 | Ticker measureTicker; |
| meikefrok | 15:caf29b6f5261 | 56 | Ticker sampleTicker_claw; |
| meikefrok | 5:3d88f7506cd9 | 57 | |
| meikefrok | 5:3d88f7506cd9 | 58 | //======== Variables =========================================================== |
| megrootens | 0:048fbd80203e | 59 | // counters |
| meikefrok | 5:3d88f7506cd9 | 60 | int num_turned_on_0 = 0; // count number of times red LED turned on |
| meikefrok | 5:3d88f7506cd9 | 61 | int num_turned_on_1 = 0; // count number of times green LED turned on |
| meikefrok | 5:3d88f7506cd9 | 62 | int num_turned_on_2 = 0; // count number of times blue LED turned on |
| meikefrok | 5:3d88f7506cd9 | 63 | |
| meikefrok | 5:3d88f7506cd9 | 64 | int num_claw_turned_on_0 = 0; // count number of times red LED turned on |
| meikefrok | 5:3d88f7506cd9 | 65 | int num_claw_turned_on_1 = 0; // count number of times green LED turned on |
| meikefrok | 5:3d88f7506cd9 | 66 | int num_claw_turned_on_2 = 0; // count number of times blue LED turned on |
| megrootens | 0:048fbd80203e | 67 | |
| meikefrok | 5:3d88f7506cd9 | 68 | // speed |
| meikefrok | 16:3c9a3ff09765 | 69 | double cart_speed = 0.2; |
| meikefrok | 6:23b1ed826b59 | 70 | double arm_speed = 0.1; |
| megrootens | 0:048fbd80203e | 71 | |
| meikefrok | 5:3d88f7506cd9 | 72 | // position |
| meikefrok | 12:b31df384b170 | 73 | float factor_cart = 0.05802; |
| meikefrok | 5:3d88f7506cd9 | 74 | float factor_arm = 0.1539; |
| meikefrok | 5:3d88f7506cd9 | 75 | int position_cart; |
| meikefrok | 5:3d88f7506cd9 | 76 | int position_arm; |
| meikefrok | 5:3d88f7506cd9 | 77 | float ain_cart; //Variable to store the analog input of the cart |
| meikefrok | 5:3d88f7506cd9 | 78 | float ain_arm; //Variable to store the analog input of the arm |
| megrootens | 0:048fbd80203e | 79 | |
| meikefrok | 6:23b1ed826b59 | 80 | int position_claw; |
| meikefrok | 6:23b1ed826b59 | 81 | |
| meikefrok | 6:23b1ed826b59 | 82 | |
| meikefrok | 5:3d88f7506cd9 | 83 | // miscellaneous |
| meikefrok | 12:b31df384b170 | 84 | const float kTimeToggle = 0.05f; // period with which to toggle the parts |
| meikefrok | 5:3d88f7506cd9 | 85 | const int LedOn = 0; // LED on if 0 |
| meikefrok | 7:9715323b20ac | 86 | volatile int part_id = 1; // ID of what part should move, begins with the cart |
| meikefrok | 5:3d88f7506cd9 | 87 | volatile int servo_id = 1; // ID to the side the servo should move, begins in center position |
| meikefrok | 3:3a671d01bcb8 | 88 | |
| meikefrok | 15:caf29b6f5261 | 89 | int waiting_claw = 1000; |
| meikefrok | 15:caf29b6f5261 | 90 | |
| meikefrok | 7:9715323b20ac | 91 | //======== Variables Filter ==================================================== |
| meikefrok | 7:9715323b20ac | 92 | /*coefficients of each filter |
| meikefrok | 7:9715323b20ac | 93 | lno = left tricep notch filter |
| meikefrok | 7:9715323b20ac | 94 | lhf = left tricep high pass filter |
| meikefrok | 7:9715323b20ac | 95 | llf = left tricep lowpass filter |
| meikefrok | 7:9715323b20ac | 96 | same goes for rno etc. |
| meikefrok | 7:9715323b20ac | 97 | */ |
| meikefrok | 7:9715323b20ac | 98 | double lno_b0 = 0.9911; |
| meikefrok | 7:9715323b20ac | 99 | double lno_b1 = -1.6036; |
| meikefrok | 7:9715323b20ac | 100 | double lno_b2 = 0.9911; |
| meikefrok | 7:9715323b20ac | 101 | double lno_a1 = -1.603; |
| meikefrok | 7:9715323b20ac | 102 | double lno_a2 = 0.9822; |
| meikefrok | 7:9715323b20ac | 103 | |
| meikefrok | 7:9715323b20ac | 104 | double rno_b0 = 0.9911; |
| meikefrok | 7:9715323b20ac | 105 | double rno_b1 = -1.6036; |
| meikefrok | 7:9715323b20ac | 106 | double rno_b2 = 0.9911; |
| meikefrok | 7:9715323b20ac | 107 | double rno_a1 = -1.603; |
| meikefrok | 7:9715323b20ac | 108 | double rno_a2 = 0.9822; |
| meikefrok | 7:9715323b20ac | 109 | |
| meikefrok | 7:9715323b20ac | 110 | double lno2_b0 = 0.9824; |
| meikefrok | 7:9715323b20ac | 111 | double lno2_b1 = -0.6071; |
| meikefrok | 7:9715323b20ac | 112 | double lno2_b2 = 0.9824; |
| meikefrok | 7:9715323b20ac | 113 | double lno2_a1 = -0.6071; |
| meikefrok | 7:9715323b20ac | 114 | double lno2_a2 = 0.9647; |
| meikefrok | 7:9715323b20ac | 115 | |
| meikefrok | 7:9715323b20ac | 116 | double rno2_b0 = 0.9824; |
| meikefrok | 7:9715323b20ac | 117 | double rno2_b1 = -0.6071; |
| meikefrok | 7:9715323b20ac | 118 | double rno2_b2 = 0.9824; |
| meikefrok | 7:9715323b20ac | 119 | double rno2_a1 = -0.6071; |
| meikefrok | 7:9715323b20ac | 120 | double rno2_a2 = 0.9647; |
| meikefrok | 7:9715323b20ac | 121 | |
| meikefrok | 7:9715323b20ac | 122 | double lhf_b0 = 0.9355; |
| meikefrok | 7:9715323b20ac | 123 | double lhf_b1 = -1.8711; |
| meikefrok | 7:9715323b20ac | 124 | double lhf_b2 = 0.9355; |
| meikefrok | 7:9715323b20ac | 125 | double lhf_a1 = -1.8669; |
| meikefrok | 7:9715323b20ac | 126 | double lhf_a2 = 0.8752; |
| meikefrok | 7:9715323b20ac | 127 | |
| meikefrok | 7:9715323b20ac | 128 | double rhf_b0 = 0.9355; |
| meikefrok | 7:9715323b20ac | 129 | double rhf_b1 = -1.8711; |
| meikefrok | 7:9715323b20ac | 130 | double rhf_b2 = 0.9355; |
| meikefrok | 7:9715323b20ac | 131 | double rhf_a1 = -1.8669; |
| meikefrok | 7:9715323b20ac | 132 | double rhf_a2 = 0.8752; |
| meikefrok | 7:9715323b20ac | 133 | |
| meikefrok | 7:9715323b20ac | 134 | |
| meikefrok | 7:9715323b20ac | 135 | double llf_b0 = 8.7656e-5; |
| meikefrok | 7:9715323b20ac | 136 | double llf_b1 = 1.17531e-4; |
| meikefrok | 7:9715323b20ac | 137 | double llf_b2 = 8.7656e-5; |
| meikefrok | 7:9715323b20ac | 138 | double llf_a1 = -1.9733; |
| meikefrok | 7:9715323b20ac | 139 | double llf_a2 = 0.9737; |
| meikefrok | 7:9715323b20ac | 140 | |
| meikefrok | 7:9715323b20ac | 141 | double rlf_b0 = 8.7656e-5; |
| meikefrok | 7:9715323b20ac | 142 | double rlf_b1 = 1.17531e-4; |
| meikefrok | 7:9715323b20ac | 143 | double rlf_b2 = 8.7656e-5; |
| meikefrok | 7:9715323b20ac | 144 | double rlf_a1 = -1.9733; |
| meikefrok | 7:9715323b20ac | 145 | double rlf_a2 = 0.9737; |
| meikefrok | 7:9715323b20ac | 146 | |
| meikefrok | 7:9715323b20ac | 147 | |
| meikefrok | 7:9715323b20ac | 148 | //starting values of the biquads of the corresponding filters |
| meikefrok | 7:9715323b20ac | 149 | double lno_v1 = 0, lno_v2 = 0; |
| meikefrok | 7:9715323b20ac | 150 | double lno2_v1 = 0, lno2_v2 = 0; |
| meikefrok | 7:9715323b20ac | 151 | double lhf_v1 = 0, lhf_v2 = 0; |
| meikefrok | 7:9715323b20ac | 152 | double llf_v1 = 0, llf_v2 = 0; |
| meikefrok | 7:9715323b20ac | 153 | |
| meikefrok | 7:9715323b20ac | 154 | double rno_v1 = 0, rno_v2 = 0; |
| meikefrok | 7:9715323b20ac | 155 | double rno2_v1 = 0, rno2_v2 = 0; |
| meikefrok | 7:9715323b20ac | 156 | double rhf_v1 = 0, rhf_v2 = 0; |
| meikefrok | 7:9715323b20ac | 157 | double rlf_v1 = 0, rlf_v2 = 0; |
| meikefrok | 7:9715323b20ac | 158 | |
| meikefrok | 7:9715323b20ac | 159 | /* declaration of the outputs of each biquad. |
| meikefrok | 7:9715323b20ac | 160 | the output of the previous biquad is the input for the next biquad. |
| meikefrok | 7:9715323b20ac | 161 | so lno_y goes into lhf_y etc. |
| meikefrok | 7:9715323b20ac | 162 | */ |
| meikefrok | 7:9715323b20ac | 163 | double lno_y; |
| meikefrok | 7:9715323b20ac | 164 | double lno2_y; |
| meikefrok | 7:9715323b20ac | 165 | double lhf_y; |
| meikefrok | 7:9715323b20ac | 166 | double llf_y; |
| meikefrok | 7:9715323b20ac | 167 | double lrect_y; |
| meikefrok | 7:9715323b20ac | 168 | double rno_y; |
| meikefrok | 7:9715323b20ac | 169 | double rno2_y; |
| meikefrok | 7:9715323b20ac | 170 | double rhf_y; |
| meikefrok | 7:9715323b20ac | 171 | double rlf_y; |
| meikefrok | 7:9715323b20ac | 172 | double rrect_y; |
| meikefrok | 7:9715323b20ac | 173 | |
| meikefrok | 16:3c9a3ff09765 | 174 | |
| meikefrok | 7:9715323b20ac | 175 | // set the threshold value for the filtered signal |
| meikefrok | 7:9715323b20ac | 176 | //if the signal exceeds this value the motors will start to rotate |
| meikefrok | 16:3c9a3ff09765 | 177 | const double threshold_value= 0.1; |
| meikefrok | 15:caf29b6f5261 | 178 | const double threshold_value_claw= 0.08; |
| meikefrok | 16:3c9a3ff09765 | 179 | const double threshold_value_cart=0.08; |
| meikefrok | 15:caf29b6f5261 | 180 | |
| meikefrok | 7:9715323b20ac | 181 | |
| meikefrok | 7:9715323b20ac | 182 | |
| meikefrok | 7:9715323b20ac | 183 | /* declaration of each biquad |
| meikefrok | 7:9715323b20ac | 184 | The coefficients will be filled in later on in void scopeSend |
| meikefrok | 7:9715323b20ac | 185 | As said before the input of each biquad is the output of the previous one |
| meikefrok | 7:9715323b20ac | 186 | The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal. |
| meikefrok | 7:9715323b20ac | 187 | This is done for both left and right so this makes two chains of 3 biquads */ |
| meikefrok | 7:9715323b20ac | 188 | |
| meikefrok | 7:9715323b20ac | 189 | double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 190 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 191 | { |
| meikefrok | 7:9715323b20ac | 192 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 193 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 194 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 195 | v1 = v; |
| meikefrok | 7:9715323b20ac | 196 | return y; |
| meikefrok | 7:9715323b20ac | 197 | } |
| meikefrok | 7:9715323b20ac | 198 | |
| meikefrok | 7:9715323b20ac | 199 | double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 200 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 201 | { |
| meikefrok | 7:9715323b20ac | 202 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 203 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 204 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 205 | v1 = v; |
| meikefrok | 7:9715323b20ac | 206 | return y; |
| meikefrok | 7:9715323b20ac | 207 | } |
| meikefrok | 7:9715323b20ac | 208 | |
| meikefrok | 7:9715323b20ac | 209 | double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 210 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 211 | { |
| meikefrok | 7:9715323b20ac | 212 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 213 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 214 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 215 | v1 = v; |
| meikefrok | 7:9715323b20ac | 216 | return y; |
| meikefrok | 7:9715323b20ac | 217 | } |
| meikefrok | 7:9715323b20ac | 218 | |
| meikefrok | 7:9715323b20ac | 219 | double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 220 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 221 | { |
| meikefrok | 7:9715323b20ac | 222 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 223 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 224 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 225 | v1 = v; |
| meikefrok | 7:9715323b20ac | 226 | return y; |
| meikefrok | 7:9715323b20ac | 227 | } |
| meikefrok | 7:9715323b20ac | 228 | double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 229 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 230 | { |
| meikefrok | 7:9715323b20ac | 231 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 232 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 233 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 234 | v1 = v; |
| meikefrok | 7:9715323b20ac | 235 | return y; |
| meikefrok | 7:9715323b20ac | 236 | } |
| meikefrok | 7:9715323b20ac | 237 | |
| meikefrok | 7:9715323b20ac | 238 | double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 239 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 240 | { |
| meikefrok | 7:9715323b20ac | 241 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 242 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 243 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 244 | v1 = v; |
| meikefrok | 7:9715323b20ac | 245 | return y; |
| meikefrok | 7:9715323b20ac | 246 | } |
| meikefrok | 7:9715323b20ac | 247 | |
| meikefrok | 7:9715323b20ac | 248 | double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 249 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 250 | { |
| meikefrok | 7:9715323b20ac | 251 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 252 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 253 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 254 | v1 = v; |
| meikefrok | 7:9715323b20ac | 255 | return y; |
| meikefrok | 7:9715323b20ac | 256 | } |
| meikefrok | 7:9715323b20ac | 257 | |
| meikefrok | 7:9715323b20ac | 258 | double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , |
| meikefrok | 7:9715323b20ac | 259 | const double b1 , const double b2 ) |
| meikefrok | 7:9715323b20ac | 260 | { |
| meikefrok | 7:9715323b20ac | 261 | double v = u - a1*v1 - a2*v2; |
| meikefrok | 7:9715323b20ac | 262 | double y = b0*v + b1*v1 + b2*v2; |
| meikefrok | 7:9715323b20ac | 263 | v2 = v1; |
| meikefrok | 7:9715323b20ac | 264 | v1 = v; |
| meikefrok | 7:9715323b20ac | 265 | return y; |
| meikefrok | 7:9715323b20ac | 266 | } |
| meikefrok | 7:9715323b20ac | 267 | |
| meikefrok | 17:18e9df406502 | 268 | int t = 0; |
| meikefrok | 17:18e9df406502 | 269 | double value_right = 0; |
| meikefrok | 17:18e9df406502 | 270 | double value_left = 0; |
| meikefrok | 17:18e9df406502 | 271 | |
| meikefrok | 17:18e9df406502 | 272 | double calibrate_right(){ |
| meikefrok | 17:18e9df406502 | 273 | for(t=0; t<200; t++){ |
| meikefrok | 17:18e9df406502 | 274 | if(value_right <= rlf_y){ |
| meikefrok | 17:18e9df406502 | 275 | value_right = rlf_y; |
| meikefrok | 17:18e9df406502 | 276 | }else{ |
| meikefrok | 17:18e9df406502 | 277 | value_right = value_right +0.0 ; |
| meikefrok | 17:18e9df406502 | 278 | } |
| meikefrok | 17:18e9df406502 | 279 | } |
| meikefrok | 17:18e9df406502 | 280 | return value_right; |
| meikefrok | 17:18e9df406502 | 281 | } |
| meikefrok | 17:18e9df406502 | 282 | |
| meikefrok | 17:18e9df406502 | 283 | double calibrate_left(){ |
| meikefrok | 17:18e9df406502 | 284 | for(t=0; t<200; t++){ |
| meikefrok | 17:18e9df406502 | 285 | if(value_left <= llf_y){ |
| meikefrok | 17:18e9df406502 | 286 | value_left = llf_y; |
| meikefrok | 17:18e9df406502 | 287 | }else{ |
| meikefrok | 17:18e9df406502 | 288 | value_left = value_left +0.0 ; |
| meikefrok | 17:18e9df406502 | 289 | } |
| meikefrok | 17:18e9df406502 | 290 | } |
| meikefrok | 17:18e9df406502 | 291 | return value_left; |
| meikefrok | 17:18e9df406502 | 292 | } |
| meikefrok | 17:18e9df406502 | 293 | |
| meikefrok | 17:18e9df406502 | 294 | |
| meikefrok | 7:9715323b20ac | 295 | /* function that calculates the filtered EMG signal from the raw EMG signal. |
| meikefrok | 7:9715323b20ac | 296 | So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal. |
| meikefrok | 7:9715323b20ac | 297 | After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like. |
| meikefrok | 7:9715323b20ac | 298 | The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/ |
| meikefrok | 7:9715323b20ac | 299 | |
| meikefrok | 17:18e9df406502 | 300 | //==========PID================ |
| meikefrok | 17:18e9df406502 | 301 | //setpoints |
| meikefrok | 17:18e9df406502 | 302 | const double Setpoint_Translation = -200; |
| meikefrok | 17:18e9df406502 | 303 | const double Setpoint_Back = 0; |
| meikefrok | 17:18e9df406502 | 304 | const double Setpoint_Rotation = 0; |
| meikefrok | 17:18e9df406502 | 305 | double M3_ControlSpeed = 0; |
| meikefrok | 17:18e9df406502 | 306 | double M2_ControlSpeed = 0; |
| meikefrok | 17:18e9df406502 | 307 | double SetpointError_Translation = 0; |
| meikefrok | 17:18e9df406502 | 308 | double SetpointError_Rotation = 0; |
| meikefrok | 17:18e9df406502 | 309 | |
| meikefrok | 17:18e9df406502 | 310 | //booleans for control |
| meikefrok | 17:18e9df406502 | 311 | bool booltranslate = false; |
| meikefrok | 17:18e9df406502 | 312 | bool boolrotate = false; |
| meikefrok | 17:18e9df406502 | 313 | |
| meikefrok | 17:18e9df406502 | 314 | |
| meikefrok | 17:18e9df406502 | 315 | double setpoint; |
| meikefrok | 17:18e9df406502 | 316 | double Arm_ControlSpeed = 0; |
| meikefrok | 17:18e9df406502 | 317 | double Cart_ControlSpeed = 0; |
| meikefrok | 17:18e9df406502 | 318 | |
| meikefrok | 17:18e9df406502 | 319 | //Cart PID |
| meikefrok | 17:18e9df406502 | 320 | const double Ts = 0.001953125; //Ts=1/fs (sample frequency) |
| meikefrok | 17:18e9df406502 | 321 | const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4; |
| meikefrok | 17:18e9df406502 | 322 | double Translation_error = 0; |
| meikefrok | 17:18e9df406502 | 323 | double Translation_e_prev = 0; |
| meikefrok | 17:18e9df406502 | 324 | |
| meikefrok | 17:18e9df406502 | 325 | //Arm PID |
| meikefrok | 17:18e9df406502 | 326 | const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429; |
| meikefrok | 17:18e9df406502 | 327 | double Rotation_error = 0; |
| meikefrok | 17:18e9df406502 | 328 | double Rotation_e_prev = 0; |
| meikefrok | 17:18e9df406502 | 329 | |
| meikefrok | 17:18e9df406502 | 330 | double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev) |
| meikefrok | 17:18e9df406502 | 331 | { |
| meikefrok | 17:18e9df406502 | 332 | double e_der = (error - e_prev) / Ts; |
| meikefrok | 17:18e9df406502 | 333 | e_prev = error; |
| meikefrok | 17:18e9df406502 | 334 | e_int = e_int + (Ts * error); |
| meikefrok | 17:18e9df406502 | 335 | |
| meikefrok | 17:18e9df406502 | 336 | return kp*error + ki + e_int + kd + e_der; |
| meikefrok | 17:18e9df406502 | 337 | } |
| meikefrok | 17:18e9df406502 | 338 | |
| meikefrok | 17:18e9df406502 | 339 | |
| meikefrok | 7:9715323b20ac | 340 | |
| meikefrok | 7:9715323b20ac | 341 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |
| meikefrok | 7:9715323b20ac | 342 | //======== Functions and main ============================================================== |
| meikefrok | 7:9715323b20ac | 343 | /* function that calculates the filtered EMG signal from the raw EMG signal. |
| meikefrok | 7:9715323b20ac | 344 | So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal. |
| meikefrok | 7:9715323b20ac | 345 | After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like. |
| meikefrok | 7:9715323b20ac | 346 | The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/ |
| meikefrok | 7:9715323b20ac | 347 | void scopeSend(void){ |
| meikefrok | 7:9715323b20ac | 348 | lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2); |
| meikefrok | 7:9715323b20ac | 349 | lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2); |
| meikefrok | 7:9715323b20ac | 350 | lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2); |
| meikefrok | 7:9715323b20ac | 351 | lrect_y = fabs(lhf_y); |
| meikefrok | 17:18e9df406502 | 352 | 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 | 353 | |
| meikefrok | 7:9715323b20ac | 354 | rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2); |
| meikefrok | 7:9715323b20ac | 355 | rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2); |
| meikefrok | 7:9715323b20ac | 356 | rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2); |
| meikefrok | 7:9715323b20ac | 357 | rrect_y = fabs(rhf_y); |
| meikefrok | 17:18e9df406502 | 358 | 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 | 359 | |
| meikefrok | 16:3c9a3ff09765 | 360 | scope.set(1, llf_y); |
| meikefrok | 16:3c9a3ff09765 | 361 | scope.set(0, rlf_y); |
| meikefrok | 16:3c9a3ff09765 | 362 | scope.send(); |
| meikefrok | 7:9715323b20ac | 363 | } |
| meikefrok | 7:9715323b20ac | 364 | |
| meikefrok | 5:3d88f7506cd9 | 365 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> |
| meikefrok | 5:3d88f7506cd9 | 366 | //======== Functions and main ============================================================== |
| meikefrok | 10:31e4c3d71ee6 | 367 | void Measure(){ |
| meikefrok | 10:31e4c3d71ee6 | 368 | // encoder |
| meikefrok | 10:31e4c3d71ee6 | 369 | position_cart = (Encoder_Cart.getPulses()*factor_cart) ; |
| meikefrok | 10:31e4c3d71ee6 | 370 | ain_cart = pot_cart.read(); |
| meikefrok | 12:b31df384b170 | 371 | |
| meikefrok | 13:dd7b41766f5f | 372 | // encoder |
| meikefrok | 13:dd7b41766f5f | 373 | position_arm = (Encoder_Arm.getPulses()*factor_arm) ; |
| meikefrok | 13:dd7b41766f5f | 374 | ain_arm = pot_arm.read(); |
| meikefrok | 13:dd7b41766f5f | 375 | |
| meikefrok | 16:3c9a3ff09765 | 376 | if (ain_arm == 0){ |
| meikefrok | 16:3c9a3ff09765 | 377 | Encoder_Arm.reset(); |
| meikefrok | 16:3c9a3ff09765 | 378 | }else {} |
| meikefrok | 13:dd7b41766f5f | 379 | |
| meikefrok | 12:b31df384b170 | 380 | |
| meikefrok | 10:31e4c3d71ee6 | 381 | |
| meikefrok | 10:31e4c3d71ee6 | 382 | } |
| meikefrok | 10:31e4c3d71ee6 | 383 | |
| meikefrok | 3:3a671d01bcb8 | 384 | |
| meikefrok | 5:3d88f7506cd9 | 385 | // Switch between Cart, Arm and Claw |
| meikefrok | 7:9715323b20ac | 386 | |
| meikefrok | 8:ac4e5afbdbcd | 387 | void SwitchCart(){ |
| meikefrok | 2:ad4b181a6422 | 388 | switch (part_id) { |
| meikefrok | 5:3d88f7506cd9 | 389 | //Cart |
| meikefrok | 3:3a671d01bcb8 | 390 | case 2: { |
| meikefrok | 15:caf29b6f5261 | 391 | led_r = LedOn; |
| meikefrok | 5:3d88f7506cd9 | 392 | if(led_r == LedOn){ |
| megrootens | 0:048fbd80203e | 393 | num_turned_on_0++; |
| meikefrok | 5:3d88f7506cd9 | 394 | |
| meikefrok | 16:3c9a3ff09765 | 395 | if (rlf_y > threshold_value_cart) { |
| meikefrok | 12:b31df384b170 | 396 | if(position_cart <= -170){ //If the cart is at the right side, it stops |
| meikefrok | 6:23b1ed826b59 | 397 | Cart.stop(1)==1; |
| meikefrok | 16:3c9a3ff09765 | 398 | |
| meikefrok | 16:3c9a3ff09765 | 399 | }else if(position_cart >= 170 && position_arm <=-100){ //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. |
| meikefrok | 6:23b1ed826b59 | 400 | Cart.stop(1) == 1; |
| meikefrok | 6:23b1ed826b59 | 401 | |
| meikefrok | 16:3c9a3ff09765 | 402 | }else if(position_cart >= 170 && position_arm <=-160 && position_claw == -18){ |
| meikefrok | 6:23b1ed826b59 | 403 | Cart.stop(1) == 1; |
| meikefrok | 5:3d88f7506cd9 | 404 | |
| meikefrok | 5:3d88f7506cd9 | 405 | }else{ |
| meikefrok | 5:3d88f7506cd9 | 406 | Cart.speed(cart_speed)==cart_speed; |
| meikefrok | 5:3d88f7506cd9 | 407 | } |
| meikefrok | 12:b31df384b170 | 408 | |
| meikefrok | 16:3c9a3ff09765 | 409 | if (llf_y > threshold_value_cart){ |
| meikefrok | 12:b31df384b170 | 410 | Cart.stop(1)==1; |
| meikefrok | 12:b31df384b170 | 411 | } |
| meikefrok | 5:3d88f7506cd9 | 412 | |
| meikefrok | 16:3c9a3ff09765 | 413 | }else if (llf_y > threshold_value_cart) { |
| meikefrok | 16:3c9a3ff09765 | 414 | if(position_cart >= 170){ //If the cart is at the left side, it stops |
| meikefrok | 6:23b1ed826b59 | 415 | Cart.stop(1)==1; |
| meikefrok | 16:3c9a3ff09765 | 416 | |
| meikefrok | 16:3c9a3ff09765 | 417 | }else if(position_cart <= -170 && position_arm >=100){ //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. |
| meikefrok | 12:b31df384b170 | 418 | Cart.stop(1) == 1; |
| meikefrok | 12:b31df384b170 | 419 | |
| meikefrok | 16:3c9a3ff09765 | 420 | }else if(position_cart <= -170 && position_arm >=160 && position_claw == 27){ |
| meikefrok | 6:23b1ed826b59 | 421 | Cart.stop(1)==1; |
| meikefrok | 6:23b1ed826b59 | 422 | |
| meikefrok | 5:3d88f7506cd9 | 423 | }else{ |
| meikefrok | 5:3d88f7506cd9 | 424 | Cart.speed(-cart_speed)==-cart_speed; |
| meikefrok | 5:3d88f7506cd9 | 425 | } |
| meikefrok | 12:b31df384b170 | 426 | |
| meikefrok | 16:3c9a3ff09765 | 427 | if (rlf_y > threshold_value_cart){ |
| meikefrok | 12:b31df384b170 | 428 | Cart.stop(1)==1; |
| meikefrok | 12:b31df384b170 | 429 | } |
| meikefrok | 5:3d88f7506cd9 | 430 | |
| meikefrok | 5:3d88f7506cd9 | 431 | }else { |
| meikefrok | 12:b31df384b170 | 432 | Cart.stop(1)==1; |
| meikefrok | 5:3d88f7506cd9 | 433 | } |
| megrootens | 0:048fbd80203e | 434 | } |
| meikefrok | 12:b31df384b170 | 435 | |
| meikefrok | 12:b31df384b170 | 436 | if(!btn && !btn2) { |
| meikefrok | 12:b31df384b170 | 437 | Arm.speed(0) == 0; |
| meikefrok | 12:b31df384b170 | 438 | if(position_cart<0){ |
| meikefrok | 12:b31df384b170 | 439 | Cart.speed(-0.1)== -0.1; |
| meikefrok | 12:b31df384b170 | 440 | }else if(position_cart>0){ |
| meikefrok | 12:b31df384b170 | 441 | Cart.speed(0.1)==0.1; |
| meikefrok | 12:b31df384b170 | 442 | }else{ |
| meikefrok | 12:b31df384b170 | 443 | Cart.stop(0)==0; |
| meikefrok | 12:b31df384b170 | 444 | } |
| meikefrok | 12:b31df384b170 | 445 | } |
| meikefrok | 12:b31df384b170 | 446 | |
| meikefrok | 17:18e9df406502 | 447 | SetpointError_Translation = setpoint - position_cart; |
| meikefrok | 17:18e9df406502 | 448 | |
| meikefrok | 17:18e9df406502 | 449 | //set direction |
| meikefrok | 17:18e9df406502 | 450 | if (SetpointError_Translation < 0) { |
| meikefrok | 17:18e9df406502 | 451 | cart_speed = 0; |
| meikefrok | 17:18e9df406502 | 452 | } else {} |
| meikefrok | 17:18e9df406502 | 453 | |
| meikefrok | 17:18e9df406502 | 454 | |
| meikefrok | 17:18e9df406502 | 455 | Cart_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev)); |
| meikefrok | 17:18e9df406502 | 456 | if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) { |
| meikefrok | 17:18e9df406502 | 457 | cart_speed = 0; |
| meikefrok | 17:18e9df406502 | 458 | |
| meikefrok | 17:18e9df406502 | 459 | } |
| meikefrok | 17:18e9df406502 | 460 | else{ |
| meikefrok | 17:18e9df406502 | 461 | cart_speed = Cart_ControlSpeed; |
| meikefrok | 17:18e9df406502 | 462 | } |
| meikefrok | 17:18e9df406502 | 463 | |
| meikefrok | 12:b31df384b170 | 464 | |
| meikefrok | 5:3d88f7506cd9 | 465 | // controle LED |
| meikefrok | 3:3a671d01bcb8 | 466 | led_g = not LedOn; |
| meikefrok | 5:3d88f7506cd9 | 467 | led_b = not LedOn; |
| meikefrok | 12:b31df384b170 | 468 | |
| meikefrok | 5:3d88f7506cd9 | 469 | pc.baud(115200); |
| meikefrok | 5:3d88f7506cd9 | 470 | pc.printf("Distance in mm: %i\n", position_cart); |
| meikefrok | 5:3d88f7506cd9 | 471 | |
| megrootens | 0:048fbd80203e | 472 | break; |
| megrootens | 0:048fbd80203e | 473 | } |
| meikefrok | 8:ac4e5afbdbcd | 474 | } |
| meikefrok | 8:ac4e5afbdbcd | 475 | } |
| meikefrok | 8:ac4e5afbdbcd | 476 | |
| meikefrok | 8:ac4e5afbdbcd | 477 | void SwitchArm(){ |
| meikefrok | 8:ac4e5afbdbcd | 478 | switch (part_id) { |
| meikefrok | 8:ac4e5afbdbcd | 479 | //Cart |
| meikefrok | 8:ac4e5afbdbcd | 480 | case 3: { |
| meikefrok | 15:caf29b6f5261 | 481 | led_g = LedOn; |
| meikefrok | 8:ac4e5afbdbcd | 482 | if(led_g == LedOn){ |
| meikefrok | 8:ac4e5afbdbcd | 483 | num_turned_on_1++; |
| meikefrok | 13:dd7b41766f5f | 484 | Arm.stop(1)==1; |
| meikefrok | 13:dd7b41766f5f | 485 | |
| meikefrok | 8:ac4e5afbdbcd | 486 | |
| meikefrok | 16:3c9a3ff09765 | 487 | if (rlf_y > threshold_value_claw) { |
| meikefrok | 17:18e9df406502 | 488 | if(position_cart > -170 && position_arm >= 90){ //If the cart is not at the end, the arm can't move any further than 45 degrees |
| meikefrok | 8:ac4e5afbdbcd | 489 | Arm.stop(1)==1; |
| meikefrok | 8:ac4e5afbdbcd | 490 | |
| meikefrok | 16:3c9a3ff09765 | 491 | }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){ |
| meikefrok | 8:ac4e5afbdbcd | 492 | Arm.stop(1)==1; |
| meikefrok | 8:ac4e5afbdbcd | 493 | |
| meikefrok | 16:3c9a3ff09765 | 494 | }else if(position_cart<= -170 && position_arm>=160){ //If the cart is at the right end, the arm can't move any further than 70 degrees |
| meikefrok | 8:ac4e5afbdbcd | 495 | Arm.stop(1)==1; |
| meikefrok | 16:3c9a3ff09765 | 496 | |
| meikefrok | 8:ac4e5afbdbcd | 497 | }else{ |
| meikefrok | 8:ac4e5afbdbcd | 498 | Arm.speed(arm_speed)==arm_speed; |
| meikefrok | 8:ac4e5afbdbcd | 499 | } |
| meikefrok | 13:dd7b41766f5f | 500 | |
| meikefrok | 13:dd7b41766f5f | 501 | if (llf_y > threshold_value){ |
| meikefrok | 13:dd7b41766f5f | 502 | Cart.stop(1)==1; |
| meikefrok | 13:dd7b41766f5f | 503 | } |
| meikefrok | 8:ac4e5afbdbcd | 504 | |
| meikefrok | 16:3c9a3ff09765 | 505 | }else if (llf_y > threshold_value_claw) { |
| meikefrok | 17:18e9df406502 | 506 | if(position_cart < 170 && position_arm <= -90){ //If the cart is not at the end, the arm can't move any further than 45 degrees |
| meikefrok | 8:ac4e5afbdbcd | 507 | Arm.stop(1)==1; |
| meikefrok | 8:ac4e5afbdbcd | 508 | |
| meikefrok | 16:3c9a3ff09765 | 509 | }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){ |
| meikefrok | 8:ac4e5afbdbcd | 510 | Arm.stop(1)==1; |
| meikefrok | 8:ac4e5afbdbcd | 511 | |
| meikefrok | 16:3c9a3ff09765 | 512 | }else if(position_cart>=170 && position_arm<=-160){ //If the cart is at the left end, the arm can't move any further than 70 degrees |
| meikefrok | 8:ac4e5afbdbcd | 513 | Arm.stop(1)==1; |
| meikefrok | 16:3c9a3ff09765 | 514 | |
| meikefrok | 8:ac4e5afbdbcd | 515 | }else{ |
| meikefrok | 8:ac4e5afbdbcd | 516 | Arm.speed(-arm_speed)==-arm_speed; |
| meikefrok | 8:ac4e5afbdbcd | 517 | } |
| meikefrok | 13:dd7b41766f5f | 518 | |
| meikefrok | 13:dd7b41766f5f | 519 | if (rlf_y > threshold_value){ |
| meikefrok | 13:dd7b41766f5f | 520 | Cart.stop(1)==1; |
| meikefrok | 13:dd7b41766f5f | 521 | } |
| meikefrok | 8:ac4e5afbdbcd | 522 | |
| meikefrok | 8:ac4e5afbdbcd | 523 | }else { |
| meikefrok | 13:dd7b41766f5f | 524 | Arm.stop(1)==1; |
| meikefrok | 13:dd7b41766f5f | 525 | } |
| meikefrok | 13:dd7b41766f5f | 526 | |
| meikefrok | 13:dd7b41766f5f | 527 | if(!btn&&!btn2){ |
| meikefrok | 13:dd7b41766f5f | 528 | Cart.speed(0) == 0; |
| meikefrok | 8:ac4e5afbdbcd | 529 | if(position_arm>0){ |
| meikefrok | 8:ac4e5afbdbcd | 530 | Arm.speed(-0.1)== -0.1; |
| meikefrok | 8:ac4e5afbdbcd | 531 | }else if(position_arm<0){ |
| meikefrok | 8:ac4e5afbdbcd | 532 | Arm.speed(0.1)==0.1; |
| meikefrok | 8:ac4e5afbdbcd | 533 | }else{ |
| meikefrok | 8:ac4e5afbdbcd | 534 | Arm.stop(0)==0; |
| meikefrok | 8:ac4e5afbdbcd | 535 | } |
| meikefrok | 13:dd7b41766f5f | 536 | } |
| meikefrok | 8:ac4e5afbdbcd | 537 | } |
| meikefrok | 17:18e9df406502 | 538 | |
| meikefrok | 17:18e9df406502 | 539 | { |
| meikefrok | 17:18e9df406502 | 540 | |
| meikefrok | 17:18e9df406502 | 541 | SetpointError_Rotation = setpoint - position_arm; |
| meikefrok | 17:18e9df406502 | 542 | |
| meikefrok | 17:18e9df406502 | 543 | //set direction |
| meikefrok | 17:18e9df406502 | 544 | if (SetpointError_Rotation > 0) { |
| meikefrok | 17:18e9df406502 | 545 | arm_speed = 0; |
| meikefrok | 17:18e9df406502 | 546 | } else {} |
| meikefrok | 17:18e9df406502 | 547 | |
| meikefrok | 17:18e9df406502 | 548 | Arm_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev)); |
| meikefrok | 17:18e9df406502 | 549 | if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) { |
| meikefrok | 17:18e9df406502 | 550 | Arm_ControlSpeed = 0; |
| meikefrok | 17:18e9df406502 | 551 | }else{ |
| meikefrok | 17:18e9df406502 | 552 | arm_speed = Arm_ControlSpeed; |
| meikefrok | 17:18e9df406502 | 553 | } |
| meikefrok | 17:18e9df406502 | 554 | } |
| meikefrok | 17:18e9df406502 | 555 | |
| meikefrok | 17:18e9df406502 | 556 | |
| meikefrok | 8:ac4e5afbdbcd | 557 | // controle LED |
| meikefrok | 8:ac4e5afbdbcd | 558 | led_r = not LedOn; |
| meikefrok | 8:ac4e5afbdbcd | 559 | led_b = not LedOn; |
| meikefrok | 8:ac4e5afbdbcd | 560 | |
| meikefrok | 13:dd7b41766f5f | 561 | pc.baud(115200); |
| meikefrok | 13:dd7b41766f5f | 562 | pc.printf("Degrees: %i\n", position_arm); |
| meikefrok | 13:dd7b41766f5f | 563 | |
| meikefrok | 8:ac4e5afbdbcd | 564 | break; |
| megrootens | 0:048fbd80203e | 565 | } |
| meikefrok | 8:ac4e5afbdbcd | 566 | } |
| meikefrok | 8:ac4e5afbdbcd | 567 | } |
| meikefrok | 8:ac4e5afbdbcd | 568 | |
| meikefrok | 15:caf29b6f5261 | 569 | void SwitchClaw(){ |
| meikefrok | 15:caf29b6f5261 | 570 | switch (part_id) { |
| meikefrok | 15:caf29b6f5261 | 571 | case 4: { |
| meikefrok | 15:caf29b6f5261 | 572 | led_b = LedOn; |
| meikefrok | 15:caf29b6f5261 | 573 | |
| meikefrok | 15:caf29b6f5261 | 574 | if(rlf_y > threshold_value_claw){ |
| meikefrok | 15:caf29b6f5261 | 575 | servo_id ++; |
| meikefrok | 15:caf29b6f5261 | 576 | |
| meikefrok | 15:caf29b6f5261 | 577 | switch (servo_id) { |
| meikefrok | 15:caf29b6f5261 | 578 | case 0: { |
| meikefrok | 15:caf29b6f5261 | 579 | led_r = LedOn; |
| meikefrok | 15:caf29b6f5261 | 580 | if (led_r == LedOn) { |
| meikefrok | 15:caf29b6f5261 | 581 | num_claw_turned_on_0++; |
| meikefrok | 15:caf29b6f5261 | 582 | } |
| meikefrok | 15:caf29b6f5261 | 583 | |
| meikefrok | 15:caf29b6f5261 | 584 | led_b = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 585 | led_g = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 586 | servo.position(-18); |
| meikefrok | 15:caf29b6f5261 | 587 | //pc.printf("Servo position is: left \r\n"); |
| meikefrok | 15:caf29b6f5261 | 588 | |
| meikefrok | 15:caf29b6f5261 | 589 | break; |
| meikefrok | 15:caf29b6f5261 | 590 | } |
| meikefrok | 15:caf29b6f5261 | 591 | case 1: { |
| meikefrok | 15:caf29b6f5261 | 592 | led_b = LedOn; |
| meikefrok | 15:caf29b6f5261 | 593 | if (led_b == LedOn) { |
| meikefrok | 15:caf29b6f5261 | 594 | num_claw_turned_on_1++; |
| meikefrok | 15:caf29b6f5261 | 595 | } |
| meikefrok | 15:caf29b6f5261 | 596 | |
| meikefrok | 15:caf29b6f5261 | 597 | led_r = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 598 | led_g = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 599 | |
| meikefrok | 15:caf29b6f5261 | 600 | servo.position(3); |
| meikefrok | 15:caf29b6f5261 | 601 | //pc.printf("Servo position is: center \r\n"); |
| meikefrok | 15:caf29b6f5261 | 602 | |
| meikefrok | 15:caf29b6f5261 | 603 | wait_ms(waiting_claw); |
| meikefrok | 15:caf29b6f5261 | 604 | |
| meikefrok | 15:caf29b6f5261 | 605 | break; |
| meikefrok | 15:caf29b6f5261 | 606 | } |
| meikefrok | 15:caf29b6f5261 | 607 | case 2: { |
| meikefrok | 15:caf29b6f5261 | 608 | led_g = LedOn; |
| meikefrok | 15:caf29b6f5261 | 609 | if (led_g == LedOn) { |
| meikefrok | 15:caf29b6f5261 | 610 | num_claw_turned_on_2++; |
| meikefrok | 15:caf29b6f5261 | 611 | } |
| meikefrok | 15:caf29b6f5261 | 612 | |
| meikefrok | 15:caf29b6f5261 | 613 | led_r = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 614 | led_b = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 615 | |
| meikefrok | 15:caf29b6f5261 | 616 | servo.position(27); |
| meikefrok | 15:caf29b6f5261 | 617 | //pc.printf("Servo position is: %i\r\n", num_claw_turned_on); |
| meikefrok | 15:caf29b6f5261 | 618 | |
| meikefrok | 15:caf29b6f5261 | 619 | break; |
| meikefrok | 15:caf29b6f5261 | 620 | } |
| meikefrok | 15:caf29b6f5261 | 621 | } |
| meikefrok | 15:caf29b6f5261 | 622 | |
| meikefrok | 15:caf29b6f5261 | 623 | |
| meikefrok | 15:caf29b6f5261 | 624 | }else if(llf_y > threshold_value_claw){ |
| meikefrok | 15:caf29b6f5261 | 625 | servo_id --; |
| meikefrok | 15:caf29b6f5261 | 626 | |
| meikefrok | 15:caf29b6f5261 | 627 | switch (servo_id) { |
| meikefrok | 15:caf29b6f5261 | 628 | case 0: { |
| meikefrok | 15:caf29b6f5261 | 629 | led_r = LedOn; |
| meikefrok | 15:caf29b6f5261 | 630 | led_b = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 631 | led_g = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 632 | |
| meikefrok | 15:caf29b6f5261 | 633 | servo.position(-18); |
| meikefrok | 15:caf29b6f5261 | 634 | //pc.printf("Servo position is: left \r\n"); |
| meikefrok | 15:caf29b6f5261 | 635 | |
| meikefrok | 15:caf29b6f5261 | 636 | |
| meikefrok | 15:caf29b6f5261 | 637 | break; |
| meikefrok | 15:caf29b6f5261 | 638 | } |
| meikefrok | 15:caf29b6f5261 | 639 | case 1: { |
| meikefrok | 15:caf29b6f5261 | 640 | led_b = LedOn; |
| meikefrok | 15:caf29b6f5261 | 641 | led_r = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 642 | led_g = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 643 | |
| meikefrok | 15:caf29b6f5261 | 644 | servo.position(3); |
| meikefrok | 15:caf29b6f5261 | 645 | //pc.printf("Servo position is: center \r\n"); |
| meikefrok | 15:caf29b6f5261 | 646 | |
| meikefrok | 15:caf29b6f5261 | 647 | wait_ms(waiting_claw); |
| meikefrok | 15:caf29b6f5261 | 648 | |
| meikefrok | 15:caf29b6f5261 | 649 | break; |
| meikefrok | 15:caf29b6f5261 | 650 | } |
| meikefrok | 15:caf29b6f5261 | 651 | case 2: { |
| meikefrok | 15:caf29b6f5261 | 652 | led_g = LedOn; |
| meikefrok | 15:caf29b6f5261 | 653 | led_r = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 654 | led_b = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 655 | |
| meikefrok | 15:caf29b6f5261 | 656 | servo.position(27); |
| meikefrok | 15:caf29b6f5261 | 657 | //pc.printf("Servo position is: right \r\n"); |
| meikefrok | 15:caf29b6f5261 | 658 | |
| meikefrok | 15:caf29b6f5261 | 659 | |
| meikefrok | 15:caf29b6f5261 | 660 | break; |
| meikefrok | 15:caf29b6f5261 | 661 | } |
| meikefrok | 15:caf29b6f5261 | 662 | } |
| meikefrok | 15:caf29b6f5261 | 663 | |
| meikefrok | 15:caf29b6f5261 | 664 | }else{} |
| meikefrok | 15:caf29b6f5261 | 665 | } |
| meikefrok | 15:caf29b6f5261 | 666 | led_r = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 667 | led_g = not LedOn; |
| meikefrok | 15:caf29b6f5261 | 668 | |
| meikefrok | 15:caf29b6f5261 | 669 | position_claw = servo.read(); |
| meikefrok | 15:caf29b6f5261 | 670 | pc.baud(115200); |
| meikefrok | 15:caf29b6f5261 | 671 | pc.printf("Servo position is: %i, %i, %i \r\n", num_claw_turned_on_0, num_claw_turned_on_1, num_claw_turned_on_2); |
| meikefrok | 15:caf29b6f5261 | 672 | |
| meikefrok | 15:caf29b6f5261 | 673 | break; |
| meikefrok | 15:caf29b6f5261 | 674 | } |
| meikefrok | 15:caf29b6f5261 | 675 | } |
| megrootens | 0:048fbd80203e | 676 | |
| megrootens | 0:048fbd80203e | 677 | |
| meikefrok | 5:3d88f7506cd9 | 678 | // Switch the part |
| meikefrok | 2:ad4b181a6422 | 679 | void SetValue2() { |
| meikefrok | 3:3a671d01bcb8 | 680 | part_id = 2; |
| meikefrok | 6:23b1ed826b59 | 681 | } |
| meikefrok | 2:ad4b181a6422 | 682 | |
| meikefrok | 2:ad4b181a6422 | 683 | void SetValue3() { |
| meikefrok | 3:3a671d01bcb8 | 684 | part_id = 3; |
| meikefrok | 6:23b1ed826b59 | 685 | } |
| meikefrok | 2:ad4b181a6422 | 686 | |
| meikefrok | 2:ad4b181a6422 | 687 | |
| meikefrok | 2:ad4b181a6422 | 688 | void SetValue4() { |
| meikefrok | 3:3a671d01bcb8 | 689 | part_id = 4; |
| megrootens | 0:048fbd80203e | 690 | } |
| megrootens | 0:048fbd80203e | 691 | |
| meikefrok | 2:ad4b181a6422 | 692 | |
| meikefrok | 5:3d88f7506cd9 | 693 | // Main |
| megrootens | 0:048fbd80203e | 694 | int main() |
| megrootens | 0:048fbd80203e | 695 | { |
| meikefrok | 3:3a671d01bcb8 | 696 | led_r = not LedOn; |
| meikefrok | 3:3a671d01bcb8 | 697 | led_g = not LedOn; |
| meikefrok | 3:3a671d01bcb8 | 698 | led_b = not LedOn; |
| megrootens | 0:048fbd80203e | 699 | |
| meikefrok | 8:ac4e5afbdbcd | 700 | tick_part.attach(&SwitchCart,kTimeToggle); |
| meikefrok | 15:caf29b6f5261 | 701 | tick_part_arm.attach(&SwitchArm,kTimeToggle); |
| meikefrok | 15:caf29b6f5261 | 702 | tick_part_claw.attach(&SwitchClaw,0.1f); |
| meikefrok | 11:97f824629da5 | 703 | measureTicker.attach(Measure, 0.005); |
| meikefrok | 15:caf29b6f5261 | 704 | sampleTicker.attach(scopeSend,0.01); |
| meikefrok | 15:caf29b6f5261 | 705 | |
| meikefrok | 5:3d88f7506cd9 | 706 | |
| meikefrok | 3:3a671d01bcb8 | 707 | btn_cart.fall(&SetValue2); |
| meikefrok | 3:3a671d01bcb8 | 708 | btn_arm.fall(&SetValue3); |
| meikefrok | 3:3a671d01bcb8 | 709 | btn_claw.fall(&SetValue4); |
| meikefrok | 2:ad4b181a6422 | 710 | |
| meikefrok | 12:b31df384b170 | 711 | if(part_id == 4){ |
| meikefrok | 12:b31df384b170 | 712 | Cart.stop(1)==1; |
| meikefrok | 12:b31df384b170 | 713 | } |
| meikefrok | 12:b31df384b170 | 714 | |
| megrootens | 0:048fbd80203e | 715 | while (true); |
| meikefrok | 12:b31df384b170 | 716 | |
| megrootens | 0:048fbd80203e | 717 | } |
| meikefrok | 2:ad4b181a6422 | 718 | |
| meikefrok | 2:ad4b181a6422 | 719 | |
| meikefrok | 2:ad4b181a6422 | 720 |