PID_controler voor het latere script
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 1:faa90344cdd8
- Parent:
- 0:57aa29917d4c
- Child:
- 2:ca645c585a03
diff -r 57aa29917d4c -r faa90344cdd8 main.cpp --- a/main.cpp Tue Nov 01 11:50:51 2016 +0000 +++ b/main.cpp Wed Nov 02 10:50:35 2016 +0000 @@ -2,6 +2,8 @@ #include "QEI.h" //bieb voor encoderfuncties in c++ #include "MODSERIAL.h" //bieb voor modserial #include "BiQuad.h" +#include "HIDScope.h" +#include "math.h" InterruptIn sw3(SW3); DigitalIn sw2(SW2); @@ -17,6 +19,7 @@ DigitalOut richting_motor2(D7); PwmOut pwm_motor2(D6); BiQuad PID_controller; +HIDScope Scope(1); //constanten voor de encoder const int CW = 2.5; //definitie rechtsom 'lage waarde' @@ -42,7 +45,7 @@ volatile float d_ref_new; const float w_ref = 3; const double Fs = 0.001; -const double Kp = 5; +const double Kp = 0.5; const double Ki = 5; const double Kd = 5; const double N = 100; @@ -52,6 +55,7 @@ volatile float voltage_motor1=0.18; volatile double error1; volatile double ctrlOutput; +volatile double sintel =0; void reference(){ d_ref = d_ref + w_ref * Fs; @@ -60,6 +64,10 @@ void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; ctrlOutput = PID_controller.step(error1); + } +void sin_teller(){ + sintel=sintel+0.01; + } // if (ctrlOutput > 1){ // ctrlOutput =1; // } @@ -69,7 +77,7 @@ // else{ // ctrlOutput = ctrlOutput; // } - } + @@ -83,19 +91,23 @@ while (true) { if (button_cw == 0){ - + t.attach(&sin_teller,1); richting_motor1 = 1; -pwm_motor1 = voltage_motor1; +pwm_motor1 = sin(sintel*10*3.141592653); +pc.printf("pwm_motor is %f\r\n", pwm_motor1); +pc.printf("sintel is %f\r\n", sintel); counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; -tick.attach(&reference,Fs); -PID_controller.PIDF(Kp,Ki,Kd,N,Fs); -controllerticker.attach(&m1_controller,Fs); -voltage_motor1=ctrlOutput; -pc.printf("%f", voltage_motor1); -pc.printf(" %f", d_ref); -pc.printf(" %f \r\n", rev_counts_motor1_rad); +//tick.attach(&reference,Fs); +//PID_controller.PIDF(Kp,Ki,Kd,N,Fs); +//controllerticker.attach(&m1_controller,Fs); +//voltage_motor1=ctrlOutput; +//pc.printf("%f", voltage_motor1); +//pc.printf(" %f", d_ref); +//pc.printf(" %f \r\n", rev_counts_motor1_rad); +Scope.set(0, rev_counts_motor1_rad); +Scope.send(); } else{