a biquad working
Dependencies: HIDScope mbed QEI
Revision 4:61d5e7417c4c, committed 2015-10-27
- Comitter:
- yohoo15
- Date:
- Tue Oct 27 14:13:48 2015 +0000
- Parent:
- 3:2f75bb307da3
- Commit message:
- Work in progress;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2f75bb307da3 -r 61d5e7417c4c main.cpp --- a/main.cpp Tue Oct 27 13:22:39 2015 +0000 +++ b/main.cpp Tue Oct 27 14:13:48 2015 +0000 @@ -6,13 +6,7 @@ #include <iostream> #include "QEI.h" -// Define pin for motor control -DigitalOut directionPin(D4); -PwmOut PWM(D5); -DigitalIn buttonccw(PTA4); -DigitalIn buttoncw(PTC6); -// define ticker Serial pc(USBTX, USBRX); @@ -40,7 +34,7 @@ } AnalogIn analog_emg_left(A0); -//AnalogIn analog_emg_right(A1); +AnalogIn analog_emg_right(A1); double input = 0; double filter_signal_hid = 0; //double input_right = 0; @@ -226,8 +220,94 @@ return(filter_signal); } -/*************************************************************motor control******************************************************************************************************/ +/*************************************************************BEGIN motor control******************************************************************************************************/ +// Define pin for motor control +DigitalOut directionPin(D4); +PwmOut PWM(D5); +DigitalIn buttonccw(PTA4); +DigitalIn buttoncw(PTC6); + +QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) +// define ticker + +// define rotation direction and begin possition +const int cw = 1; +const int ccw = 0; +double setpoint = 0; //setpoint is in pulses + +// saying buttons are not pressed +const int Buttoncw_pressed = 0; +const int Buttonccw_pressed = 0; + +// Controller gain proportional and intergrator +const double motor1_Kp = 5; // more or les random number. +const double motor1_Ki = 0; +const double M1_timestep = 0.01; // reason ticker works with 100 Hz. +double motor1_error_integraal = 0; // initial value of integral error +// Defining pulses per revolution (calculating pulses to rotations in degree.) +const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4. 10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice) +/* +double Rotation = -2; // rotation +double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree. +*/ + +// defining flags +volatile bool flag_motor = false; + +// making function flags. +void Go_flag_motor() +{ + flag_motor = true; +} + +// To make a new setpoint +double making_setpoint(double direction){ + if ( cw == direction){ + setpoint = setpoint + (pulses_per_revolution/40000); + } + if ( ccw == direction){ + setpoint = setpoint - (pulses_per_revolution/40000); + } + return(setpoint); + + } + +// Reusable P controller +double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) +{ + + e_int = e_int + Ts * error; + + double PI_output = (Kp * error) + (Ki * e_int); + return PI_output; +} +// Next task, measure the error and apply the output to the plant +void motor1_Controller() +{ + double reference = setpoint; // setpoint is in pulses + double position = wheel.getPulses(); + double error_pulses = (reference - position); // calculate the error in pulses + double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn + + double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); + + if(error_pulses > 0) { + directionPin.write(cw); + + } + else if(error_pulses < 0) { + directionPin.write(ccw); + } + else{ + output = 0; + } +PWM.write(output); // out of the if loop due to abs output + + +} + +/*************************************************************END motor control******************************************************************************************************/ void HIDScope_kijken() { @@ -237,21 +317,43 @@ } int main() { - HIDScope_timer.attach(&Go_flag_HIDScope, 0.002); - Filteren_timer.attach(&Go_flag_filteren,0.004); + aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning + HIDScope_timer.attach(&Go_flag_HIDScope, 0.02); + Filteren_timer.attach(&Go_flag_filteren,0.04); while(1) { if(Flag_filteren) { Flag_filteren = false; filter_signal_hid = Filteren(); + } if(Flag_HIDScope) { Flag_HIDScope = false; HIDScope_kijken(); + } - if(left_movement) { - pc.printf("left = true /n"); + + if(flag_motor) { + flag_motor = false; + motor1_Controller(); + + } + + + +// Pussing buttons to get input signal + + if(left_movement){ + setpoint = making_setpoint(cw); + + } +if(buttonccw.read() == Buttonccw_pressed) { + setpoint = making_setpoint(ccw); } } } + + + + \ No newline at end of file