Marjolein Scheffers / Mbed 2 deprecated BioRobotics_Main

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of BioRobotics_Main by Casper Berkhout

main.cpp@5:224c1fe73a8f, 2018-09-28 (annotated)

Committer:
s1680897
Date:
Fri Sep 28 09:43:45 2018 +0000
Revision:
5:224c1fe73a8f
Parent:
4:de0923cf6bcc 
Biorobotics 2017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperBerkhout 0:4141aef83f4b 1 #include "QEI.h"
CasperBerkhout 0:4141aef83f4b 2 #include "math.h"
CasperBerkhout 0:4141aef83f4b 3 #include "mbed.h"
CasperBerkhout 0:4141aef83f4b 4 //#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
CasperBerkhout 0:4141aef83f4b 5 #include "MODSERIAL.h"
CasperBerkhout 0:4141aef83f4b 6 #include "BiQuad.h"
CasperBerkhout 0:4141aef83f4b 7
CasperBerkhout 0:4141aef83f4b 8 //left pot to set reference position.
CasperBerkhout 0:4141aef83f4b 9 //right pot to set Kp, right pot sets Ki when (right)button is pressed down
CasperBerkhout 0:4141aef83f4b 10
CasperBerkhout 0:4141aef83f4b 11
CasperBerkhout 0:4141aef83f4b 12 //--------------object creation------------------
CasperBerkhout 0:4141aef83f4b 13 Serial pc(USBTX, USBRX);
CasperBerkhout 0:4141aef83f4b 14 //Use X4 encoding.
CasperBerkhout 0:4141aef83f4b 15 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
CasperBerkhout 0:4141aef83f4b 16 //Use X2 encoding by default.
CasperBerkhout 0:4141aef83f4b 17 QEI enc1(D13, D12, NC, 32); //enable the encoder
CasperBerkhout 0:4141aef83f4b 18 QEI enc2(D15, D14, NC, 32); //enable the encoder
CasperBerkhout 0:4141aef83f4b 19 PwmOut M1_speed(D6);
CasperBerkhout 0:4141aef83f4b 20 PwmOut M2_speed(D5);
CasperBerkhout 0:4141aef83f4b 21 DigitalOut M1_direction(D7);
CasperBerkhout 0:4141aef83f4b 22 DigitalOut M2_direction(D4);
CasperBerkhout 0:4141aef83f4b 23 AnalogIn potmeter(A0); //left pot
CasperBerkhout 0:4141aef83f4b 24 AnalogIn potmeter2(A1); //right pot
CasperBerkhout 1:d7299175a12e 25 InterruptIn button1(D2); //hardware interrupt for stopping motors - right button
CasperBerkhout 1:d7299175a12e 26 DigitalIn LimSW1(D9);
CasperBerkhout 1:d7299175a12e 27 DigitalIn LimSW2(D8);
CasperBerkhout 1:d7299175a12e 28 DigitalIn HomStart(D3); // - left button
CasperBerkhout 1:d7299175a12e 29
CasperBerkhout 4:de0923cf6bcc 30 BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
CasperBerkhout 0:4141aef83f4b 31 //create scope objects - note: script won't run when HID usb port is not connected
CasperBerkhout 0:4141aef83f4b 32 //HIDScope scope(5); //set # of channels
CasperBerkhout 1:d7299175a12e 33
CasperBerkhout 1:d7299175a12e 34 Ticker motor_update1;
CasperBerkhout 1:d7299175a12e 35 Ticker motor_update2;
CasperBerkhout 1:d7299175a12e 36 Ticker error_update;
CasperBerkhout 0:4141aef83f4b 37
s1680897 5:224c1fe73a8f 38 //Define objects EMG filter
s1680897 5:224c1fe73a8f 39 AnalogIn emg2( A2 );
s1680897 5:224c1fe73a8f 40 AnalogIn emg3( A3 );
s1680897 5:224c1fe73a8f 41 AnalogIn emg4( A4 );
s1680897 5:224c1fe73a8f 42 AnalogIn emg5( A5 );
CasperBerkhout 0:4141aef83f4b 43
s1680897 5:224c1fe73a8f 44
s1680897 5:224c1fe73a8f 45 Ticker sample_timer;
s1680897 5:224c1fe73a8f 46 Ticker scope_timer;
s1680897 5:224c1fe73a8f 47
s1680897 5:224c1fe73a8f 48 DigitalOut ledred(LED_RED);
s1680897 5:224c1fe73a8f 49 DigitalOut ledblue(LED_BLUE);
s1680897 5:224c1fe73a8f 50 DigitalOut ledgreen(LED_GREEN);
s1680897 5:224c1fe73a8f 51 DigitalOut led2(LED_RED);
s1680897 5:224c1fe73a8f 52 DigitalIn button_cal (SW2);
s1680897 5:224c1fe73a8f 53
s1680897 5:224c1fe73a8f 54
s1680897 5:224c1fe73a8f 55 //-----------------variable declaration----------------
CasperBerkhout 0:4141aef83f4b 56 int pwm_freq = 500;
CasperBerkhout 0:4141aef83f4b 57 float set_speed;
CasperBerkhout 0:4141aef83f4b 58 float reference_pos;
CasperBerkhout 0:4141aef83f4b 59
CasperBerkhout 0:4141aef83f4b 60
CasperBerkhout 0:4141aef83f4b 61
CasperBerkhout 0:4141aef83f4b 62 double M1_home;
CasperBerkhout 0:4141aef83f4b 63 double M1_error_pos = 0;
CasperBerkhout 1:d7299175a12e 64 float M1_Kp = 2;
CasperBerkhout 1:d7299175a12e 65 float M1_Ki = 4;
CasperBerkhout 3:455b79d42636 66 float M1_Kd = 0.19;
CasperBerkhout 0:4141aef83f4b 67 double M1_e_int=0;
CasperBerkhout 0:4141aef83f4b 68 double M1_e_prior=0;
CasperBerkhout 0:4141aef83f4b 69
CasperBerkhout 0:4141aef83f4b 70 double M2_home;
CasperBerkhout 0:4141aef83f4b 71 double M2_error_pos = 0;
CasperBerkhout 4:de0923cf6bcc 72 float M2_Kp = 1.5;
CasperBerkhout 4:de0923cf6bcc 73 float M2_Ki = 0.5;
CasperBerkhout 1:d7299175a12e 74 float M2_Kd = 0;
CasperBerkhout 0:4141aef83f4b 75 double M2_e_int=0;
CasperBerkhout 0:4141aef83f4b 76 double M2_e_prior=0;
CasperBerkhout 0:4141aef83f4b 77
CasperBerkhout 2:c7369b41f7ae 78 double setpoint = 0;
CasperBerkhout 2:c7369b41f7ae 79
CasperBerkhout 2:c7369b41f7ae 80 double M1_rel_pos;
CasperBerkhout 2:c7369b41f7ae 81 double M2_rel_pos;
CasperBerkhout 2:c7369b41f7ae 82
CasperBerkhout 0:4141aef83f4b 83 float Ts = 0.002; //500hz sample freq
CasperBerkhout 0:4141aef83f4b 84
s1680897 5:224c1fe73a8f 85 float gain_factor = 1.5;
s1680897 5:224c1fe73a8f 86 double x_vel;
s1680897 5:224c1fe73a8f 87 double y_vel;
s1680897 5:224c1fe73a8f 88
CasperBerkhout 1:d7299175a12e 89 bool M1homflag;
CasperBerkhout 1:d7299175a12e 90 bool M2homflag;
CasperBerkhout 1:d7299175a12e 91 bool Homstartflag;
CasperBerkhout 1:d7299175a12e 92
s1680897 5:224c1fe73a8f 93 /** Sample function
s1680897 5:224c1fe73a8f 94 * this function samples the emg and sends it to HIDScope
s1680897 5:224c1fe73a8f 95 **/
s1680897 5:224c1fe73a8f 96 int P= 200;
s1680897 5:224c1fe73a8f 97 double A[200];
s1680897 5:224c1fe73a8f 98
s1680897 5:224c1fe73a8f 99 double sig_2_abs;
s1680897 5:224c1fe73a8f 100 double sig_3_abs;
s1680897 5:224c1fe73a8f 101 double sig_4_abs;
s1680897 5:224c1fe73a8f 102 double sig_5_abs;
s1680897 5:224c1fe73a8f 103 double movmean2;
s1680897 5:224c1fe73a8f 104 double movmean3;
s1680897 5:224c1fe73a8f 105 double movmean4;
s1680897 5:224c1fe73a8f 106 double movmean5;
s1680897 5:224c1fe73a8f 107 double emgsample2;
s1680897 5:224c1fe73a8f 108 double emgsample3;
s1680897 5:224c1fe73a8f 109 double emgsample4;
s1680897 5:224c1fe73a8f 110 double emgsample5;
s1680897 5:224c1fe73a8f 111
s1680897 5:224c1fe73a8f 112 enum States {cal2,cal3,cal4,cal5};
s1680897 5:224c1fe73a8f 113 int state;
CasperBerkhout 1:d7299175a12e 114
s1680897 5:224c1fe73a8f 115 double movmean_cal[5000];
s1680897 5:224c1fe73a8f 116 double movmean_max2=0;
s1680897 5:224c1fe73a8f 117 double movmean_max3=0;
s1680897 5:224c1fe73a8f 118 double movmean_max4=0;
s1680897 5:224c1fe73a8f 119 double movmean_max5=0;
s1680897 5:224c1fe73a8f 120
s1680897 5:224c1fe73a8f 121
s1680897 5:224c1fe73a8f 122 // Filters
s1680897 5:224c1fe73a8f 123 BiQuadChain bqc;
s1680897 5:224c1fe73a8f 124 BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
s1680897 5:224c1fe73a8f 125 BiQuad bq2( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp?
s1680897 5:224c1fe73a8f 126 BiQuad bq3( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch
s1680897 5:224c1fe73a8f 127
s1680897 5:224c1fe73a8f 128
s1680897 5:224c1fe73a8f 129 //Function emgfilter signal 2
s1680897 5:224c1fe73a8f 130 double emgfilter2(double emgsample2)
s1680897 5:224c1fe73a8f 131 {
s1680897 5:224c1fe73a8f 132 //filter and take abs of input
s1680897 5:224c1fe73a8f 133 double sig_2_filt = bqc.step(emgsample2);
s1680897 5:224c1fe73a8f 134 sig_2_abs = abs(sig_2_filt);
s1680897 5:224c1fe73a8f 135 //calculate moving mean of the filtered signal
s1680897 5:224c1fe73a8f 136 for(int i = P-1; i >= 0; i--){
s1680897 5:224c1fe73a8f 137 if (i == 0) {
s1680897 5:224c1fe73a8f 138 A[i] = sig_2_abs;
s1680897 5:224c1fe73a8f 139 }
s1680897 5:224c1fe73a8f 140 else {
s1680897 5:224c1fe73a8f 141 A[i] = A[i-1];
s1680897 5:224c1fe73a8f 142 }
s1680897 5:224c1fe73a8f 143 }
s1680897 5:224c1fe73a8f 144 double sum2 = 0;
s1680897 5:224c1fe73a8f 145 // deze for-loop sommeert de array
s1680897 5:224c1fe73a8f 146 for (int n = 0; n < P-1; n++)
s1680897 5:224c1fe73a8f 147 {
s1680897 5:224c1fe73a8f 148 sum2 = sum2 + A[n];
s1680897 5:224c1fe73a8f 149 movmean2 = sum2/P; //dit is de moving average waarde
s1680897 5:224c1fe73a8f 150 }
s1680897 5:224c1fe73a8f 151 double set_speed2=movmean2/movmean_max2;
s1680897 5:224c1fe73a8f 152 return(set_speed2);
s1680897 5:224c1fe73a8f 153 }
s1680897 5:224c1fe73a8f 154
s1680897 5:224c1fe73a8f 155
s1680897 5:224c1fe73a8f 156 //Signal 3
s1680897 5:224c1fe73a8f 157 double emgfilter3(double emgsample3){
s1680897 5:224c1fe73a8f 158 //filter and take abs of input
s1680897 5:224c1fe73a8f 159 double sig_3_filt = bqc.step(emgsample3);
s1680897 5:224c1fe73a8f 160 sig_3_abs = abs(sig_3_filt);
s1680897 5:224c1fe73a8f 161
s1680897 5:224c1fe73a8f 162 //calculate moving mean of the filtered signal
s1680897 5:224c1fe73a8f 163 for(int i = P-1; i >= 0; i--){
s1680897 5:224c1fe73a8f 164 if (i == 0) {
s1680897 5:224c1fe73a8f 165 A[i] = sig_3_abs;
s1680897 5:224c1fe73a8f 166 }
s1680897 5:224c1fe73a8f 167 else {
s1680897 5:224c1fe73a8f 168 A[i] = A[i-1];
s1680897 5:224c1fe73a8f 169 }
s1680897 5:224c1fe73a8f 170 }
s1680897 5:224c1fe73a8f 171 double sum3 = 0;
s1680897 5:224c1fe73a8f 172 // deze for-loop sommeert de array
s1680897 5:224c1fe73a8f 173 for (int n = 0; n < P-1; n++) {
s1680897 5:224c1fe73a8f 174 sum3 = sum3 + A[n];
s1680897 5:224c1fe73a8f 175 movmean3 = sum3/P; //dit is de moving average waarde
s1680897 5:224c1fe73a8f 176 }
s1680897 5:224c1fe73a8f 177 double set_speed3=movmean3/movmean_max3;
s1680897 5:224c1fe73a8f 178 return(set_speed3);
s1680897 5:224c1fe73a8f 179 }
s1680897 5:224c1fe73a8f 180 //Signal 4
s1680897 5:224c1fe73a8f 181 double emgfilter4(double emgsample4)
s1680897 5:224c1fe73a8f 182 {
s1680897 5:224c1fe73a8f 183 //filter and take abs of input
s1680897 5:224c1fe73a8f 184 double sig_4_filt = bqc.step(emgsample4);
s1680897 5:224c1fe73a8f 185 sig_4_abs = abs(sig_4_filt);
s1680897 5:224c1fe73a8f 186
s1680897 5:224c1fe73a8f 187 //calculate moving mean of the filtered signal
s1680897 5:224c1fe73a8f 188 for(int i = P-1; i >= 0; i--){
s1680897 5:224c1fe73a8f 189 if (i == 0) {
s1680897 5:224c1fe73a8f 190 A[i] = sig_4_abs;
s1680897 5:224c1fe73a8f 191 }
s1680897 5:224c1fe73a8f 192 else {
s1680897 5:224c1fe73a8f 193 A[i] = A[i-1];
s1680897 5:224c1fe73a8f 194 }
s1680897 5:224c1fe73a8f 195 }
s1680897 5:224c1fe73a8f 196 double sum4 = 0;
s1680897 5:224c1fe73a8f 197 // deze for-loop sommeert de array
s1680897 5:224c1fe73a8f 198 for (int n = 0; n < P-1; n++) {
s1680897 5:224c1fe73a8f 199 sum4 = sum4 + A[n];
s1680897 5:224c1fe73a8f 200 movmean4 = sum4/P; //dit is de moving average waarde
s1680897 5:224c1fe73a8f 201 }
s1680897 5:224c1fe73a8f 202 double set_speed4=movmean4/movmean_max4;
s1680897 5:224c1fe73a8f 203 return(set_speed4);
s1680897 5:224c1fe73a8f 204 }
s1680897 5:224c1fe73a8f 205 //Signal 5
s1680897 5:224c1fe73a8f 206 double emgfilter5(double emgsample5)
s1680897 5:224c1fe73a8f 207 {
s1680897 5:224c1fe73a8f 208 //filter and take abs of input
s1680897 5:224c1fe73a8f 209 double sig_5_filt = bqc.step(emgsample5);
s1680897 5:224c1fe73a8f 210 sig_5_abs = abs(sig_5_filt);
s1680897 5:224c1fe73a8f 211
s1680897 5:224c1fe73a8f 212 //calculate moving mean of the filtered signal
s1680897 5:224c1fe73a8f 213 for(int i = P-1; i >= 0; i--){
s1680897 5:224c1fe73a8f 214 if (i == 0) {
s1680897 5:224c1fe73a8f 215 A[i] = sig_5_abs;
s1680897 5:224c1fe73a8f 216 }
s1680897 5:224c1fe73a8f 217 else {
s1680897 5:224c1fe73a8f 218 A[i] = A[i-1];
s1680897 5:224c1fe73a8f 219 }
s1680897 5:224c1fe73a8f 220 }
s1680897 5:224c1fe73a8f 221 double sum5 = 0;
s1680897 5:224c1fe73a8f 222 // deze for-loop sommeert de array
s1680897 5:224c1fe73a8f 223 for (int n = 0; n < P-1; n++) {
s1680897 5:224c1fe73a8f 224 sum5 = sum5 + A[n];
s1680897 5:224c1fe73a8f 225 movmean5 = sum5/P; //dit is de moving average waarde
s1680897 5:224c1fe73a8f 226 }
s1680897 5:224c1fe73a8f 227 double set_speed5=movmean5/movmean_max5;
s1680897 5:224c1fe73a8f 228 return(set_speed5);
s1680897 5:224c1fe73a8f 229 }
s1680897 5:224c1fe73a8f 230
s1680897 5:224c1fe73a8f 231 void sample(){
s1680897 5:224c1fe73a8f 232 emgsample2 = emg2.read();
s1680897 5:224c1fe73a8f 233 emgsample3 = emg3.read();
s1680897 5:224c1fe73a8f 234 emgsample4 = emg4.read();
s1680897 5:224c1fe73a8f 235 emgsample5 = emg5.read();
s1680897 5:224c1fe73a8f 236 double x_vel = emgfilter2(emgsample2)-emgfilter3(emgsample3);
s1680897 5:224c1fe73a8f 237 double y_vel = emgfilter4(emgsample4)-emgfilter5(emgsample5);
s1680897 5:224c1fe73a8f 238 }
s1680897 5:224c1fe73a8f 239
s1680897 5:224c1fe73a8f 240 //Calibration
s1680897 5:224c1fe73a8f 241 void calibration_switch()
s1680897 5:224c1fe73a8f 242 {
s1680897 5:224c1fe73a8f 243 switch(state) {
s1680897 5:224c1fe73a8f 244 case cal2:
s1680897 5:224c1fe73a8f 245 ledred=0;
s1680897 5:224c1fe73a8f 246 for (int i=0; i<=3500; i++){
s1680897 5:224c1fe73a8f 247 if (movmean2>movmean_max2) {
s1680897 5:224c1fe73a8f 248 movmean_max2=movmean2;
s1680897 5:224c1fe73a8f 249 }}
s1680897 5:224c1fe73a8f 250 state=cal3;
s1680897 5:224c1fe73a8f 251 break;
s1680897 5:224c1fe73a8f 252
s1680897 5:224c1fe73a8f 253 case cal3:
s1680897 5:224c1fe73a8f 254 ledred=1;
s1680897 5:224c1fe73a8f 255 ledblue=0;
s1680897 5:224c1fe73a8f 256 for (int i=0; i<=3500; i++){
s1680897 5:224c1fe73a8f 257 if (movmean3>movmean_max3) {
s1680897 5:224c1fe73a8f 258 movmean_max3=movmean3;
s1680897 5:224c1fe73a8f 259 }}
s1680897 5:224c1fe73a8f 260 state=cal4;
s1680897 5:224c1fe73a8f 261 break;
s1680897 5:224c1fe73a8f 262
s1680897 5:224c1fe73a8f 263 case cal4:
s1680897 5:224c1fe73a8f 264 ledblue=1;
s1680897 5:224c1fe73a8f 265 ledgreen=0;
s1680897 5:224c1fe73a8f 266 for (int i=0; i<=3500; i++){
s1680897 5:224c1fe73a8f 267 if (movmean4>movmean_max4) {
s1680897 5:224c1fe73a8f 268 movmean_max4=movmean4;
s1680897 5:224c1fe73a8f 269 }}
s1680897 5:224c1fe73a8f 270 state=cal5;
s1680897 5:224c1fe73a8f 271 break;
s1680897 5:224c1fe73a8f 272
s1680897 5:224c1fe73a8f 273 case cal5:
s1680897 5:224c1fe73a8f 274 ledgreen=1;
s1680897 5:224c1fe73a8f 275 ledred=0;
s1680897 5:224c1fe73a8f 276 ledblue=0;
s1680897 5:224c1fe73a8f 277 for (int i=0; i<=3500; i++){
s1680897 5:224c1fe73a8f 278 if (movmean5>movmean_max5) {
s1680897 5:224c1fe73a8f 279 movmean_max5=movmean5;
s1680897 5:224c1fe73a8f 280 }}
s1680897 5:224c1fe73a8f 281 break;
s1680897 5:224c1fe73a8f 282 } //end switch
s1680897 5:224c1fe73a8f 283 } //end calibration_switch
s1680897 5:224c1fe73a8f 284
s1680897 5:224c1fe73a8f 285 void calibration () {
s1680897 5:224c1fe73a8f 286 while(button1==0) {
s1680897 5:224c1fe73a8f 287 state=cal2;
s1680897 5:224c1fe73a8f 288 calibration_switch();
s1680897 5:224c1fe73a8f 289 }
s1680897 5:224c1fe73a8f 290 }
CasperBerkhout 0:4141aef83f4b 291
CasperBerkhout 0:4141aef83f4b 292
CasperBerkhout 0:4141aef83f4b 293 void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
s1680897 5:224c1fe73a8f 294 vdx=x_vel*gain_factor;
s1680897 5:224c1fe73a8f 295 vdy=y_vel*gain_factor;
CasperBerkhout 0:4141aef83f4b 296 double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
CasperBerkhout 0:4141aef83f4b 297 double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q2))/(0.12*sin(q2))*vdy;
CasperBerkhout 0:4141aef83f4b 298 q1_new = q1 +q1_dot*Ts;
CasperBerkhout 0:4141aef83f4b 299 q2_new = q2 +q2_dot*Ts;
CasperBerkhout 0:4141aef83f4b 300 return;
CasperBerkhout 0:4141aef83f4b 301 }
CasperBerkhout 0:4141aef83f4b 302
CasperBerkhout 0:4141aef83f4b 303 float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
CasperBerkhout 0:4141aef83f4b 304 e_int += Ts*e;
CasperBerkhout 0:4141aef83f4b 305 double e_diff = (e-e_prior)/Ts;
CasperBerkhout 0:4141aef83f4b 306 e_prior = e;
CasperBerkhout 0:4141aef83f4b 307 double e_diff_filter = bqlowpass.step(e_diff);
CasperBerkhout 0:4141aef83f4b 308 return(Kp*e+Ki*e_int+Kd*e_diff_filter);
CasperBerkhout 0:4141aef83f4b 309 }
CasperBerkhout 0:4141aef83f4b 310
CasperBerkhout 0:4141aef83f4b 311 void M1_control(){
CasperBerkhout 0:4141aef83f4b 312
CasperBerkhout 0:4141aef83f4b 313 //call PID func and set new motor speed
CasperBerkhout 0:4141aef83f4b 314 set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
CasperBerkhout 0:4141aef83f4b 315 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 316 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 317 M1_direction.write(0);
CasperBerkhout 0:4141aef83f4b 318 }
CasperBerkhout 0:4141aef83f4b 319 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 320 M1_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 321 M1_direction.write(1);
CasperBerkhout 0:4141aef83f4b 322 }
CasperBerkhout 0:4141aef83f4b 323 else{M1_speed.write(0);}
CasperBerkhout 4:de0923cf6bcc 324
CasperBerkhout 0:4141aef83f4b 325 }
CasperBerkhout 0:4141aef83f4b 326
CasperBerkhout 0:4141aef83f4b 327 void M2_control(){
CasperBerkhout 0:4141aef83f4b 328 set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
CasperBerkhout 0:4141aef83f4b 329 if(set_speed > 0){
CasperBerkhout 0:4141aef83f4b 330 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 331 M2_direction.write(0);
CasperBerkhout 0:4141aef83f4b 332 }
CasperBerkhout 0:4141aef83f4b 333 else if (set_speed < 0){
CasperBerkhout 0:4141aef83f4b 334 M2_speed.write(abs(set_speed));
CasperBerkhout 0:4141aef83f4b 335 M2_direction.write(1);
CasperBerkhout 0:4141aef83f4b 336 }
CasperBerkhout 0:4141aef83f4b 337 else{M2_speed.write(0);}
CasperBerkhout 4:de0923cf6bcc 338 pc.printf("M2 integral = %f\n\r", M2_e_int);
CasperBerkhout 0:4141aef83f4b 339 }
CasperBerkhout 0:4141aef83f4b 340
CasperBerkhout 2:c7369b41f7ae 341 void homing_system () {
CasperBerkhout 2:c7369b41f7ae 342 LimSW1.mode(PullDown);
CasperBerkhout 2:c7369b41f7ae 343 LimSW2.mode(PullDown);
CasperBerkhout 2:c7369b41f7ae 344 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 345 M2_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 346 M1_direction.write(0);
CasperBerkhout 2:c7369b41f7ae 347 M2_direction.write(1);
CasperBerkhout 2:c7369b41f7ae 348
CasperBerkhout 2:c7369b41f7ae 349 while(1){
CasperBerkhout 3:455b79d42636 350 if (HomStart.read() == 0){
CasperBerkhout 3:455b79d42636 351 setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
CasperBerkhout 4:de0923cf6bcc 352 pc.printf("Homing... \n\r");
CasperBerkhout 4:de0923cf6bcc 353 }
CasperBerkhout 4:de0923cf6bcc 354
CasperBerkhout 4:de0923cf6bcc 355 double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416;
CasperBerkhout 4:de0923cf6bcc 356 double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;
CasperBerkhout 3:455b79d42636 357 if(LimSW1.read() == 0){
CasperBerkhout 3:455b79d42636 358 M1_error_pos = 1.5*setpoint - M1_rel_pos;
CasperBerkhout 3:455b79d42636 359 }
CasperBerkhout 3:455b79d42636 360 if(LimSW2.read() == 0){
CasperBerkhout 3:455b79d42636 361 M2_error_pos = -(0.5*setpoint - M2_rel_pos);
CasperBerkhout 3:455b79d42636 362 }
CasperBerkhout 3:455b79d42636 363 M1_control();
CasperBerkhout 3:455b79d42636 364 M2_control();
CasperBerkhout 4:de0923cf6bcc 365
CasperBerkhout 4:de0923cf6bcc 366
CasperBerkhout 2:c7369b41f7ae 367 if(LimSW1.read() == 1){
CasperBerkhout 2:c7369b41f7ae 368 M1_error_pos = 0;
CasperBerkhout 2:c7369b41f7ae 369 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 370 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 371 }
CasperBerkhout 2:c7369b41f7ae 372 if (LimSW2.read() == 1) {
CasperBerkhout 2:c7369b41f7ae 373 M2_error_pos = 0;
CasperBerkhout 2:c7369b41f7ae 374 M2_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 375 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 376 }
CasperBerkhout 2:c7369b41f7ae 377
CasperBerkhout 2:c7369b41f7ae 378 if (LimSW1.read() == 1 && LimSW2.read() ==1) {
CasperBerkhout 2:c7369b41f7ae 379 pc.printf("Homing finished \n\r");
CasperBerkhout 2:c7369b41f7ae 380 M1_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 381 M2_speed.write(0);
CasperBerkhout 2:c7369b41f7ae 382 wait(0.5);
CasperBerkhout 2:c7369b41f7ae 383 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 2:c7369b41f7ae 384 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
CasperBerkhout 4:de0923cf6bcc 385 //break;
CasperBerkhout 4:de0923cf6bcc 386 while(1); //stop after homing.
CasperBerkhout 2:c7369b41f7ae 387 }
CasperBerkhout 4:de0923cf6bcc 388 wait(0.01);
CasperBerkhout 2:c7369b41f7ae 389 }
CasperBerkhout 0:4141aef83f4b 390
CasperBerkhout 2:c7369b41f7ae 391 }
CasperBerkhout 0:4141aef83f4b 392
CasperBerkhout 0:4141aef83f4b 393 void scopesend(){
CasperBerkhout 0:4141aef83f4b 394
CasperBerkhout 0:4141aef83f4b 395
CasperBerkhout 0:4141aef83f4b 396
CasperBerkhout 0:4141aef83f4b 397 }
CasperBerkhout 0:4141aef83f4b 398 void StopMotors(){
CasperBerkhout 0:4141aef83f4b 399 while(1){
CasperBerkhout 0:4141aef83f4b 400 M1_speed.write(0);
CasperBerkhout 0:4141aef83f4b 401 M2_speed.write(0);
CasperBerkhout 0:4141aef83f4b 402 }
CasperBerkhout 0:4141aef83f4b 403 }
CasperBerkhout 0:4141aef83f4b 404
CasperBerkhout 0:4141aef83f4b 405 void geterror(){
CasperBerkhout 0:4141aef83f4b 406 double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 0:4141aef83f4b 407 double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
CasperBerkhout 2:c7369b41f7ae 408
CasperBerkhout 2:c7369b41f7ae 409 float Arm1_home = 122.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
CasperBerkhout 2:c7369b41f7ae 410 float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
CasperBerkhout 0:4141aef83f4b 411
CasperBerkhout 2:c7369b41f7ae 412 double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
CasperBerkhout 2:c7369b41f7ae 413 double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
CasperBerkhout 0:4141aef83f4b 414
CasperBerkhout 0:4141aef83f4b 415 double q1 = M1_actual_pos;
CasperBerkhout 2:c7369b41f7ae 416 double q2 = q1 + M2_actual_pos; //see drawing
CasperBerkhout 0:4141aef83f4b 417
CasperBerkhout 1:d7299175a12e 418 //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
CasperBerkhout 3:455b79d42636 419 double M1_reference_pos = 0.5*3.1416-potmeter.read(); //should cover the right range - radians
CasperBerkhout 1:d7299175a12e 420 double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
CasperBerkhout 0:4141aef83f4b 421
CasperBerkhout 3:455b79d42636 422
CasperBerkhout 1:d7299175a12e 423 //M2_error_pos = M2_reference_pos - M2_actual_pos;
CasperBerkhout 1:d7299175a12e 424 M2_error_pos = 0;
CasperBerkhout 3:455b79d42636 425 if(M1_reference_pos > Arm1_home){
CasperBerkhout 3:455b79d42636 426 M1_reference_pos = Arm1_home;
CasperBerkhout 3:455b79d42636 427 }
CasperBerkhout 3:455b79d42636 428 else{
CasperBerkhout 3:455b79d42636 429 M1_error_pos = M1_reference_pos - M1_actual_pos;
CasperBerkhout 3:455b79d42636 430 }
CasperBerkhout 3:455b79d42636 431 if(M2_reference_pos > Arm2_home){
CasperBerkhout 3:455b79d42636 432 M2_reference_pos = Arm2_home;
CasperBerkhout 3:455b79d42636 433 }
CasperBerkhout 3:455b79d42636 434 else{
CasperBerkhout 3:455b79d42636 435 M2_error_pos = M2_reference_pos - M2_actual_pos;
CasperBerkhout 3:455b79d42636 436 }
CasperBerkhout 0:4141aef83f4b 437 }
CasperBerkhout 0:4141aef83f4b 438
CasperBerkhout 0:4141aef83f4b 439 int main() {
s1680897 5:224c1fe73a8f 440 calibration();
s1680897 5:224c1fe73a8f 441 //add biquad sections to the chain
s1680897 5:224c1fe73a8f 442 bqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
CasperBerkhout 0:4141aef83f4b 443
s1680897 5:224c1fe73a8f 444 sample_timer.attach(&sample, 0.002);
s1680897 5:224c1fe73a8f 445 //scope_timer.attach(&scopesend, 0.002);
s1680897 5:224c1fe73a8f 446
s1680897 5:224c1fe73a8f 447 /*empty loop, sample() is executed periodically*/
s1680897 5:224c1fe73a8f 448 while(1) {}
CasperBerkhout 0:4141aef83f4b 449 //initialize serial comm and set motor PWM freq
CasperBerkhout 0:4141aef83f4b 450 M1_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 451 M2_speed.period(1.0/pwm_freq);
CasperBerkhout 0:4141aef83f4b 452 pc.baud(115200);
CasperBerkhout 1:d7299175a12e 453 pc.printf("starting homing function now. Push button to start procedure \n\r");
CasperBerkhout 0:4141aef83f4b 454 //commence homing procedure
CasperBerkhout 0:4141aef83f4b 455 homing_system();
CasperBerkhout 1:d7299175a12e 456 pc.printf("Setting home position complete\n\r");
CasperBerkhout 0:4141aef83f4b 457 //attach all interrupt
CasperBerkhout 1:d7299175a12e 458 pc.printf("attaching interrupt tickers now \n\r");
CasperBerkhout 0:4141aef83f4b 459 button1.fall(StopMotors); //stop motor interrupt
CasperBerkhout 1:d7299175a12e 460 motor_update1.attach(&M1_control,0.01);
CasperBerkhout 1:d7299175a12e 461 motor_update2.attach(&M2_control,0.01);
CasperBerkhout 1:d7299175a12e 462 error_update.attach(&geterror,0.01);
CasperBerkhout 0:4141aef83f4b 463
CasperBerkhout 1:d7299175a12e 464 pc.printf("initialization complete - position control of motors now active\n\r");
CasperBerkhout 0:4141aef83f4b 465
CasperBerkhout 0:4141aef83f4b 466 while(1){
CasperBerkhout 0:4141aef83f4b 467
CasperBerkhout 0:4141aef83f4b 468
CasperBerkhout 0:4141aef83f4b 469 }
CasperBerkhout 0:4141aef83f4b 470
CasperBerkhout 0:4141aef83f4b 471 }