Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

main.cpp@2:821ae727bf8c, 2017-10-31 (annotated)

Committer:
JornJan
Date:
Tue Oct 31 12:16:16 2017 +0000
Revision:
2:821ae727bf8c
Parent:
1:b6a079ca16e0
Child:
3:329acce2487c
niet werkend  ochtend 31-10;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:4c07bb5a9f9f 1 #include "mbed.h"
JornJan 0:4c07bb5a9f9f 2 #include "Serial.h"
JornJan 0:4c07bb5a9f9f 3 #include "math.h"
JornJan 0:4c07bb5a9f9f 4 #include "QEI.h"
JornJan 0:4c07bb5a9f9f 5
JornJan 0:4c07bb5a9f9f 6 // Connecties
JornJan 0:4c07bb5a9f9f 7 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 2:821ae727bf8c 8 DigitalOut led_g(LED_GREEN); //Groene led op k64f bord
JornJan 0:4c07bb5a9f9f 9 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:4c07bb5a9f9f 10 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 2:821ae727bf8c 11 DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord)
JornJan 2:821ae727bf8c 12 PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord)
JornJan 0:4c07bb5a9f9f 13 QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
JornJan 0:4c07bb5a9f9f 14 QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
JornJan 0:4c07bb5a9f9f 15 const double pi = 3.1415926535897; // waarde voor pi aanmaken
JornJan 0:4c07bb5a9f9f 16
JornJan 0:4c07bb5a9f9f 17 // globale gegevens
JornJan 0:4c07bb5a9f9f 18 Ticker tick_sample; // ticker voor aanroepen aansturing
JornJan 2:821ae727bf8c 19 Ticker tick_wasd; //ticker voor toetsenbord aansturing
JornJan 0:4c07bb5a9f9f 20 char key;
JornJan 0:4c07bb5a9f9f 21 double ts=0.001;
JornJan 0:4c07bb5a9f9f 22 int q1_puls;
JornJan 0:4c07bb5a9f9f 23 int q2_puls;
JornJan 2:821ae727bf8c 24 int n = 0;
JornJan 0:4c07bb5a9f9f 25
JornJan 0:4c07bb5a9f9f 26 // kinematica gegevens
JornJan 0:4c07bb5a9f9f 27 // lengte armen
JornJan 0:4c07bb5a9f9f 28 double L1 = 0.250;
JornJan 0:4c07bb5a9f9f 29 double L2 = 0.355;
JornJan 0:4c07bb5a9f9f 30 double L3 = 0.150;
JornJan 0:4c07bb5a9f9f 31
JornJan 0:4c07bb5a9f9f 32 // reference position
JornJan 0:4c07bb5a9f9f 33 double q1=0; // positie q2 in radialen
JornJan 0:4c07bb5a9f9f 34 double q2=0; // positie q2 in radialen
JornJan 0:4c07bb5a9f9f 35 double q1_pos;
JornJan 0:4c07bb5a9f9f 36 double q2_pos;
JornJan 0:4c07bb5a9f9f 37
JornJan 0:4c07bb5a9f9f 38 // EMG Input_k
JornJan 0:4c07bb5a9f9f 39 double vx = 0;
JornJan 0:4c07bb5a9f9f 40 double vy = 0;
JornJan 0:4c07bb5a9f9f 41
JornJan 0:4c07bb5a9f9f 42
JornJan 0:4c07bb5a9f9f 43 // PID gegevens
JornJan 0:4c07bb5a9f9f 44 double pulses2rad;
JornJan 0:4c07bb5a9f9f 45 double position;
JornJan 0:4c07bb5a9f9f 46 double ref1;
JornJan 0:4c07bb5a9f9f 47 double ref2;
JornJan 0:4c07bb5a9f9f 48 double PD1;
JornJan 0:4c07bb5a9f9f 49 double PD2;
JornJan 2:821ae727bf8c 50 double error1=0;
JornJan 2:821ae727bf8c 51 double error1m=0;
JornJan 2:821ae727bf8c 52 double error2=0;
JornJan 2:821ae727bf8c 53 double error2m=0;
JornJan 0:4c07bb5a9f9f 54
JornJan 0:4c07bb5a9f9f 55 double Kp; // proportional coefficient (
JornJan 0:4c07bb5a9f9f 56 double Kd; // differential coefficient
JornJan 0:4c07bb5a9f9f 57
JornJan 0:4c07bb5a9f9f 58
JornJan 2:821ae727bf8c 59 void wasd()
JornJan 0:4c07bb5a9f9f 60 {
JornJan 2:821ae727bf8c 61 static char oldkey = 'p';
JornJan 2:821ae727bf8c 62 static double oldvx = 0;
JornJan 2:821ae727bf8c 63 static double oldvy = 0;
JornJan 2:821ae727bf8c 64
JornJan 0:4c07bb5a9f9f 65 if(pc.readable()==true)
JornJan 0:4c07bb5a9f9f 66 { key = pc.getc();
JornJan 2:821ae727bf8c 67
JornJan 0:4c07bb5a9f9f 68 if (key=='a')
JornJan 0:4c07bb5a9f9f 69 {
JornJan 1:b6a079ca16e0 70 vx = 0.04; //referentie snelheid m/s
JornJan 0:4c07bb5a9f9f 71 vy = 0;
JornJan 0:4c07bb5a9f9f 72 }
JornJan 0:4c07bb5a9f9f 73 else if(key=='d')
JornJan 0:4c07bb5a9f9f 74 {
JornJan 1:b6a079ca16e0 75 vx = -0.04;
JornJan 0:4c07bb5a9f9f 76 vy = 0;
JornJan 0:4c07bb5a9f9f 77 }
JornJan 0:4c07bb5a9f9f 78 else if(key=='w')
JornJan 0:4c07bb5a9f9f 79 {
JornJan 0:4c07bb5a9f9f 80 vx = 0;
JornJan 1:b6a079ca16e0 81 vy = 0.04;
JornJan 0:4c07bb5a9f9f 82 }
JornJan 0:4c07bb5a9f9f 83 else if(key=='s')
JornJan 0:4c07bb5a9f9f 84 {
JornJan 0:4c07bb5a9f9f 85 vx = 0;
JornJan 1:b6a079ca16e0 86 vy = -0.04;
JornJan 0:4c07bb5a9f9f 87 }
JornJan 2:821ae727bf8c 88 else
JornJan 0:4c07bb5a9f9f 89 {
JornJan 2:821ae727bf8c 90 key = oldkey;
JornJan 2:821ae727bf8c 91 vx = oldvx;
JornJan 2:821ae727bf8c 92 vy = oldvy;
JornJan 0:4c07bb5a9f9f 93 }
JornJan 2:821ae727bf8c 94
JornJan 2:821ae727bf8c 95 } //einde eerste if statemnet
JornJan 2:821ae727bf8c 96
JornJan 2:821ae727bf8c 97 else if (pc.readable()==false)
JornJan 0:4c07bb5a9f9f 98 {
JornJan 0:4c07bb5a9f9f 99 vx=0;
JornJan 2:821ae727bf8c 100 vy=0;
JornJan 2:821ae727bf8c 101 key='p';
JornJan 0:4c07bb5a9f9f 102 }
JornJan 2:821ae727bf8c 103 oldkey = key;
JornJan 2:821ae727bf8c 104 oldvx = vx;
JornJan 2:821ae727bf8c 105 oldvy = vy;
JornJan 2:821ae727bf8c 106
JornJan 0:4c07bb5a9f9f 107 }
JornJan 0:4c07bb5a9f9f 108
JornJan 2:821ae727bf8c 109 void kinematics()
JornJan 0:4c07bb5a9f9f 110 {
JornJan 0:4c07bb5a9f9f 111
JornJan 2:821ae727bf8c 112 q1 = ((-(L3 + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vx + ((L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q1_pos;
JornJan 2:821ae727bf8c 113 q2 = (((L3 - L1*sin(q1) + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1))*vx) + ((L1*cos(q1) - L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q2_pos;
JornJan 0:4c07bb5a9f9f 114 }
JornJan 0:4c07bb5a9f9f 115
JornJan 2:821ae727bf8c 116 void controller(double q1ref, double q2ref)
JornJan 0:4c07bb5a9f9f 117 {
JornJan 0:4c07bb5a9f9f 118 // error = qset
JornJan 0:4c07bb5a9f9f 119 // referentie bepalen
JornJan 0:4c07bb5a9f9f 120
JornJan 2:821ae727bf8c 121 error1 = q1ref - q1_pos;
JornJan 2:821ae727bf8c 122 error2 = q2ref - q2_pos;
JornJan 0:4c07bb5a9f9f 123
JornJan 0:4c07bb5a9f9f 124 //PD
JornJan 2:821ae727bf8c 125 PD1 = Kp*(error1) + Kd*((error1m-error1)/ts) ; // PD sum; PD = proportianal + differential
JornJan 2:821ae727bf8c 126 error1m = error1; // waarde voor qset wordt opgeslagen (m = memory)
JornJan 2:821ae727bf8c 127 PD2 = Kp*(error2) + Kd*((error2m-error2)/ts) ;
JornJan 2:821ae727bf8c 128 error2m = error2;
JornJan 0:4c07bb5a9f9f 129
JornJan 0:4c07bb5a9f9f 130 //Motor control
JornJan 0:4c07bb5a9f9f 131 if (PD1<0 && PD2<0 )
JornJan 0:4c07bb5a9f9f 132 {
JornJan 0:4c07bb5a9f9f 133 motor1MagnitudePin = fabs(PD1);
JornJan 2:821ae727bf8c 134 motor1DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 135 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 136 motor2DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 137 }
JornJan 0:4c07bb5a9f9f 138 else if (PD1>0 && PD2<0)
JornJan 0:4c07bb5a9f9f 139 {
JornJan 0:4c07bb5a9f9f 140 motor1MagnitudePin = fabs(PD1);
JornJan 2:821ae727bf8c 141 motor1DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 142 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 143 motor2DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 144 }
JornJan 0:4c07bb5a9f9f 145
JornJan 0:4c07bb5a9f9f 146 else if (PD1<0 && PD2>0)
JornJan 0:4c07bb5a9f9f 147 {
JornJan 0:4c07bb5a9f9f 148 motor1MagnitudePin = fabs(PD1);
JornJan 2:821ae727bf8c 149 motor1DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 150 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 151 motor2DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 152 }
JornJan 0:4c07bb5a9f9f 153
JornJan 0:4c07bb5a9f9f 154 else if (PD1>0 && PD2>0)
JornJan 0:4c07bb5a9f9f 155 {
JornJan 0:4c07bb5a9f9f 156 motor1MagnitudePin = fabs(PD1);
JornJan 2:821ae727bf8c 157 motor1DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 158 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 159 motor2DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 160 }
JornJan 0:4c07bb5a9f9f 161
JornJan 0:4c07bb5a9f9f 162
JornJan 0:4c07bb5a9f9f 163
JornJan 0:4c07bb5a9f9f 164 }
JornJan 0:4c07bb5a9f9f 165
JornJan 0:4c07bb5a9f9f 166 void aansturing()
JornJan 0:4c07bb5a9f9f 167 {
JornJan 0:4c07bb5a9f9f 168 // encoders uitlezen
JornJan 0:4c07bb5a9f9f 169 q1_puls = q1_enc.getPulses();
JornJan 0:4c07bb5a9f9f 170 q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
JornJan 0:4c07bb5a9f9f 171 q2_puls = q2_enc.getPulses();
JornJan 0:4c07bb5a9f9f 172 q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
JornJan 2:821ae727bf8c 173
JornJan 0:4c07bb5a9f9f 174 // kinematica bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
JornJan 2:821ae727bf8c 175 kinematics();
JornJan 2:821ae727bf8c 176
JornJan 0:4c07bb5a9f9f 177 // PD controller gebruikt PD control en stuurt motor aan
JornJan 0:4c07bb5a9f9f 178 controller(q1, q2);
JornJan 2:821ae727bf8c 179
JornJan 2:821ae727bf8c 180 if(n==500){
JornJan 2:821ae727bf8c 181 printf("motor1 = %f motor2 = %f\n\r", PD1, PD2);
JornJan 2:821ae727bf8c 182 n=0;
JornJan 2:821ae727bf8c 183 }
JornJan 2:821ae727bf8c 184 else{
JornJan 2:821ae727bf8c 185 n=n+1;
JornJan 2:821ae727bf8c 186 }
JornJan 0:4c07bb5a9f9f 187 }
JornJan 0:4c07bb5a9f9f 188
JornJan 0:4c07bb5a9f9f 189
JornJan 0:4c07bb5a9f9f 190 int main()
JornJan 0:4c07bb5a9f9f 191 {
JornJan 0:4c07bb5a9f9f 192 pc.baud(115200);
JornJan 0:4c07bb5a9f9f 193 pulses2rad=(2*pi)/4200;
JornJan 2:821ae727bf8c 194 Kp = 2.5;
JornJan 2:821ae727bf8c 195 Kd = 0.2*Kp;
JornJan 2:821ae727bf8c 196 tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
JornJan 2:821ae727bf8c 197 tick_wasd.attach(&wasd, 0.1);
JornJan 2:821ae727bf8c 198 led_g = 0;
JornJan 2:821ae727bf8c 199
JornJan 0:4c07bb5a9f9f 200 while(true)
JornJan 0:4c07bb5a9f9f 201 {
JornJan 2:821ae727bf8c 202
JornJan 0:4c07bb5a9f9f 203 }
JornJan 0:4c07bb5a9f9f 204
JornJan 0:4c07bb5a9f9f 205
JornJan 0:4c07bb5a9f9f 206 }