kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar2 by
PROJECT_main.cpp
- Committer:
- Hooglugt
- Date:
- 2014-11-04
- Revision:
- 28:1e3fe7204529
- Parent:
- 27:e33c352d6bb2
File content as of revision 28:1e3fe7204529:
#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 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 = 2.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 = 0.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; } 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 motor2cal: //motorarm naar nul-positie blink.attach(kalbi, 0.4); 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; 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; 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; 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; 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; 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; 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; goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 1; 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; goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 2; 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; goto directionchoice; } else { if(bi_result>FACTOR*bi_max) { force = 3; 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(); 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 */ 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 switch(state) { case 1: switch (direction) { case 1: pwm_motor2.write(0.8); motor2dir = 1; count++; if (count>170) { state=2; count = 0; pwm_motor2.write(0); } break; case 2: pwm_motor2.write(0.8); motor2dir = 1; count++; if (count>100) { state=2; count = 0; pwm_motor2.write(0); } break; case 3: pwm_motor2.write(0.8); motor2dir = 1; count++; if (count>50) { state=2; count = 0; pwm_motor2.write(0); } break; } case 2: { count++; if(count>1000) { count = 0; state = 3; } break; } case 3: { switch (force) { case 1: pwm_motor1.write(1); motor1dir = 1; break; case 2: pwm_motor1.write(1); //net niet haalbaar motor1dir = 1; break; case 3: pwm_motor1.write(1); //niet haalbaar motor1dir = 1; break; } if(fabs(motor1.getPosition()*omrekenfactor1)>7.0) { pwm_motor1.write(0); state = 4; } break; } case 4: { count++; if(count>4000) { count = 0; state = 1; goto motor2cal; } break; } } } } // end main