verslag
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_FINAL by
PROJECT_main.cpp
- Committer:
- Hooglugt
- Date:
- 2014-11-03
- Revision:
- 11:b517e73a98ab
- Parent:
- 10:6bf3e25f020a
- Child:
- 12:b09b7fe5550c
- Child:
- 16:a0a39512bd47
File content as of revision 11:b517e73a98ab:
#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(5); 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 pwm1 =0; float integral1 = 0; float derivative1 = 0; float controlerror1 = 0; float previouserror1 = 0; int state = 1; int count = 0; float angle = 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 = 12.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag float Kd1 = 0.0; float Kp2 = 8.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return float Ki2 = 0.0; //0.30 en 0.20 float Kd2 = 0.0; volatile bool looptimerflag; //voor motorcontrol TSAMP //functies void setlooptimerflag(void) { looptimerflag = true; } Ticker hid; void hidscope(void){ 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() { hid.attach(hidscope, 0.01); 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); direction = 1; force = 1; goto motorcontrol; 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 */ // FORMAT_CODE_START setpoint1=0; setpoint2=0; integral1 = integral = 0; previouserror1 = previouserror = 0; while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag); looptimerflag = false; //clear flag setpoint1=0; setpoint2=0; integral1 = integral = 0; previouserror1 = previouserror = 0; // FORMAT_CODE_START scope.set(0, motor2.getPosition()*omrekenfactor2); scope.set(1, setpoint2); scope.set(2, motor1.getPosition()*omrekenfactor1); scope.set(3, setpoint1); scope.set(4, state); switch(state) { case 1: { setpoint1=0; setpoint2 += 0.4*TSAMP; switch (direction) { case 1: angle = 0.436332313+0.197222205; //(0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel break; case 2: angle = 0.436332313; break; case 3: angle = 0.436332313-0.197222205; break; } if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1 setpoint2 = angle; count = 0; state=2; } /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) { state = 2; }*/ break; } case 2: { setpoint1 = 0; count++; if(count>1000) { count = 0; state = 3; } break; } case 3: { switch (force) { case 1: setpoint1 += 2*TSAMP; //6.8*TSAMP; break; case 2: setpoint1 += 0.4*TSAMP; //7.4*TSAMP; break; case 3: setpoint1 += 0.4*TSAMP; //8.0*TSAMP; break; } if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) { state = 4; } break; } case 4: { setpoint2 -= 0.25*TSAMP; if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm state = 5; } break; } case 5: { setpoint1 -= 0.5*TSAMP; if(setpoint1 < 0) { state = 6; } break; } case 6: { setpoint1 = 0; count++; if(count>3000) { count = 0; state = 1; goto directionchoice; } break; } } //motor regeling //regelaar motor1, bepaalt positie controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1; integral1 = integral1 + controlerror1*TSAMP; derivative1 = (controlerror1 - previouserror1)/TSAMP; pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1; previouserror1 = controlerror1; keep_in_range(&pwm1, -1,1); pwm_motor1.write(fabs(pwm1)); if(pwm1 > 0) { motor1dir = 1; } else { motor1dir = 0; } //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(fabs(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; } } // FORMAT_CODE_END 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