Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@0:2b481a8db14f, 2015-10-08 (annotated)

Committer:
yohoo15
Date:
Thu Oct 08 11:35:49 2015 +0000
Revision:
0:2b481a8db14f
Child:
1:71617831bc4e
eerste versie, doet het. maar nog niet precies

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 0:2b481a8db14f 13
yohoo15 0:2b481a8db14f 14 // define rotation direction
yohoo15 0:2b481a8db14f 15 const int cw = 1;
yohoo15 0:2b481a8db14f 16 const int ccw = 0;
yohoo15 0:2b481a8db14f 17
yohoo15 0:2b481a8db14f 18 // Controller gain proportional
yohoo15 0:2b481a8db14f 19 const double motor1_Kp = 2.5;
yohoo15 0:2b481a8db14f 20
yohoo15 0:2b481a8db14f 21 // calculating pulses to rotations in degree.
yohoo15 0:2b481a8db14f 22 const double Offset = 10341 ;//8400 counts is aangegeven op de motor ( is te weinig) 10511 schiet iets over
yohoo15 0:2b481a8db14f 23 const double resolution = Offset / 360;
yohoo15 0:2b481a8db14f 24 double Rotation = 4; // rotation in degree
yohoo15 0:2b481a8db14f 25 double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree.
yohoo15 0:2b481a8db14f 26
yohoo15 0:2b481a8db14f 27
yohoo15 0:2b481a8db14f 28
yohoo15 0:2b481a8db14f 29 // Reusable P controller
yohoo15 0:2b481a8db14f 30 double P(double error, double Kp)
yohoo15 0:2b481a8db14f 31 {
yohoo15 0:2b481a8db14f 32 double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
yohoo15 0:2b481a8db14f 33 double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
yohoo15 0:2b481a8db14f 34
yohoo15 0:2b481a8db14f 35 double P_output = Kp * error_normalised_rotation;
yohoo15 0:2b481a8db14f 36 return P_output;
yohoo15 0:2b481a8db14f 37 }
yohoo15 0:2b481a8db14f 38 // Next task, measure the error and apply the output to the plant
yohoo15 0:2b481a8db14f 39 void motor1_Controller()
yohoo15 0:2b481a8db14f 40 {
yohoo15 0:2b481a8db14f 41 double reference = movement;
yohoo15 0:2b481a8db14f 42 double position = wheel.getPulses();
yohoo15 0:2b481a8db14f 43 double error = reference - position;
yohoo15 0:2b481a8db14f 44
yohoo15 0:2b481a8db14f 45 double output = P( error, motor1_Kp );
yohoo15 0:2b481a8db14f 46
yohoo15 0:2b481a8db14f 47 if(error > 0) {
yohoo15 0:2b481a8db14f 48 directionPin.write(cw);
yohoo15 0:2b481a8db14f 49 PWM.write(output);
yohoo15 0:2b481a8db14f 50 } else {
yohoo15 0:2b481a8db14f 51 directionPin.write(ccw);
yohoo15 0:2b481a8db14f 52 PWM.write(output);
yohoo15 0:2b481a8db14f 53 }
yohoo15 0:2b481a8db14f 54
yohoo15 0:2b481a8db14f 55
yohoo15 0:2b481a8db14f 56
yohoo15 0:2b481a8db14f 57 }
yohoo15 0:2b481a8db14f 58
yohoo15 0:2b481a8db14f 59
yohoo15 0:2b481a8db14f 60
yohoo15 0:2b481a8db14f 61 int main()
yohoo15 0:2b481a8db14f 62 {
yohoo15 0:2b481a8db14f 63 aansturen.attach( &motor1_Controller, 0.01f ); // 100 Hz
yohoo15 0:2b481a8db14f 64 while( 1 ) { }
yohoo15 0:2b481a8db14f 65 }
yohoo15 0:2b481a8db14f 66
yohoo15 0:2b481a8db14f 67
yohoo15 0:2b481a8db14f 68