Dominique Clevers
the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of
The_SLAP_5_1
by 
Daan
Diff: main.cpp
- Revision:
- 6:cb5daf35ba9b
- Parent:
- 5:5383d9a80307
- Child:
- 7:dac6b30d43b3
--- a/main.cpp Mon Oct 27 15:43:34 2014 +0000
+++ b/main.cpp Tue Oct 28 09:46:57 2014 +0000
@@ -21,6 +21,13 @@
#define M1_PWM PTA5 //kleine motor
#define M1_DIR PTA4 //kleine motor
#define TSAMP 0.005 // Sampletijd, 200Hz
+#define K_P_KM (14)
+#define K_I_KM (28 *TSAMP)
+#define K_D_KM (0.01 /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.
TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x4); // rs, e, d4-d7 ok
@@ -38,7 +45,7 @@
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; //variable to store value in for triceps
+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;
@@ -76,8 +83,48 @@
looptimerflag = true;
}
-void keep_in_range(float * in, float min, float max); //keep in range
+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;
+ 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;
+ scope.set(1,out_p_km);
+ scope.set(2,out_i_km);
+ scope.set(3,out_d_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;
+ scope.set(1,out_p_gm);
+ scope.set(2,out_i_gm);
+ scope.set(3,out_d_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();
@@ -97,6 +144,7 @@
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()
{
@@ -118,8 +166,16 @@
switch(state) {
case RUST: { //Aanzetten
-
- state = KALIBRATIE;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf(" GROEP 5 ");
+ wait(5);
+ if (metingstatus<5);
+ state = KALIBRATIE;
+ if (metingstatus==5);
+ state = RICHTEN;
break;
}
@@ -170,6 +226,7 @@
tijdtimer.reset();
metingstatus=3;
}
+ }
if (metingstatus==3) { //BICEPS KRACHT METING
lcd.cls();
lcd.locate(0,0);
@@ -185,6 +242,7 @@
tijdtimer.reset();
metingstatus=4;
}
+ }
if (metingstatus==4) {//TRICEPS KRACHT METING
lcd.cls();
lcd.locate(0,0);
@@ -198,7 +256,8 @@
tijdtimer.stop();
tijdtimer.reset();
metingstatus=5;
- }
+ }
+ }
if (metingstatus==5) {
lcd.cls();
lcd.locate(0,0);
@@ -216,41 +275,153 @@
lcd.printf("RICHTEN"); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf("KIES GOAL"); //regel 2 LCD scherm
+ int16_t setpointkm;
+ float new_pwm_km;
wait(1);
- int goalkeuze=1; //midden goadl
+ int goalkeuze=1; //midden goal
float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps); //RUSTWAARDES NOG NIET GEBRUIKT
kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
tijdtimer.start();
- if (kalibratiewaarde_biceps>0.3);{
- goalkeuze = 0; //linker goal
+
+ if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_triceps<0.3){
+ goalkeuze = 0; //goalkeuze
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("LINKER GOAL!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf(""); //regel 2 LCD scherm
+ setpointkm = -241;
+ new_pwm_km = pidkm(setpointkm, motor1.getPosition());
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir1 = 1;
+ else
+ motordir1 = 0;
+ pwm_motor1.write(abs(new_pwm_km));
+ wait(1);
+ state = SLAAN;
+ //-241 encoder tics tov midden goal
+
}
- if (kalibratiewaarde_triceps>0.3); {
+ if (kalibratiewaarde_triceps>0.3 && kalibratiewaarde_biceps<0.3) {
goalkeuze = 2; //rechter goal
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("RECHTER GOAL!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf(""); //regel 2 LCD scherm
+ //+11 encoder tics tov midden goal
+ setpointkm = 11;
+ new_pwm_km = pidkm(setpointkm, motor1.getPosition());
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir1 = 1;
+ else
+ motordir1 = 0;
+ pwm_motor1.write(abs(new_pwm_km));
+ wait(1);
+ state = SLAAN;
+ }
+ if (kalibratiewaarde_biceps <0.3 && kalibratiewaarde_triceps<0.3) {
+ goalkeuze=1; // middelste goal!
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("MIDDEN GOAL!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf(""); //regel 2 LCD scherm
+ //0 encoder tics. Ook start positie!
+ setpointkm = 0;
+ new_pwm_km = pidkm(setpointkm, motor1.getPosition());
+ clamp(&new_pwm_km, -1,1);
+ if(new_pwm_km < 0)
+ motordir1 = 1;
+ else
+ motordir1 = 0;
+ pwm_motor1.write(abs(new_pwm_km));
+ wait(1);
+ state = SLAAN;
}
if (tijdtimer == 5) {
tijdtimer.stop();
tijdtimer.reset();
state = SLAAN;
- }
-
-
-
-
-
- keep_in_range(&pwm_to_motor, -1,1);
- if(pwm_to_motor > 0)
- motordir1.write(1);
- else
- motordir1.write(0);
- pwm_motor1.write(abs(pwm_to_motor));
+ }
break;
}
case SLAAN: { //Balletje slaan
- lcd.printf("Slaan!");
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("SLAAN!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("KIES GOAL!"); //regel 2 LCD scherm
wait(1);
- state = RICHTEN;
+ int16_t setpointgm;
+ float new_pwm_gm;
+ int goalhoogte=0;
+ float kalibratiewaarde_biceps;
+ kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
+ if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
+ goalhoogte = 0; //goalkeuze, onderste goal
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("ONDERSTE GOAL"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
+ setpointgm = //onbekend;
+ new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
+ clamp(&new_pwm_gm, -1,1);
+ if(new_pwm_gm < 0)
+ motordir2 = 0;
+ else
+ motordir2 = 1;
+ pwm_motor2.write(abs(new_pwm_gm));
+ wait(2);
+ state = RUST;
+ }
+ if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
+ goalhoogte = 1; //goalkeuze, middelste goal
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
+ setpointgm = //onbekend;
+ new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
+ clamp(&new_pwm_gm, -1,1);
+ if(new_pwm_gm < 0)
+ motordir2 = 0;
+ else
+ motordir2 = 1;
+ pwm_motor2.write(abs(new_pwm_gm));
+ wait(2);
+ state = RUST;
+ }
+ if (kalibratiewaarde_biceps>0.6){
+ goalhoogte = 2; //goalkeuze, Bovenste goal
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
+ setpointgm = //onbekend;
+ new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
+ clamp(&new_pwm_gm, -1,1);
+ if(new_pwm_gm < 0)
+ motordir2 = 0;
+ else
+ motordir2 = 1;
+ pwm_motor2.write(abs(new_pwm_gm));
+ wait(2);
+ state = RUST;
+
+ }
+
+
+
+
+ state = RUST;
break;
}
@@ -258,9 +429,8 @@
state = RUST;
}
- }
+ } //switch sate
- }
-}
+ } //while
+} //int main
-}
\ No newline at end of file