Rianne Bulthuis / Mbed 2 deprecated Cases_homepos_picontrol_EMG_2

EMG

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Cases_homepos_picontrol_EMG by Arun Raveenthiran

main.cpp@4:b4530fb376dd, 2015-10-21 (annotated)

Committer:
arunr
Date:
Wed Oct 21 11:50:15 2015 +0000
Revision:
4:b4530fb376dd
Parent:
3:5f59cbe53d7d
Child:
5:b9d5d7311dac
home position werkt nauwkeurig, cases werken ook. Motor draait nu niet meer door wanneer 2 knoppen worden ingedrukt.

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
arunr 4:b4530fb376dd 29 enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
arunr 4:b4530fb376dd 30 uint8_t state = HOME;
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 4:b4530fb376dd 78 void gethome(){
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 4:b4530fb376dd 84 motor1_speed = 1;
arunr 0:65ab9f79a4cc 85 }
arunr 0:65ab9f79a4cc 86
arunr 0:65ab9f79a4cc 87 void move_motor1_cw (){
arunr 0:65ab9f79a4cc 88 motor1_direction = 1;
arunr 4:b4530fb376dd 89 motor1_speed = 1;
arunr 0:65ab9f79a4cc 90 }
arunr 0:65ab9f79a4cc 91
arunr 0:65ab9f79a4cc 92 void movetohome(){
arunr 0:65ab9f79a4cc 93 P = motor1.getPosition();
arunr 4:b4530fb376dd 94
arunr 4:b4530fb376dd 95 if (P >= -28 && P <= 28){
arunr 0:65ab9f79a4cc 96 motor1_speed = 0;
arunr 0:65ab9f79a4cc 97 }
arunr 4:b4530fb376dd 98 else if (P > 24){
riannebulthuis 3:5f59cbe53d7d 99 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 100 motor1_speed = 0.1;
arunr 0:65ab9f79a4cc 101 }
arunr 4:b4530fb376dd 102 else if (P < -24){
riannebulthuis 3:5f59cbe53d7d 103 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 104 motor1_speed = 0.1;
arunr 0:65ab9f79a4cc 105 }
arunr 0:65ab9f79a4cc 106 }
arunr 0:65ab9f79a4cc 107
arunr 0:65ab9f79a4cc 108 void move_motor1()
arunr 0:65ab9f79a4cc 109 {
arunr 0:65ab9f79a4cc 110 if (button_1 == pressed) {
arunr 0:65ab9f79a4cc 111 move_motor1_cw ();
riannebulthuis 1:7d5e6bc2b314 112 } else if (button_2 == pressed) {
riannebulthuis 1:7d5e6bc2b314 113 move_motor1_ccw ();
riannebulthuis 1:7d5e6bc2b314 114 } else {
riannebulthuis 1:7d5e6bc2b314 115 motor1_speed = 0;
arunr 0:65ab9f79a4cc 116 }
arunr 0:65ab9f79a4cc 117 }
arunr 0:65ab9f79a4cc 118
arunr 0:65ab9f79a4cc 119 void read_encoder1 () // aflezen van encoder via hidscope??
arunr 0:65ab9f79a4cc 120 {
arunr 0:65ab9f79a4cc 121 scope.set(0,motor1.getPosition());
arunr 0:65ab9f79a4cc 122 led.write(motor1.getPosition()/100.0);
arunr 0:65ab9f79a4cc 123 scope.send();
arunr 0:65ab9f79a4cc 124 wait(0.2f);
arunr 0:65ab9f79a4cc 125 }
arunr 0:65ab9f79a4cc 126
arunr 4:b4530fb376dd 127 void print_position(){
arunr 4:b4530fb376dd 128 pc.printf("move motor \n\r");
arunr 4:b4530fb376dd 129 wait(0.2f);
arunr 4:b4530fb376dd 130 }
arunr 0:65ab9f79a4cc 131 int main()
riannebulthuis 2:866a8a9f2b93 132 {
arunr 0:65ab9f79a4cc 133 while (true) {
riannebulthuis 1:7d5e6bc2b314 134 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 1:7d5e6bc2b314 135
riannebulthuis 1:7d5e6bc2b314 136 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
riannebulthuis 1:7d5e6bc2b314 137
arunr 4:b4530fb376dd 138 case HOME: //positie op 0 zetten voor arm 1
riannebulthuis 1:7d5e6bc2b314 139 {
arunr 4:b4530fb376dd 140 pc.printf("home\n\r");
riannebulthuis 1:7d5e6bc2b314 141 read_encoder1();
arunr 4:b4530fb376dd 142 gethome();
arunr 4:b4530fb376dd 143 pc.printf("Home-position is %f\n\r", H);
riannebulthuis 2:866a8a9f2b93 144 state = MOVE_MOTOR;
arunr 4:b4530fb376dd 145 wait(5);
riannebulthuis 1:7d5e6bc2b314 146 break;
riannebulthuis 1:7d5e6bc2b314 147 }
riannebulthuis 1:7d5e6bc2b314 148
riannebulthuis 2:866a8a9f2b93 149 case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons
riannebulthuis 1:7d5e6bc2b314 150 {
riannebulthuis 2:866a8a9f2b93 151 pc.printf("move_motor\n\r");
riannebulthuis 2:866a8a9f2b93 152 while (state == MOVE_MOTOR){
riannebulthuis 2:866a8a9f2b93 153 move_motor1();
arunr 4:b4530fb376dd 154 print_position();
riannebulthuis 2:866a8a9f2b93 155 if (button_1 == pressed && button_2 == pressed){
riannebulthuis 2:866a8a9f2b93 156 state = BACKTOHOMEPOSITION;
riannebulthuis 2:866a8a9f2b93 157 }
riannebulthuis 2:866a8a9f2b93 158 }
riannebulthuis 2:866a8a9f2b93 159 wait (1);
riannebulthuis 2:866a8a9f2b93 160 break;
riannebulthuis 1:7d5e6bc2b314 161 }
riannebulthuis 1:7d5e6bc2b314 162
riannebulthuis 2:866a8a9f2b93 163 case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
riannebulthuis 1:7d5e6bc2b314 164 {
riannebulthuis 2:866a8a9f2b93 165 pc.printf("backhomeposition\n\r");
riannebulthuis 2:866a8a9f2b93 166 wait (1);
riannebulthuis 2:866a8a9f2b93 167
riannebulthuis 2:866a8a9f2b93 168 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
riannebulthuis 2:866a8a9f2b93 169
riannebulthuis 2:866a8a9f2b93 170 while(state == BACKTOHOMEPOSITION){
riannebulthuis 3:5f59cbe53d7d 171 movetohome();
riannebulthuis 2:866a8a9f2b93 172 while(regelaar_ticker_flag != true);
riannebulthuis 2:866a8a9f2b93 173 regelaar_ticker_flag = false;
riannebulthuis 2:866a8a9f2b93 174
riannebulthuis 2:866a8a9f2b93 175 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
arunr 4:b4530fb376dd 176 pc.printf("referentie %f\n\r", H);
riannebulthuis 2:866a8a9f2b93 177
arunr 4:b4530fb376dd 178 if (P <=24 && P >= -24){
arunr 4:b4530fb376dd 179 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
arunr 4:b4530fb376dd 180 pc.printf("referentie %f\n\r", H);
riannebulthuis 3:5f59cbe53d7d 181 state = STOP;
arunr 4:b4530fb376dd 182 pc.printf("Laatste positie %f\n\r", motor1.getPosition());
riannebulthuis 3:5f59cbe53d7d 183 break;
riannebulthuis 2:866a8a9f2b93 184 }
riannebulthuis 3:5f59cbe53d7d 185 }
riannebulthuis 3:5f59cbe53d7d 186 }
riannebulthuis 3:5f59cbe53d7d 187 case STOP:
riannebulthuis 3:5f59cbe53d7d 188 {
arunr 4:b4530fb376dd 189 static int c;
riannebulthuis 3:5f59cbe53d7d 190 while(state == STOP){
riannebulthuis 3:5f59cbe53d7d 191 motor1_speed = 0;
arunr 4:b4530fb376dd 192 if (c++ == 0){
riannebulthuis 3:5f59cbe53d7d 193 pc.printf("einde\n\r");
riannebulthuis 3:5f59cbe53d7d 194 }
riannebulthuis 3:5f59cbe53d7d 195 }
riannebulthuis 1:7d5e6bc2b314 196 break;
riannebulthuis 3:5f59cbe53d7d 197 }
riannebulthuis 3:5f59cbe53d7d 198 }
riannebulthuis 2:866a8a9f2b93 199 }
arunr 0:65ab9f79a4cc 200 }