Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@6:8ab07cce3098, 2015-10-14 (annotated)

Committer:
yohoo15
Date:
Wed Oct 14 13:26:48 2015 +0000
Revision:
6:8ab07cce3098
Parent:
5:d1ab07fd3355
Child:
7:6006a473ea0b
The pi controler working  inlcusive go flag, variable is rotations

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 1:71617831bc4e 13 Ticker Printen;
yohoo15 0:2b481a8db14f 14
yohoo15 0:2b481a8db14f 15 // define rotation direction
yohoo15 0:2b481a8db14f 16 const int cw = 1;
yohoo15 0:2b481a8db14f 17 const int ccw = 0;
yohoo15 0:2b481a8db14f 18
yohoo15 3:4abcd40682fa 19 // Controller gain proportional and intergrator
yohoo15 1:71617831bc4e 20 const double motor1_Kp = 5; // more or les random number.
yohoo15 6:8ab07cce3098 21 const double motor1_Ki = 0.5;
yohoo15 6:8ab07cce3098 22 const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
yohoo15 3:4abcd40682fa 23 double motor1_error_integraal = 0; // initial value of integral error
yohoo15 0:2b481a8db14f 24
yohoo15 6:8ab07cce3098 25
yohoo15 0:2b481a8db14f 26 // calculating pulses to rotations in degree.
yohoo15 6:8ab07cce3098 27 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 6:8ab07cce3098 28 double Rotation = -2; // rotation in degree
yohoo15 6:8ab07cce3098 29 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
yohoo15 0:2b481a8db14f 30
yohoo15 5:d1ab07fd3355 31 // defining flags
yohoo15 5:d1ab07fd3355 32 volatile bool flag_motor = false;
yohoo15 5:d1ab07fd3355 33 volatile bool flag_pcprint = false;
yohoo15 0:2b481a8db14f 34
yohoo15 6:8ab07cce3098 35 // making function flags.
yohoo15 5:d1ab07fd3355 36 void Go_flag_motor()
yohoo15 5:d1ab07fd3355 37 {
yohoo15 5:d1ab07fd3355 38 flag_motor = true;
yohoo15 5:d1ab07fd3355 39 }
yohoo15 5:d1ab07fd3355 40
yohoo15 5:d1ab07fd3355 41 void Go_flag_pcprint()
yohoo15 5:d1ab07fd3355 42 {
yohoo15 5:d1ab07fd3355 43 flag_pcprint = true;
yohoo15 5:d1ab07fd3355 44 }
yohoo15 0:2b481a8db14f 45
yohoo15 0:2b481a8db14f 46 // Reusable P controller
yohoo15 3:4abcd40682fa 47 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 6:8ab07cce3098 48 {
yohoo15 6:8ab07cce3098 49
yohoo15 6:8ab07cce3098 50 e_int = e_int + Ts * error;
yohoo15 6:8ab07cce3098 51
yohoo15 6:8ab07cce3098 52 double PI_output = (Kp * error) + (Ki * e_int);
yohoo15 3:4abcd40682fa 53 return PI_output;
yohoo15 0:2b481a8db14f 54 }
yohoo15 0:2b481a8db14f 55 // Next task, measure the error and apply the output to the plant
yohoo15 0:2b481a8db14f 56 void motor1_Controller()
yohoo15 0:2b481a8db14f 57 {
yohoo15 6:8ab07cce3098 58 double reference = movement; // movement is in pulses
yohoo15 0:2b481a8db14f 59 double position = wheel.getPulses();
yohoo15 6:8ab07cce3098 60 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 6:8ab07cce3098 61 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 6:8ab07cce3098 62
yohoo15 6:8ab07cce3098 63 double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
yohoo15 0:2b481a8db14f 64
yohoo15 6:8ab07cce3098 65 if(error_pulses > 0) {
yohoo15 0:2b481a8db14f 66 directionPin.write(cw);
yohoo15 6:8ab07cce3098 67
yohoo15 6:8ab07cce3098 68 }
yohoo15 6:8ab07cce3098 69 else if(error_pulses < 0) {
yohoo15 6:8ab07cce3098 70 directionPin.write(ccw);
yohoo15 0:2b481a8db14f 71 }
yohoo15 6:8ab07cce3098 72 else{
yohoo15 6:8ab07cce3098 73 output = 0;
yohoo15 6:8ab07cce3098 74 }
yohoo15 6:8ab07cce3098 75 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:2b481a8db14f 76
yohoo15 0:2b481a8db14f 77
yohoo15 0:2b481a8db14f 78 }
yohoo15 0:2b481a8db14f 79
yohoo15 0:2b481a8db14f 80
yohoo15 1:71617831bc4e 81 void counts_showing()
yohoo15 6:8ab07cce3098 82 {
yohoo15 1:71617831bc4e 83
yohoo15 6:8ab07cce3098 84 double kijken = wheel.getPulses() / pulses_per_revolution;
yohoo15 6:8ab07cce3098 85 pc.printf("ref %.0f rounds%.2f \n",Rotation,kijken);
yohoo15 6:8ab07cce3098 86
yohoo15 6:8ab07cce3098 87 }
yohoo15 0:2b481a8db14f 88
yohoo15 0:2b481a8db14f 89 int main()
yohoo15 0:2b481a8db14f 90 {
yohoo15 6:8ab07cce3098 91 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 5:d1ab07fd3355 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 5:d1ab07fd3355 93 while( 1 ) {
yohoo15 5:d1ab07fd3355 94
yohoo15 5:d1ab07fd3355 95 if(flag_motor) {
yohoo15 5:d1ab07fd3355 96 flag_motor = false;
yohoo15 5:d1ab07fd3355 97 motor1_Controller();
yohoo15 5:d1ab07fd3355 98 }
yohoo15 5:d1ab07fd3355 99
yohoo15 5:d1ab07fd3355 100 if(flag_pcprint) {
yohoo15 5:d1ab07fd3355 101 flag_pcprint = false;
yohoo15 5:d1ab07fd3355 102 counts_showing();
yohoo15 5:d1ab07fd3355 103 }
yohoo15 5:d1ab07fd3355 104
yohoo15 5:d1ab07fd3355 105 }
yohoo15 0:2b481a8db14f 106 }
yohoo15 0:2b481a8db14f 107
yohoo15 0:2b481a8db14f 108
yohoo15 0:2b481a8db14f 109