Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@4:0844ab0d7d93, 2015-10-09 (annotated)

Committer:
yohoo15
Date:
Fri Oct 09 11:29:40 2015 +0000
Revision:
4:0844ab0d7d93
Parent:
3:4abcd40682fa
Child:
5:d1ab07fd3355
Controller futhers (PI)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yohoo15 0:2b481a8db14f 1 #include "mbed.h"
yohoo15 0:2b481a8db14f 2 #include "QEI.h"
yohoo15 0:2b481a8db14f 3
yohoo15 0:2b481a8db14f 4 Serial pc(USBTX, USBRX);
yohoo15 0:2b481a8db14f 5 QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
yohoo15 0:2b481a8db14f 6
yohoo15 0:2b481a8db14f 7 // Define pin for motor control
yohoo15 0:2b481a8db14f 8 DigitalOut directionPin(D4);
yohoo15 0:2b481a8db14f 9 PwmOut PWM(D5);
yohoo15 0:2b481a8db14f 10
yohoo15 0:2b481a8db14f 11 // define ticker
yohoo15 0:2b481a8db14f 12 Ticker aansturen;
yohoo15 1:71617831bc4e 13 Ticker Printen;
yohoo15 0:2b481a8db14f 14
yohoo15 0:2b481a8db14f 15 // define rotation direction
yohoo15 0:2b481a8db14f 16 const int cw = 1;
yohoo15 0:2b481a8db14f 17 const int ccw = 0;
yohoo15 0:2b481a8db14f 18
yohoo15 3:4abcd40682fa 19 // Controller gain proportional and intergrator
yohoo15 1:71617831bc4e 20 const double motor1_Kp = 5; // more or les random number.
yohoo15 4:0844ab0d7d93 21 const double motor1_Ki = 0;
yohoo15 4:0844ab0d7d93 22 const double M1_timestep = 0.001; // reason ticker works with 100 Hz.
yohoo15 3:4abcd40682fa 23 double motor1_error_integraal = 0; // initial value of integral error
yohoo15 0:2b481a8db14f 24
yohoo15 0:2b481a8db14f 25 // calculating pulses to rotations in degree.
yohoo15 2:22f0ce494b16 26 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)
yohoo15 0:2b481a8db14f 27 const double resolution = Offset / 360;
yohoo15 2:22f0ce494b16 28 double Rotation = 2; // rotation in degree
yohoo15 0:2b481a8db14f 29 double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree.
yohoo15 0:2b481a8db14f 30
yohoo15 0:2b481a8db14f 31
yohoo15 0:2b481a8db14f 32
yohoo15 0:2b481a8db14f 33 // Reusable P controller
yohoo15 3:4abcd40682fa 34 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 0:2b481a8db14f 35 {
yohoo15 0:2b481a8db14f 36 double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
yohoo15 0:2b481a8db14f 37 double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
yohoo15 0:2b481a8db14f 38
yohoo15 3:4abcd40682fa 39 e_int = e_int + Ts * error_normalised_rotation;
yohoo15 3:4abcd40682fa 40
yohoo15 4:0844ab0d7d93 41 double PI_output = Kp * error_normalised_rotation + Ki * e_int; //
yohoo15 3:4abcd40682fa 42 return PI_output;
yohoo15 0:2b481a8db14f 43 }
yohoo15 0:2b481a8db14f 44 // Next task, measure the error and apply the output to the plant
yohoo15 0:2b481a8db14f 45 void motor1_Controller()
yohoo15 0:2b481a8db14f 46 {
yohoo15 0:2b481a8db14f 47 double reference = movement;
yohoo15 0:2b481a8db14f 48 double position = wheel.getPulses();
yohoo15 0:2b481a8db14f 49 double error = reference - position;
yohoo15 0:2b481a8db14f 50
yohoo15 3:4abcd40682fa 51 double output = PI( error, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal );
yohoo15 0:2b481a8db14f 52
yohoo15 1:71617831bc4e 53
yohoo15 1:71617831bc4e 54
yohoo15 0:2b481a8db14f 55 if(error > 0) {
yohoo15 0:2b481a8db14f 56 directionPin.write(cw);
yohoo15 0:2b481a8db14f 57 PWM.write(output);
yohoo15 1:71617831bc4e 58 //pc.printf("ref %.0f count%.0f cw\n",movement,position);
yohoo15 1:71617831bc4e 59
yohoo15 1:71617831bc4e 60 } if(error < 0) {
yohoo15 0:2b481a8db14f 61 directionPin.write(ccw);
yohoo15 1:71617831bc4e 62 PWM.write(-output); //min output to get output possitif
yohoo15 1:71617831bc4e 63 //pc.printf("a");
yohoo15 0:2b481a8db14f 64 }
yohoo15 0:2b481a8db14f 65
yohoo15 0:2b481a8db14f 66
yohoo15 0:2b481a8db14f 67
yohoo15 0:2b481a8db14f 68 }
yohoo15 0:2b481a8db14f 69
yohoo15 0:2b481a8db14f 70
yohoo15 1:71617831bc4e 71 void counts_showing()
yohoo15 1:71617831bc4e 72 {
yohoo15 1:71617831bc4e 73
yohoo15 1:71617831bc4e 74 double kijken = wheel.getPulses();
yohoo15 3:4abcd40682fa 75 pc.printf("ref %.0f count%.0f int err %.2f \n",movement,kijken, motor1_error_integraal);
yohoo15 1:71617831bc4e 76
yohoo15 1:71617831bc4e 77 }
yohoo15 0:2b481a8db14f 78
yohoo15 0:2b481a8db14f 79 int main()
yohoo15 0:2b481a8db14f 80 {
yohoo15 4:0844ab0d7d93 81 aansturen.attach( &motor1_Controller, 0.001f ); // 100 Hz
yohoo15 4:0844ab0d7d93 82 Printen.attach(&counts_showing,0.1); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
yohoo15 0:2b481a8db14f 83 while( 1 ) { }
yohoo15 0:2b481a8db14f 84 }
yohoo15 0:2b481a8db14f 85
yohoo15 0:2b481a8db14f 86
yohoo15 0:2b481a8db14f 87