Diff: main.cpp

Revision:

0:653af6a8a659

Child:

1:0d5864272412

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 30 16:59:01 2014 +0000
@@ -0,0 +1,228 @@
+/***************************************/
+/*                                     */
+/*   BRONCODE GROEP 5, MODULE 9, 2014  */
+/*       *****-THE SLAP-******         */
+/*                                     */
+/* -Dominique Clevers                  */
+/* -Rianne van Dommelen                */
+/* -Daan de Muinck Keizer              */
+/* -David den Houting                  */
+/* -Marjolein Thijssen                 */
+/***************************************/
+#include "mbed.h"
+#include "HIDScope.h"
+#include "arm_math.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+//include "TextLCD.h"
+
+#define M2_PWM PTC8 //blauw
+#define M2_DIR PTC9 //groen
+#define M1_PWM PTA5 //kleine motor
+#define M1_DIR PTA4 //kleine motor
+#define TSAMP 0.005  // Sampletijd, 200Hz
+#define K_P_KM (0.1)
+#define K_I_KM (0.03  *TSAMP)
+#define K_D_KM (0.001 /TSAMP)
+#define K_P_GM (2.9)
+#define K_I_GM (0.3  *TSAMP)
+#define K_D_GM (0.003 /TSAMP)
+#define I_LIMIT 1.
+#define RADTICKGM 0.003927
+#define THETADOT0 6.85
+#define THETADOT1 7.77
+#define THETADOT2 9.21
+
+//TextLCD pc(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //Textpc pc(p15, p16, p17, p18, p19, p20, Textpc::pc16x4); // rs, e, d4-d7 ok
+
+Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
+Encoder motor1(PTD5,PTA13);//geel,wit
+PwmOut pwm_motor1(M1_PWM);
+PwmOut pwm_motor2(M2_PWM);
+DigitalOut motordir2(M2_DIR);
+DigitalOut motordir1(M1_DIR);
+AnalogIn emg0(PTB0); //Biceps
+AnalogIn emg1(PTB1); //Triceps
+HIDScope scope(6);
+
+MODSERIAL pc(USBTX,USBRX,64,1024);
+
+
+float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0,pwm_to_motor1,max_value_biceps,min_value_biceps; //variable to store value in for biceps
+float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_triceps,min_value_triceps,metingstatus; //variable to store value in for triceps
+
+arm_biquad_casd_df1_inst_f32 notch_biceps;
+arm_biquad_casd_df1_inst_f32 notch_triceps;
+// constants for 50 Hz notch (bandbreedte 2 Hz)
+float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
+//state values
+float notch_biceps_states[4];
+float notch_triceps_states[4];
+
+arm_biquad_casd_df1_inst_f32 highpass_biceps;
+arm_biquad_casd_df1_inst_f32 highpass_triceps;
+//constants for 20Hz highpass
+float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
+//state values
+float highpass_biceps_states[4];
+float highpass_triceps_states[4];
+
+arm_biquad_casd_df1_inst_f32 lowpass_biceps;
+arm_biquad_casd_df1_inst_f32 lowpass_triceps;
+//constants for 80Hz lowpass
+float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
+//state values
+float lowpass_biceps_states[4];
+float lowpass_triceps_states[4];
+
+arm_biquad_casd_df1_inst_f32 envelop_biceps;
+arm_biquad_casd_df1_inst_f32 envelop_triceps;
+//constants for envelop
+float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
+// state values
+float envelop_biceps_states[4];
+float envelop_triceps_states[4];
+
+enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
+uint8_t state=RUST;
+
+enum kalibratiestates {BICEPSMAX,TRICEPSMAX};
+
+volatile bool looptimerflag;
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor
+{
+    float error_km;
+    static float prev_error_km = 0;
+    float           out_p_km = 0;
+    static float    out_i_km = 0;              //static, want dan wordt vorige waarde onthouden
+    float           out_d_km = 0;
+    error_km  = setpointkm-measurementkm;
+    out_p_km  = error_km*K_P_KM;
+    out_i_km += error_km*K_I_KM;
+    out_d_km  = (error_km-prev_error_km)*K_D_KM;
+    clamp(&out_i_km,-I_LIMIT,I_LIMIT);
+    prev_error_km = error_km;
+    return out_p_km + out_i_km + out_d_km;
+}
+
+float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
+{
+    float error_gm;
+    static float prev_error_gm = 0;
+    float           out_p_gm = 0;
+    static float    out_i_gm = 0;
+    float           out_d_gm = 0;
+    error_gm  = setpointgm-measurementgm;
+    out_p_gm  = error_gm*K_P_GM;
+    out_i_gm += error_gm*K_I_GM;
+    out_d_gm  = (error_gm-prev_error_gm)*K_D_GM;
+    clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
+    prev_error_gm = error_gm;
+    return out_p_gm + out_i_gm + out_d_gm;
+}
+void emgmeten()
+{
+    /*put raw emg value in emg_value*/
+    emg0_value_f32 = emg0.read();
+    emg1_value_f32 = emg1.read();
+
+    //process emg biceps
+    arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
+    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
+    arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
+    filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass);  //gelijkrichter
+    arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
+
+    //process emg triceps
+    arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
+    arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
+    arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
+    filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass);  //gelijkrichter
+    arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
+}
+
+
+int main()
+{
+    pc.baud(38400); //PC baud rate is 38400 bits/seconde
+    Ticker emg_timer;
+    emg_timer.attach(emgmeten, TSAMP);
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,TSAMP);
+    Timer tijdtimer;
+    Timer tijdslaan;
+    arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
+    arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
+    arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
+    arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
+    while(true) {
+        switch(state) {
+            case RUST: {                            //Aanzetten
+                pc.printf("--THE SLAP -- GROEP 5");        //pc scherm
+                wait(5);
+                state = KALIBRATIE;
+                break;
+            }
+
+            case KALIBRATIE: {                                  //kalibreren met maximale inspanning
+                max_value_biceps=0;
+                max_value_triceps=0;
+                state = BICEPSMAX;
+                switch(state) {
+                    case BICEPSMAX: {                           //maximale inspanning biceps
+                        pc.printf("Kalibratie. 1:BICEPS MAX");  //pc scherm
+                        wait(1);
+                        tijdtimer.start();
+                        pc.printf("Biceps meting, meting loopt"); //pc scherm
+                        while (tijdtimer <= 3) {
+                            if (envelop_emg0 > max_value_biceps) {
+                                max_value_biceps = envelop_emg0;
+                            }
+                        }
+                        if (tijdtimer >= 3) {
+                            tijdtimer.stop();
+                            tijdtimer.reset();
+                            pc.printf("max value %f\n\r", max_value_biceps);        //pc scherm
+                            wait(1);
+                        state = TRICEPSMAX;
+                        }                                       //einde if statement
+                        break;
+                    }                                           //einde case bicepsmax
+                    case TRICEPSMAX: {                          //maximale inspanning triceps
+                        pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
+                        wait(1);
+                        tijdtimer.start();
+                        pc.printf("Triceps meting, meting loopt!"); //pc scherm
+                        while (tijdtimer <= 3) {
+                            if (envelop_emg1 > max_value_triceps) {
+                                max_value_triceps = envelop_emg1;
+                            }
+                        }
+                        if (tijdtimer >= 3) {
+                            tijdtimer.stop();
+                            tijdtimer.reset();
+                            pc.printf("max value %f\n\r", max_value_triceps);
+                            wait(1);
+                        } //einde if statement
+                        break;
+                    } //einde case tricepsmax
+                    default: {
+                        state = BICEPSMAX;
+                    }  //einde default
+                } //einde switch states
+                break;
+            }}}}       // einde kalibratie case