Dominique Clevers
/
testen_eindscript
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Revision 2:8596695c56df, committed 2014-10-30
- Comitter:
- DominiqueC
- Date:
- Thu Oct 30 22:57:50 2014 +0000
- Parent:
- 1:0d5864272412
- Child:
- 3:b06ada67fa4f
- Commit message:
- testscript, posities nog toevoegen de rest doet het!
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 30 19:40:26 2014 +0000
+++ b/main.cpp Thu Oct 30 22:57:50 2014 +0000
@@ -35,7 +35,7 @@
//TextLCD pc(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
+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);
@@ -182,52 +182,105 @@
case KALIBRATIE: { //kalibreren met maximale inspanning
max_value_biceps=0;
max_value_triceps=0;
- state = BICEPSMAX;
- switch(state) {
- case BICEPSMAX: { //maximale inspanning biceps
- pc.printf("Kalibratie. 1:BICEPS MAX"); //pc scherm
- wait(1);
- tijdtimer.start();
- pc.printf("Biceps meting, meting loopt"); //pc scherm
- while (tijdtimer <= 3) {
- if (envelop_emg0 > max_value_biceps);
- {
- max_value_biceps = envelop_emg0;
- }
- }
- if (tijdtimer >= 3) {
- tijdtimer.stop();
- tijdtimer.reset();
- pc.printf("max value %f\n\r", max_value_biceps); //pc scherm
- wait(1);
- state = TRICEPSMAX;
- break;
- } //einde if statement
- } //einde case bicepsmax
- case TRICEPSMAX: { //maximale inspanning triceps
- pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
- wait(1);
- tijdtimer.start();
- pc.printf("Triceps meting, meting loopt!"); //pc scherm
- while (tijdtimer <= 3) {
- if (envelop_emg1 > max_value_triceps) {
- max_value_triceps = envelop_emg1;
- }
- }
- if (tijdtimer >= 3) {
- tijdtimer.stop();
- tijdtimer.reset();
- pc.printf("max value %f\n\r", max_value_triceps);
- wait(1);
- } //einde if statement
- break;
- } //einde case tricepsmax
- default: {
- state = BICEPSMAX;
- } //einde default
- } //einde switch states
+ //maximale inspanning biceps
+ pc.printf("Kalibratie. 1:BICEPS MAX"); //pc scherm
+ wait(1);
+ tijdtimer.start();
+ pc.printf("Biceps meting, meting loopt"); //pc scherm
+ while (tijdtimer <= 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); //pc scherm
+ wait(1);
+
+ //maximale inspanning triceps
+ pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
+ wait(1);
+ tijdtimer.start();
+ pc.printf("Triceps meting, meting loopt!"); //pc scherm
+ while (tijdtimer <= 3) {
+ if (envelop_emg1 > max_value_triceps) {
+ max_value_triceps = envelop_emg1;
+ }
+ }
+ tijdtimer.stop();
+ tijdtimer.reset();
+ pc.printf("max value %f\n\r", max_value_triceps);
+ wait(1);
+ state = RICHTEN;
break;
}// einde kalibratie case
+
+ case RICHTEN: { //batje richten (gebruik biceps en triceps)
+ pc.printf("Richten"); //regel 1 LCD scherm
+ pc.printf("Kies goal!"); //regel 2 LCD scherm
+ float setpointkm;
+ float new_pwm_km;
+ wait(3);
+ 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");
+ } else if (kalibratiewaarde_biceps <= 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
+ setpointkm = 127; //11,12graden naar rechts
+ pc.printf("rechts");
+ } else { //middelste goal!
+ setpointkm = 0;
+ pc.printf("midden");
+ }
+ //pc.printf("setpoint %f ", setpointkm);
+
+ // NU MOTOR 2 LATEN BEWEGEN NAAR setpointkm
+
+ 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;
+ 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");
+ } else if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
+ thetadot=THETADOT1;
+ pc.printf("MIDDELSTE GOAL");
+ } else { //goal bovenin
+ thetadot=THETADOT2;
+ pc.printf("BOVENSTE GOAL");
+ }
+ //pc.printf("thetadot %f ", thetadot);
+ pc.printf("Daar gaat ie!");
+
+ // NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+
+ state = HOME;
+ break;
+ }
+
+ case HOME: {
+ // NU MOTOR 1 LATEN BEWEGEN NAAR 0
+ // NU MOTOR 2 LATEN BEWEGEN NAAR 0
+ // state = RICHTEN; //optioneel
+ break;
+ }
+
}
}
-}
+}
\ No newline at end of file