kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar2 by
PROJECT_main.cpp
- Committer:
- Hooglugt
- Date:
- 2014-11-03
- Revision:
- 7:ca1ade91bd14
- Parent:
- 6:14051758db6f
- Child:
- 8:75980dc35763
File content as of revision 7:ca1ade91bd14:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "arm_math.h" #include "encoder.h" #define TSAMP 0.001 // sample freq encoder motor #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan #define TIMEBETWEENBLINK 100 // sec voor volgende blink #define TSAMP_EMG 0.002 //sample frequency emg #define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde #define FACTOR 0.6 //factor*max_waarde = threshold emg //Define objects HIDScope scope(2); AnalogIn emg0(PTB1); //Analog input biceps AnalogIn emg1(PTB2); //Analog input triceps Ticker log_timer; //sample emg Ticker blink; //ledjes aan/uit Ticker blink2; //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen Ticker looptimer; //motor regelaar MODSERIAL pc(USBTX,USBRX); arm_biquad_casd_df1_inst_f32 bihighpass; float bihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071 float bihighpass_states[4]; arm_biquad_casd_df1_inst_f32 binotch; float binotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10 float binotch_states[4]; arm_biquad_casd_df1_inst_f32 trihighpass; float trihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071 float trihighpass_states[4]; arm_biquad_casd_df1_inst_f32 trinotch; float trinotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10 float trinotch_states[4]; float bi_result = 0; float tri_result = 0; float bi_max = 0; float tri_max = 0; // variables for biceps MAF float y0 = 0; float y1 = 0; float y2 = 0; float y3 = 0; float y4 = 0; float y5 = 0; float y6 = 0; float y7 = 0; float y8 = 0; float y9 = 0; // variables for triceps MAF float x0 = 0; float x1 = 0; float x2 = 0; float x3 = 0; float x4 = 0; float x5 = 0; float x6 = 0; float x7 = 0; float x8 = 0; float x9 = 0; //LED interface DigitalOut dir1(PTA1); DigitalOut dir2(PTA2); DigitalOut dir3(PTD4); DigitalOut for1(PTA12); DigitalOut for2(PTA13); DigitalOut for3(PTD1); uint8_t direction = 0; uint8_t force = 0; //motorcontrol objects //motor 1, voltage pins op M2 Encoder motor1(PTD3, PTD5); DigitalOut motor1dir(PTC9); PwmOut pwm_motor1(PTC8); //motor 2, voltage pins op M1 Encoder motor2(PTD2,PTD0); DigitalOut motor2dir(PTA4); PwmOut pwm_motor2(PTA5); float integral = 0; float derivative = 0; float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd float kalibratie = 0; float controlerror = 0; float previouserror = 0; float pwm = 0; float omrekenfactor1 = 0.0028035714; // 6.28/(32*70) float omrekenfactor2 = 0.0015213178; // 6.28/(24*172); float setpoint1 = 0; //te behalen speed van motor1 (37D) float setpoint2 = 0; //te behalen hoek van motor2 (25D) float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag float Kd1 = 0.0; float Kp3 = 0.15; //Kp en Ki van motor1, voor de return float Ki3 = 0.05; //0.09, 0.05 float Kd3 = 0.1; float Kp2 = 1.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return float Ki2 = 0.8; //0.30 en 0.20 float Kd2 = 0.1; float Kp4 = 1.0; //Kp en Ki van motor2, voor de return float Ki4 = 0.8; float Kd4 = 0.1; volatile bool looptimerflag; //voor motorcontrol TSAMP //functies void setlooptimerflag(void) { looptimerflag = true; scope.set(0, motor2.getPosition()*omrekenfactor2); scope.set(0, motor1.getPosition()*omrekenfactor1); scope.send(); } void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = max; } void looper() { //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively float emg_biceps; //Float voor EMG-waarde biceps float emg_triceps; //Float voor EMG-waarde triceps emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps //process emg biceps arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 ); arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 ); y0 = fabs(emg_biceps); bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1); y9=y8; y8=y7; y7=y6; y6=y5; y5=y4; y4=y3; y3=y2; y2=y1; y1=y0; //process emg triceps arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 ); arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 ); x0 = fabs(emg_triceps); tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1); x9=x8; x8=x7; x7=x6; x6=x5; x5=x4; x4=x3; x3=x2; x2=x1; x1=x0; } void kalbi() //blinking three lights, first row - 2nd row unlit { if(dir1==0) { dir1 = dir2 = dir3 = 1; } else { dir1 = dir2 = dir3 = 0; } } void kaltri() //blinking three lights, 2nd row - first row lit { if(for1==0) { for1 = for2 = for3 = 1; } else { for1 = for2 = for3 = 0; } } void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P) { if(direction == 1 && force == 1) { // links zwak if(for1 == 0 && dir1 == 0) { for1 = dir1 = 1; } else { for1 = dir1 = 0; } } if(direction == 1 && force == 2) { // links normaal if(for2 == 0 && dir1 == 0) { for2 = dir1 = 1; } else { for2 = dir1 = 0; } } if(direction == 1 && force == 3) { // links sterk if(for3 == 0 && dir1 == 0) { for3 = dir1 = 1; } else { for3 = dir1 = 0; } } if(direction == 2 && force == 1) { // mid zwak if(for1 == 0 && dir2 == 0) { for1 = dir2 = 1; } else { for1 = dir2 = 0; } } if(direction == 2 && force == 2) { // mid normaal if(for2 == 0 && dir2 == 0) { for2 = dir2 = 1; } else { for2 = dir2 = 0; } } if(direction == 2 && force == 3) { // mid sterk if(for3 == 0 && dir2 == 0) { for3 = dir2 = 1; } else { for3 = dir2 = 0; } } if(direction == 3 && force == 1) { // rechts zwak if(for1 == 0 && dir3 == 0) { for1 = dir3 = 1; } else { for1 = dir3 = 0; } } if(direction == 3 && force == 2) { // rechts normaal if(for2 == 0 && dir3 == 0) { for2 = dir3 = 1; } else { for2 = dir3 = 0; } } if(direction == 3 && force == 3) { // rechts sterk if(for3 == 0 && dir3 == 0) { for3 = dir3 = 1; } else { for3 = dir3 = 0; } } } int main() { pc.baud(115200); //baudrate instellen log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz looptimer.attach(setlooptimerflag,TSAMP); pwm_motor1.period_us(100); //10kHz PWM frequency pwm_motor2.period_us(100); //10kHz PWM frequency //set up filters arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states); arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states); arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states); arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states); //kalibratie //motorarm naar nul-positie blink.attach(kalbi, 0.2); blink2.attach(kaltri, 0.2); //calibration motor 2 pwm_motor2.write(0.65); //lage PWM motor2dir = 0; //rechtsom wait(1); // anders wordt de while(1) meteen onderbroken while(1) { if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // motor2.getSpeed()*omrekenfactor2 > -0.70), 0.70 is nog aan te passen pwm_motor2.write(0); motor2.setPosition(0); goto motor1cal; } wait(0.01); } motor1cal: //calibration motor 1 pwm_motor1.write(0.65); //lage PWM motor1dir = 0; //linksom wait(1); // anders wordt de while(1) meteen onderbroken while(1) { if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen pwm_motor1.write(0); motor1.setPosition(0); goto emgcal; } wait(0.01); } emgcal: blink.detach(); blink2.detach(); dir1 = dir2 = dir3 = 1; for1 = for2 = for3 = 1; pc.printf("kalmoarm "); wait (1); for1 = for2 = for3 = 0; if(kalibratie==0) { //biceps kalibratie blink.attach(kalbi, 0.2); for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { if (bi_max < bi_result) { bi_max = bi_result; } wait (0.01); } blink.detach(); dir1 = dir2 = dir3 = 1; pc.printf("kalbi "); wait (1); //triceps kalibratie blink.attach(kaltri, 0.2); for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { if (tri_max < tri_result) { tri_max = tri_result; } wait (0.01); } blink.detach(); for1 = for2 = for3 = 1; pc.printf("kaltri "); wait (1); for1 = for2 = for3 = 0; kalibratie = 1; } directionchoice: log_timer.attach(looper, TSAMP_EMG); while(1) { //Loop keuze DIRECTION for(int i=1; i<4; i++) { if(i==1) { //red dir1=1; dir2=0; dir3=0; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(bi_result>FACTOR*bi_max) { direction = 1; pc.printf("links "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie goto forcechoice; // goes to second while(1) for the deciding the force } else { wait(0.01); } } } if(i==2) { //green dir1 =0; dir2 =1; dir3 =0; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(bi_result>FACTOR*bi_max) { direction = 2; pc.printf("mid "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie goto forcechoice; } else { wait(0.01); } } } if(i==3) { //blue dir1 =0; dir2 =0; dir3 =1; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(bi_result>FACTOR*bi_max) { direction = 3; pc.printf("rechts "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie goto forcechoice; } else { wait(0.01); } } } } } forcechoice: while(1) { //Loop keuze FORCE for(int j=1; j<4; j++) { if(j==1) { //red for1=1; for2=0; for3=0; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(tri_result>FACTOR*tri_max) { for1 = for2 = for3 = 0; pc.printf("reset "); goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 1; pc.printf("zwak "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie goto choicesmade; } else { wait(0.01); } } } } if(j==2) { //green for1=0; for2=1; for3=0; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(tri_result>FACTOR*tri_max) { for1 = for2 = for3 = 0; pc.printf("reset "); goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 2; pc.printf("normaal "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie goto choicesmade; } else { wait(0.01); } } } } if(j==3) { //blue for1=0; for2=0; for3=1; for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { if(tri_result>FACTOR*tri_max) { for1 = for2 = for3 = 0; pc.printf("reset "); goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 3; pc.printf("sterk "); wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie goto choicesmade; } else { wait(0.01); } } } } } } choicesmade: blink.attach(okay, 0.2); while(1) { if(tri_result>FACTOR*tri_max) { blink.detach(); pc.printf("reset "); switch (direction) { case 1: dir1 = 1; for1 = 1; for2 = for3 = 0; break; case 2: dir2 = 1; for1 = 1; for2 = for3 = 0; break; case 3: dir3 = 1; for1 = 1; for2 = for3 = 0; break; } wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze goto forcechoice; } else { if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) { blink.detach(); log_timer.detach(); goto motorcontrol; } else { wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje) } } } motorcontrol: /* Vanaf hier komt de aansturing van de motor */ switch (direction) { case 1: setpoint2 = (0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel break; case 2: setpoint2 = (0.436332313); //25 graden vanaf nul-punt (precies midden) break; case 3: setpoint2 = (0.436332313-0.197222205); // 25 graden - 11,3 graden, slag naar rechterdoel break; } switch (force) { case 1: setpoint1 = 6.8; break; case 2: setpoint1 = 7.4; break; case 3: setpoint1 = 8.0; break; } while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag); looptimerflag = false; //clear flag //regelaar motor2, bepaalt positie controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative; previouserror = controlerror; keep_in_range(&pwm, -1,1); pwm_motor2.write(abs(pwm)); if(pwm > 0) { motor2dir = 1; } else { motor2dir = 0; } //controleert of batje positie heeft bepaald if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) { batjeset = 0; } else { batjeset++; } } else { pwm_motor2.write(0); batjeset = integral = derivative = previouserror = 0; wait(1); goto motor1control; } } motor1control: while(1) { // loop voor het slaan mbv motor1 (batje snelheid) while(!looptimerflag); looptimerflag = false; //clear flag if (balhit == 0) { //regelaar motor1, bepaalt snelheid controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative; previouserror = controlerror; } else { //regelaar motor1, bepaalt positie balhit = integral = derivative = previouserror = 0; goto resetpositionmotor1; } keep_in_range(&pwm, -1,1); pwm_motor1.write(abs(pwm)); if(pwm > 0) { motor1dir = 1; } else { motor1dir = 0; } //controleert of batje balletje heeft bereikt //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle if (motor1.getPosition()*omrekenfactor1 > 1.60) { balhit = 1; } } resetpositionmotor1: while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst while(!looptimerflag); looptimerflag = false; //clear flag //regelaar motor1, bepaalt positie controlerror = -1*motor1.getPosition()*omrekenfactor1; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative; previouserror = controlerror; keep_in_range(&pwm, -1,1); if(pwm > 0) { motor1dir = 1; } else { motor1dir = 0; //1 = rechtsom, 0 = linksom } pwm_motor1.write(abs(pwm)); //controleert of arm terug in positie is if(batjeset < 200) { if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) { batjeset = 0; } else { batjeset++; } } else { pwm_motor1.write(0); batjeset = integral = derivative = previouserror = 0; wait(1); goto resetpositionmotor2; } } resetpositionmotor2: while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag); looptimerflag = false; //clear flag //regelaar motor2, bepaalt positie controlerror = -1*motor2.getPosition()*omrekenfactor2; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative; previouserror = controlerror; keep_in_range(&pwm, -1,1); if(pwm > 0) { motor2dir = 1; } else { motor2dir = 0; } pwm_motor2.write(abs(pwm)); //controleert of batje positie heeft bepaald if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) { batjeset = 0; } else { batjeset++; } } else { pwm_motor2.write(0); batjeset = integral = derivative = previouserror = 0; wait(1); direction = force = 0; goto motor1cal; } } } // end main