Arun Raveenthiran / Mbed 2 deprecated home_position_control

Home position met PI control

Dependencies:   Encoder HIDScope mbed

main.cpp@0:239a6daf84e4, 2015-10-20 (annotated)

Committer:
arunr
Date:
Tue Oct 20 11:46:31 2015 +0000
Revision:
0:239a6daf84e4
Child:
1:d5e81fb97eca
Home position op snelheid 0.1;  fout is +/- 5 graden;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arunr 0:239a6daf84e4 1 #include "mbed.h"
arunr 0:239a6daf84e4 2 #include "encoder.h"
arunr 0:239a6daf84e4 3 #include "HIDScope.h"
arunr 0:239a6daf84e4 4
arunr 0:239a6daf84e4 5 DigitalOut motor1_direction(D4);
arunr 0:239a6daf84e4 6 PwmOut motor1_speed(D5);
arunr 0:239a6daf84e4 7 PwmOut led(D9);
arunr 0:239a6daf84e4 8 DigitalIn button_1(PTC6); //counterclockwise
arunr 0:239a6daf84e4 9 DigitalIn button_2(PTA4); //clockwise
arunr 0:239a6daf84e4 10 Encoder motor1(D12,D13);
arunr 0:239a6daf84e4 11 HIDScope scope(1);
arunr 0:239a6daf84e4 12
arunr 0:239a6daf84e4 13 AnalogIn PotMeter1(A1);
arunr 0:239a6daf84e4 14 Ticker controller;
arunr 0:239a6daf84e4 15
arunr 0:239a6daf84e4 16 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
arunr 0:239a6daf84e4 17 const double g = 360; // Aantal graden 1 rotatie
arunr 0:239a6daf84e4 18 const double c = 4200; // Aantal counts 1 rotatie
arunr 0:239a6daf84e4 19 const double q = c/(g);
arunr 0:239a6daf84e4 20
arunr 0:239a6daf84e4 21 //PI-controller constante
arunr 0:239a6daf84e4 22 const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
arunr 0:239a6daf84e4 23 const double motor1_Ki = 0.002; // Integrating gain m1.
arunr 0:239a6daf84e4 24 const double motor1_Ts = 0.01; // Time step m1
arunr 0:239a6daf84e4 25 double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
arunr 0:239a6daf84e4 26
arunr 0:239a6daf84e4 27 // Reusable P controller
arunr 0:239a6daf84e4 28 double Pc (double error, const double Kp)
arunr 0:239a6daf84e4 29 {
arunr 0:239a6daf84e4 30 return motor1_Kp * error;
arunr 0:239a6daf84e4 31 }
arunr 0:239a6daf84e4 32
arunr 0:239a6daf84e4 33 // Measure the error and apply output to the plant
arunr 0:239a6daf84e4 34 void motor1_controlP()
arunr 0:239a6daf84e4 35 {
arunr 0:239a6daf84e4 36 double referenceP1 = PotMeter1.read();
arunr 0:239a6daf84e4 37 double positionP1 = q*motor1.getPosition();
arunr 0:239a6daf84e4 38 double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
arunr 0:239a6daf84e4 39 }
arunr 0:239a6daf84e4 40
arunr 0:239a6daf84e4 41 // Reusable PI controller
arunr 0:239a6daf84e4 42 double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
arunr 0:239a6daf84e4 43 {
arunr 0:239a6daf84e4 44 err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
arunr 0:239a6daf84e4 45 return motor1_Kp*error + motor1_Ki*err_int;
arunr 0:239a6daf84e4 46 } // De totale fout die wordt hersteld met behulp van PI control.
arunr 0:239a6daf84e4 47
arunr 0:239a6daf84e4 48 void motor1_controlPI()
arunr 0:239a6daf84e4 49 {
arunr 0:239a6daf84e4 50 double referencePI1 = PotMeter1.read();
arunr 0:239a6daf84e4 51 double positionPI1 = q*motor1.getPosition();
arunr 0:239a6daf84e4 52 double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
arunr 0:239a6daf84e4 53 }
arunr 0:239a6daf84e4 54
arunr 0:239a6daf84e4 55 const int pressed = 0;
arunr 0:239a6daf84e4 56
arunr 0:239a6daf84e4 57 double H;
arunr 0:239a6daf84e4 58 double P;
arunr 0:239a6daf84e4 59 double D;
arunr 0:239a6daf84e4 60
arunr 0:239a6daf84e4 61 void sethome(){
arunr 0:239a6daf84e4 62 motor1.setPosition(0);
arunr 0:239a6daf84e4 63 H = motor1.getPosition();
arunr 0:239a6daf84e4 64 }
arunr 0:239a6daf84e4 65
arunr 0:239a6daf84e4 66 void move_motor1_ccw (){
arunr 0:239a6daf84e4 67 motor1_direction = 0;
arunr 0:239a6daf84e4 68 motor1_speed = 0.1;
arunr 0:239a6daf84e4 69 }
arunr 0:239a6daf84e4 70
arunr 0:239a6daf84e4 71 void move_motor1_cw (){
arunr 0:239a6daf84e4 72 motor1_direction = 1;
arunr 0:239a6daf84e4 73 motor1_speed = 0.1;
arunr 0:239a6daf84e4 74 }
arunr 0:239a6daf84e4 75
arunr 0:239a6daf84e4 76 void movetohome(){
arunr 0:239a6daf84e4 77 P = motor1.getPosition();
arunr 0:239a6daf84e4 78 D = (P - H);
arunr 0:239a6daf84e4 79
arunr 0:239a6daf84e4 80 if (D >= -48 && D <= 48){
arunr 0:239a6daf84e4 81 motor1_speed = 0;
arunr 0:239a6daf84e4 82 }
arunr 0:239a6daf84e4 83 else if (D > 60){
arunr 0:239a6daf84e4 84 move_motor1_cw();
arunr 0:239a6daf84e4 85 }
arunr 0:239a6daf84e4 86 else if (D < -60){
arunr 0:239a6daf84e4 87 move_motor1_ccw();
arunr 0:239a6daf84e4 88 }
arunr 0:239a6daf84e4 89 }
arunr 0:239a6daf84e4 90
arunr 0:239a6daf84e4 91 void move_motor1(){
arunr 0:239a6daf84e4 92 P = motor1.getPosition();
arunr 0:239a6daf84e4 93 D = (P - H);
arunr 0:239a6daf84e4 94
arunr 0:239a6daf84e4 95 if (button_1 == pressed) {
arunr 0:239a6daf84e4 96 move_motor1_cw ();
arunr 0:239a6daf84e4 97 }
arunr 0:239a6daf84e4 98 else if (button_2 == pressed) {
arunr 0:239a6daf84e4 99 move_motor1_ccw ();
arunr 0:239a6daf84e4 100 }
arunr 0:239a6daf84e4 101 else {
arunr 0:239a6daf84e4 102 movetohome();
arunr 0:239a6daf84e4 103 }
arunr 0:239a6daf84e4 104 }
arunr 0:239a6daf84e4 105
arunr 0:239a6daf84e4 106
arunr 0:239a6daf84e4 107 void read_encoder1 () // aflezen van encoder via hidscope??
arunr 0:239a6daf84e4 108 {
arunr 0:239a6daf84e4 109 scope.set(0,motor1.getPosition());
arunr 0:239a6daf84e4 110 led.write(motor1.getPosition()/100.0);
arunr 0:239a6daf84e4 111 scope.send();
arunr 0:239a6daf84e4 112 wait(0.2f);
arunr 0:239a6daf84e4 113 }
arunr 0:239a6daf84e4 114
arunr 0:239a6daf84e4 115 int main()
arunr 0:239a6daf84e4 116 {
arunr 0:239a6daf84e4 117
arunr 0:239a6daf84e4 118 controller.attach_us(&motor1_controlP, 1e5);
arunr 0:239a6daf84e4 119 sethome();
arunr 0:239a6daf84e4 120 while (true) {
arunr 0:239a6daf84e4 121 read_encoder1();
arunr 0:239a6daf84e4 122 move_motor1();
arunr 0:239a6daf84e4 123 }
arunr 0:239a6daf84e4 124 }