Dominique Clevers
voorlopige script getest (posities nog toevoegen)
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Revision 3:b06ada67fa4f, committed 2014-10-31
- Comitter:
- DominiqueC
- Date:
- Fri Oct 31 15:34:56 2014 +0000
- Parent:
- 2:8596695c56df
- Child:
- 4:527e5b5283c2
- Commit message:
- EINDSCRIPT versie 1
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 30 22:57:50 2014 +0000
+++ b/main.cpp Fri Oct 31 15:34:56 2014 +0000
@@ -21,12 +21,12 @@
#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 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
@@ -86,9 +86,6 @@
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)
{
@@ -162,6 +159,8 @@
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);
@@ -170,6 +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(true) {
switch(state) {
case RUST: { //Aanzetten
@@ -225,8 +225,6 @@
float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
- //pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
- //pc.printf("triceps %f\n\r", kalibratiewaarde_triceps);
if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
setpointkm = -127; //11,12graden naar links
pc.printf("links");
@@ -237,50 +235,120 @@
setpointkm = 0;
pc.printf("midden");
}
- //pc.printf("setpoint %f ", setpointkm);
-
- // NU MOTOR 2 LATEN BEWEGEN NAAR setpointkm
-
+ //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));
+ }
state = SLAAN;
break;
}
-
+
case SLAAN: {
pc.printf("Slaan PingPong!"); //regel 1 LCD scherm
pc.printf("Kies goal!"); //regel 2 LCD scherm
float thetadot;
float setpointgm;
float new_pwm_gm;
+ float setpointkm;
+ float new_pwm_km;
wait(3);
float kalibratiewaarde_biceps;
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
//pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
if (kalibratiewaarde_biceps <= 0.3) { //kalibratiewaarde_biceps<0.3 goal onderin
thetadot=THETADOT0;
- pc.printf("Onderste goal");
+ pc.printf("Onderste goal");
} else if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
thetadot=THETADOT1;
- pc.printf("MIDDELSTE GOAL");
+ pc.printf("MIDDELSTE GOAL");
} else { //goal bovenin
thetadot=THETADOT2;
- pc.printf("BOVENSTE GOAL");
+ pc.printf("BOVENSTE GOAL");
}
- //pc.printf("thetadot %f ", thetadot);
pc.printf("Daar gaat ie!");
-
+
// NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
-
- state = HOME;
+ 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 LATEN BEWEGEN NAAR setpointkm
+ 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;
+
+ // NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+ tijdtimer.reset();
+ tijdslaan.reset();
+ while (tijdtimer.read() <=3) {
+ 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 LATEN BEWEGEN NAAR setpointkm
+ 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));
+ }
+ //state = RICHTEN; //optioneel
break;
}
-
- case HOME: {
- // NU MOTOR 1 LATEN BEWEGEN NAAR 0
- // NU MOTOR 2 LATEN BEWEGEN NAAR 0
- // state = RICHTEN; //optioneel
- break;
+ default: {
+ state = RUST;
}
-
}
}
}
\ No newline at end of file
--- a/mbed.bld Thu Oct 30 22:57:50 2014 +0000 +++ b/mbed.bld Fri Oct 31 15:34:56 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file