BMT M9 Groep01
robot
Dependencies: Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed
Fork of
Robot2false
by 
BMT M9 Groep01
Diff: main.cpp
- Revision:
- 61:6bf3935b9e74
- Parent:
- 60:c4cf57749f2e
- Child:
- 62:bdcc3328b13e
--- a/main.cpp Mon Nov 03 20:50:58 2014 +0000
+++ b/main.cpp Mon Nov 03 21:05:53 2014 +0000
@@ -9,9 +9,9 @@
#define K_I (0.0 *TSAMP1)
#define K_D (0 /TSAMP1)
#define I_LIMIT 1.
-#define K_Pp (0.003) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-#define K_Ip (0.0 *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
-#define K_Dp (0.000003 /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
+#define K_Pp (0.003)
+#define K_Ip (0.0 *TSAMP2)
+#define K_Dp (0.000003 /TSAMP2)
#define TSAMP1 0.01
#define TSAMP2 0.01
@@ -35,7 +35,7 @@
//alle initiaties voor EMG
MODSERIAL pc(USBTX,USBRX);
-HIDScope scope(6);
+HIDScope scope(5);
AnalogIn emgB(PTB1); //biceps
AnalogIn emgT(PTB2); //tricep
@@ -83,18 +83,17 @@
float pwm;
float new_pwmpos;
float new_pwm;
-float PWM2 = 0.3; //PWM voor instellen hoek batje, kan waarschijnlijk een stuk langzamer
-int toestand = WACHTEN; //terugkeren?
+float PWM2 = 0.3; //PWM voor instellen hoek batje
+int toestand = WACHTEN;
float setspeed = 0, V3=150, V2=40, V1 =30;//V in counts/s
-
Encoder motor1(PTD5,PTD3, true);
Encoder motor2(PTD0,PTD2);
-
DigitalOut motordir1(PTA4);
DigitalOut motordir2(PTC9);
PwmOut pwm_motor1(PTA5);
PwmOut pwm_motor2(PTC8);
+
float prevgetpos=0;
float omega;
float deltacounts;
@@ -118,7 +117,6 @@
* key 2 will light Green LED -> CALIBRATIE BICEPS
* key 3 will light Blue LED -> START*/
-
enum standen {STAND1=0, STAND2=1, STAND3=2};
standen hoek2 = STAND1;
@@ -139,12 +137,12 @@
int key=0;
- pc.printf("key 1 calibratie triceps\n");
- pc.printf("key 2 caliratie biceps\n");
- pc.printf("key 3 START\n");
+ pc.printf("key 1 calibratie tricep\r\n");
+ pc.printf("key 2 caliratie biceps\r\n");
+ pc.printf("key 3 START\r\n");
while(true) {
-
+
key = TButton.PressedButton();
if (key==1) {
@@ -154,7 +152,7 @@
myled3 = 1;
rood=1;
- /*pc.printf("calibratie tricep aan\n");
+ pc.printf("calibratie tricep aan\n");
wait(2);
Calibratie_Triceps();
@@ -173,7 +171,7 @@
myled2=1;
myled3=1;
rood=0;
- wit=0;*/
+ wit=0;
}
if (key==2) {
//green
@@ -182,7 +180,7 @@
myled3 = 1;
groen=1;
- /*pc.printf("calibratie bicep snelheid 1 aan\n");
+ pc.printf("calibratie bicep snelheid 1 aan\n");
wait(2);
Calibratie_Biceps();
@@ -234,16 +232,15 @@
groen=0;
myled1=1;
myled2=1;
- myled3=1;*/
+ myled3=1;
}
-
if (key==3) {
//blue
myled1 = 1;
myled2 = 1;
myled3 = 0;
blauw=1;
- /*wait(3);
+ wait(3);
if(drempelwaardeT==0) {
pc.printf("geen waarde calibratie TRICEPS \n");
@@ -276,194 +273,184 @@
myled3 = 0;
wit=1;
groen=1;
- } else {*/
-
- /*pc.printf("eerst positie dan snelheid aangeven /n");
+ } else {
+ pc.printf("eerst positie dan snelheid aangeven /n");
- //bepaling van positie met triceps 1
- Ticker log_timerT1;
-
- arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
- arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
- arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+ //bepaling van positie met triceps 1
+ Ticker log_timerT1;
- myled1 = 0;
- myled2 = 1;
- myled3 = 1;
- rood=1;
+ arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
- log_timerT1.attach(Triceps, 0.005);
- wait(2);
- log_timerT1.detach();
-
- // positie van batje met behulp van Triceps
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
+ rood=1;
- if (MOVAVG_T >= drempelwaardeT) {
- yT1=1;
- } else {
- yT1=0;
- }
+ log_timerT1.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT1.detach();
- pc.printf("Triceps meting 1 is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
- rood=0;
+ // positie van batje met behulp van Triceps
- wait(3);
-
- //bepaling van positie met tricep 2
- Ticker log_timerT2;
-
- arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
- arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
- arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT1=1;
+ } else {
+ yT1=0;
+ }
- myled1 = 0;
- myled2 = 1;
- myled3 = 1;
- rood=1;
-
- log_timerT2.attach(Triceps, 0.005);
- wait(2);
- log_timerT2.detach();
+ pc.printf("Triceps meting 1 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ rood=0;
- if (MOVAVG_T >= drempelwaardeT) {
- yT2=1;
- } else {
- yT2=0;
- }
+ wait(3);
- pc.printf("Triceps meting 2 is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
- rood=0;
+ //bepaling van positie met tricep 2
+ Ticker log_timerT2;
- // *** INPUT MOTOR 2 ***
- positie=yT1+yT2; */
-positie=1;
- //controle positie op scherm
- if (positie==0) {
- pc.printf("Motor 2 blijft op stand 1\n");
- } else {
- if (positie==1) {
- pc.printf("Motor 2 gaat naar stand 2\n");
- } else {
- if (positie==2) {
- pc.printf("Motor 2 gaat naar stand 3\n");
- }
- }
- }
+ arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
+
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
+ rood=1;
- Ticker looptimer2;
- looptimer2.attach(motor2aansturing,TSAMP1);
- wait(8);
- looptimer2.detach();
- pc.printf("Detach Motor 2\n");
-
-
- // ------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
- wait(2);
- /* Ticker log_timerB;
+ log_timerT2.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT2.detach();
- arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states);
- arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
- arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
-
- myled1 = 1;
- myled2 = 0;
- myled3 = 1;
- groen=1;
-
- log_timerB.attach(Biceps,0.005);
- wait(2);
- log_timerB.detach();
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT2=1;
+ } else {
+ yT2=0;
+ }
- //bepaling van snelheidsstand met biceps
+ pc.printf("Triceps meting 2 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ rood=0;
- if (MOVAVG_B >= drempelwaardeB1) {
- yB1=1;
- if (MOVAVG_B >= drempelwaardeB2) {
- yB2=1;
- if (MOVAVG_B >= drempelwaardeB3) {
- yB3=1;
- } else {
- yB3=0;
- }
- } else {
- yB2=0;
- }
- } else {
- yB1=0;
- }
+ // *** INPUT MOTOR 2 ***
+ positie=yT1+yT2;
- pc.printf("Biceps meting is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
- groen=0;
-
- // *** INPUT MOTOR 1 ***
- snelheidsstand=yB1+yB2+yB3; */
-
- snelheidsstand=3;
-
- //controle snelheidsstand op scherm
- if (snelheidsstand==0) {
- pc.printf("Motor 1 beweegt niet \n");
- } else {
- if (snelheidsstand==1) {
- pc.printf("Motor 1 beweegt met snelheid 1 \n");
+ //controle positie op scherm
+ if (positie==0) {
+ pc.printf("Motor 2 blijft op stand 1\n");
} else {
- if (snelheidsstand==2) {
- pc.printf("Motor 1 beweegt met snelheid 2 \n");
+ if (positie==1) {
+ pc.printf("Motor 2 gaat naar stand 2\n");
} else {
- if (snelheidsstand==3) {
- pc.printf("Motor 1 beweegt met snelheid 3 \n");
+ if (positie==2) {
+ pc.printf("Motor 2 gaat naar stand 3\n");
}
}
}
- }
+
+ Ticker looptimer2;
+ looptimer2.attach(motor2aansturing,TSAMP1);
+ wait(8);
+ looptimer2.detach();
+ pc.printf("Detach Motor 2\n");
+
+ // eind aansturing motor 2
+
+ wait(2);
+ Ticker log_timerB;
+
+ arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states);
+ arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+ arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
+
+ myled1 = 1;
+ myled2 = 0;
+ myled3 = 1;
+ groen=1;
- Ticker looptimer1;
- //pwm_motor1.write(0.3);
- motordir1 = 1;
- //stop = 0;
- looptimer1.attach(motor1aansturing,TSAMP1);
- wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
- looptimer1.detach();
- pc.printf("detachMotor1\n");
+ log_timerB.attach(Biceps,0.005);
+ wait(2);
+ log_timerB.detach();
+
+ //bepaling van snelheidsstand met biceps
- pid(0,0,true);
+ if (MOVAVG_B >= drempelwaardeB1) {
+ yB1=1;
+ if (MOVAVG_B >= drempelwaardeB2) {
+ yB2=1;
+ if (MOVAVG_B >= drempelwaardeB3) {
+ yB3=1;
+ } else {
+ yB3=0;
+ }
+ } else {
+ yB2=0;
+ }
+ } else {
+ yB1=0;
+ }
- Ticker looptimer3;
- looptimer3.attach(motor1aansturingdeel2,TSAMP1); //of TSAMP2?....
- wait(4); ////is aan te passen (tijd die nodig is om balletje te slaan
- looptimer3.detach();
- pc.printf("detachMotor1\n");
+ pc.printf("Biceps meting is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ groen=0;
+
+ // *** INPUT MOTOR 1 ***
+ snelheidsstand=yB1+yB2+yB3;
+
+ //controle snelheidsstand op scherm
+ if (snelheidsstand==0) {
+ pc.printf("Motor 1 beweegt niet \n");
+ } else {
+ if (snelheidsstand==1) {
+ pc.printf("Motor 1 beweegt met snelheid 1 \n");
+ } else {
+ if (snelheidsstand==2) {
+ pc.printf("Motor 1 beweegt met snelheid 2 \n");
+ } else {
+ if (snelheidsstand==3) {
+ pc.printf("Motor 1 beweegt met snelheid 3 \n");
+ }
+ }
+ }
+ }
- pidpositie(0,0,true);
- pwm_motor1.write(0);
- toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
+ Ticker looptimer1;
+ motordir1 = 1;
+ looptimer1.attach(motor1aansturing,TSAMP1);
+ wait(2);
+ looptimer1.detach();
+ pc.printf("detachMotor1\n");
+ pid(0,0,true);
- myled1=1;
- myled2=1;
- myled3=1;
- groen=0;
- blauw=0;
+ Ticker looptimer3;
+ looptimer3.attach(motor1aansturingdeel2,TSAMP1);
+ wait(3); //is aan te passen (tijd die nodig is om balletje te slaan)
+ looptimer3.detach();
+ pc.printf("detachMotor1\n");
+ pidpositie(0,0,true);
+ pwm_motor1.write(0);
+ toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
+
+ myled1=1;
+ myled2=1;
+ myled3=1;
+ groen=0;
+ blauw=0;
+ }
}
}
- //}
}//end int main
-
float pid(float setspeed, float measurement, bool reset )
{
-
-
float error;
static float prev_error = 0;
float out_p = 0;
@@ -710,23 +697,20 @@
case SLAAN:
pc.printf("SLAAN\n");
new_pwm = pid(setspeed, omega);
- pwm_motor1.write(new_pwm); //===================================================================================================
+ pwm_motor1.write(new_pwm);
motordir1 = 1;
if (motor1.getPosition() <= ANGLEMAX) {
toestand = TERUGKEREN;
- pwm_motor1.write(0.0);//arvid had hier 0,0 gezet?!
+ pwm_motor1.write(0.0);
pc.printf("toestand = terugkeren, wacht tot 2e ticker\n\r");
- //stop = 1;
-
}
break;
case WACHTEN:
- pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
- //?? motor1.getPosition(nieuwe positie);
+ pidpositie(ANGLEMIN,motor1.getPosition());
pwm_motor1.write(0);
pc.printf("ifwachten\n");
- if (snelheidsstand != 0 /*&& stop == 0*/) {
+ if (snelheidsstand != 0 ) {
toestand = SLAAN;
pc.printf("slaan \n");
switch(snelheidsstand) {
@@ -745,19 +729,6 @@
}//end switch
} //end if
break;
-
- /*case TERUGKEREN:
- if (motor1.getPosition()<=ANGLEMAX) {
- new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
- pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
- motordir1 = 0;
- }
- if (motor1.getPosition()>= ANGLEMAX) {
- new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
- pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
- motordir1 = 1;
- }
- break;*///overbodig!!!
} //end switch
scope.set(0,motor1.getPosition());
@@ -772,24 +743,23 @@
deltacounts = motor1.getPosition()- prevgetpos;
prevgetpos=motor1.getPosition();
omega=(deltacounts/TSAMP1);
-
+
switch(toestand) {
- case TERUGKEREN: //deze case moet blijven ookal is het de enige case
- new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());//new_PWM benaming zorgt mogelijk voor problemen
+ case TERUGKEREN:
+ new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());
if (new_pwmpos > 0) {
motordir1 = 0;
- //pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
+ //pc.printf("motor1.getPosition %d\r\n", motor1.getPosition()); voor controle
} else {
motordir1 = 1;
- //pc.printf("if2\n");
}
- pwm_motor1.write(fabs(new_pwmpos)); //mogelijk moet dit -new_pwm zijn???
+ pwm_motor1.write(fabs(new_pwmpos));
break;
} //end switch
- scope.set(0,motor1.getPosition()); //ruwe data
+ scope.set(0,motor1.getPosition());
scope.set(2,ANGLEMIN);
scope.set(3, omega);
scope.set(4, setspeed);
scope.send();
-}//let op. Geen pidposition(0,0,true) deze moet zelf zorgen dat hij 0 wordt, en daar genoeg tijd voor hebben!!!
\ No newline at end of file
+}
\ No newline at end of file