Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@2:22f0ce494b16, 2015-10-08 (annotated)

Committer:
yohoo15
Date:
Thu Oct 08 12:35:56 2015 +0000
Revision:
2:22f0ce494b16
Parent:
1:71617831bc4e
Child:
3:4abcd40682fa
P controller with right values;

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 0:2b481a8db14f 19 // Controller gain proportional
yohoo15 1:71617831bc4e 20 const double motor1_Kp = 5; // more or les random number.
yohoo15 0:2b481a8db14f 21
yohoo15 0:2b481a8db14f 22 // calculating pulses to rotations in degree.
yohoo15 2:22f0ce494b16 23 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 24 const double resolution = Offset / 360;
yohoo15 2:22f0ce494b16 25 double Rotation = 2; // rotation in degree
yohoo15 0:2b481a8db14f 26 double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree.
yohoo15 0:2b481a8db14f 27
yohoo15 0:2b481a8db14f 28
yohoo15 0:2b481a8db14f 29
yohoo15 0:2b481a8db14f 30 // Reusable P controller
yohoo15 0:2b481a8db14f 31 double P(double error, double Kp)
yohoo15 0:2b481a8db14f 32 {
yohoo15 0:2b481a8db14f 33 double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
yohoo15 0:2b481a8db14f 34 double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
yohoo15 0:2b481a8db14f 35
yohoo15 0:2b481a8db14f 36 double P_output = Kp * error_normalised_rotation;
yohoo15 0:2b481a8db14f 37 return P_output;
yohoo15 0:2b481a8db14f 38 }
yohoo15 0:2b481a8db14f 39 // Next task, measure the error and apply the output to the plant
yohoo15 0:2b481a8db14f 40 void motor1_Controller()
yohoo15 0:2b481a8db14f 41 {
yohoo15 0:2b481a8db14f 42 double reference = movement;
yohoo15 0:2b481a8db14f 43 double position = wheel.getPulses();
yohoo15 0:2b481a8db14f 44 double error = reference - position;
yohoo15 0:2b481a8db14f 45
yohoo15 0:2b481a8db14f 46 double output = P( error, motor1_Kp );
yohoo15 0:2b481a8db14f 47
yohoo15 1:71617831bc4e 48
yohoo15 1:71617831bc4e 49
yohoo15 0:2b481a8db14f 50 if(error > 0) {
yohoo15 0:2b481a8db14f 51 directionPin.write(cw);
yohoo15 0:2b481a8db14f 52 PWM.write(output);
yohoo15 1:71617831bc4e 53 //pc.printf("ref %.0f count%.0f cw\n",movement,position);
yohoo15 1:71617831bc4e 54
yohoo15 1:71617831bc4e 55 } if(error < 0) {
yohoo15 0:2b481a8db14f 56 directionPin.write(ccw);
yohoo15 1:71617831bc4e 57 PWM.write(-output); //min output to get output possitif
yohoo15 1:71617831bc4e 58 //pc.printf("a");
yohoo15 0:2b481a8db14f 59 }
yohoo15 0:2b481a8db14f 60
yohoo15 0:2b481a8db14f 61
yohoo15 0:2b481a8db14f 62
yohoo15 0:2b481a8db14f 63 }
yohoo15 0:2b481a8db14f 64
yohoo15 0:2b481a8db14f 65
yohoo15 1:71617831bc4e 66 void counts_showing()
yohoo15 1:71617831bc4e 67 {
yohoo15 1:71617831bc4e 68
yohoo15 1:71617831bc4e 69 double kijken = wheel.getPulses();
yohoo15 1:71617831bc4e 70 pc.printf("ref %.0f count%.0f\n",movement,kijken);
yohoo15 1:71617831bc4e 71
yohoo15 1:71617831bc4e 72 }
yohoo15 0:2b481a8db14f 73
yohoo15 0:2b481a8db14f 74 int main()
yohoo15 0:2b481a8db14f 75 {
yohoo15 0:2b481a8db14f 76 aansturen.attach( &motor1_Controller, 0.01f ); // 100 Hz
yohoo15 1:71617831bc4e 77 Printen.attach(&counts_showing,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
yohoo15 0:2b481a8db14f 78 while( 1 ) { }
yohoo15 0:2b481a8db14f 79 }
yohoo15 0:2b481a8db14f 80
yohoo15 0:2b481a8db14f 81
yohoo15 0:2b481a8db14f 82