Biorobotics / Mbed 2 deprecated P_controler

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

main.cpp@0:bb0fd6c1d178, 2015-10-09 (annotated)

Committer:
yohoo15
Date:
Fri Oct 09 07:49:40 2015 +0000
Revision:
0:bb0fd6c1d178
Child:
1:63edfaecc73e
P controler working;

Who changed what in which revision?

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