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@3:b9e2c7d52953, 2015-10-14 (annotated)

Committer:
yohoo15
Date:
Wed Oct 14 12:47:44 2015 +0000
Revision:
3:b9e2c7d52953
Parent:
2:5ec7af981b32
Child:
4:1b4885298ade
Werkt, Voor de verandering naar rotaties;

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 2:5ec7af981b32 24 const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4. 10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
yohoo15 2:5ec7af981b32 25 const double resolution = pulses_per_revolution / 360;
yohoo15 3:b9e2c7d52953 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 1:63edfaecc73e 29 // defining flags
yohoo15 1:63edfaecc73e 30 volatile bool flag_motor = false;
yohoo15 1:63edfaecc73e 31 volatile bool flag_pcprint = false;
yohoo15 0:bb0fd6c1d178 32
yohoo15 2:5ec7af981b32 33 // making function flags.
yohoo15 1:63edfaecc73e 34 void Go_flag_motor()
yohoo15 1:63edfaecc73e 35 {
yohoo15 1:63edfaecc73e 36 flag_motor = true;
yohoo15 1:63edfaecc73e 37 }
yohoo15 1:63edfaecc73e 38
yohoo15 1:63edfaecc73e 39 void Go_flag_pcprint()
yohoo15 1:63edfaecc73e 40 {
yohoo15 1:63edfaecc73e 41 flag_pcprint = true;
yohoo15 1:63edfaecc73e 42 }
yohoo15 0:bb0fd6c1d178 43
yohoo15 0:bb0fd6c1d178 44 // Reusable P controller
yohoo15 0:bb0fd6c1d178 45 double P(double error, const double Kp)
yohoo15 0:bb0fd6c1d178 46 {
yohoo15 0:bb0fd6c1d178 47 double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
yohoo15 0:bb0fd6c1d178 48 double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
yohoo15 1:63edfaecc73e 49
yohoo15 0:bb0fd6c1d178 50 double P_output = Kp * error_normalised_rotation;
yohoo15 0:bb0fd6c1d178 51 return P_output;
yohoo15 0:bb0fd6c1d178 52 }
yohoo15 0:bb0fd6c1d178 53 // Next task, measure the error and apply the output to the plant
yohoo15 0:bb0fd6c1d178 54 void motor1_Controller()
yohoo15 0:bb0fd6c1d178 55 {
yohoo15 0:bb0fd6c1d178 56 double reference = movement;
yohoo15 0:bb0fd6c1d178 57 double position = wheel.getPulses();
yohoo15 0:bb0fd6c1d178 58 double error = reference - position;
yohoo15 0:bb0fd6c1d178 59
yohoo15 0:bb0fd6c1d178 60 double output = P( error, motor1_Kp);
yohoo15 0:bb0fd6c1d178 61
yohoo15 0:bb0fd6c1d178 62
yohoo15 0:bb0fd6c1d178 63
yohoo15 0:bb0fd6c1d178 64 if(error > 0) {
yohoo15 0:bb0fd6c1d178 65 directionPin.write(cw);
yohoo15 0:bb0fd6c1d178 66 PWM.write(output);
yohoo15 0:bb0fd6c1d178 67 //pc.printf("ref %.0f count%.0f cw\n",movement,position);
yohoo15 1:63edfaecc73e 68
yohoo15 1:63edfaecc73e 69 }
yohoo15 1:63edfaecc73e 70 if(error < 0) {
yohoo15 0:bb0fd6c1d178 71 directionPin.write(ccw);
yohoo15 1:63edfaecc73e 72 PWM.write(-output); //min output to get output positive
yohoo15 0:bb0fd6c1d178 73 //pc.printf("a");
yohoo15 0:bb0fd6c1d178 74 }
yohoo15 0:bb0fd6c1d178 75
yohoo15 0:bb0fd6c1d178 76
yohoo15 0:bb0fd6c1d178 77
yohoo15 0:bb0fd6c1d178 78 }
yohoo15 0:bb0fd6c1d178 79
yohoo15 0:bb0fd6c1d178 80
yohoo15 0:bb0fd6c1d178 81 void counts_showing()
yohoo15 1:63edfaecc73e 82 {
yohoo15 0:bb0fd6c1d178 83
yohoo15 1:63edfaecc73e 84 double kijken = wheel.getPulses();
yohoo15 1:63edfaecc73e 85 pc.printf("ref %.0f count%.0f \n",movement,kijken);
yohoo15 1:63edfaecc73e 86
yohoo15 1:63edfaecc73e 87 }
yohoo15 0:bb0fd6c1d178 88
yohoo15 0:bb0fd6c1d178 89 int main()
yohoo15 0:bb0fd6c1d178 90 {
yohoo15 2:5ec7af981b32 91 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 1:63edfaecc73e 92 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 1:63edfaecc73e 93 while( 1 ) {
yohoo15 1:63edfaecc73e 94
yohoo15 1:63edfaecc73e 95 if(flag_motor) {
yohoo15 1:63edfaecc73e 96 flag_motor = false;
yohoo15 1:63edfaecc73e 97 motor1_Controller();
yohoo15 1:63edfaecc73e 98 }
yohoo15 1:63edfaecc73e 99
yohoo15 1:63edfaecc73e 100 if(flag_pcprint) {
yohoo15 1:63edfaecc73e 101 flag_pcprint = false;
yohoo15 1:63edfaecc73e 102 counts_showing();
yohoo15 1:63edfaecc73e 103 }
yohoo15 1:63edfaecc73e 104
yohoo15 1:63edfaecc73e 105 }
yohoo15 0:bb0fd6c1d178 106 }
yohoo15 0:bb0fd6c1d178 107
yohoo15 0:bb0fd6c1d178 108