Dominique Clevers
the slap
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of
The_SLAP_5_1
by 
Daan
Diff: main.cpp
- Revision:
- 7:dac6b30d43b3
- Parent:
- 6:cb5daf35ba9b
- Child:
- 8:aa27423f4a4a
--- a/main.cpp Tue Oct 28 09:46:57 2014 +0000
+++ b/main.cpp Tue Oct 28 14:53:54 2014 +0000
@@ -21,13 +21,17 @@
#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_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 THETA0 6.85
+#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
@@ -77,6 +81,8 @@
enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
uint8_t state=RUST;
+enum kalibratiestates {BICEPSMAX,TRICEPSMAX};
+
volatile bool looptimerflag;
void setlooptimerflag(void)
{
@@ -148,13 +154,13 @@
int main()
{
- while(1) {
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(¬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);
@@ -163,7 +169,7 @@
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(1) {
switch(state) {
case RUST: { //Aanzetten
lcd.cls();
@@ -172,177 +178,128 @@
lcd.locate(0,1);
lcd.printf(" GROEP 5 ");
wait(5);
- if (metingstatus<5);
- state = KALIBRATIE;
- if (metingstatus==5);
- state = RICHTEN;
+ state = KALIBRATIE;
break;
}
case KALIBRATIE: {
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("Kalibratie"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf("1:BICEPS RUST"); //regel 2 LCD scherm
- wait(1);
max_value_biceps=0;
max_value_triceps=0;
- min_value_biceps=0;
- min_value_triceps=0;
- int metingstatus=1;
-
- if (metingstatus==1) { //BICEPS RUST METING
-
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("BICEPS RUST"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
- wait(1);
- tijdtimer.start();
-
- if (envelop_emg0 > min_value_biceps) {
- min_value_biceps = envelop_emg0;
- }
- if (tijdtimer == 3 ) {
- tijdtimer.stop();
- tijdtimer.reset();
- metingstatus=2;
- }
- }
- if (metingstatus==2) { //TRICEPS RUST METING
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("TRICEPS RUST"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf("ONTSPAN! (3SEC)"); //regel 2 LCD scherm
- wait(1);
- tijdtimer.start();
- if (envelop_emg1 > min_value_triceps) {
- min_value_triceps = envelop_emg1; }
- if (tijdtimer == 3 ) {
- tijdtimer.stop();
- tijdtimer.reset();
- metingstatus=3;
- }
- }
- if (metingstatus==3) { //BICEPS KRACHT METING
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("BICEPS MAX"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
- wait(1);
- tijdtimer.start();
- if (envelop_emg0 > max_value_biceps) {
- max_value_biceps = envelop_emg0; }
- if (tijdtimer == 3 ) {
- tijdtimer.stop();
- tijdtimer.reset();
- metingstatus=4;
- }
- }
- if (metingstatus==4) {//TRICEPS KRACHT METING
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("TRICEPS MAX"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf("SPAN AAN! (3SEC)"); //regel 2 LCD scherm
- tijdtimer.start();
- if (envelop_emg1 > max_value_triceps) {
- max_value_triceps = envelop_emg1; }
- if (tijdtimer == 3 ) {
- tijdtimer.stop();
- tijdtimer.reset();
- metingstatus=5;
- }
- }
- if (metingstatus==5) {
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("Kalibratie OK"); //regel 1 LCD scherm
- lcd.locate(0,1);
- lcd.printf(""); //regel 2 LCD scherm
- wait(1);
- state = RICHTEN; }
+ state = BICEPSMAX;
+ switch(state) {
+ case BICEPSMAX: {
+ lcd.cls();
+ lcd.locate(0,0);
+ 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.locate(0,1);
+ lcd.printf("Meting loopt!"); //regel 2 LCD scherm
+ while (tijdtimer <= 3){
+ if (envelop_emg0 > max_value_biceps) {
+ max_value_biceps = envelop_emg0; }
+ }
+ if (tijdtimer >= 3) {
+ tijdtimer.stop();
+ tijdtimer.reset();
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Einde meting!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
+ wait(1);
+ state = TRICEPSMAX;
+ }//einde if statement
+ break;
+ }//einde case bicepsmax
+ case TRICEPSMAX: {
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Kalibratie"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ 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.locate(0,1);
+ lcd.printf("Meting loopt!"); //regel 2 LCD scherm
+ while (tijdtimer <= 3){
+ if (envelop_emg1 > max_value_triceps) {
+ max_value_triceps = envelop_emg1; }
+ }
+ if (tijdtimer >= 3) {
+ tijdtimer.stop();
+ tijdtimer.reset();
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Einde meting!"); //regel 1 LCD scherm
+ lcd.locate(0,1);
+ lcd.printf("waarde"); //METING WAARDE TOEVOEGEN
+ wait(1);
+ state = RICHTEN;
+ } //einde if statement
+ break;
+ } //einde case tricepsmax
+ default: {
+ state = BICEPSMAX;
+ } //einde default
+ } //einde switch states
break;
- }
+ } // einde kalibratie case
case RICHTEN: { //Batje richten
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);
- 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 && 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
-
+ while( tijdtimer <= 3) {
+ if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps < 0.3) { //linker goal!
+ setpointkm = -168;
+ 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));
+ }
+ if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
+ setpointkm = 168;
+ 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));
+ }
+ if (kalibratiewaarde_biceps < 0.3 && kalibratiewaarde_triceps < 0.3) { //middelste goal!
+ 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));
+ }
}
- 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) {
+ if (tijdtimer >= 3) {
tijdtimer.stop();
tijdtimer.reset();
state = SLAAN;
@@ -353,23 +310,26 @@
case SLAAN: { //Balletje slaan
lcd.cls();
lcd.locate(0,0);
- lcd.printf("SLAAN!"); //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;
- int goalhoogte=0;
float kalibratiewaarde_biceps;
+ float theta;
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
- if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
- goalhoogte = 0; //goalkeuze, onderste goal
+ tijdtimer.start();
+ while (tijdtimer <=3 ) {
+ if (kalibratiewaarde_biceps>0.1 && kalibratiewaarde_biceps<0.3){
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
- setpointgm = //onbekend;
+ lcd.printf("Daar gaat ie!"); //regel 2 LCD scherm
+ tijdslaan.start();
+ theta=THETA0;
+ setpointgm = (theta*tijdslaan/RADTICKGM);
new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
clamp(&new_pwm_gm, -1,1);
if(new_pwm_gm < 0)
@@ -380,14 +340,14 @@
wait(2);
state = RUST;
}
- if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
- goalhoogte = 1; //goalkeuze, middelste goal
+ if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<0.6){
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;
+ theta=THETA1;
+ setpointgm = ((theta*tijdslaan)/RADTICKGM);
new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
clamp(&new_pwm_gm, -1,1);
if(new_pwm_gm < 0)
@@ -398,14 +358,14 @@
wait(2);
state = RUST;
}
- if (kalibratiewaarde_biceps>0.6){
- goalhoogte = 2; //goalkeuze, Bovenste goal
+ if (kalibratiewaarde_biceps>0.6){
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;
+ theta=THETA2;
+ setpointgm = ((theta*tijdslaan)/RADTICKGM);
new_pwm_gm = pidgm(setpointgm, motor2.getPosition());
clamp(&new_pwm_gm, -1,1);
if(new_pwm_gm < 0)
@@ -417,8 +377,14 @@
state = RUST;
}
-
-
+ } //einde whilelus
+ if (tijdtimer >=3 ) {
+ tijdtimer.stop();
+ tijdtimer.reset();
+
+ //+ TERUGKEREN BEGINPOSITIE!
+ state = RUST;
+ } //einde if statement
state = RUST;