Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@5:d1ab07fd3355, 2015-10-14 (annotated)

Committer:
yohoo15
Date:
Wed Oct 14 10:33:34 2015 +0000
Revision:
5:d1ab07fd3355
Parent:
4:0844ab0d7d93
Child:
6:8ab07cce3098
whit go flag ( needs to be tested)

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 5:d1ab07fd3355 31 // defining flags
yohoo15 5:d1ab07fd3355 32 volatile bool flag_motor = false;
yohoo15 5:d1ab07fd3355 33 volatile bool flag_pcprint = false;
yohoo15 0:2b481a8db14f 34
yohoo15 5:d1ab07fd3355 35 // making flags.
yohoo15 5:d1ab07fd3355 36 void Go_flag_motor()
yohoo15 5:d1ab07fd3355 37 {
yohoo15 5:d1ab07fd3355 38 flag_motor = true;
yohoo15 5:d1ab07fd3355 39 }
yohoo15 5:d1ab07fd3355 40
yohoo15 5:d1ab07fd3355 41 void Go_flag_pcprint()
yohoo15 5:d1ab07fd3355 42 {
yohoo15 5:d1ab07fd3355 43 flag_pcprint = true;
yohoo15 5:d1ab07fd3355 44 }
yohoo15 0:2b481a8db14f 45
yohoo15 0:2b481a8db14f 46 // Reusable P controller
yohoo15 3:4abcd40682fa 47 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 0:2b481a8db14f 48 {
yohoo15 0:2b481a8db14f 49 double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
yohoo15 0:2b481a8db14f 50 double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
yohoo15 0:2b481a8db14f 51
yohoo15 3:4abcd40682fa 52 e_int = e_int + Ts * error_normalised_rotation;
yohoo15 3:4abcd40682fa 53
yohoo15 4:0844ab0d7d93 54 double PI_output = Kp * error_normalised_rotation + Ki * e_int; //
yohoo15 3:4abcd40682fa 55 return PI_output;
yohoo15 0:2b481a8db14f 56 }
yohoo15 0:2b481a8db14f 57 // Next task, measure the error and apply the output to the plant
yohoo15 0:2b481a8db14f 58 void motor1_Controller()
yohoo15 0:2b481a8db14f 59 {
yohoo15 0:2b481a8db14f 60 double reference = movement;
yohoo15 0:2b481a8db14f 61 double position = wheel.getPulses();
yohoo15 0:2b481a8db14f 62 double error = reference - position;
yohoo15 0:2b481a8db14f 63
yohoo15 3:4abcd40682fa 64 double output = PI( error, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal );
yohoo15 0:2b481a8db14f 65
yohoo15 1:71617831bc4e 66
yohoo15 1:71617831bc4e 67
yohoo15 0:2b481a8db14f 68 if(error > 0) {
yohoo15 0:2b481a8db14f 69 directionPin.write(cw);
yohoo15 0:2b481a8db14f 70 PWM.write(output);
yohoo15 1:71617831bc4e 71 //pc.printf("ref %.0f count%.0f cw\n",movement,position);
yohoo15 1:71617831bc4e 72
yohoo15 1:71617831bc4e 73 } if(error < 0) {
yohoo15 0:2b481a8db14f 74 directionPin.write(ccw);
yohoo15 1:71617831bc4e 75 PWM.write(-output); //min output to get output possitif
yohoo15 1:71617831bc4e 76 //pc.printf("a");
yohoo15 0:2b481a8db14f 77 }
yohoo15 0:2b481a8db14f 78
yohoo15 0:2b481a8db14f 79
yohoo15 0:2b481a8db14f 80
yohoo15 0:2b481a8db14f 81 }
yohoo15 0:2b481a8db14f 82
yohoo15 0:2b481a8db14f 83
yohoo15 1:71617831bc4e 84 void counts_showing()
yohoo15 1:71617831bc4e 85 {
yohoo15 1:71617831bc4e 86
yohoo15 1:71617831bc4e 87 double kijken = wheel.getPulses();
yohoo15 3:4abcd40682fa 88 pc.printf("ref %.0f count%.0f int err %.2f \n",movement,kijken, motor1_error_integraal);
yohoo15 1:71617831bc4e 89
yohoo15 1:71617831bc4e 90 }
yohoo15 0:2b481a8db14f 91
yohoo15 0:2b481a8db14f 92 int main()
yohoo15 0:2b481a8db14f 93 {
yohoo15 5:d1ab07fd3355 94 aansturen.attach( &Go_flag_motor, 0.001f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 5:d1ab07fd3355 95 Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
yohoo15 5:d1ab07fd3355 96 while( 1 ) {
yohoo15 5:d1ab07fd3355 97
yohoo15 5:d1ab07fd3355 98 if(flag_motor) {
yohoo15 5:d1ab07fd3355 99 flag_motor = false;
yohoo15 5:d1ab07fd3355 100 motor1_Controller();
yohoo15 5:d1ab07fd3355 101 }
yohoo15 5:d1ab07fd3355 102
yohoo15 5:d1ab07fd3355 103 if(flag_pcprint) {
yohoo15 5:d1ab07fd3355 104 flag_pcprint = false;
yohoo15 5:d1ab07fd3355 105 counts_showing();
yohoo15 5:d1ab07fd3355 106 }
yohoo15 5:d1ab07fd3355 107
yohoo15 5:d1ab07fd3355 108 }
yohoo15 0:2b481a8db14f 109 }
yohoo15 0:2b481a8db14f 110
yohoo15 0:2b481a8db14f 111
yohoo15 0:2b481a8db14f 112