P, PI, PID-control
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of control by
main.cpp
- Committer:
- bjonkheer
- Date:
- 2018-10-15
- Revision:
- 3:59d6069fb0f6
- Parent:
- 2:3d4fc510f3d5
File content as of revision 3:59d6069fb0f6:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" #include "BiQuad.h" HIDScope scope(2); PwmOut pwmpin(D5); DigitalOut directionpin(D4); AnalogIn potmeter1(A0); AnalogIn potmeter2(A1); DigitalIn button(D0); Ticker Sample; Timer t; MODSERIAL pc(USBTX, USBRX); QEI wheel (D13, D12, NC, 32); float angle_resolution = (360.0/32.0)*(1.0/131.0); //degrees/counts float samplingfreq = 100.0; //Hz float u = 0; int pulses = 0; float angle; float oldangle; double time_s; volatile double Kp = 7.5; volatile double Ki = 1.02; double Kd = 10; double refangle; double error_angle; double PID_controller(double error) { Kp = potmeter2.read()*3; double u_k = Kp * error; 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); error_integral = error_integral + error * 1/samplingfreq; double u_i = Ki * error_integral; double error_derivative = (error - error_prev)*samplingfreq; double filtered_error_derivative = LowPassFilter.step(error_derivative); double u_d = Kd * filtered_error_derivative; error_prev = error; return u_k+u_i+u_d; } float mean(float *samples, int n) { float sum = 0.0; for (int i=0; i<n; i++) { sum += samples[i]; } return sum / (float)n; } void sampling() { } void changedirection() { directionpin = !directionpin; } int main() { t.start(); pc.baud(115200); pwmpin.period_us(60); //button.fall(&changedirection); //refvel.attach(&sampling, 1/samplingfreq); while (true) { pulses = wheel.getPulses(); refangle = potmeter1.read()*360.0; scope.set(0,refangle); angle = pulses*angle_resolution; scope.set(1,angle); scope.send(); error_angle = refangle - angle; u = PID_controller(error_angle)/360.0; directionpin = u > 0.0f; pwmpin = fabs(u); pc.printf("Reference velocity is: %f degrees per second \r\n", u); oldangle = angle; wait(1/samplingfreq); } }