Dominique Clevers
eindscript the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Revision 0:8d63bed88781, committed 2014-11-02
- Comitter:
- DominiqueC
- Date:
- Sun Nov 02 20:14:29 2014 +0000
- Child:
- 1:5187f57ad4b0
- Commit message:
- klaar!
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Daanmk/code/Encoder/#9a8b76f0908c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/tomlankhorst/code/HIDScope/#e44574634162
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#180e968a751e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/simon/code/TextLCD/#308d188a2d3a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Nov 02 20:14:29 2014 +0000
@@ -0,0 +1,400 @@
+/***************************************/
+/* */
+/* 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 "arm_math.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+#include "TextLCD.h"
+
+#define M2_PWM PTC8 //kleine motor
+#define M2_DIR PTC9 //kleine motor
+#define M1_PWM PTA5 //grote motor
+#define M1_DIR PTA4 //grote motor
+#define TSAMP 0.005 // Sampletijd, 200Hz
+#define K_P_KM (0.01)
+#define K_I_KM (0.0003 *TSAMP)
+#define K_D_KM (0.0 /TSAMP)
+#define K_P_GM (0.0022)
+#define K_I_GM (0.0001 *TSAMP)
+#define K_D_GM (0.00000001 /TSAMP)
+#define I_LIMIT 1.
+#define RADTICKGM 0.003927
+#define THETADOT0 6.85
+#define THETADOT1 7.77
+#define THETADOT2 9.21
+
+TextLCD lcd(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 MOTOR 2
+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); //deltoid
+
+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_deltoid,min_value_deltoid,metingstatus; //variable to store value in for deltoid
+
+arm_biquad_casd_df1_inst_f32 notch_biceps;
+arm_biquad_casd_df1_inst_f32 notch_deltoid;
+// 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_deltoid_states[4];
+
+arm_biquad_casd_df1_inst_f32 highpass_biceps;
+arm_biquad_casd_df1_inst_f32 highpass_deltoid;
+//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_deltoid_states[4];
+
+arm_biquad_casd_df1_inst_f32 lowpass_biceps;
+arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
+//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_deltoid_states[4];
+
+arm_biquad_casd_df1_inst_f32 envelop_biceps;
+arm_biquad_casd_df1_inst_f32 envelop_deltoid;
+//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_deltoid_states[4];
+
+enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
+uint8_t state=RUST;
+
+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(¬ch_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 deltoid
+ arm_biquad_cascade_df1_f32(¬ch_deltoid, &emg1_value_f32, &filtered_emg1_notch, 1 );
+ arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
+ arm_biquad_cascade_df1_f32(&lowpass_deltoid, &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_deltoid, &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;
+ tijdtimer.start();
+ tijdslaan.start();
+ arm_biquad_cascade_df1_init_f32(¬ch_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(¬ch_deltoid,1 , notch_const, notch_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&highpass_deltoid,1 ,highpass_const,highpass_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 ,lowpass_const,lowpass_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&envelop_deltoid,1 ,envelop_const,envelop_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
+
+ while(true) {
+ switch(state) {
+ case RUST: { //Aanzetten
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf(" --THE SLAP -- "); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf(" GROEP 5 "); //regel 2 LCD scherm
+ wait(5);
+ state = KALIBRATIE;
+ break;
+ }
+
+ case KALIBRATIE: { //kalibreren met maximale inspanning
+ max_value_biceps=0;
+ max_value_deltoid=0;
+ //maximale inspanning biceps
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Kalibratie1:"); //LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("Span biceps!");
+ wait(5);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Meting loopt");
+ tijdtimer.reset();
+ tijdtimer.start();
+ while (tijdtimer.read() <= 3) {
+ if (envelop_emg0 > max_value_biceps) {
+ max_value_biceps = envelop_emg0;
+ }
+ }
+ tijdtimer.stop();
+ tijdtimer.reset();
+ //pc.printf("max value %f\n\r", max_value_biceps);
+ wait(5);
+
+ //maximale inspanning deltoid
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Kalibratie2:");
+ lcd.locate(0,1);
+ lcd.printf("Span deltoid!");
+ wait(5);
+ tijdtimer.start();
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Meting loopt");
+ while (tijdtimer.read() <= 3) {
+ if (envelop_emg1 > max_value_deltoid) {
+ max_value_deltoid = envelop_emg1;
+ }
+ }
+ // tijdtimer.stop();
+ tijdtimer.reset();
+ //pc.printf("max value %f\n\r", max_value_deltoid);
+ wait(5);
+
+ state = RICHTEN;
+ break;
+ }// einde kalibratie case
+
+ case RICHTEN: { //batje richten (gebruik biceps en deltoid)
+ wait(3);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Richten"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ pc.printf("Kies goal!"); //regel 2 LCD scherm
+ float setpointkm;
+ float new_pwm_km;
+ wait(5);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Meting loopt");
+ float kalibratiewaarde_deltoid;
+ kalibratiewaarde_deltoid=(envelop_emg1/max_value_deltoid);
+ //pc.printf("deltoid %f\n\r", kalibratiewaarde_deltoid); //WEGHALEN LATER
+ if (kalibratiewaarde_deltoid >= 0.35) {
+ setpointkm = -127; //11,12graden naar links
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("links");
+ } else if (kalibratiewaarde_deltoid>0.1 && kalibratiewaarde_deltoid<=0.35) {
+ setpointkm = 0; //11,12graden naar rechts
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("midden");
+ } else {
+ setpointkm = 127;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("rechts");
+ }
+ //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
+ tijdtimer.reset();
+ while (tijdtimer.read() <= 3) {
+ while(looptimerflag == false);
+ looptimerflag = false;
+ new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir2 = 1; //links
+ else
+ motordir2 = 0; //rechts
+ pwm_motor2.write(abs(new_pwm_km));
+ }
+ wait(2);
+ state = SLAAN;
+ break;
+ }
+
+ case SLAAN: {
+ wait(3);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Slaan"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ pc.printf("Kies goal"); //regel 2 LCD scherm
+ float thetadot;
+ float setpointgm;
+ float new_pwm_gm;
+ float setpointkm;
+ float new_pwm_km;
+ wait(5);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Meting loopt");
+ float kalibratiewaarde_biceps;
+ kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
+ //pc.printf("biceps %f\n\r", kalibratiewaarde_biceps); //WEGHALEN LATER
+ if (kalibratiewaarde_biceps <= 0.2) { //kalibratiewaarde_biceps<0.3 goal onderin
+ thetadot=THETADOT0;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("ONDERSTE GOAL");
+ } else if (kalibratiewaarde_biceps>0.2 && kalibratiewaarde_biceps<=0.4) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
+ thetadot=THETADOT1;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("MIDDELSTE GOAL");
+ } else { //goal bovenin
+ thetadot=THETADOT2;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("BOVENSTE GOAL");
+ }
+ wait(3);
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Daar gaat ie!");
+
+ //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+ tijdtimer.reset();
+ tijdslaan.reset();
+ while (tijdtimer.read() <=3) {
+ while(looptimerflag == false);
+ looptimerflag = false;
+ if (motor1.getPosition()>= 444 ) {
+ setpointgm = 444;
+ } else {
+ setpointgm = ((thetadot*tijdslaan.read())/RADTICKGM);
+ }
+ new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
+ clamp(&new_pwm_gm, -1,1);
+ if(new_pwm_gm < 0) {
+ motordir1 = 1; //links
+ } else {
+ motordir1 = 0; //rechts
+ }
+ pwm_motor1.write(abs(new_pwm_gm));
+ }
+ //pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm);
+
+ //MOTOR 2 OP POSITIE HOUDEN
+ new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir2 = 1; //links
+ else
+ motordir2 = 0; //rechts
+ pwm_motor2.write(abs(new_pwm_km));
+
+ }
+ state = HOME;
+ break;
+
+ case HOME: {
+ //pc.printf("HOMESTATE");
+ float setpointgm;
+ float new_pwm_gm;
+ float setpointkm;
+ float new_pwm_km;
+
+ //MOTOR 1 naar home
+ tijdtimer.reset();
+ tijdslaan.reset();
+ while (tijdtimer.read() <=6) {
+ while(looptimerflag == false);
+ looptimerflag = false;
+ setpointgm = 0;
+ new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
+ clamp(&new_pwm_gm, -1,1);
+ if(new_pwm_gm < 0)
+ motordir1 = 1; //links
+ else
+ motordir1 = 0; //rechts
+ pwm_motor1.write(abs(new_pwm_gm));
+
+ //MOTOR 2 naar home
+ setpointkm=0;
+ new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir2 = 1; //links
+ else
+ motordir2 = 0; //rechts
+ pwm_motor2.write(abs(new_pwm_km));
+ }
+ wait(10);
+ state = RICHTEN; //optioneel
+ break;
+ }
+ default: {
+ state = RUST;
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dsp.lib Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/mbed-official/code/mbed-dsp/#7a284390b0ce
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Nov 02 20:14:29 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file