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@4:1b4885298ade, 2015-10-14 (annotated)

Committer:
yohoo15
Date:
Wed Oct 14 12:58:25 2015 +0000
Revision:
4:1b4885298ade
Parent:
3:b9e2c7d52953
Child:
5:d8195d9d098f
Everything is in rotations now;

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 3:b9e2c7d52953 25 double Rotation = -2; // rotation in degree
yohoo15 4:1b4885298ade 26 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
yohoo15 0:bb0fd6c1d178 27
yohoo15 1:63edfaecc73e 28 // defining flags
yohoo15 1:63edfaecc73e 29 volatile bool flag_motor = false;
yohoo15 1:63edfaecc73e 30 volatile bool flag_pcprint = false;
yohoo15 0:bb0fd6c1d178 31
yohoo15 2:5ec7af981b32 32 // making function flags.
yohoo15 1:63edfaecc73e 33 void Go_flag_motor()
yohoo15 1:63edfaecc73e 34 {
yohoo15 1:63edfaecc73e 35 flag_motor = true;
yohoo15 1:63edfaecc73e 36 }
yohoo15 1:63edfaecc73e 37
yohoo15 1:63edfaecc73e 38 void Go_flag_pcprint()
yohoo15 1:63edfaecc73e 39 {
yohoo15 1:63edfaecc73e 40 flag_pcprint = true;
yohoo15 1:63edfaecc73e 41 }
yohoo15 0:bb0fd6c1d178 42
yohoo15 0:bb0fd6c1d178 43 // Reusable P controller
yohoo15 0:bb0fd6c1d178 44 double P(double error, const double Kp)
yohoo15 4:1b4885298ade 45 {
yohoo15 1:63edfaecc73e 46
yohoo15 4:1b4885298ade 47 double P_output = Kp * error;
yohoo15 0:bb0fd6c1d178 48 return P_output;
yohoo15 0:bb0fd6c1d178 49 }
yohoo15 0:bb0fd6c1d178 50 // Next task, measure the error and apply the output to the plant
yohoo15 0:bb0fd6c1d178 51 void motor1_Controller()
yohoo15 0:bb0fd6c1d178 52 {
yohoo15 0:bb0fd6c1d178 53 double reference = movement;
yohoo15 0:bb0fd6c1d178 54 double position = wheel.getPulses();
yohoo15 4:1b4885298ade 55 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 4:1b4885298ade 56 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 4:1b4885298ade 57
yohoo15 4:1b4885298ade 58 double output = abs(P( error_rotation, motor1_Kp));
yohoo15 0:bb0fd6c1d178 59
yohoo15 4:1b4885298ade 60 if(error_pulses > 0) {
yohoo15 0:bb0fd6c1d178 61 directionPin.write(cw);
yohoo15 4:1b4885298ade 62
yohoo15 1:63edfaecc73e 63 }
yohoo15 4:1b4885298ade 64 if(error_pulses < 0) {
yohoo15 4:1b4885298ade 65 directionPin.write(ccw);
yohoo15 0:bb0fd6c1d178 66 }
yohoo15 4:1b4885298ade 67 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:bb0fd6c1d178 68
yohoo15 0:bb0fd6c1d178 69
yohoo15 0:bb0fd6c1d178 70 }
yohoo15 0:bb0fd6c1d178 71
yohoo15 0:bb0fd6c1d178 72
yohoo15 0:bb0fd6c1d178 73 void counts_showing()
yohoo15 1:63edfaecc73e 74 {
yohoo15 0:bb0fd6c1d178 75
yohoo15 1:63edfaecc73e 76 double kijken = wheel.getPulses();
yohoo15 1:63edfaecc73e 77 pc.printf("ref %.0f count%.0f \n",movement,kijken);
yohoo15 1:63edfaecc73e 78
yohoo15 1:63edfaecc73e 79 }
yohoo15 0:bb0fd6c1d178 80
yohoo15 0:bb0fd6c1d178 81 int main()
yohoo15 0:bb0fd6c1d178 82 {
yohoo15 2:5ec7af981b32 83 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 1:63edfaecc73e 84 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 85 while( 1 ) {
yohoo15 1:63edfaecc73e 86
yohoo15 1:63edfaecc73e 87 if(flag_motor) {
yohoo15 1:63edfaecc73e 88 flag_motor = false;
yohoo15 1:63edfaecc73e 89 motor1_Controller();
yohoo15 1:63edfaecc73e 90 }
yohoo15 1:63edfaecc73e 91
yohoo15 1:63edfaecc73e 92 if(flag_pcprint) {
yohoo15 1:63edfaecc73e 93 flag_pcprint = false;
yohoo15 1:63edfaecc73e 94 counts_showing();
yohoo15 1:63edfaecc73e 95 }
yohoo15 1:63edfaecc73e 96
yohoo15 1:63edfaecc73e 97 }
yohoo15 0:bb0fd6c1d178 98 }
yohoo15 0:bb0fd6c1d178 99
yohoo15 0:bb0fd6c1d178 100