Code om de PID controller af te stellen aan de hand van een sinus golf
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Diff: main.cpp
- Revision:
- 7:9a1007e35bac
- Parent:
- 6:14a9c4f30c86
- Child:
- 8:bf5192a22c64
- Child:
- 9:aa5d6636197b
--- a/main.cpp Tue Mar 19 16:12:42 2019 +0000 +++ b/main.cpp Wed Mar 20 13:21:56 2019 +0000 @@ -6,6 +6,8 @@ // Algemeen DigitalIn button2(SW2); DigitalIn button3(SW3); +AnalogIn But2(A5); +AnalogIn But1(A3); DigitalOut led(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); @@ -36,6 +38,7 @@ //Ticker Ticker Pwm; Ticker PotRead; +Ticker Kdc; //Kinematica @@ -54,14 +57,11 @@ float upperlim = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor // VARIABLES PID CONTROLLER -double Kp = Pot1*10; // -double Ki = 5; // -double Kd = 5; // -double Ts = 0.1; // Sample time in seconds +double Kp = 6; // Zonder arm: 6,0,1 +double Ki = 0; // +double Kd = 1; // +double Ts = 0.001; // Sample time in seconds - - -} float Kinematics(float KPot) { @@ -110,7 +110,19 @@ static double error_integral = 0; static double error_prev = error; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); - + + /* PID testing + Kp = 10 * Pot2; + Ki = 10 * Pot1; + + if (!But2) { + Kd = Kd + 0.01; + } + if (!But1){ + Kd = Kd - 0.01; + } + */ + // Proportional part: double u_k = Kp * error; @@ -141,22 +153,16 @@ void PwmMotor(void) { + // Reference hoek berekenen, in graden float Ellebooghoek1 = Kinematics(pwm2); float Ellebooghoek4 = Limits(Ellebooghoek1); ElbowReference = Ellebooghoek4; - //pc.printf("ElbowReference:%f\n", ElbowReference); - //pc.printf("pwm2=%f\r\n",pwm2); - + // Positie motor berekenen, in graden Pulses2 = encoder2.getPulses(); motor_position2 = -(Pulses2/8400)*360; - - double error = reference_rotation - motor_position2; - //double error = ElbowReference - motor_position2; - //pc.printf("Degrees is:%f, error = %f\n", motor_position2, error); - //pwmpin2 = fabs(pwm2); //zonder PID - + double error = ElbowReference - motor_position2; double u = PID_controller(error); moter2_control(u); @@ -178,12 +184,21 @@ { Pot2 = pot.read(); Pot1 = pot0.read(); - pwm2 =(Pot2*2)-1; //scaling - - //pc.printf("%f\r\n",Pot2); + pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1 } - +/* +void Kdcount (void) // Voor het testen van de PID waardes +{ + int count = 0; + ElbowReference = ElbowReference + 10; + if (count == 7) { + ElbowReference = 0; + count = 0; + } + count ++; +} +*/ int main() { @@ -192,6 +207,7 @@ int counter = 0; pwmpin2.period_us(60); PotRead.attach(ContinuousReader,Ts); + //Kdc.attach(Kdcount,5); //Voor PID waarde testen pc.baud(115200); //pc.printf("start\r\n"); led = 1; @@ -214,7 +230,7 @@ led = 0; if(counter==10) { float tmp = t.read(); - printf("%f,%f,%f,%f\n\r",tmp,motor_position2,reference_rotation,Pot1); + printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Kp,Ki,Kd); counter = 0; } counter++;