Functie van het slaan van de bal
Dependencies: Encoder HIDScope mbed-dsp mbed MODSERIAL
main.cpp
- Committer:
- BasvanBuuren
- Date:
- 2014-10-29
- Revision:
- 7:36a7a7136be3
- Parent:
- 6:3c4a2afb11e5
- Child:
- 8:eaa3e91dc534
File content as of revision 7:36a7a7136be3:
#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.03 *SAMP_TIME) #define K_D (0.0005 /SAMP_TIME) #define I_LIMIT 1. #define MAXENCO 300 #define MINENCO 0 //define in and output Encoder encoderA(PTD0,PTD2); PwmOut m1_speed(PTA5); DigitalOut m1_dir(PTA4); //define functions //void slam(); void slam_II(); void clamp(float * in, float min, float max); float pid(float rev_value, float mea_value); //define global variables int y1; int main() { while(1) { cin >> y1; cout << y1 << endl; //slam(); slam_II(); } } /*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() { float new_speed; float max_speed; float enca; switch (y1) { case 1: m1_dir=1; max_speed=0.33; m1_speed.write(0.33); break; case 2: m1_dir=1; max_speed=0.66; m1_speed.write(0.66); break; case 3: m1_dir=1; max_speed=1; m1_speed.write(1); break; default: m1_dir=1; m1_speed=0; break; } enca=encoderA.getPosition(); while(enca!=MAXENCO) { 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)); enca=encoderA.getPosition(); } while(enca!=MINENCO) { 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)); enca=encoderA.getPosition(); } } 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; }