Functie van het slaan van de bal
Dependencies: Encoder HIDScope mbed-dsp mbed MODSERIAL
main.cpp
- Committer:
- BasvanBuuren
- Date:
- 2014-10-29
- Revision:
- 12:5ea8de059fa1
- Parent:
- 11:a9f4f296d7ed
- Child:
- 13:b35f3553f210
File content as of revision 12:5ea8de059fa1:
#include "mbed.h" #include "HIDScope.h" //#include "mbed-dsp.h" #include "encoder.h" #include <iostream> #define SAMP_TIME 0.01 #define K_P (0.1) #define K_I (0.04 *SAMP_TIME) #define K_D (0.0001 /SAMP_TIME) #define I_LIMIT 1. #define MAXENCO 256 #define MINENCO 0 //define in and output Encoder encoderA(PTD0,PTD2); PwmOut m1_speed(PTA5); DigitalOut m1_dir(PTA4); //define functions void slam(); void clamp(float * in, float min, float max); float pid(float rev_value, float mea_value); //define global variables int y1; float new_speed; float max_speed; float enca; int main() { while(1) { cin >> y1; cout << y1 << endl; slam(); } } void slam() { 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); cout << "Motor 1 draait" << endl; break; case 2: cout << "zaak2"<< endl; m1_dir=1; max_speed=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); 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()); 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) { m1_dir=1; } else { m1_dir=0; } m1_speed.write(fabs(new_speed)); //cout << new_speed << endl; } cout << "Ik ben uit de eerste whilelus" << endl; cout << encoderA.getPosition() << endl << "check" << endl; new_speed=pid(MINENCO,encoderA.getPosition()); 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); if (new_speed>0) { m1_dir=1; } else { m1_dir=0; } 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) { *in > min ? * in < max? : *in = max : *in = min; } float pid(float rev_value, float mea_value) { float error; static float prev_error = 0; float p_out = 0; static float i_out = 0; float d_out = 0; error = rev_value - mea_value; p_out = error * K_P; i_out += error * K_I; d_out = (error - prev_error) * K_D; clamp(&i_out,-I_LIMIT,I_LIMIT); prev_error=error; return p_out + i_out + d_out; }