Dominique Clevers
voorlopige script getest (posities nog toevoegen)
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Revision 4:527e5b5283c2, committed 2014-10-31
- Comitter:
- DominiqueC
- Date:
- Fri Oct 31 16:52:26 2014 +0000
- Parent:
- 3:b06ada67fa4f
- Child:
- 5:5085197c02be
- Commit message:
- Eindscript nog hangend in Home case;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 31 15:34:56 2014 +0000
+++ b/main.cpp Fri Oct 31 16:52:26 2014 +0000
@@ -10,16 +10,15 @@
/* -Marjolein Thijssen */
/***************************************/
#include "mbed.h"
-#include "HIDScope.h"
#include "arm_math.h"
#include "encoder.h"
#include "MODSERIAL.h"
//include "TextLCD.h"
-#define M2_PWM PTC8 //blauw
-#define M2_DIR PTC9 //groen
-#define M1_PWM PTA5 //kleine motor
-#define M1_DIR PTA4 //kleine motor
+#define M2_PWM PTC8 //kleine motor
+#define M2_DIR PTC9 //kleine motor
+#define M1_PWM PTA5 //grote motor
+#define M1_DIR PTA4 //grote motor
#define TSAMP 0.005 // Sampletijd, 200Hz
#define K_P_KM (0.01)
#define K_I_KM (0.0003 *TSAMP)
@@ -43,7 +42,6 @@
DigitalOut motordir1(M1_DIR);
AnalogIn emg0(PTB0); //Biceps
AnalogIn emg1(PTB1); //Triceps
-HIDScope scope(6);
MODSERIAL pc(USBTX,USBRX,64,1024);
@@ -183,35 +181,36 @@
max_value_biceps=0;
max_value_triceps=0;
//maximale inspanning biceps
- pc.printf("Kalibratie. 1:BICEPS MAX"); //pc scherm
- wait(1);
+ pc.printf("Kalibratie1: Span Biceps!"); //pc scherm
+ wait(5);
+ pc.printf("Meting loopt"); //pc scherm
+ tijdtimer.reset();
tijdtimer.start();
- pc.printf("Biceps meting, meting loopt"); //pc scherm
- while (tijdtimer <= 3) {
- if (envelop_emg0 > max_value_biceps);
- {
+ while (tijdtimer.read() <= 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);
+ pc.printf("max value %f\n\r", max_value_biceps);
+ wait(5);
//maximale inspanning triceps
- pc.printf("Kalibratie. 2:TRICEPS MAX"); //pc scherm
- wait(1);
+ pc.printf("Kalibratie2: Span Triceps!"); //pc scherm
+ wait(5);
tijdtimer.start();
- pc.printf("Triceps meting, meting loopt!"); //pc scherm
- while (tijdtimer <= 3) {
+ pc.printf("Meting loopt"); //pc scherm
+ while (tijdtimer.read() <= 3) {
if (envelop_emg1 > max_value_triceps) {
max_value_triceps = envelop_emg1;
}
}
- tijdtimer.stop();
+ // tijdtimer.stop();
tijdtimer.reset();
pc.printf("max value %f\n\r", max_value_triceps);
- wait(1);
+ wait(5);
+
state = RICHTEN;
break;
}// einde kalibratie case
@@ -225,6 +224,7 @@
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); //WEGHALEN LATER
if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
setpointkm = -127; //11,12graden naar links
pc.printf("links");
@@ -248,13 +248,14 @@
motordir2 = 0; //rechts
pwm_motor2.write(abs(new_pwm_km));
}
+ wait(2);
state = SLAAN;
break;
}
case SLAAN: {
- pc.printf("Slaan PingPong!"); //regel 1 LCD scherm
- pc.printf("Kies goal!"); //regel 2 LCD scherm
+ pc.printf("Slaan"); //regel 1 LCD scherm
+ pc.printf("Kies goal"); //regel 2 LCD scherm
float thetadot;
float setpointgm;
float new_pwm_gm;
@@ -263,10 +264,10 @@
wait(3);
float kalibratiewaarde_biceps;
kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
- //pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
+ pc.printf("biceps %f\n\r", kalibratiewaarde_biceps); //WEGHALEN LATER
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");
@@ -276,7 +277,7 @@
}
pc.printf("Daar gaat ie!");
- // NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+ //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
tijdtimer.reset();
tijdslaan.reset();
while (tijdtimer.read() <=3) {
@@ -298,7 +299,7 @@
}
pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm);
- //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
+ //MOTOR 2 OP POSITIE HOUDEN
new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
clamp(&new_pwm_km, -1,1);
if(new_pwm_km < 0)
@@ -318,7 +319,7 @@
float setpointkm;
float new_pwm_km;
- // NU MOTOR 1 LATEN BEWEGEN met snelheid thetadot
+ //MOTOR 1 naar home
tijdtimer.reset();
tijdslaan.reset();
while (tijdtimer.read() <=3) {
@@ -333,7 +334,7 @@
motordir1 = 0; //rechts
pwm_motor1.write(abs(new_pwm_gm));
- //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
+ //MOTOR 2 naar home
setpointkm=0;
new_pwm_km = pidkm(setpointkm, motor2.getPosition()); //bewegen naar setpoint
clamp(&new_pwm_km, -1,1);