![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-11-02
- Revision:
- 26:8fc7e88603f8
- Parent:
- 25:c719346df3cd
File content as of revision 26:8fc7e88603f8:
#include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "HIDScope.h" #include <math.h> //ATTENTION: set mBed to version 151 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) // set MODSERIAL to version 44 // set HIDScope to version 7 // set biquadFilter to version 7 InterruptIn encoderA (D9); InterruptIn encoderB (D8); DigitalOut directionpin1 (D7); DigitalOut directionpin2 (D4); DigitalIn button2 (D10); PwmOut pwmpin1 (D6); PwmOut pwmpin2 (D5); MODSERIAL pc(USBTX, USBRX); //HIDScope scope( 3 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Global variables int encoder = 1; //Start value encoder double T = 0.01f; //Ticker period double Kp = 17.5; //Motor 1 double Ki = 1.02; double Kd = 23.2; double PI = 3.14159; //double err = 0; volatile double reference_rotation = PI; //pi , in radians double encoder_radians; //double u_total = 0; double i=0; //Tickers Ticker encoder_tick; Ticker engine_tick; //Functions /* void HIDScope_sample() { scope.set(0,sinout); scope.set(1,encoder); //alles in array? scope.set(2,err); scope.send(); } */ 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(double err) { 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 start_your_engines(double u) { if(encoder<5000 && encoder>-5000) //limieten van de translatie 11760 encoder counts { pwmpin1 = fabs(u); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! directionpin1.write(u < 0.0f); } else { pwmpin1 = 0; } } void engine_control() { encoder_radians = encoder*(2*PI)/8400; double err = reference_rotation - encoder_radians; double u = PID_controller(err); //PID controller function call start_your_engines(u); //Call start_your_engines function } // Main function start. int main() { pc.baud(115200); pc.printf("hello\n\r"); pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz encoder_tick.attach(&encoder_count,T); engine_tick.attach(&engine_control,T); //HIDScope ticker toevoegen // DEFININING VARIABLES //float reference; float foo[17]; float length = 9.0f; int a; int q; while (true) { // EXECTURION IN MAIN // SIN ALS INPUT GEVEN if (button2 == false) { wait (0.2f); for (q=0; q<4; q++) { float b = 8.0f; for (a=0; a<10; a++) { foo[a] = 3.14f/length*(a); //printf("%f\n\r", foo[a]); reference_rotation = foo[a]; wait(0.2f); } for(a=10; a<18; a++) { foo[a] = 3.14f/length*b; //printf("%f\n\r", foo[a]); reference_rotation = foo[a]; b = b-1.0f; wait(0.2f); } } reference_rotation = 0; } } }