![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
Diff: main.cpp
- Revision:
- 25:c719346df3cd
- Parent:
- 24:b9dd6cf5c366
- Child:
- 26:8fc7e88603f8
diff -r b9dd6cf5c366 -r c719346df3cd main.cpp --- a/main.cpp Tue Oct 30 15:02:26 2018 +0000 +++ b/main.cpp Tue Oct 30 21:27:35 2018 +0000 @@ -1,39 +1,70 @@ #include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" +#include "HIDScope.h" +#include <math.h> -AnalogIn potmetervalue1 (A1); -AnalogIn potmetervalue2 (A2); - -DigitalIn button2 (D10); //Let op, is deze niet bezet? +//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 = 0; //Start value encoder -int T = 0.002f; //Ticker period +int encoder = 1; //Start value encoder -double Kp = 17.5; +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; +//double err = 0; + +volatile double reference_rotation = PI; //pi , in radians +double encoder_radians; + +//double u_total = 0; + +double pi=3.14159; +double i=0; //Tickers -Ticker function_tick; +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) @@ -90,7 +121,7 @@ encoderB.fall(&encoderB_fall); } -double PID_controller() +double PID_controller(double err) { static double err_integral = 0; static double err_prev = err; // initialization with this value only done once! @@ -111,31 +142,84 @@ err_prev = err; // Sum all parts and return it - return u_k + u_i + u_d; + return u_k + u_i + u_d; } -void call_all_functions() +void start_your_engines(double u) { - PID_controller(); - encoder_count(); + 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 + 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) +{ - function_tick.attach(&call_all_functions,T); - - - 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; + } - - } } - +}