Arun Raveenthiran / Mbed 2 deprecated home_position_control

Home position met PI control

Dependencies:   Encoder HIDScope mbed

main.cpp@1:d5e81fb97eca, 2015-10-21 (annotated)

Committer:
arunr
Date:
Wed Oct 21 11:49:33 2015 +0000
Revision:
1:d5e81fb97eca
Parent:
0:239a6daf84e4 
home position werkt nauwkeurig;

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 1:d5e81fb97eca 68 motor1_speed = 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 1:d5e81fb97eca 73 motor1_speed = 1;
arunr 0:239a6daf84e4 74 }
arunr 0:239a6daf84e4 75
arunr 0:239a6daf84e4 76 void movetohome(){
arunr 0:239a6daf84e4 77 P = motor1.getPosition();
arunr 1:d5e81fb97eca 78
arunr 1:d5e81fb97eca 79 if (P >= -28 && P <= 28){
arunr 0:239a6daf84e4 80 motor1_speed = 0;
arunr 0:239a6daf84e4 81 }
arunr 1:d5e81fb97eca 82 else if (P > 24){
arunr 1:d5e81fb97eca 83 motor1_direction = 1;
arunr 1:d5e81fb97eca 84 motor1_speed = 0.1;
arunr 0:239a6daf84e4 85 }
arunr 1:d5e81fb97eca 86 else if (P < -24){
arunr 1:d5e81fb97eca 87 motor1_direction = 0;
arunr 1:d5e81fb97eca 88 motor1_speed = 0.1;
arunr 0:239a6daf84e4 89 }
arunr 0:239a6daf84e4 90 }
arunr 0:239a6daf84e4 91
arunr 1:d5e81fb97eca 92
arunr 0:239a6daf84e4 93 void move_motor1(){
arunr 0:239a6daf84e4 94 P = motor1.getPosition();
arunr 0:239a6daf84e4 95 D = (P - H);
arunr 0:239a6daf84e4 96
arunr 0:239a6daf84e4 97 if (button_1 == pressed) {
arunr 0:239a6daf84e4 98 move_motor1_cw ();
arunr 0:239a6daf84e4 99 }
arunr 0:239a6daf84e4 100 else if (button_2 == pressed) {
arunr 0:239a6daf84e4 101 move_motor1_ccw ();
arunr 0:239a6daf84e4 102 }
arunr 0:239a6daf84e4 103 else {
arunr 0:239a6daf84e4 104 movetohome();
arunr 0:239a6daf84e4 105 }
arunr 0:239a6daf84e4 106 }
arunr 0:239a6daf84e4 107
arunr 0:239a6daf84e4 108
arunr 0:239a6daf84e4 109 void read_encoder1 () // aflezen van encoder via hidscope??
arunr 0:239a6daf84e4 110 {
arunr 0:239a6daf84e4 111 scope.set(0,motor1.getPosition());
arunr 0:239a6daf84e4 112 led.write(motor1.getPosition()/100.0);
arunr 0:239a6daf84e4 113 scope.send();
arunr 0:239a6daf84e4 114 wait(0.2f);
arunr 0:239a6daf84e4 115 }
arunr 0:239a6daf84e4 116
arunr 0:239a6daf84e4 117 int main()
arunr 0:239a6daf84e4 118 {
arunr 0:239a6daf84e4 119
arunr 0:239a6daf84e4 120 controller.attach_us(&motor1_controlP, 1e5);
arunr 0:239a6daf84e4 121 sethome();
arunr 0:239a6daf84e4 122 while (true) {
arunr 0:239a6daf84e4 123 read_encoder1();
arunr 0:239a6daf84e4 124 move_motor1();
arunr 0:239a6daf84e4 125 }
arunr 0:239a6daf84e4 126 }