BMT M9 Groep01
alles in elkaar met de mooie manier van de regelaar
Dependencies: Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed
Fork of
Robot2mooiemanier
by 
BMT M9 Groep01
Diff: main.cpp
- Revision:
- 22:f3a827faa135
- Parent:
- 21:b7fb79882cb8
- Child:
- 23:8f7ce4894c58
diff -r b7fb79882cb8 -r f3a827faa135 main.cpp
--- a/main.cpp Fri Oct 31 13:30:21 2014 +0000
+++ b/main.cpp Fri Oct 31 13:59:31 2014 +0000
@@ -108,7 +108,7 @@
standen hoek2 = STAND1;
int main ()
-{
+{
pc.baud(115200);
drempelwaardeT=0;
@@ -239,169 +239,169 @@
myled3 = 0;
} else {*/
- /*pc.printf("eerst positie dan snelheid aangeven /n");
+ /*pc.printf("eerst positie dan snelheid aangeven /n");
- //bepaling van positie met triceps 1
- Ticker log_timerT1;
+ //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);
- 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;
+
+ log_timerT1.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT1.detach();
- myled1 = 0;
- myled2 = 1;
- myled3 = 1;
+ // positie van batje met behulp van Triceps
- log_timerT1.attach(Triceps, 0.005);
- wait(2);
- log_timerT1.detach();
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT1=1;
+ } else {
+ yT1=0;
+ }
- // positie van batje met behulp van Triceps
+ pc.printf("Triceps meting 1 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+ wait(3);
- if (MOVAVG_T >= drempelwaardeT) {
- yT1=1;
- } else {
- yT1=0;
- }
+ //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);
+
+ myled1 = 0;
+ myled2 = 1;
+ myled3 = 1;
- pc.printf("Triceps meting 1 is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
- wait(3);
+ log_timerT2.attach(Triceps, 0.005);
+ wait(2);
+ log_timerT2.detach();
- //bepaling van positie met tricep 2
- Ticker log_timerT2;
+ if (MOVAVG_T >= drempelwaardeT) {
+ yT2=1;
+ } else {
+ yT2=0;
+ }
+
+ pc.printf("Triceps meting 2 is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 0;
+
+ //*** INPUT MOTOR 2 ***
+ positie=yT1+yT2;
- 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;
+ //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");
+ }
+ }
+ }
- log_timerT2.attach(Triceps, 0.005);
- wait(2);
- log_timerT2.detach();
+ Ticker looptimer2;
+ looptimer2.attach(motor2aansturing,TSAMP1);
+ wait(8);
+ looptimer2.detach();
+ pc.printf("Detach Motor 1\n"); */
+
+//------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
+ wait(2);
+ /* Ticker log_timerB;
- if (MOVAVG_T >= drempelwaardeT) {
- yT2=1;
- } else {
- yT2=0;
- }
+ 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;
+
+ log_timerB.attach(Biceps,0.005);
+ wait(2);
+ log_timerB.detach();
+
+ //bepaling van snelheidsstand met biceps
- pc.printf("Triceps meting 2 is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
-
- //*** INPUT MOTOR 2 ***
- positie=yT1+yT2;
+ 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;
+ }
- //controle positie op scherm
- if (positie==0) {
- pc.printf("Motor 2 blijft op stand 1\n");
+ pc.printf("Biceps meting is klaar.\n");
+ myled1 = 1;
+ myled2 = 1;
+ myled3 = 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");
} else {
- if (positie==1) {
- pc.printf("Motor 2 gaat naar stand 2\n");
+ if (snelheidsstand==2) {
+ pc.printf("Motor 1 beweegt met snelheid 2 \n");
} else {
- if (positie==2) {
- pc.printf("Motor 2 gaat naar stand 3\n");
+ if (snelheidsstand==3) {
+ pc.printf("Motor 1 beweegt met snelheid 3 \n");
}
}
}
-
- Ticker looptimer2;
- looptimer2.attach(motor2aansturing,TSAMP1);
- wait(8);
- looptimer2.detach();
- pc.printf("Detach Motor 1\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;
+ }
- log_timerB.attach(Biceps,0.005);
- wait(2);
- log_timerB.detach();
-
- //bepaling van snelheidsstand met biceps
-
- 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 looptimer1;
+ //pwm_motor1.write(0.3);
+ motordir1 = 1;
+ stop = 0;
+ looptimer1.attach(motor1aansturing,TSAMP1);
+ wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+ looptimer1.detach();
+ pc.printf("detachMotor1\n");
- pc.printf("Biceps meting is klaar.\n");
- myled1 = 1;
- myled2 = 1;
- myled3 = 0;
-
- //*** INPUT MOTOR 1 ***
- snelheidsstand=yB1+yB2+yB3; */
+ Ticker looptimer3;
+ looptimer3.attach(motor1aansturingdeel2,TSAMP1);
+ wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+ looptimer3.detach();
+ pc.printf("detachMotor1\n");
- 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");
- } 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");
- }
- }
- }
- }
+ pwm_motor1.write(0);
- Ticker looptimer1;
- //pwm_motor1.write(0.3);
- motordir1 = 1;
- stop = 0;
- looptimer1.attach(motor1aansturing,TSAMP1);
- wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
- looptimer1.detach();
- pc.printf("detachMotor1\n");
-
- Ticker looptimer3;
- looptimer3.attach(motor1aansturingdeel2,TSAMP1);
- wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
- looptimer3.detach();
- pc.printf("detachMotor1\n");
-
- pwm_motor1.write(0);
-
- myled1=1;
- myled2=1;
- myled3=1;
- }
+ myled1=1;
+ myled2=1;
+ myled3=1;
}
}
+ //}
}//end int main
@@ -412,8 +412,7 @@
float out_p = 0;
static float out_i = 0;
float out_d = 0;
- if(reset==true)
- {
+ if(reset==true) {
out_i = 0;
prev_error = 0;
}
@@ -629,27 +628,30 @@
if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
toestand = WACHTEN;
//motor1.setPosition(0);
- pid(0,0,true);
+ pid(0,0,true);
pc.printf("if1\n");
}
if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
toestand = SLAAN;
pc.printf("slaan \n");
if ( snelheidsstand==3) {
- setspeed = V3; pc.printf("Snel 3 \n");
+ setspeed = V3;
+ pc.printf("Snel 3 \n");
}
if ( snelheidsstand==2) {
- setspeed = V2; pc.printf("Snel 2\n");
+ setspeed = V2;
+ pc.printf("Snel 2\n");
}
if ( snelheidsstand==1) {
- setspeed = V1; pc.printf("Snel 1 \n");
+ setspeed = V1;
+ pc.printf("Snel 1 \n");
}
}
if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
toestand = TERUGKEREN;
//motor1.setPosition(0);
pwm_motor1.write(0);
- pid(0,0,true);
+ pid(0,0,true);
stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
pc.printf("toestand = terugkeren\n\r");
}
@@ -662,15 +664,18 @@
pwm_motor1.write(new_pwm);
motordir1 = 1;
pc.printf("SLAAN\n");
-
- /*if (toestand == TERUGKEREN) {
- new_pwm = pid(setspeed, motor1.getSpeed(),false);
- pwm_motor1.write(new_pwm);
- motordir1 = 0;
- pc.printf("motor gaat terugkeren\n\r");
- pc.printf("new pwm %f\r\n",new_pwm);*/
+ scope.set(0,motor1.getPosition()); //ruwe data
+ scope.set(1,-244); //filtered
+ scope.send();
+
+ /*if (toestand == TERUGKEREN) {
+ new_pwm = pid(setspeed, motor1.getSpeed(),false);
+ pwm_motor1.write(new_pwm);
+ motordir1 = 0;
+ pc.printf("motor gaat terugkeren\n\r");
+ pc.printf("new pwm %f\r\n",new_pwm);*/
}
-
+
}
void motor1aansturingdeel2()
@@ -678,14 +683,17 @@
if (motor1.getPosition()>= ANGLEMAX && toestand != SLAAN) {
toestand = WACHTEN;
motor1.setPosition(0);
- pid(0,0,true);
+ pid(0,0,true);
//pc.printf("if2\n");
}
if (toestand == TERUGKEREN) {
- new_pwm = pid(setspeed, motor1.getSpeed(),false);
+ new_pwm = pid(setspeed, motor1.getSpeed(),false);
pwm_motor1.write(new_pwm);
motordir1 = 0;
-
+ //sturen naar HID Scope
+ scope.set(0,motor1.getPosition()); //ruwe data
+ scope.set(2,motor1.getPosition()); //filtered
+ scope.send();
pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
}
if (toestand == WACHTEN) {