BMT M9 Groep01
alles in elkaar met de mooie manier van de regelaar
Dependencies: Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed
Fork of
Robot2mooiemanier
by 
BMT M9 Groep01
Diff: main.cpp
- Revision:
- 0:1594bb11fa13
- Child:
- 1:5d30a2ea2e11
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Oct 30 12:21:27 2014 +0000
@@ -0,0 +1,647 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "arm_math.h"
+#include "TouchButton.h"
+
+#define K_P (0.5)
+#define K_I (0.02 *TSAMP1)
+#define K_D (0 /TSAMP1)
+#define I_LIMIT 1.
+
+#define TSAMP1 0.01
+#define TSAMP2 0.01
+#define WACHTEN 1
+#define SLAAN 2
+#define TERUGKEREN 3
+#define ANGLEMAX 251
+#define ANGLEMIN 0
+
+//initiating functions
+void Triceps();
+void Biceps();
+void Calibratie_Triceps();
+void Calibratie_Biceps();
+float pid(float setspeed, float measurement);
+void motor2aansturing();
+void motor1aansturing();
+
+//alle initiaties voor EMG
+MODSERIAL pc(USBTX,USBRX);
+
+HIDScope scope(4);
+
+AnalogIn emgB(PTB1); //biceps
+AnalogIn emgT(PTB2); //tricep
+
+//*** OBJECTS ***
+//bicep
+uint16_t emg_valueB;
+float emg_value_f32B;
+float filtered_emgB;
+float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
+int yB1, yB2, yB3;
+float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects
+float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
+int snelheidsstand;
+//tricep
+uint16_t emg_valueT;
+float emg_value_f32T;
+float filtered_emgT;
+float drempelwaardeT;
+int yT1, yT2;
+float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
+float MOVAVG_Positie1, MOVAVG_Positie2;
+int positie;
+
+//*** FILTERS ***
+arm_biquad_casd_df1_inst_f32 notchT;
+arm_biquad_casd_df1_inst_f32 notchB;
+//constants for 50Hz
+float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
+float notch_states[4];
+
+arm_biquad_casd_df1_inst_f32 lowpassT;
+arm_biquad_casd_df1_inst_f32 lowpassB;
+//constants for 60Hz lowpass
+float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
+float lowpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 highpassT;
+arm_biquad_casd_df1_inst_f32 highpassB;
+//constants for 20Hz highpass
+float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
+float highpass_states[4];
+
+bool stop;
+float new_pwm;
+float PWM2 = 0.3; //PWM voor instellen hoek batje
+int toestand = TERUGKEREN;
+float setspeed = 0, V3=60, V2=40, V1 =30, Vreturn= 15;//V in counts/s
+
+
+Encoder motor1(PTD5,PTD3);
+Encoder motor2(PTD0,PTD2);
+DigitalOut motordir1(PTA4);
+DigitalOut motordir2(PTC9);
+PwmOut pwm_motor1(PTA5);
+PwmOut pwm_motor2(PTC8);
+
+DigitalOut myled1(LED1);//red
+DigitalOut myled2(LED2);//green
+DigitalOut myled3(LED3);//blue
+
+/* FRDM-KL25Z built-in touch slider
+*******************
+* * * *
+* 1 * 2 * 3 *
+* * * *
+*******************
+* key 1 will light Red LED -> CALIBRATIE TRICEPS
+* key 2 will light Green LED -> CALIBRATIE BICEPS
+* key 3 will light Blue LED -> START*/
+
+
+enum standen {STAND1=0, STAND2=1, STAND3=2};
+standen hoek2 = STAND1;
+
+int main ()
+{
+ pc.baud(115200);
+
+ drempelwaardeT=0;
+ drempelwaardeB1=0;
+ drempelwaardeB2=0;
+ drempelwaardeB3=0;
+
+ TouchButton TButton;
+
+ myled1=1;
+ myled2=1;
+ myled3=1;
+
+ int key=0;
+
+ pc.printf("key 1 calibratie triceps\n");
+ pc.printf("key 2 caliratie biceps\n");
+ pc.printf("key 3 START\n");
+
+ while(true) {
+
+ key = TButton.PressedButton();
+
+ if (key==1) {
+ //rood
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
+
+ pc.printf("calibratie tricep aan\n");
+ wait(2);
+
+ Calibratie_Triceps();
+ drempelwaardeT=MOVAVG_T-1;
+ pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
+
+ pc.printf("calibratie tricep klaar,\n");
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ wait(2);
+
+ myled1=1;
+ myled2=1;
+ myled3=1;
+ }
+ if (key==2) {
+ //green
+ myled1 = 1;
+ myled2 = 0;
+ myled3 = 1;
+
+ pc.printf("calibratie bicep snelheid 1 aan\n");
+ wait(2);
+
+ Calibratie_Biceps();
+ drempelwaardeB1=MOVAVG_B-1;
+ pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ wait(2);
+
+ myled1 = 1;
+ myled2 = 0;
+ myled3 = 1;
+
+ pc.printf("calibratie biceps snelheid 2 aan\n");
+ wait(2);
+
+ Calibratie_Biceps();
+ drempelwaardeB2=MOVAVG_B-1;
+ pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ wait(2);
+
+ myled1 = 1;
+ myled2 = 0;
+ myled3 = 1;
+
+ pc.printf("calibratie biceps snelheid 3 aan\n");
+ wait(2);
+ Calibratie_Biceps();
+ drempelwaardeB3=MOVAVG_B-1;
+ pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ wait(2);
+
+ pc.printf("caliratie biceps is klaar\n");
+ myled1=1;
+ myled2=1;
+ myled3=1;
+ }
+
+ if (key==3) {
+ //blue
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ wait(3);
+
+ if(drempelwaardeT==0) {
+ pc.printf("geen waarde calibratie TRICEPS \n");
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ }
+ if (drempelwaardeB1==0) {
+ pc.printf("geen waarde calibratie BICEPS 1 \n");
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ }
+ if (drempelwaardeB2==0) {
+ pc.printf("geen waarde calibratie BICEPS 2 \n");
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ }
+ if (drempelwaardeB3==0) {
+ pc.printf("geen waarde calibratie BICEPS 3 \n");
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ } else {
+
+ pc.printf("eerst positie dan snelheid aangeven /n");
+
+ //bepaling van positie met triceps 1
+ Ticker log_timerT1;
+
+ arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
+
+ log_timerT1.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT1.detach();
+
+ // positie van batje met behulp van Triceps
+
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT1=1;
+ } else {
+ yT1=0;
+ }
+
+ pc.printf("Triceps meting 1 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ wait(3);
+
+ //bepaling van positie met tricep 2
+ Ticker log_timerT2;
+
+ arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
+
+ log_timerT2.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT2.detach();
+
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT2=1;
+ } else {
+ yT2=0;
+ }
+
+ pc.printf("Triceps meting 2 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+
+ //*** INPUT MOTOR 2 ***
+ positie=yT1+yT2;
+
+ //controle positie op scherm
+ if (positie==0) {
+ pc.printf("Motor 2 blijft op stand 1\n");
+ } else {
+ if (positie==1) {
+ pc.printf("Motor 2 gaat naar stand 2\n");
+ } else {
+ if (positie==2) {
+ pc.printf("Motor 2 gaat naar stand 3\n");
+ }
+ }
+ }
+
+ Ticker looptimer2;
+ looptimer2.attach(motor2aansturing,TSAMP1);
+ wait(8);
+ looptimer2.detach();
+ pc.printf("Detach Motor 1\n");
+
+//------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
+ wait(2);
+ Ticker log_timerB;
+
+ arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
+
+ myled1 = 1;
+ myled2 = 0;
+ myled3 = 1;
+
+ log_timerB.attach(Biceps,0.005);
+ wait(2);
+ log_timerB.detach();
+
+ //bepaling van snelheidsstand met biceps
+
+ if (MOVAVG_B >= drempelwaardeB1) {
+ yB1=1;
+ if (MOVAVG_B >= drempelwaardeB2) {
+ yB2=1;
+ if (MOVAVG_B >= drempelwaardeB3) {
+ yB3=1;
+ } else {
+ yB3=0;
+ }
+ } else {
+ yB2=0;
+ }
+ } else {
+ yB1=0;
+ }
+
+ pc.printf("Biceps meting is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+
+ //*** INPUT MOTOR 1 ***
+ snelheidsstand=yB1+yB2+yB3;
+
+ //controle snelheidsstand op scherm
+ if (snelheidsstand==0) {
+ pc.printf("Motor 1 beweegt niet \n");
+ } else {
+ if (snelheidsstand==1) {
+ pc.printf("Motor 1 beweegt met snelheid 1 \n");
+ } else {
+ if (snelheidsstand==2) {
+ pc.printf("Motor 1 beweegt met snelheid 2 \n");
+ } else {
+ if (snelheidsstand==3) {
+ pc.printf("Motor 1 beweegt met snelheid 3 \n");
+ }
+ }
+ }
+ }
+
+ Ticker looptimer1;
+ //pwm_motor1.write(0.3);
+ motordir1 = 1;
+ stop = 0;
+ looptimer1.attach(motor1aansturing,TSAMP1);
+ wait(8); ////is aan te passen (tijd die nodig is om balletje te slaan
+ looptimer1.detach();
+ pc.printf("detachMotor1\n");
+ pwm_motor1.write(0);
+
+ myled1=1;
+ myled2=1;
+ myled3=1;
+ }
+ }
+ }
+}//end int main
+
+
+float pid(float setspeed, float measurement)
+{
+ float error;
+ static float prev_error = 0;
+ float out_p = 0;
+ static float out_i = 0;
+ float out_d = 0;
+ error = setspeed-measurement;
+ out_p = error*K_P;
+ out_i += error*K_I;
+ out_d = (error-prev_error)*K_D;
+ prev_error = error;
+ return out_p + out_i + out_d;
+}
+
+void Triceps()
+{
+ //Triceps lezen
+ emg_valueT = emgT.read_u16();
+ emg_value_f32T = emgT.read();
+
+ //Triceps filteren
+ arm_biquad_cascade_df1_f32(¬chT, &emg_value_f32T, &filtered_emgT, 1);
+ arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
+ filtered_emgT = fabs(filtered_emgT);
+ arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
+ filtered_emgT = fabs(filtered_emgT);
+
+ //Triceps moving average
+ T0=filtered_emgT*1000;
+ MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
+ T29=T28;
+ T28=T27;
+ T27=T26;
+ T26=T25;
+ T25=T24;
+ T24=T23;
+ T23=T22;
+ T22=T21;
+ T21=T20;
+ T20=T19;
+ T19=T18;
+ T18=T17;
+ T17=T16;
+ T16=T15;
+ T15=T14;
+ T14=T13;
+ T13=T12;
+ T12=T11;
+ T11=T10;
+ T10=T9;
+ T9=T8;
+ T8=T7;
+ T7=T6;
+ T6=T5;
+ T5=T4;
+ T4=T3;
+ T3=T2;
+ T2=T1;
+ T1=T0;
+
+ //sturen naar scherm (Realterm)
+ pc.printf("Moving average T %f\r\n",MOVAVG_T);
+
+ //sturen naar HID Scope
+ scope.set(0,emg_valueT); //ruwe data
+ scope.set(1,filtered_emgT); //filtered
+ scope.send();
+}
+
+void Biceps()
+{
+ //Biceps lezen
+ emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+ emg_value_f32B = emgB.read();
+
+ //Biceps filteren
+ arm_biquad_cascade_df1_f32(¬chB, &emg_value_f32B, &filtered_emgB, 1 );
+ arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
+ filtered_emgB = fabs(filtered_emgB);
+ arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
+ filtered_emgB = fabs(filtered_emgB);
+
+ //Biceps moving average
+ B0=filtered_emgB*1000;
+ MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
+ B59=B58;
+ B58=B57;
+ B57=B56;
+ B56=B55;
+ B55=B54;
+ B54=B53;
+ B53=B52;
+ B52=B51;
+ B51=B50;
+ B50=B48;
+ B49=B49;
+ B48=B47;
+ B47=B46;
+ B46=B45;
+ B45=B44;
+ B44=B43;
+ B43=B42;
+ B42=B41;
+ B41=B40;
+ B40=B39;
+ B39=B38;
+ B38=B37;
+ B37=B36;
+ B36=B35;
+ B35=B34;
+ B34=B33;
+ B33=B32;
+ B32=B31;
+ B31=B30;
+ B30=B29;
+ B29=B28;
+ B28=B27;
+ B27=B26;
+ B26=B25;
+ B25=B24;
+ B24=B23;
+ B23=B22;
+ B22=B21;
+ B21=B20;
+ B20=B19;
+ B19=B18;
+ B18=B17;
+ B17=B16;
+ B16=B15;
+ B15=B14;
+ B14=B13;
+ B13=B12;
+ B12=B11;
+ B11=B10;
+ B10=B9;
+ B9=B8;
+ B8=B7;
+ B7=B6;
+ B6=B5;
+ B5=B4;
+ B4=B3;
+ B3=B2;
+ B2=B1;
+ B1=B0;
+
+ //sturen naar scherm
+ pc.printf("Moving average B %f\r\n",MOVAVG_B);
+
+ //naar HID Scope
+ scope.set(2,emg_valueB); //ruwe data
+ scope.set(3,filtered_emgB); //filtered
+ scope.send();
+}
+
+void Calibratie_Triceps()
+{
+ Ticker log_timerT;
+
+ arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+
+ log_timerT.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT.detach();
+}
+
+void Calibratie_Biceps()
+{
+ Ticker log_timerB;
+
+ arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
+
+ log_timerB.attach(Biceps, 0.005);
+ wait(2);
+ log_timerB.detach();
+}
+
+void motor2aansturing()
+{
+ if (positie == 0) {
+ if (motor2.getPosition()>= 0) {
+ motordir2 = 1;
+ pwm_motor2.write(PWM2);
+ } else {
+ pwm_motor2.write(0);
+ }
+ }
+ if (positie ==1) {
+ if (motor2.getPosition()>= 2) {
+ motordir2 = 1;
+ pwm_motor2.write(PWM2);
+ }
+ if (motor2.getPosition()<= 2) {
+ motordir2 = 0;
+ pwm_motor2.write(PWM2);
+ } else {
+ pwm_motor2.write(0);
+ }
+ }
+ if (positie ==2) {
+ if (motor2.getPosition()<= 4) {
+ motordir2 = 0;
+ pwm_motor2.write(PWM2);
+ } else {
+ pwm_motor2.write(0);
+ }
+ } //end if
+}
+
+void motor1aansturing()
+{
+
+ if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN) {
+ toestand = WACHTEN;
+ }
+ if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
+ toestand = SLAAN;
+ pc.printf("slaan \n");
+ if ( snelheidsstand==3) {
+ setspeed = V3; pc.printf("Snel 3 \n");
+ }
+ if ( snelheidsstand==2) {
+ setspeed = V2; pc.printf("Snel 2\n");
+ }
+ if ( snelheidsstand==1) {
+ setspeed = V1; pc.printf("Snel 1 \n");
+ }
+ }
+ if (toestand == SLAAN && abs(motor1.getPosition()) >= ANGLEMAX) {
+ toestand = TERUGKEREN;
+ stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
+ }
+ if (toestand == TERUGKEREN) {
+ new_pwm = pid(Vreturn, motor1.getSpeed());
+ pwm_motor1.write(new_pwm);
+ motordir1 = 1;
+ }
+ if (toestand == WACHTEN) {
+ pwm_motor1.write(0);
+ }
+ if (toestand == SLAAN) {
+ new_pwm = pid(setspeed, motor1.getSpeed());
+ motordir1 = 0;
+ pwm_motor1.write(new_pwm);
+ }
+}
\ No newline at end of file