main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "arm_math.h"
00004 #include "MODSERIAL.h"
00005 #include "encoder.h"
00006 #include "PwmOut.h"
00007 
00008 #define TSAMP 0.005
00009 #define K_P1 (0.8)
00010 #define K_I1 (0.001)
00011 #define K_P2 (4.0)
00012 #define K_I2 (0.01)
00013 #define I_LIMIT 1.
00014 #define l_arm 0.5
00015 
00016 #define M1_PWM PTC8
00017 #define M1_DIR PTC9
00018 #define M2_PWM PTA5
00019 #define M2_DIR PTA4
00020 
00021 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
00022 
00023 // Define objects
00024 Serial pc(USBTX, USBRX);
00025 
00026 // LED
00027 DigitalOut myledred(PTB3);
00028 DigitalOut myledgreen(PTB1);
00029 DigitalOut myledblue(PTB2);
00030 
00031 //EMG
00032 AnalogIn    emg0(PTB0); //Analog input
00033 AnalogIn    emg1(PTC2); //Analog input
00034 HIDScope scope(4);
00035 
00036 //motor1 25D
00037 Encoder motor1(PTD3,PTD5); //wit, geel
00038 PwmOut pwm_motor1(M2_PWM);
00039 DigitalOut motordir1(M2_DIR);
00040 
00041 //motor2 37D
00042 Encoder motor2(PTD2, PTD0); //wit, geel
00043 PwmOut pwm_motor2(M1_PWM);
00044 DigitalOut motordir2(M1_DIR);
00045 
00046 // Motor variabelen
00047 float pwm_out1 = 0;
00048 float pwm_out2 = 0;
00049 int cur_pos_motor1;
00050 int prev_pos_motor1 = 0;
00051 int cur_pos_motor2;
00052 int prev_pos_motor2 = 0;
00053 float speed1_rad;
00054 float speed2_rad;
00055 float pos_motor1_rad;
00056 float pos_motor2_rad;
00057 int staat1 = 0;
00058 int staat2 = 0;
00059 volatile float arm_hoogte = 0;
00060 volatile float batje_hoek = 0;
00061 int wait_iterator1 = 0;
00062 int wait_iterator2 = 0;
00063 
00064 
00065 // EMG Filters (settings en variabelen)
00066 
00067 // Filters
00068 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
00069 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
00070 //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
00071 float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
00072 //state values
00073 float lowpass_biceps_states[4];
00074 float lowpass_deltoid_states[4];
00075 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
00076 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
00077 //highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
00078 float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
00079 //state values
00080 float highnotch_biceps_states[8];
00081 float highnotch_deltoid_states[8];
00082 
00083 //De globale variabele voor het gefilterde EMG signaal
00084 float filtered_biceps;
00085 float filtered_deltoid;
00086 float filtered_average_bi;
00087 float filtered_average_del;
00088 
00089 //gemiddelde EMG waarden over 250 sample stappen
00090 void average_biceps(float filtered_biceps,float *average)
00091 {
00092     static float total=0;
00093     static float number=0;
00094     total = total + filtered_biceps;
00095     number = number + 1;
00096     if ( number == 250) {
00097         *average = total/250;
00098         total = 0;
00099         number = 0;
00100     }
00101 }
00102 
00103 void average_deltoid(float filtered_input,float *average_output)
00104 {
00105     static float total=0;
00106     static float number=0;
00107     total = total + filtered_input;
00108     number = number + 1;
00109     if ( number == 250) {
00110         *average_output = total/250;
00111         total = 0;
00112         number = 0;
00113     }
00114 }
00115 
00116 // EMG looper
00117 void looper()
00118 {
00119     /*variable to store value in*/
00120     uint16_t emg_value1;
00121     uint16_t emg_value2;
00122 
00123     float emg_value1_f32;
00124     float emg_value2_f32;
00125     /*put raw emg value both in red and in emg_value*/
00126     emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
00127     emg_value1_f32 = emg0.read();
00128 
00129     emg_value2 = emg1.read_u16();
00130     emg_value2_f32 = emg1.read();
00131 
00132     //process emg biceps
00133     arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
00134     filtered_biceps = fabs(filtered_biceps);
00135     arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
00136     average_biceps(filtered_biceps,&filtered_average_bi);
00137     //process emg deltoid
00138     arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value2_f32, &filtered_deltoid, 1 );
00139     filtered_deltoid = fabs(filtered_deltoid);
00140     arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
00141     average_deltoid(filtered_deltoid, &filtered_average_del);
00142 
00143     /*send value to PC. */
00144     scope.set(0,emg_value1);     //Raw EMG signal biceps
00145     //scope.set(1,emg_value2);    //Raw EMG signal Deltoid
00146     scope.set(1,filtered_biceps);  //processed float biceps
00147     scope.set(2,filtered_average_bi); //processed float deltoid
00148     //scope.set(2,filtered_deltoid);  //processed float biceps
00149     scope.set(3,filtered_average_del); //processed float deltoid
00150     scope.send();
00151 
00152 }
00153 
00154 // LED AANSTURING
00155 
00156 void BlinkRed(int n)
00157 {
00158     for (int i=0; i<n; i++) {
00159         myledred = 0;
00160         myledgreen = 0;
00161         myledblue = 0;
00162         wait(0.1);
00163         myledred = 1;
00164         myledgreen = 0;
00165         myledblue = 0;
00166         wait(0.1);
00167     }
00168 }
00169 
00170 // Ticker voor groen knipperen, zodat tijdens dit knipperen presets gekozen kunnen worden
00171 Ticker ledticker;
00172 
00173 void greenblink()
00174 {
00175     if(myledgreen.read())
00176         myledgreen = 0;
00177     else
00178         myledgreen = 1;
00179 }
00180 
00181 void BlinkGreen()
00182 {
00183     myledred= 0;
00184     myledblue =0;
00185     ledticker.attach(&greenblink,.5);
00186 }
00187 
00188 void stopblinkgreen()
00189 {
00190     ledticker.detach();
00191 }
00192 
00193 // Groen schijnen
00194 void ShineGreen ()
00195 {
00196     myledred = 0;
00197     myledgreen = 1;
00198     myledblue = 0;
00199 }
00200 
00201 // Blauw schijnen
00202 void ShineBlue ()
00203 {
00204     myledred = 0;
00205     myledgreen = 0;
00206     myledblue = 1;
00207 }
00208 
00209 // Rood schijnen
00210 void ShineRed ()
00211 {
00212     myledred = 1;
00213     myledgreen = 0;
00214     myledblue = 0;
00215 }
00216 
00217 // MOTORFUNCTIES
00218 
00219 // Motor1 = batje
00220 // Motor2 = arm
00221 
00222 void clamp(float* in, float min, float max) //Clamp geeft een maximum en minimum limiet aan een functie
00223 {
00224 *in > min ? /*(*/*in < max? : *in = max : *in = min;
00225 }
00226 
00227 // PI-regelaar motor1: batje
00228 float pid1(float setpoint1, float measurement1)
00229 {
00230     float error1;
00231     float           out_p1 = 0;
00232     static float    out_i1 = 0;
00233     error1  = (setpoint1 - measurement1);
00234     out_p1  = error1*K_P1;
00235     out_i1 += error1*K_I1;
00236     clamp(&out_i1,-I_LIMIT,I_LIMIT);
00237     return out_p1 + out_i1;
00238 }
00239 
00240 // PI-regelaar motor2: arm
00241 float pid2(float setpoint2, float measurement2)
00242 {
00243     float error2;
00244     float           out_p2 = 0;
00245     static float    out_i2 = 0;
00246     error2  = (setpoint2 - measurement2);
00247     out_p2  = error2*K_P2;
00248     out_i2 += error2*K_I2;
00249     clamp(&out_i2,-I_LIMIT,I_LIMIT);
00250     return out_p2 + out_i2;
00251 }
00252 
00253 // Variabelen
00254 float prev_setpoint1 = 0;
00255 float setpoint1 = 0;
00256 float prev_setpoint2 = 0;
00257 float setpoint2 = 0;
00258 
00259 // Functies motoren
00260 
00261 // Motor1 links draaien
00262 void batje_links ()
00263 {
00264     speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
00265     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint
00266     if(setpoint1 > (11*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/
00267         setpoint1 = (11*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
00268     }
00269     if(setpoint1 < -(11*2.3*2.0*PI/360)) {
00270         setpoint1 = -((11*2.3*2.0*PI/360));
00271     }
00272     if(setpoint1 <= -((11*2.3*2.0*PI/360)-0.1)) {
00273         staat1 = 1;
00274         prev_setpoint1 = setpoint1;
00275     }
00276 }
00277 // Motor1 rechts draaien
00278 void batje_rechts ()
00279 {
00280     speed1_rad = 1.0;
00281     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
00282     if(setpoint1 > (11.3*2.3*2.0*PI/360)) {
00283         setpoint1 = (11.3*2.3*2.0*PI/360);
00284     }
00285     if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
00286         setpoint1 = -(11.3*2.3*2.0*PI/360);
00287     }
00288     prev_setpoint1 = setpoint1;
00289     if(setpoint1 >= ((11.3*2.3*2.0*PI/360)-0.1)) {
00290         staat1 = 1;
00291     }
00292 }
00293 
00294 //Motor1 na links draaien weer terug laten draaien naar beginstand
00295 void batje_begin_links ()
00296 {
00297     speed1_rad = 1.0;
00298     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
00299     if(setpoint1 > (0*2.3*2.0*PI/360)) {
00300         setpoint1 = (0*2.3*2.0*PI/360);
00301     }
00302     //if(setpoint1 < -(0*2.3*2.0*PI/360)) {
00303     //  setpoint1 = -(0*2.3*2.0*PI/360);
00304     //}
00305     prev_setpoint1 = setpoint1;
00306 }
00307 
00308 //Motor1 na links draaien weer terug laten draaien naar beginstand
00309 void batje_begin_rechts ()
00310 {
00311     speed1_rad = -1.0;
00312     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
00313     //if(setpoint1 > (0*2.3*2.0*PI/360)) {
00314     //  setpoint1 = (0*2.3*2.0*PI/360);
00315     //}
00316     if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
00317         setpoint1 = -((0.0*2.3*2.0*PI/360));
00318     }
00319     prev_setpoint1 = setpoint1;
00320 }
00321 
00322 // Motor2 balletje op zn hoogst slaan
00323 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
00324 {
00325     speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht)
00326     setpoint2 = -(120*2.0*PI/360);
00327     /*setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
00328     if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
00329         setpoint2 = (100.0*2.0*PI/360);
00330     }
00331     if(setpoint2 < -(100.0*2.0*PI/360)) {
00332         setpoint2 = -(100.0*2.0*PI/360);
00333     }
00334 */
00335     prev_setpoint2 = setpoint2;
00336     if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
00337         staat2 = 1;
00338     }
00339 }
00340 
00341 // Motor2 balletje in het midden slaan
00342 void arm_mid ()
00343 {
00344     speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht)
00345     setpoint2 = -(120*2.0*PI/360);
00346     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
00347     if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
00348         setpoint2 = (100.0*2.0*PI/360);
00349     }
00350     if(setpoint2 < -(100.0*2.0*PI/360)) {
00351         setpoint2 = -(100.0*2.0*PI/360);
00352     }
00353 
00354     prev_setpoint2 = setpoint2;
00355     if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
00356         staat2 = 1;
00357     }
00358 }
00359 
00360 // Motor2 balletje op het laagst slaan
00361 void arm_laag ()
00362 {
00363     speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht)
00364     setpoint2 = -(120*2.0*PI/360);
00365     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
00366     if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
00367         setpoint2 = (100.0*2.0*PI/360);
00368     }
00369     if(setpoint2 < -(100.0*2.0*PI/360)) {
00370         setpoint2 = -(100.0*2.0*PI/360);
00371     }
00372 
00373     prev_setpoint2 = setpoint2;
00374     if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
00375         staat2 = 1;
00376     }
00377 }
00378 
00379 // Motor2 arm terug zetten in beginstand
00380 void arm_begin ()
00381 {
00382     speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
00383     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
00384     if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
00385         setpoint2 = (0.0*2.0*PI/360);
00386     }
00387     //if(setpoint2 < -(0.0*2.0*PI/360)) {
00388     //   setpoint2 = -(0.0*2.0*PI/360);
00389     //}
00390     prev_setpoint2 = setpoint2;
00391 }
00392 
00393 // MOTOR aansturing
00394 void looper_motor()
00395 {
00396     pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005
00397 
00398     //MOTOR1
00399     cur_pos_motor1 = motor1.getPosition();
00400     pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //voor 1 rotatie van de motoras geldt 24(aantal cpr vd encoder)*172(gearbox ratio)=4128 counts.
00401     pwm_out1 = pid1(setpoint1, pos_motor1_rad);
00402     if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld.
00403         pwm_out1 = -1.0;
00404     }
00405     if (pwm_out1 > 1.0) {
00406         pwm_out1 = 1.0;
00407     }
00408     pwm_motor1.write(abs(pwm_out1));
00409     if(pwm_out1 > 0) {
00410         motordir1 = 1;
00411     } else {
00412         motordir1 = 0;
00413     }
00414 
00415     //MOTOR2
00416     cur_pos_motor2 = motor2.getPosition();
00417     pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
00418     pwm_out2 = pid2(setpoint2, pos_motor2_rad); //
00419     if (pwm_out2 < -1.0) {
00420         pwm_out2 = -1.0;
00421     }
00422     if (pwm_out2 > 1.0) {
00423         pwm_out2 = 1.0;
00424     }
00425     pwm_motor2.write(abs(pwm_out2));
00426     if(pwm_out2 > 0) {
00427         motordir2 = 0;
00428     } else {
00429         motordir2 = 1;
00430     }
00431 
00432 
00433     //STATES
00434 
00435     //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug
00436     if (batje_hoek == 1) {
00437         if(staat1 == 0) {
00438             batje_rechts();
00439             wait_iterator1 = 0;
00440         } else if(staat1 ==1) {
00441             wait_iterator1++;
00442         }
00443         if(wait_iterator1 > 1200) {
00444             staat1 = 2;
00445             batje_begin_rechts();
00446         }
00447     }
00448 
00449     if (batje_hoek == 2) {
00450         if(staat1 == 0) {
00451             batje_links();
00452             wait_iterator1 = 0;
00453         } else if(staat1 ==1) {
00454             wait_iterator1++;
00455         }
00456         if(wait_iterator1 > 1200) {
00457             staat1 = 2;
00458 
00459             batje_begin_links ();
00460         }
00461     }
00462 
00463 
00464     if(arm_hoogte == 1) {
00465         if(staat2 == 0) {
00466             arm_laag();
00467             wait_iterator2 = 0;
00468         } else if(staat2 == 1) {
00469             wait_iterator2++;
00470         }
00471         if(wait_iterator2 > 400) {
00472             staat2 = 2;
00473 
00474             arm_begin();
00475         }
00476     }
00477 
00478     if(arm_hoogte == 2) {
00479         if(staat2 == 0) {
00480             arm_mid();
00481             wait_iterator2 = 0;
00482         } else if(staat2 == 1) {
00483         }          wait_iterator2++;
00484         if(wait_iterator2 > 400) {
00485             staat2 = 2;
00486 
00487             arm_begin();
00488         }
00489     }
00490 
00491     if(arm_hoogte == 3) {
00492         if(staat2 == 0) {
00493             arm_hoog();
00494             wait_iterator2 = 0;
00495         } else if(staat2 == 1) {
00496             wait_iterator2++;
00497         }
00498         if(wait_iterator2 > 400) {
00499             staat2 = 2;
00500             arm_begin();
00501         }
00502     }
00503 }
00504 
00505 
00506 
00507 // Hoofdprogramma, hierin staat de aansturing vd LED
00508 int main()
00509 {
00510     float emg_arm1 = 0.03;
00511     float emg_arm2 = 0.02;
00512     pwm_motor1.period_us(100);
00513     motor1.setPosition(0);
00514     pwm_motor2.period_us(100);
00515     motor2.setPosition(0);
00516     pc.baud(115200);
00517     // Ticker EMG signaal meten
00518     Ticker log_timer;
00519     //set up filters. Use external array for constants
00520     arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
00521     arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
00522     arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
00523     arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
00524     // Uitvoeren van ticker EMG, sample frequentie 500Hz
00525     log_timer.attach(looper, 0.002);
00526 
00527     // Aanroepen van motoraansturing in motor ticker
00528     Ticker looptimer;
00529     looptimer.attach(looper_motor,TSAMP);
00530 
00531     while(1) {
00532 
00533         while(1) {
00534             pc.printf("Span de biceps aan om het instellen te starten.\n");
00535             do {
00536                 ShineRed();
00537             } while(filtered_average_bi < emg_arm1 && filtered_average_del < emg_arm2); // In rust, geen meting
00538             if (filtered_average_bi > 0.035) { // Beginnen met meting wanneer biceps wordt aangespannen
00539                 BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
00540                 BlinkGreen(); // groen knipperen, meten van spieraanspanning
00541                 while (1) { // eerste loop, keuze voor de positie van het batje
00542                     pc.printf("In de loop.\n");
00543                     if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { //bi en del aangespannen --> batje in het midden
00544                         stopblinkgreen();
00545                         pc.printf("ShineGreen.\n");
00546                         ShineGreen();
00547                         wait (4);
00548                         break;
00549                     }
00550                     if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> batje naar links
00551                         stopblinkgreen();
00552                         pc.printf("ShineBlue.\n");
00553                         ShineBlue();
00554                         //batje_hoek = 2;
00555                         wait(4);
00556                         break;
00557                     } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> batje naar rechts
00558                         stopblinkgreen();
00559                         pc.printf("ShineRed.\n");
00560                         ShineRed();
00561                         //batje_hoek = 1;
00562                         wait (4);
00563                         break;
00564                     }
00565                 }
00566                 BlinkGreen();
00567                 while (1) { // loop voor het instellen van de kracht
00568                     pc.printf("In de loop.\n");
00569                     if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { // bi en del aanspannen --> hoog slaan
00570                         stopblinkgreen();
00571                         pc.printf("ShineGreen.\n");
00572                         ShineGreen();
00573                         arm_hoogte = 3;
00574                         wait (4);
00575                         break;
00576                     }
00577                     if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> laag slaan
00578                         stopblinkgreen();
00579                         pc.printf("ShineBlue.\n");
00580                         ShineBlue();
00581                         arm_hoogte = 1;
00582                         wait(4);
00583                         break;
00584                     } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> midden slaan
00585                         stopblinkgreen();
00586                         pc.printf("ShineRed.\n");
00587                         ShineRed();
00588                         arm_hoogte = 2;
00589                         wait (4);
00590                         break;
00591                     }
00592                 }
00593 
00594             }
00595         }
00596     }
00597 }