Dominique Clevers
the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of
The_SLAP_5_1
by 
Daan
Diff: main.cpp
- Revision:
- 9:9000c5c1a0d6
- Parent:
- 8:aa27423f4a4a
- Child:
- 10:d156dc2efe5c
--- a/main.cpp Wed Oct 29 16:41:46 2014 +0000
+++ b/main.cpp Wed Oct 29 19:13:44 2014 +0000
@@ -29,13 +29,13 @@
#define K_D_GM (0.003 /TSAMP)
#define I_LIMIT 1.
#define RADTICKGM 0.003927
-#define THETA0 6.85
+#define THETA0 6.85 //hoe bepaald?????
#define THETA1 7.77
#define THETA2 9.21
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
-Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
+Encoder motor2(PTD2,PTD0); //geel,wit kleine motor
Encoder motor1(PTD5,PTA13);//geel,wit
PwmOut pwm_motor1(M1_PWM);
PwmOut pwm_motor2(M2_PWM);
@@ -53,8 +53,10 @@
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
-float notch_biceps_states[4]; //state values
+//state values
+float notch_biceps_states[4];
float notch_triceps_states[4];
arm_biquad_casd_df1_inst_f32 highpass_biceps;
@@ -64,9 +66,10 @@
//state values
float highpass_biceps_states[4];
float highpass_triceps_states[4];
-//constants for 80Hz lowpass
+
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];
@@ -74,7 +77,9 @@
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];
@@ -171,35 +176,35 @@
arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
while(true) {
switch(state) {
- case RUST: { //Aanzetten
+ case RUST: { //Aanzetten
lcd.cls();
lcd.locate(0,0);
- lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm
+ lcd.printf(" -- THE SLAP -- "); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf(" GROEP 5 ");
+ lcd.printf(" GROEP 5 "); //regel 2 LCD scherm
wait(5);
state = KALIBRATIE;
break;
}
- case KALIBRATIE: {
+ case KALIBRATIE: { //kalibreren met maximale inspanning
max_value_biceps=0;
max_value_triceps=0;
state = BICEPSMAX;
switch(state) {
- case BICEPSMAX: {
+ case BICEPSMAX: { //maximale inspanning biceps
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Kalibratie"); //regel 1 LCD scherm
+ lcd.printf("Kalibratie"); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf("1:BICEPS MAX"); //regel 2 LCD scherm
wait(1);
tijdtimer.start();
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Biceps meting"); //regel 1 LCD scherm
+ lcd.printf("Biceps meting"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("Meting loopt!"); //regel 2 LCD scherm
+ lcd.printf("Meting loopt!"); //regel 2 LCD scherm
while (tijdtimer <= 3){
if (envelop_emg0 > max_value_biceps) {
max_value_biceps = envelop_emg0; }
@@ -209,27 +214,27 @@
tijdtimer.reset();
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Einde meting!"); //regel 1 LCD scherm
+ lcd.printf("Einde meting!"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
+ lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
wait(1);
state = TRICEPSMAX;
}//einde if statement
break;
}//einde case bicepsmax
- case TRICEPSMAX: {
+ case TRICEPSMAX: { //maximale inspanning biceps
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Kalibratie"); //regel 1 LCD scherm
+ lcd.printf("Kalibratie"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("2:TRICEPS MAX"); //regel 2 LCD scherm
+ lcd.printf("2:TRICEPS MAX"); //regel 2 LCD scherm
wait(1);
tijdtimer.start();
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Triceps meting"); //regel 1 LCD scherm
+ lcd.printf("Triceps meting"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("Meting loopt!"); //regel 2 LCD scherm
+ lcd.printf("Meting loopt!"); //regel 2 LCD scherm
while (tijdtimer <= 3){
if (envelop_emg1 > max_value_triceps) {
max_value_triceps = envelop_emg1; }
@@ -239,9 +244,9 @@
tijdtimer.reset();
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Einde meting!"); //regel 1 LCD scherm
+ lcd.printf("Einde meting!"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
+ lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
wait(1);
state = RICHTEN;
} //einde if statement
@@ -254,12 +259,12 @@
break;
} // einde kalibratie case
- case RICHTEN: { //Batje richten
+ case RICHTEN: { //batje richten (gebruik biceps en triceps)
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Richten"); //regel 1 LCD scherm
+ lcd.printf("Richten"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("Kies goal!"); //regel 2 LCD scherm
+ lcd.printf("Kies goal!"); //regel 2 LCD scherm
int16_t setpointkm;
float new_pwm_km;
wait(1);
@@ -269,7 +274,7 @@
tijdtimer.start();
while( tijdtimer <= 3) {
if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal!
- setpointkm = -125;
+ setpointkm = -127; //11,12graden naar rechts???????
new_pwm_km = pidkm(setpointkm, motor1.getPosition());
clamp(&new_pwm_km, -1,1);
if(new_pwm_km < 0)
@@ -282,7 +287,7 @@
}
}
if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
- setpointkm = 125;
+ setpointkm = 127; //11,12graden naar links??????
new_pwm_km = pidkm(setpointkm, motor1.getPosition());
clamp(&new_pwm_km, -1,1);
if(new_pwm_km < 0)
@@ -313,12 +318,12 @@
break;
}
- case SLAAN: { //Balletje slaan
+ case SLAAN: { //snelheid bepalen (gebruik alleen biceps)
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Slaan PingPong!"); //regel 1 LCD scherm
+ lcd.printf("Slaan PingPong!"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("Kies goal!"); //regel 2 LCD scherm
+ lcd.printf("Kies goal!"); //regel 2 LCD scherm
wait(1);
int16_t setpointgm;
float new_pwm_gm;
@@ -327,12 +332,12 @@
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
tijdtimer.start();
while (tijdtimer <=3 ) {
- if (kalibratiewaarde_biceps<0.3){ //kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
+ if (kalibratiewaarde_biceps<0.3){ //kalibratiewaarde_biceps<0.3 goal onderin
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Onderste goal"); //regel 1 LCD scherm
+ lcd.printf("Onderste goal"); //regel 1 LCD scherm
lcd.locate(0,1);
- lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm
+ lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm
tijdslaan.start();
theta=THETA0;
setpointgm = (theta*tijdslaan/RADTICKGM);
@@ -349,10 +354,10 @@
wait(2);
state = RUST;
}
- if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
+ if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){ //0.3<kalibratiewaarde_biceps<0.6 goal midden
lcd.cls();
lcd.locate(0,0);
- lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
+ lcd.printf("MIDDELSTE GOAL"); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
theta=THETA1;
@@ -369,10 +374,10 @@
wait(2);
state = RUST;
}
- if (kalibratiewaarde_biceps>0.6){
+ if (kalibratiewaarde_biceps>0.6){ //kalibratiewaarde_biceps>0.6 goal bovenin
lcd.cls();
lcd.locate(0,0);
- lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
+ lcd.printf("BOVENSTE GOAL"); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf("DAAR GAAT IE!"); //regel 2 LCD scherm
theta=THETA2;
@@ -398,7 +403,7 @@
//+ TERUGKEREN BEGINPOSITIE!
lcd.cls();
lcd.locate(0,0);
- lcd.printf("Goed Gedaan!"); //regel 1 LCD scherm
+ lcd.printf("Goed Gedaan!"); //regel 1 LCD scherm
lcd.locate(0,1);
lcd.printf("Terug naar begin"); //regel 2 LCD scherm
setpointgm = (0);