Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

main.cpp@0:4c07bb5a9f9f, 2017-10-27 (annotated)

Committer:
JornJan
Date:
Fri Oct 27 11:42:57 2017 +0000
Revision:
0:4c07bb5a9f9f
Child:
1:b6a079ca16e0
First try ;

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 0:4c07bb5a9f9f 8 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:4c07bb5a9f9f 9 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 0:4c07bb5a9f9f 10 DigitalOut motor2DirectionPin(D6); //Motorrichting op D4 (connected op het bord)
JornJan 0:4c07bb5a9f9f 11 PwmOut motor2MagnitudePin(D7); //Motorkracht op D5 (connected op het bord)
JornJan 0:4c07bb5a9f9f 12 QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
JornJan 0:4c07bb5a9f9f 13 QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
JornJan 0:4c07bb5a9f9f 14 const double pi = 3.1415926535897; // waarde voor pi aanmaken
JornJan 0:4c07bb5a9f9f 15
JornJan 0:4c07bb5a9f9f 16 // globale gegevens
JornJan 0:4c07bb5a9f9f 17 Ticker tick_sample; // ticker voor aanroepen aansturing
JornJan 0:4c07bb5a9f9f 18 char key;
JornJan 0:4c07bb5a9f9f 19 double ts=0.001;
JornJan 0:4c07bb5a9f9f 20 int q1_puls;
JornJan 0:4c07bb5a9f9f 21 int q2_puls;
JornJan 0:4c07bb5a9f9f 22
JornJan 0:4c07bb5a9f9f 23 // kinematica gegevens
JornJan 0:4c07bb5a9f9f 24 // lengte armen
JornJan 0:4c07bb5a9f9f 25 double L1 = 0.250;
JornJan 0:4c07bb5a9f9f 26 double L2 = 0.355;
JornJan 0:4c07bb5a9f9f 27 double L3 = 0.150;
JornJan 0:4c07bb5a9f9f 28
JornJan 0:4c07bb5a9f9f 29 // reference position
JornJan 0:4c07bb5a9f9f 30 double q1=0; // positie q2 in radialen
JornJan 0:4c07bb5a9f9f 31 double q2=0; // positie q2 in radialen
JornJan 0:4c07bb5a9f9f 32 double q1_pos;
JornJan 0:4c07bb5a9f9f 33 double q2_pos;
JornJan 0:4c07bb5a9f9f 34
JornJan 0:4c07bb5a9f9f 35 // EMG Input_k
JornJan 0:4c07bb5a9f9f 36 double vx = 0;
JornJan 0:4c07bb5a9f9f 37 double vy = 0;
JornJan 0:4c07bb5a9f9f 38
JornJan 0:4c07bb5a9f9f 39
JornJan 0:4c07bb5a9f9f 40 // PID gegevens
JornJan 0:4c07bb5a9f9f 41 double pulses2rad;
JornJan 0:4c07bb5a9f9f 42 double position;
JornJan 0:4c07bb5a9f9f 43 double ref1;
JornJan 0:4c07bb5a9f9f 44 double ref2;
JornJan 0:4c07bb5a9f9f 45 double PD1;
JornJan 0:4c07bb5a9f9f 46 double PD2;
JornJan 0:4c07bb5a9f9f 47 double qset1=0;
JornJan 0:4c07bb5a9f9f 48 double qset1m=0;
JornJan 0:4c07bb5a9f9f 49 double qset2=0;
JornJan 0:4c07bb5a9f9f 50 double qset2m=0;
JornJan 0:4c07bb5a9f9f 51
JornJan 0:4c07bb5a9f9f 52 double Kp; // proportional coefficient (
JornJan 0:4c07bb5a9f9f 53 double Kd; // differential coefficient
JornJan 0:4c07bb5a9f9f 54
JornJan 0:4c07bb5a9f9f 55
JornJan 0:4c07bb5a9f9f 56 void wasd(double& vx, double& vy)
JornJan 0:4c07bb5a9f9f 57 {
JornJan 0:4c07bb5a9f9f 58 if(pc.readable()==true)
JornJan 0:4c07bb5a9f9f 59 { key = pc.getc();
JornJan 0:4c07bb5a9f9f 60 if (key=='a')
JornJan 0:4c07bb5a9f9f 61 {
JornJan 0:4c07bb5a9f9f 62 vx = 0.02; //referentie snelheid m/s
JornJan 0:4c07bb5a9f9f 63 vy = 0;
JornJan 0:4c07bb5a9f9f 64 }
JornJan 0:4c07bb5a9f9f 65 else if(key=='d')
JornJan 0:4c07bb5a9f9f 66 {
JornJan 0:4c07bb5a9f9f 67 vx = -0.02;
JornJan 0:4c07bb5a9f9f 68 vy = 0;
JornJan 0:4c07bb5a9f9f 69 }
JornJan 0:4c07bb5a9f9f 70 else if(key=='w')
JornJan 0:4c07bb5a9f9f 71 {
JornJan 0:4c07bb5a9f9f 72 vx = 0;
JornJan 0:4c07bb5a9f9f 73 vy = 0.02;
JornJan 0:4c07bb5a9f9f 74 }
JornJan 0:4c07bb5a9f9f 75 else if(key=='s')
JornJan 0:4c07bb5a9f9f 76 {
JornJan 0:4c07bb5a9f9f 77 vx = 0;
JornJan 0:4c07bb5a9f9f 78 vy = -0.02;
JornJan 0:4c07bb5a9f9f 79 }
JornJan 0:4c07bb5a9f9f 80 else
JornJan 0:4c07bb5a9f9f 81 {
JornJan 0:4c07bb5a9f9f 82 vx=0;
JornJan 0:4c07bb5a9f9f 83 vy=0;
JornJan 0:4c07bb5a9f9f 84 }
JornJan 0:4c07bb5a9f9f 85 }
JornJan 0:4c07bb5a9f9f 86 else
JornJan 0:4c07bb5a9f9f 87 {
JornJan 0:4c07bb5a9f9f 88 vx=0;
JornJan 0:4c07bb5a9f9f 89 vy=0;
JornJan 0:4c07bb5a9f9f 90 }
JornJan 0:4c07bb5a9f9f 91 }
JornJan 0:4c07bb5a9f9f 92
JornJan 0:4c07bb5a9f9f 93 double kinematics(double& q1, double& q2, double q1_pos, double q2_pos, double vx, double vy )
JornJan 0:4c07bb5a9f9f 94 {
JornJan 0:4c07bb5a9f9f 95
JornJan 0:4c07bb5a9f9f 96 q1 = ((-(L3 + L2*cos(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vx + (-(L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q1_pos;
JornJan 0:4c07bb5a9f9f 97 q2 = (((L3 + L2*cos(q2_pos) - L1*sin(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos)) * vx) + ((L1*cos(q1_pos) + L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q2_pos;
JornJan 0:4c07bb5a9f9f 98 return 0;
JornJan 0:4c07bb5a9f9f 99 }
JornJan 0:4c07bb5a9f9f 100
JornJan 0:4c07bb5a9f9f 101 void controller(double qset1, double qset2)
JornJan 0:4c07bb5a9f9f 102 {
JornJan 0:4c07bb5a9f9f 103 // error = qset
JornJan 0:4c07bb5a9f9f 104 // referentie bepalen
JornJan 0:4c07bb5a9f9f 105 ref1 = qset1 + q1_pos; // genereert het referentiesignaal
JornJan 0:4c07bb5a9f9f 106 ref2 = qset2 + q2_pos;
JornJan 0:4c07bb5a9f9f 107
JornJan 0:4c07bb5a9f9f 108
JornJan 0:4c07bb5a9f9f 109 //PD
JornJan 0:4c07bb5a9f9f 110 PD1 = Kp*(qset1) + Kd*((qset1m-qset1)/ts) ; // PD sum; PD = proportianal + differential
JornJan 0:4c07bb5a9f9f 111 qset1m = qset1; // waarde voor qset wordt opgeslagen (m = memory)
JornJan 0:4c07bb5a9f9f 112 PD2 = Kp*(qset2) + Kd*((qset2m-qset2)/ts) ;
JornJan 0:4c07bb5a9f9f 113 qset2m = qset2;
JornJan 0:4c07bb5a9f9f 114
JornJan 0:4c07bb5a9f9f 115 //Motor control
JornJan 0:4c07bb5a9f9f 116 if (PD1<0 && PD2<0 )
JornJan 0:4c07bb5a9f9f 117 {
JornJan 0:4c07bb5a9f9f 118 motor1MagnitudePin = fabs(PD1);
JornJan 0:4c07bb5a9f9f 119 motor1DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 120 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 121 motor2DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 122 }
JornJan 0:4c07bb5a9f9f 123 else if (PD1>0 && PD2<0)
JornJan 0:4c07bb5a9f9f 124 {
JornJan 0:4c07bb5a9f9f 125 motor1MagnitudePin = fabs(PD1);
JornJan 0:4c07bb5a9f9f 126 motor1DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 127 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 128 motor2DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 129 }
JornJan 0:4c07bb5a9f9f 130
JornJan 0:4c07bb5a9f9f 131 else if (PD1<0 && PD2>0)
JornJan 0:4c07bb5a9f9f 132 {
JornJan 0:4c07bb5a9f9f 133 motor1MagnitudePin = fabs(PD1);
JornJan 0:4c07bb5a9f9f 134 motor1DirectionPin = 1;
JornJan 0:4c07bb5a9f9f 135 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 136 motor2DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 137 }
JornJan 0:4c07bb5a9f9f 138
JornJan 0:4c07bb5a9f9f 139 else if (PD1>0 && PD2>0)
JornJan 0:4c07bb5a9f9f 140 {
JornJan 0:4c07bb5a9f9f 141 motor1MagnitudePin = fabs(PD1);
JornJan 0:4c07bb5a9f9f 142 motor1DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 143 motor2MagnitudePin = fabs(PD2);
JornJan 0:4c07bb5a9f9f 144 motor2DirectionPin = 0;
JornJan 0:4c07bb5a9f9f 145 }
JornJan 0:4c07bb5a9f9f 146
JornJan 0:4c07bb5a9f9f 147
JornJan 0:4c07bb5a9f9f 148
JornJan 0:4c07bb5a9f9f 149 }
JornJan 0:4c07bb5a9f9f 150
JornJan 0:4c07bb5a9f9f 151 void aansturing()
JornJan 0:4c07bb5a9f9f 152 {
JornJan 0:4c07bb5a9f9f 153 // encoders uitlezen
JornJan 0:4c07bb5a9f9f 154 q1_puls = q1_enc.getPulses();
JornJan 0:4c07bb5a9f9f 155 q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
JornJan 0:4c07bb5a9f9f 156 q2_puls = q2_enc.getPulses();
JornJan 0:4c07bb5a9f9f 157 q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
JornJan 0:4c07bb5a9f9f 158
JornJan 0:4c07bb5a9f9f 159 // pijltjestoetsen
JornJan 0:4c07bb5a9f9f 160 wasd(vx, vy);
JornJan 0:4c07bb5a9f9f 161
JornJan 0:4c07bb5a9f9f 162 // kinematica bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
JornJan 0:4c07bb5a9f9f 163 kinematics(q1, q2, q1_pos, q2_pos, vx, vy);
JornJan 0:4c07bb5a9f9f 164
JornJan 0:4c07bb5a9f9f 165 // PD controller gebruikt PD control en stuurt motor aan
JornJan 0:4c07bb5a9f9f 166 controller(q1, q2);
JornJan 0:4c07bb5a9f9f 167
JornJan 0:4c07bb5a9f9f 168 }
JornJan 0:4c07bb5a9f9f 169
JornJan 0:4c07bb5a9f9f 170
JornJan 0:4c07bb5a9f9f 171 int main()
JornJan 0:4c07bb5a9f9f 172 {
JornJan 0:4c07bb5a9f9f 173 pc.baud(115200);
JornJan 0:4c07bb5a9f9f 174 pulses2rad=(2*pi)/4200;
JornJan 0:4c07bb5a9f9f 175 Kp = 0.5/pi;
JornJan 0:4c07bb5a9f9f 176 Kd = 0.1*Kp;
JornJan 0:4c07bb5a9f9f 177 tick_sample.attach_us(&aansturing, 1000); //sample frequency 1 mHz;
JornJan 0:4c07bb5a9f9f 178
JornJan 0:4c07bb5a9f9f 179 while(true)
JornJan 0:4c07bb5a9f9f 180 {
JornJan 0:4c07bb5a9f9f 181 }
JornJan 0:4c07bb5a9f9f 182
JornJan 0:4c07bb5a9f9f 183
JornJan 0:4c07bb5a9f9f 184 }