![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Werkend voor onze motor
Dependencies: Encoder MODSERIAL QEI mbed
Fork of feed_forward_PI_copy by
main.cpp
- Committer:
- RoyvZ
- Date:
- 2017-10-20
- Revision:
- 3:91e4ed7b6748
- Parent:
- 2:68de33d10c67
- Child:
- 4:840ce3b21767
File content as of revision 3:91e4ed7b6748:
#include "mbed.h" #include "MODSERIAL.h" #include "math.h" #include "encoder.h" DigitalOut motorDirection(D4); PwmOut motorSpeed(D5); AnalogIn potMeterIn1(A1); AnalogIn potMeterIn2(A2); InterruptIn button(D11); Ticker m1_Ticker; Ticker SP_m1_Ticker; Encoder encoder1(D13,D12); float M1_KP = 2.5; float M1_KI = 1.0; const float M1_TS = 0.01; const float SP_TS = 0.01; const float RAD_PER_PULSE = 0.000119; float m1_err_int = 0; int motorD = 0; float motor1 = 0; float reference = 0; float position = 0; int outOfEncod = 0; int KP_changer = 1; float RotSpeed = 0; int outOfEncod_SP_1 = 0; int outOfEncod_SP_2 = 0; MODSERIAL pc(USBTX, USBRX); // Reusable PI controller float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){ e_int += Ts * e; // e_int is changed globally because it’s ’by reference’ (&) return Kp * e + Ki * e_int; } void Change() { if(KP_changer == 60){ KP_changer = 0; } else{ KP_changer++; } } void m1_Velocity(){ outOfEncod_SP_1 = encoder1.getPosition(); float RotDiff = (outOfEncod_SP_1 - outOfEncod_SP_2)*RAD_PER_PULSE; outOfEncod_SP_2 = outOfEncod_SP_1; RotSpeed = RotDiff/SP_TS; } // Next task, measure the error and apply the output to the plant void m1_Controller() { reference = 5 * potMeterIn1.read(); M1_KI = 2 * potMeterIn2.read(); M1_KP = KP_changer*0.1; outOfEncod = encoder1.getPosition(); position = RAD_PER_PULSE * outOfEncod; float ref_pos = reference - position; // Don’t use magic numbers! motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); if (ref_pos <= 0) { //float motor1DirectionPin1 = 1; motorD=1; } else { //float motor1DirectionPin1 = 0; motorD=0; } motorDirection.write(motorD); motorSpeed.write(motor1); } int main() { m1_Ticker.attach( &m1_Controller, M1_TS ); SP_m1_Ticker.attach( &m1_Velocity, SP_TS ); // 100 Hz pc.baud(115200); pc.printf("Fuck you pc"); m1_Controller(); button.rise(&Change); while(true) { pc.printf("%f \t", RotSpeed); pc.printf("%f \t", M1_KI); pc.printf("%f \t", M1_KP); pc.printf("%f \t", position); pc.printf("%f \r\n", reference); //wait(0.5); } }