Rianne Bulthuis / Mbed 2 deprecated home_position_1

homeposition definieren

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of home_position by Rianne Bulthuis

main.cpp@3:5f59cbe53d7d, 2015-10-20 (annotated)

Committer:
riannebulthuis
Date:
Tue Oct 20 13:15:35 2015 +0000
Revision:
3:5f59cbe53d7d
Parent:
2:866a8a9f2b93 
motor stopt op gewenste positie. Echter blijven de regels op de putty nog wel doorlopen (pc.printf)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arunr 0:65ab9f79a4cc 1 #include "mbed.h"
arunr 0:65ab9f79a4cc 2 #include "encoder.h"
arunr 0:65ab9f79a4cc 3 #include "HIDScope.h"
riannebulthuis 1:7d5e6bc2b314 4 #include "QEI.h"
riannebulthuis 1:7d5e6bc2b314 5 #include "MODSERIAL.h"
arunr 0:65ab9f79a4cc 6
arunr 0:65ab9f79a4cc 7 DigitalOut motor1_direction(D4);
arunr 0:65ab9f79a4cc 8 PwmOut motor1_speed(D5);
arunr 0:65ab9f79a4cc 9 PwmOut led(D9);
arunr 0:65ab9f79a4cc 10 DigitalIn button_1(PTC6); //counterclockwise
arunr 0:65ab9f79a4cc 11 DigitalIn button_2(PTA4); //clockwise
arunr 0:65ab9f79a4cc 12 Encoder motor1(D12,D13);
arunr 0:65ab9f79a4cc 13 HIDScope scope(1);
riannebulthuis 3:5f59cbe53d7d 14 AnalogIn PotMeter1(A1);
riannebulthuis 3:5f59cbe53d7d 15 Ticker controller;
riannebulthuis 3:5f59cbe53d7d 16 Ticker ticker_regelaar;
arunr 0:65ab9f79a4cc 17
riannebulthuis 2:866a8a9f2b93 18 MODSERIAL pc(USBTX, USBRX);
riannebulthuis 2:866a8a9f2b93 19 volatile bool regelaar_ticker_flag;
riannebulthuis 2:866a8a9f2b93 20
riannebulthuis 2:866a8a9f2b93 21 void setregelaar_ticker_flag()
riannebulthuis 2:866a8a9f2b93 22 {
riannebulthuis 2:866a8a9f2b93 23 regelaar_ticker_flag = true;
riannebulthuis 2:866a8a9f2b93 24 }
riannebulthuis 2:866a8a9f2b93 25
riannebulthuis 2:866a8a9f2b93 26 #define SAMPLETIME_REGELAAR 0.005
riannebulthuis 2:866a8a9f2b93 27
riannebulthuis 2:866a8a9f2b93 28 //define states
riannebulthuis 3:5f59cbe53d7d 29 enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
riannebulthuis 2:866a8a9f2b93 30 uint8_t state = GOTOHOMEPOSITION;
arunr 0:65ab9f79a4cc 31
riannebulthuis 3:5f59cbe53d7d 32 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
riannebulthuis 3:5f59cbe53d7d 33 const double g = 360; // Aantal graden 1 rotatie
riannebulthuis 3:5f59cbe53d7d 34 const double c = 4200; // Aantal counts 1 rotatie
riannebulthuis 3:5f59cbe53d7d 35 const double q = c/(g);
riannebulthuis 3:5f59cbe53d7d 36
riannebulthuis 3:5f59cbe53d7d 37 //PI-controller constante
riannebulthuis 3:5f59cbe53d7d 38 const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
riannebulthuis 3:5f59cbe53d7d 39 const double motor1_Ki = 0.002; // Integrating gain m1.
riannebulthuis 3:5f59cbe53d7d 40 const double motor1_Ts = 0.01; // Time step m1
riannebulthuis 3:5f59cbe53d7d 41 double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
riannebulthuis 3:5f59cbe53d7d 42
riannebulthuis 3:5f59cbe53d7d 43 // Reusable P controller
riannebulthuis 3:5f59cbe53d7d 44 double Pc (double error, const double Kp)
riannebulthuis 3:5f59cbe53d7d 45 {
riannebulthuis 3:5f59cbe53d7d 46 return motor1_Kp * error;
riannebulthuis 3:5f59cbe53d7d 47 }
riannebulthuis 3:5f59cbe53d7d 48
riannebulthuis 3:5f59cbe53d7d 49 // Measure the error and apply output to the plant
riannebulthuis 3:5f59cbe53d7d 50 void motor1_controlP()
riannebulthuis 3:5f59cbe53d7d 51 {
riannebulthuis 3:5f59cbe53d7d 52 double referenceP1 = PotMeter1.read();
riannebulthuis 3:5f59cbe53d7d 53 double positionP1 = q*motor1.getPosition();
riannebulthuis 3:5f59cbe53d7d 54 double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
riannebulthuis 3:5f59cbe53d7d 55 }
riannebulthuis 3:5f59cbe53d7d 56
riannebulthuis 3:5f59cbe53d7d 57 // Reusable PI controller
riannebulthuis 3:5f59cbe53d7d 58 double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
riannebulthuis 3:5f59cbe53d7d 59 {
riannebulthuis 3:5f59cbe53d7d 60 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
riannebulthuis 3:5f59cbe53d7d 61 return motor1_Kp*error + motor1_Ki*err_int;
riannebulthuis 3:5f59cbe53d7d 62 } // De totale fout die wordt hersteld met behulp van PI control.
riannebulthuis 3:5f59cbe53d7d 63
riannebulthuis 3:5f59cbe53d7d 64 void motor1_controlPI()
riannebulthuis 3:5f59cbe53d7d 65 {
riannebulthuis 3:5f59cbe53d7d 66 double referencePI1 = PotMeter1.read();
riannebulthuis 3:5f59cbe53d7d 67 double positionPI1 = q*motor1.getPosition();
riannebulthuis 3:5f59cbe53d7d 68 double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
riannebulthuis 3:5f59cbe53d7d 69 }
riannebulthuis 3:5f59cbe53d7d 70
arunr 0:65ab9f79a4cc 71 const int pressed = 0;
arunr 0:65ab9f79a4cc 72
arunr 0:65ab9f79a4cc 73 double H;
arunr 0:65ab9f79a4cc 74 double P;
arunr 0:65ab9f79a4cc 75 double D;
arunr 0:65ab9f79a4cc 76
arunr 0:65ab9f79a4cc 77
arunr 0:65ab9f79a4cc 78 void sethome(){
arunr 0:65ab9f79a4cc 79 H = motor1.getPosition();
arunr 0:65ab9f79a4cc 80 }
arunr 0:65ab9f79a4cc 81
arunr 0:65ab9f79a4cc 82 void move_motor1_ccw (){
arunr 0:65ab9f79a4cc 83 motor1_direction = 0;
arunr 0:65ab9f79a4cc 84 motor1_speed = 0.8;
arunr 0:65ab9f79a4cc 85 }
arunr 0:65ab9f79a4cc 86
arunr 0:65ab9f79a4cc 87 void move_motor1_cw (){
arunr 0:65ab9f79a4cc 88 motor1_direction = 1;
arunr 0:65ab9f79a4cc 89 motor1_speed = 0.8;
arunr 0:65ab9f79a4cc 90 }
arunr 0:65ab9f79a4cc 91
arunr 0:65ab9f79a4cc 92 void movetohome(){
arunr 0:65ab9f79a4cc 93 P = motor1.getPosition();
arunr 0:65ab9f79a4cc 94 D = (P - H);
riannebulthuis 3:5f59cbe53d7d 95
riannebulthuis 3:5f59cbe53d7d 96 if (D >= -36 && D <= 36){
arunr 0:65ab9f79a4cc 97 motor1_speed = 0;
arunr 0:65ab9f79a4cc 98 }
riannebulthuis 3:5f59cbe53d7d 99 else if (D > 24){
riannebulthuis 3:5f59cbe53d7d 100 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 101 motor1_speed = 0.1;
arunr 0:65ab9f79a4cc 102 }
riannebulthuis 3:5f59cbe53d7d 103 else if (D < -24){
riannebulthuis 3:5f59cbe53d7d 104 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 105 motor1_speed = 0.1;
arunr 0:65ab9f79a4cc 106 }
arunr 0:65ab9f79a4cc 107 }
arunr 0:65ab9f79a4cc 108
arunr 0:65ab9f79a4cc 109 void move_motor1()
arunr 0:65ab9f79a4cc 110 {
arunr 0:65ab9f79a4cc 111 if (button_1 == pressed) {
arunr 0:65ab9f79a4cc 112 move_motor1_cw ();
riannebulthuis 1:7d5e6bc2b314 113 } else if (button_2 == pressed) {
riannebulthuis 1:7d5e6bc2b314 114 move_motor1_ccw ();
riannebulthuis 1:7d5e6bc2b314 115 } else {
riannebulthuis 1:7d5e6bc2b314 116 motor1_speed = 0;
arunr 0:65ab9f79a4cc 117 }
arunr 0:65ab9f79a4cc 118 }
arunr 0:65ab9f79a4cc 119
arunr 0:65ab9f79a4cc 120 void read_encoder1 () // aflezen van encoder via hidscope??
arunr 0:65ab9f79a4cc 121 {
arunr 0:65ab9f79a4cc 122 scope.set(0,motor1.getPosition());
arunr 0:65ab9f79a4cc 123 led.write(motor1.getPosition()/100.0);
arunr 0:65ab9f79a4cc 124 scope.send();
arunr 0:65ab9f79a4cc 125 wait(0.2f);
arunr 0:65ab9f79a4cc 126 }
arunr 0:65ab9f79a4cc 127
arunr 0:65ab9f79a4cc 128 int main()
riannebulthuis 2:866a8a9f2b93 129 {
arunr 0:65ab9f79a4cc 130 while (true) {
riannebulthuis 1:7d5e6bc2b314 131 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 1:7d5e6bc2b314 132
riannebulthuis 1:7d5e6bc2b314 133 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
riannebulthuis 1:7d5e6bc2b314 134
riannebulthuis 2:866a8a9f2b93 135 case GOTOHOMEPOSITION: //positie op 0 zetten voor arm 1
riannebulthuis 1:7d5e6bc2b314 136 {
riannebulthuis 2:866a8a9f2b93 137 pc.printf("gotohomeposition\n\r");
riannebulthuis 1:7d5e6bc2b314 138 read_encoder1();
riannebulthuis 1:7d5e6bc2b314 139 sethome();
riannebulthuis 2:866a8a9f2b93 140 state = MOVE_MOTOR;
riannebulthuis 1:7d5e6bc2b314 141 break;
riannebulthuis 1:7d5e6bc2b314 142 }
riannebulthuis 1:7d5e6bc2b314 143
riannebulthuis 2:866a8a9f2b93 144 case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons
riannebulthuis 1:7d5e6bc2b314 145 {
riannebulthuis 2:866a8a9f2b93 146 pc.printf("move_motor\n\r");
riannebulthuis 2:866a8a9f2b93 147 while (state == MOVE_MOTOR){
riannebulthuis 2:866a8a9f2b93 148 pc.printf("whiletrue\n\r");
riannebulthuis 2:866a8a9f2b93 149 move_motor1();
riannebulthuis 2:866a8a9f2b93 150 if (button_1 == pressed && button_2 == pressed){
riannebulthuis 2:866a8a9f2b93 151 state = BACKTOHOMEPOSITION;
riannebulthuis 2:866a8a9f2b93 152 }
riannebulthuis 2:866a8a9f2b93 153 }
riannebulthuis 2:866a8a9f2b93 154 wait (1);
riannebulthuis 2:866a8a9f2b93 155 break;
riannebulthuis 1:7d5e6bc2b314 156 }
riannebulthuis 1:7d5e6bc2b314 157
riannebulthuis 2:866a8a9f2b93 158 case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
riannebulthuis 1:7d5e6bc2b314 159 {
riannebulthuis 2:866a8a9f2b93 160 pc.printf("backhomeposition\n\r");
riannebulthuis 2:866a8a9f2b93 161 wait (1);
riannebulthuis 2:866a8a9f2b93 162
riannebulthuis 2:866a8a9f2b93 163 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
riannebulthuis 2:866a8a9f2b93 164
riannebulthuis 2:866a8a9f2b93 165 while(state == BACKTOHOMEPOSITION){
riannebulthuis 3:5f59cbe53d7d 166 movetohome();
riannebulthuis 2:866a8a9f2b93 167 while(regelaar_ticker_flag != true);
riannebulthuis 2:866a8a9f2b93 168 regelaar_ticker_flag = false;
riannebulthuis 2:866a8a9f2b93 169
riannebulthuis 2:866a8a9f2b93 170 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
riannebulthuis 3:5f59cbe53d7d 171 pc.printf("referentie %d\n\r", H);
riannebulthuis 2:866a8a9f2b93 172
riannebulthuis 2:866a8a9f2b93 173 if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
riannebulthuis 3:5f59cbe53d7d 174 motor1.setPosition(0);
riannebulthuis 3:5f59cbe53d7d 175 H = motor1.getPosition();
riannebulthuis 3:5f59cbe53d7d 176 state = STOP;
riannebulthuis 3:5f59cbe53d7d 177 break;
riannebulthuis 2:866a8a9f2b93 178 }
riannebulthuis 3:5f59cbe53d7d 179 }
riannebulthuis 3:5f59cbe53d7d 180 }
riannebulthuis 3:5f59cbe53d7d 181 case STOP:
riannebulthuis 3:5f59cbe53d7d 182 {
riannebulthuis 3:5f59cbe53d7d 183 const int einde = 1;
riannebulthuis 3:5f59cbe53d7d 184 while(state == STOP){
riannebulthuis 3:5f59cbe53d7d 185 motor1_speed = 0;
riannebulthuis 3:5f59cbe53d7d 186 if (einde == 1){
riannebulthuis 3:5f59cbe53d7d 187 pc.printf("homepositie %d\n\r", H);
riannebulthuis 3:5f59cbe53d7d 188 pc.printf("huidige positie %d\n\r", P);
riannebulthuis 3:5f59cbe53d7d 189 pc.printf("einde\n\r");
riannebulthuis 3:5f59cbe53d7d 190
riannebulthuis 3:5f59cbe53d7d 191 }
riannebulthuis 3:5f59cbe53d7d 192 }
riannebulthuis 1:7d5e6bc2b314 193 break;
riannebulthuis 3:5f59cbe53d7d 194 }
riannebulthuis 3:5f59cbe53d7d 195 }
riannebulthuis 2:866a8a9f2b93 196 }
arunr 0:65ab9f79a4cc 197 }