Functie van het slaan van de bal
Dependencies: Encoder HIDScope mbed-dsp mbed MODSERIAL
Diff: main.cpp
- Revision:
- 12:5ea8de059fa1
- Parent:
- 11:a9f4f296d7ed
- Child:
- 13:b35f3553f210
diff -r a9f4f296d7ed -r 5ea8de059fa1 main.cpp --- a/main.cpp Wed Oct 29 11:01:55 2014 +0000 +++ b/main.cpp Wed Oct 29 13:41:21 2014 +0000 @@ -6,11 +6,11 @@ #define SAMP_TIME 0.01 #define K_P (0.1) -#define K_I (0.03 *SAMP_TIME) -#define K_D (0.0005 /SAMP_TIME) +#define K_I (0.04 *SAMP_TIME) +#define K_D (0.0001 /SAMP_TIME) #define I_LIMIT 1. -#define MAXENCO 300 +#define MAXENCO 256 #define MINENCO 0 @@ -20,8 +20,7 @@ DigitalOut m1_dir(PTA4); //define functions -//void slam(); -void slam_II(); +void slam(); void clamp(float * in, float min, float max); float pid(float rev_value, float mea_value); @@ -37,87 +36,58 @@ { cin >> y1; cout << y1 << endl; - //slam(); - slam_II(); + slam(); } } -/*void slam() +void slam() { - float enca; - enca=encoderA.getPosition(); - - //if (enca <= 0) - { - switch(y1) - { - - case 1: - m1_dir=1; - m1_speed=0.33; - m1_speedout=m1_speed; - break; - case 2: - m1_dir=1; - m1_speed=0.66; - m1_speedout=m1_speed; - break; - case 3: - m1_dir=1; - m1_speed=1.0; - m1_speedout=m1_speed; - break; - default : - m1_speed=0; - } - while (enca < MAXENCO) - { - enca=encoderA.getPosition(); - cout << enca << endl; - } - m1_speed=0; - m1_speedout=m1_speed; - m1_dir=0; - cout << "blieb!" << endl; - m1_speed=0.4; - m1_speedout=m1_speed; - while(enca > MINENCO) - { - enca=encoderA.getPosition(); - cout << enca << endl; - } - m1_speed=0; - m1_speedout=m1_speed; - } -}*/ - -void slam_II() -{ + cout << "Ik begin met de slafunctie" << endl; + cout << encoderA.getPosition() << endl << "check" << endl; switch (y1) { case 1: + cout << "zaak1" << endl; m1_dir=1; max_speed=0.33; - m1_speed.write(0.33); + //m1_speed.write(0.33); + cout << "Motor 1 draait" << endl; break; case 2: + cout << "zaak2"<< endl; m1_dir=1; max_speed=0.66; - m1_speed.write(0.66); + //m1_speed.write(0.66); + cout << "Motor 1 draait" << endl; break; case 3: + cout << "zaak3"<< endl; m1_dir=1; max_speed=1; - m1_speed.write(1); + //m1_speed.write(1); + cout << "Motor 1 draait" << endl; break; default: m1_dir=1; m1_speed=0; break; } + cout << "Ik ben uit de switch" << endl; new_speed=pid(MAXENCO,encoderA.getPosition()); - while(new_speed!=0) + clamp(&new_speed,-max_speed,max_speed); + if (new_speed>0) + { + m1_dir=1; + } + else { + m1_dir=0; + } + m1_speed.write(fabs(new_speed)); + + while(new_speed<-0.1 || new_speed>0.1) + { + //cout << "Eat, Sleep, Rave, Repeat" << endl; new_speed=pid(MAXENCO,encoderA.getPosition()); clamp(&new_speed,-max_speed,max_speed); if (new_speed>0) @@ -129,10 +99,23 @@ m1_dir=0; } m1_speed.write(fabs(new_speed)); - enca=encoderA.getPosition(); + //cout << new_speed << endl; } + cout << "Ik ben uit de eerste whilelus" << endl; + cout << encoderA.getPosition() << endl << "check" << endl; new_speed=pid(MINENCO,encoderA.getPosition()); - while(new_speed!=0) + clamp(&new_speed,-max_speed,max_speed); + if (new_speed>0) + { + m1_dir=1; + } + else + { + m1_dir=0; + } + m1_speed.write(fabs(new_speed)); + + while(new_speed<-0.1 || new_speed>0.1) { new_speed=pid(MINENCO,encoderA.getPosition()); clamp(&new_speed,-0.5,0.5); @@ -146,6 +129,9 @@ } m1_speed.write(fabs(new_speed)); } + m1_speed.write(0); + cout << "Ik ben uit de tweede whilelus" << endl; + cout << encoderA.getPosition() << endl << "check" << endl; } void clamp(float * in, float min, float max)