encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-10-30
- Revision:
- 24:b9dd6cf5c366
- Parent:
- 23:c027e5c57cc8
- Child:
- 25:c719346df3cd
File content as of revision 24:b9dd6cf5c366:
#include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" AnalogIn potmetervalue1 (A1); AnalogIn potmetervalue2 (A2); DigitalIn button2 (D10); //Let op, is deze niet bezet? InterruptIn encoderA (D9); InterruptIn encoderB (D8); DigitalOut directionpin1 (D7); DigitalOut directionpin2 (D4); PwmOut pwmpin1 (D6); PwmOut pwmpin2 (D5); MODSERIAL pc(USBTX, USBRX); //Global variables int encoder = 0; //Start value encoder int T = 0.002f; //Ticker period double Kp = 17.5; double Ki = 1.02; double Kd = 23.2; double err = 0; //Tickers Ticker function_tick; //Functions void encoderA_rise() { if(encoderB==false) { encoder++; } else { encoder--; } } void encoderA_fall() { if(encoderB==true) { encoder++; } else { encoder--; } } void encoderB_rise() { if(encoderA==true) { encoder++; } else { encoder--; } } void encoderB_fall() { if(encoderA==false) { encoder++; } else { encoder--; } } void encoder_count() { encoderA.rise(&encoderA_rise); encoderA.fall(&encoderA_fall); encoderB.rise(&encoderB_rise); encoderB.fall(&encoderB_fall); } double PID_controller() { static double err_integral = 0; static double err_prev = err; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: double u_k = Kp * err; // Integral part err_integral = err_integral + err * T; double u_i = Ki * err_integral; // Derivative part double err_derivative = (err - err_prev)/T; double filtered_err_derivative = LowPassFilter.step(err_derivative); double u_d = Kd * filtered_err_derivative; err_prev = err; // Sum all parts and return it return u_k + u_i + u_d; } void call_all_functions() { PID_controller(); encoder_count(); } // Main function start. int main() { pc.baud(115200); pc.printf("hello\n\r"); pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz function_tick.attach(&call_all_functions,T); while (true) { } }