Biorobotics
/
Controllerfunction
movemotor function. still to finish input variables and how to pass PWM and Direction pin and QEI
movemotor.cpp@0:7098cdb29180, 2015-10-19 (annotated)
- Committer:
- roosbulthuis
- Date:
- Mon Oct 19 13:43:58 2015 +0000
- Revision:
- 0:7098cdb29180
function for motor control we still need values for inputs and we dont know how to refer to PWM and directionPin. And how can we declare QEI wheel differently for two motors?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roosbulthuis | 0:7098cdb29180 | 1 | #include "QEI.h" |
roosbulthuis | 0:7098cdb29180 | 2 | #include "mbed.h" |
roosbulthuis | 0:7098cdb29180 | 3 | |
roosbulthuis | 0:7098cdb29180 | 4 | // Defining pulses per revolution (calculating pulses to rotations in degree.) |
roosbulthuis | 0:7098cdb29180 | 5 | 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) |
roosbulthuis | 0:7098cdb29180 | 6 | const double M1_timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS |
roosbulthuis | 0:7098cdb29180 | 7 | |
roosbulthuis | 0:7098cdb29180 | 8 | // define rotation direction and begin possition |
roosbulthuis | 0:7098cdb29180 | 9 | const int cw = 1; |
roosbulthuis | 0:7098cdb29180 | 10 | const int ccw = 0; |
roosbulthuis | 0:7098cdb29180 | 11 | |
roosbulthuis | 0:7098cdb29180 | 12 | // define ticker |
roosbulthuis | 0:7098cdb29180 | 13 | Ticker aansturen; |
roosbulthuis | 0:7098cdb29180 | 14 | |
roosbulthuis | 0:7098cdb29180 | 15 | 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 |
roosbulthuis | 0:7098cdb29180 | 16 | |
roosbulthuis | 0:7098cdb29180 | 17 | // defining flags |
roosbulthuis | 0:7098cdb29180 | 18 | volatile bool flag_motor = false; |
roosbulthuis | 0:7098cdb29180 | 19 | |
roosbulthuis | 0:7098cdb29180 | 20 | |
roosbulthuis | 0:7098cdb29180 | 21 | // making function flags. |
roosbulthuis | 0:7098cdb29180 | 22 | void Go_flag_motor() |
roosbulthuis | 0:7098cdb29180 | 23 | { |
roosbulthuis | 0:7098cdb29180 | 24 | flag_motor = true; |
roosbulthuis | 0:7098cdb29180 | 25 | } |
roosbulthuis | 0:7098cdb29180 | 26 | |
roosbulthuis | 0:7098cdb29180 | 27 | |
roosbulthuis | 0:7098cdb29180 | 28 | // defining reusable P controller |
roosbulthuis | 0:7098cdb29180 | 29 | double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) |
roosbulthuis | 0:7098cdb29180 | 30 | { |
roosbulthuis | 0:7098cdb29180 | 31 | |
roosbulthuis | 0:7098cdb29180 | 32 | e_int = e_int + Ts * error; |
roosbulthuis | 0:7098cdb29180 | 33 | |
roosbulthuis | 0:7098cdb29180 | 34 | double PI_output = (Kp * error) + (Ki * e_int); |
roosbulthuis | 0:7098cdb29180 | 35 | return PI_output; |
roosbulthuis | 0:7098cdb29180 | 36 | } |
roosbulthuis | 0:7098cdb29180 | 37 | |
roosbulthuis | 0:7098cdb29180 | 38 | // Measure the error and apply the output to the plant |
roosbulthuis | 0:7098cdb29180 | 39 | void motor1_Controller(DigitalOut directionPin(), PwmOut PWM(), double position, double Setpoint, double Kp, double Ki) //How to pass Digitalout and PWM, by reference? |
roosbulthuis | 0:7098cdb29180 | 40 | { |
roosbulthuis | 0:7098cdb29180 | 41 | double reference = setpoint; // setpoint is in pulses |
roosbulthuis | 0:7098cdb29180 | 42 | double position = wheel.getPulses(); |
roosbulthuis | 0:7098cdb29180 | 43 | double error_pulses = (reference - position); // calculate the error in pulses |
roosbulthuis | 0:7098cdb29180 | 44 | double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn |
roosbulthuis | 0:7098cdb29180 | 45 | |
roosbulthuis | 0:7098cdb29180 | 46 | double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); |
roosbulthuis | 0:7098cdb29180 | 47 | |
roosbulthuis | 0:7098cdb29180 | 48 | if(error_pulses > 0) { |
roosbulthuis | 0:7098cdb29180 | 49 | directionPin.write(cw); |
roosbulthuis | 0:7098cdb29180 | 50 | } |
roosbulthuis | 0:7098cdb29180 | 51 | else if(error_pulses < 0) { |
roosbulthuis | 0:7098cdb29180 | 52 | directionPin.write(ccw); |
roosbulthuis | 0:7098cdb29180 | 53 | } |
roosbulthuis | 0:7098cdb29180 | 54 | else{ |
roosbulthuis | 0:7098cdb29180 | 55 | output = 0; |
roosbulthuis | 0:7098cdb29180 | 56 | } |
roosbulthuis | 0:7098cdb29180 | 57 | PWM.write(output); // out of the if loop due to abs output |
roosbulthuis | 0:7098cdb29180 | 58 | } |
roosbulthuis | 0:7098cdb29180 | 59 | |
roosbulthuis | 0:7098cdb29180 | 60 | |
roosbulthuis | 0:7098cdb29180 | 61 | |
roosbulthuis | 0:7098cdb29180 | 62 | void move_motor(std::string state) //state can be left, right, or keypress |
roosbulthuis | 0:7098cdb29180 | 63 | { |
roosbulthuis | 0:7098cdb29180 | 64 | |
roosbulthuis | 0:7098cdb29180 | 65 | if (state == "left") |
roosbulthuis | 0:7098cdb29180 | 66 | { |
roosbulthuis | 0:7098cdb29180 | 67 | DigitalOut directionPin(D4); //direction |
roosbulthuis | 0:7098cdb29180 | 68 | PwmOut PWM(D5); //speed |
roosbulthuis | 0:7098cdb29180 | 69 | double position = wheel.getPulses(); //actual position |
roosbulthuis | 0:7098cdb29180 | 70 | double Setpoint; //desired position |
roosbulthuis | 0:7098cdb29180 | 71 | Kp; //controller gain |
roosbulthuis | 0:7098cdb29180 | 72 | Ki; //set to zero |
roosbulthuis | 0:7098cdb29180 | 73 | } |
roosbulthuis | 0:7098cdb29180 | 74 | |
roosbulthuis | 0:7098cdb29180 | 75 | else if (state == "right") |
roosbulthuis | 0:7098cdb29180 | 76 | { |
roosbulthuis | 0:7098cdb29180 | 77 | DigitalOut directionPin(D4); |
roosbulthuis | 0:7098cdb29180 | 78 | PwmOut PWM(D5); |
roosbulthuis | 0:7098cdb29180 | 79 | double position = wheel.getPulses(); //actual position |
roosbulthuis | 0:7098cdb29180 | 80 | double Setpoint; //desired position |
roosbulthuis | 0:7098cdb29180 | 81 | double Kp; //controller gain, same as for left |
roosbulthuis | 0:7098cdb29180 | 82 | double Ki; //set to zero |
roosbulthuis | 0:7098cdb29180 | 83 | } |
roosbulthuis | 0:7098cdb29180 | 84 | |
roosbulthuis | 0:7098cdb29180 | 85 | else if (state == "keypress") //different encoder than left and right |
roosbulthuis | 0:7098cdb29180 | 86 | { |
roosbulthuis | 0:7098cdb29180 | 87 | DigitalOut directionPin(D6); //direction |
roosbulthuis | 0:7098cdb29180 | 88 | PwmOut PWM(D7); //speed |
roosbulthuis | 0:7098cdb29180 | 89 | double position = wheel.getPulses(); //actual position, but different motor! |
roosbulthuis | 0:7098cdb29180 | 90 | double Setpoint; //desired position, always the same actually |
roosbulthuis | 0:7098cdb29180 | 91 | Kp; //controller gain, different from left and right |
roosbulthuis | 0:7098cdb29180 | 92 | Ki; //set to zero |
roosbulthuis | 0:7098cdb29180 | 93 | } |
roosbulthuis | 0:7098cdb29180 | 94 | |
roosbulthuis | 0:7098cdb29180 | 95 | |
roosbulthuis | 0:7098cdb29180 | 96 | |
roosbulthuis | 0:7098cdb29180 | 97 | //call the flags and p-controller |
roosbulthuis | 0:7098cdb29180 | 98 | |
roosbulthuis | 0:7098cdb29180 | 99 | |
roosbulthuis | 0:7098cdb29180 | 100 | aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker |
roosbulthuis | 0:7098cdb29180 | 101 | while (1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm |
roosbulthuis | 0:7098cdb29180 | 102 | { |
roosbulthuis | 0:7098cdb29180 | 103 | // to make the motor move |
roosbulthuis | 0:7098cdb29180 | 104 | if(flag_motor) { |
roosbulthuis | 0:7098cdb29180 | 105 | flag_motor = false; |
roosbulthuis | 0:7098cdb29180 | 106 | motor1_Controller(directionPin, PWM, position, Setpoint, Kp, Ki); //How to pass Digitalout and PWM, by reference? |
roosbulthuis | 0:7098cdb29180 | 107 | } |
roosbulthuis | 0:7098cdb29180 | 108 | } |
roosbulthuis | 0:7098cdb29180 | 109 | } |