Biorobotics
/
Controller
Controller futhers (PI)
main.cpp
- Committer:
- yohoo15
- Date:
- 2015-10-14
- Revision:
- 5:d1ab07fd3355
- Parent:
- 4:0844ab0d7d93
- Child:
- 6:8ab07cce3098
File content as of revision 5:d1ab07fd3355:
#include "mbed.h" #include "QEI.h" Serial pc(USBTX, USBRX); QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // Define pin for motor control DigitalOut directionPin(D4); PwmOut PWM(D5); // define ticker Ticker aansturen; Ticker Printen; // define rotation direction const int cw = 1; const int ccw = 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.001; // reason ticker works with 100 Hz. double motor1_error_integraal = 0; // initial value of integral error // calculating pulses to rotations in degree. const double Offset = 3450 ;//8400 counts is aangegeven op de motor. 3500 is almost 1 cirkel with a bit of overshoot 3450 looks good. about 10 - 20 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice) const double resolution = Offset / 360; double Rotation = 2; // rotation in degree double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree. // defining flags volatile bool flag_motor = false; volatile bool flag_pcprint = false; // making flags. void Go_flag_motor() { flag_motor = true; } void Go_flag_pcprint() { flag_pcprint = true; } // Reusable P controller double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) { double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn. double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn e_int = e_int + Ts * error_normalised_rotation; double PI_output = Kp * error_normalised_rotation + Ki * e_int; // return PI_output; } // Next task, measure the error and apply the output to the plant void motor1_Controller() { double reference = movement; double position = wheel.getPulses(); double error = reference - position; double output = PI( error, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ); if(error > 0) { directionPin.write(cw); PWM.write(output); //pc.printf("ref %.0f count%.0f cw\n",movement,position); } if(error < 0) { directionPin.write(ccw); PWM.write(-output); //min output to get output possitif //pc.printf("a"); } } void counts_showing() { double kijken = wheel.getPulses(); pc.printf("ref %.0f count%.0f int err %.2f \n",movement,kijken, motor1_error_integraal); } int main() { aansturen.attach( &Go_flag_motor, 0.001f ); // 100 Hz // timer 0.00001f motor keeps spinning Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed while( 1 ) { if(flag_motor) { flag_motor = false; motor1_Controller(); } if(flag_pcprint) { flag_pcprint = false; counts_showing(); } } }