Biorobotics
/
Controllerfunction
movemotor function. still to finish input variables and how to pass PWM and Direction pin and QEI
Diff: movemotor.cpp
- Revision:
- 0:7098cdb29180
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/movemotor.cpp Mon Oct 19 13:43:58 2015 +0000 @@ -0,0 +1,109 @@ +#include "QEI.h" +#include "mbed.h" + +// 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) +const double M1_timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS + +// define rotation direction and begin possition +const int cw = 1; +const int ccw = 0; + +// define ticker +Ticker aansturen; + +QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // ask victor whether it can be in if-loop for the two different motors + +// defining flags +volatile bool flag_motor = false; + + +// making function flags. +void Go_flag_motor() +{ + flag_motor = true; +} + + +// defining 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; +} + +// Measure the error and apply the output to the plant +void motor1_Controller(DigitalOut directionPin(), PwmOut PWM(), double position, double Setpoint, double Kp, double Ki) //How to pass Digitalout and PWM, by reference? +{ + 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 +} + + + +void move_motor(std::string state) //state can be left, right, or keypress +{ + +if (state == "left") +{ +DigitalOut directionPin(D4); //direction +PwmOut PWM(D5); //speed +double position = wheel.getPulses(); //actual position +double Setpoint; //desired position +Kp; //controller gain +Ki; //set to zero +} + +else if (state == "right") +{ +DigitalOut directionPin(D4); +PwmOut PWM(D5); +double position = wheel.getPulses(); //actual position +double Setpoint; //desired position +double Kp; //controller gain, same as for left +double Ki; //set to zero +} + +else if (state == "keypress") //different encoder than left and right +{ +DigitalOut directionPin(D6); //direction +PwmOut PWM(D7); //speed +double position = wheel.getPulses(); //actual position, but different motor! +double Setpoint; //desired position, always the same actually +Kp; //controller gain, different from left and right +Ki; //set to zero +} + + + +//call the flags and p-controller + + +aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker +while (1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm +{ +// to make the motor move + if(flag_motor) { + flag_motor = false; + motor1_Controller(directionPin, PWM, position, Setpoint, Kp, Ki); //How to pass Digitalout and PWM, by reference? + } + } +} \ No newline at end of file