Biorobotics / Mbed 2 deprecated P_controler

P controller working

Dependencies:   QEI mbed

main.cpp@5:d8195d9d098f, 2015-10-14 (annotated)

Committer:
yohoo15
Date:
Wed Oct 14 13:26:00 2015 +0000
Revision:
5:d8195d9d098f
Parent:
4:1b4885298ade 
The p controler working  inlcusive go flag, variable is rotations

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 5:d8195d9d098f 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 5:d8195d9d098f 47
yohoo15 5:d8195d9d098f 48 double P_output = (Kp * error);
yohoo15 0:bb0fd6c1d178 49 return P_output;
yohoo15 0:bb0fd6c1d178 50 }
yohoo15 0:bb0fd6c1d178 51 // Next task, measure the error and apply the output to the plant
yohoo15 0:bb0fd6c1d178 52 void motor1_Controller()
yohoo15 0:bb0fd6c1d178 53 {
yohoo15 0:bb0fd6c1d178 54 double reference = movement;
yohoo15 0:bb0fd6c1d178 55 double position = wheel.getPulses();
yohoo15 4:1b4885298ade 56 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 4:1b4885298ade 57 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 4:1b4885298ade 58
yohoo15 4:1b4885298ade 59 double output = abs(P( error_rotation, motor1_Kp));
yohoo15 0:bb0fd6c1d178 60
yohoo15 4:1b4885298ade 61 if(error_pulses > 0) {
yohoo15 0:bb0fd6c1d178 62 directionPin.write(cw);
yohoo15 4:1b4885298ade 63
yohoo15 1:63edfaecc73e 64 }
yohoo15 4:1b4885298ade 65 if(error_pulses < 0) {
yohoo15 4:1b4885298ade 66 directionPin.write(ccw);
yohoo15 0:bb0fd6c1d178 67 }
yohoo15 4:1b4885298ade 68 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:bb0fd6c1d178 69
yohoo15 0:bb0fd6c1d178 70
yohoo15 0:bb0fd6c1d178 71 }
yohoo15 0:bb0fd6c1d178 72
yohoo15 0:bb0fd6c1d178 73
yohoo15 0:bb0fd6c1d178 74 void counts_showing()
yohoo15 1:63edfaecc73e 75 {
yohoo15 0:bb0fd6c1d178 76
yohoo15 5:d8195d9d098f 77 double kijken = wheel.getPulses() / pulses_per_revolution;
yohoo15 5:d8195d9d098f 78 pc.printf("ref %.0f rounds%.4f \n",Rotation,kijken);
yohoo15 1:63edfaecc73e 79
yohoo15 1:63edfaecc73e 80 }
yohoo15 0:bb0fd6c1d178 81
yohoo15 0:bb0fd6c1d178 82 int main()
yohoo15 0:bb0fd6c1d178 83 {
yohoo15 2:5ec7af981b32 84 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 1:63edfaecc73e 85 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 86 while( 1 ) {
yohoo15 1:63edfaecc73e 87
yohoo15 1:63edfaecc73e 88 if(flag_motor) {
yohoo15 1:63edfaecc73e 89 flag_motor = false;
yohoo15 1:63edfaecc73e 90 motor1_Controller();
yohoo15 1:63edfaecc73e 91 }
yohoo15 1:63edfaecc73e 92
yohoo15 1:63edfaecc73e 93 if(flag_pcprint) {
yohoo15 1:63edfaecc73e 94 flag_pcprint = false;
yohoo15 1:63edfaecc73e 95 counts_showing();
yohoo15 1:63edfaecc73e 96 }
yohoo15 1:63edfaecc73e 97
yohoo15 1:63edfaecc73e 98 }
yohoo15 0:bb0fd6c1d178 99 }
yohoo15 0:bb0fd6c1d178 100
yohoo15 0:bb0fd6c1d178 101
yohoo15 5:d8195d9d098f 102