Arthur de Lange
/
Final_Project_G5
Begin Final script
main.cpp@0:8336c89b41ab, 2017-10-26 (annotated)
- Committer:
- arthurdelange
- Date:
- Thu Oct 26 13:32:38 2017 +0000
- Revision:
- 0:8336c89b41ab
PID en Kinematica samengevoegd (op de T vervangen door ts in de kinematica void na is er nog niks veranderd en zijn de scripts alleen maar samengevoegd).
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
arthurdelange | 0:8336c89b41ab | 1 | #include "mbed.h" |
arthurdelange | 0:8336c89b41ab | 2 | #include "Serial.h" |
arthurdelange | 0:8336c89b41ab | 3 | #include "QEI.h" |
arthurdelange | 0:8336c89b41ab | 4 | #include "math.h" |
arthurdelange | 0:8336c89b41ab | 5 | |
arthurdelange | 0:8336c89b41ab | 6 | |
arthurdelange | 0:8336c89b41ab | 7 | Serial pc(USBTX, USBRX); //Serial PC connectie |
arthurdelange | 0:8336c89b41ab | 8 | QEI encoder(D13, D12, NC, 32); //encoder instellen |
arthurdelange | 0:8336c89b41ab | 9 | DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) |
arthurdelange | 0:8336c89b41ab | 10 | PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) |
arthurdelange | 0:8336c89b41ab | 11 | Ticker controller; //toets sample tijd |
arthurdelange | 0:8336c89b41ab | 12 | double fs = 10000; // sample frequentie |
arthurdelange | 0:8336c89b41ab | 13 | double ts = 1/fs; |
arthurdelange | 0:8336c89b41ab | 14 | |
arthurdelange | 0:8336c89b41ab | 15 | // variablen voor void |
arthurdelange | 0:8336c89b41ab | 16 | int pulses; |
arthurdelange | 0:8336c89b41ab | 17 | int ref; |
arthurdelange | 0:8336c89b41ab | 18 | int er; |
arthurdelange | 0:8336c89b41ab | 19 | float Kp=0.0005; |
arthurdelange | 0:8336c89b41ab | 20 | float P; |
arthurdelange | 0:8336c89b41ab | 21 | float D; |
arthurdelange | 0:8336c89b41ab | 22 | float Dif; |
arthurdelange | 0:8336c89b41ab | 23 | float Kd=0.00012; |
arthurdelange | 0:8336c89b41ab | 24 | float PID; |
arthurdelange | 0:8336c89b41ab | 25 | int er2=0; |
arthurdelange | 0:8336c89b41ab | 26 | |
arthurdelange | 0:8336c89b41ab | 27 | // constantes_k |
arthurdelange | 0:8336c89b41ab | 28 | float L1 = 0.250; |
arthurdelange | 0:8336c89b41ab | 29 | float L2 = 0.355; |
arthurdelange | 0:8336c89b41ab | 30 | float L3 = 0.150; |
arthurdelange | 0:8336c89b41ab | 31 | |
arthurdelange | 0:8336c89b41ab | 32 | |
arthurdelange | 0:8336c89b41ab | 33 | // Reference position_k |
arthurdelange | 0:8336c89b41ab | 34 | float q1_1 = 0; |
arthurdelange | 0:8336c89b41ab | 35 | float q2_1 = 0; |
arthurdelange | 0:8336c89b41ab | 36 | float q1_2; |
arthurdelange | 0:8336c89b41ab | 37 | float q2_2; |
arthurdelange | 0:8336c89b41ab | 38 | |
arthurdelange | 0:8336c89b41ab | 39 | // EMG Input_k |
arthurdelange | 0:8336c89b41ab | 40 | float vx ; |
arthurdelange | 0:8336c89b41ab | 41 | float vy ; |
arthurdelange | 0:8336c89b41ab | 42 | |
arthurdelange | 0:8336c89b41ab | 43 | |
arthurdelange | 0:8336c89b41ab | 44 | // Encoder input_k |
arthurdelange | 0:8336c89b41ab | 45 | float q1_enc; //encoder actuator 1 |
arthurdelange | 0:8336c89b41ab | 46 | float q2_enc; //encoder actuator 2 |
arthurdelange | 0:8336c89b41ab | 47 | |
arthurdelange | 0:8336c89b41ab | 48 | |
arthurdelange | 0:8336c89b41ab | 49 | void PD() |
arthurdelange | 0:8336c89b41ab | 50 | { |
arthurdelange | 0:8336c89b41ab | 51 | char key; |
arthurdelange | 0:8336c89b41ab | 52 | if(pc.readable()==true) |
arthurdelange | 0:8336c89b41ab | 53 | { key = pc.getc(); |
arthurdelange | 0:8336c89b41ab | 54 | if (key=='q') |
arthurdelange | 0:8336c89b41ab | 55 | { |
arthurdelange | 0:8336c89b41ab | 56 | ref=-500; //reference wordt 500 pulses |
arthurdelange | 0:8336c89b41ab | 57 | } |
arthurdelange | 0:8336c89b41ab | 58 | else if(key=='w') |
arthurdelange | 0:8336c89b41ab | 59 | { |
arthurdelange | 0:8336c89b41ab | 60 | ref=0; |
arthurdelange | 0:8336c89b41ab | 61 | } |
arthurdelange | 0:8336c89b41ab | 62 | else if(key=='e') |
arthurdelange | 0:8336c89b41ab | 63 | { |
arthurdelange | 0:8336c89b41ab | 64 | ref=500; //reference wordt 0 pulses |
arthurdelange | 0:8336c89b41ab | 65 | } |
arthurdelange | 0:8336c89b41ab | 66 | } |
arthurdelange | 0:8336c89b41ab | 67 | //error bepalen |
arthurdelange | 0:8336c89b41ab | 68 | pulses=encoder.getPulses(); |
arthurdelange | 0:8336c89b41ab | 69 | er=ref-pulses; |
arthurdelange | 0:8336c89b41ab | 70 | |
arthurdelange | 0:8336c89b41ab | 71 | //PID |
arthurdelange | 0:8336c89b41ab | 72 | //Proportional part |
arthurdelange | 0:8336c89b41ab | 73 | P = Kp*er; |
arthurdelange | 0:8336c89b41ab | 74 | |
arthurdelange | 0:8336c89b41ab | 75 | //Differential part |
arthurdelange | 0:8336c89b41ab | 76 | Dif=(er2-er)/ts; |
arthurdelange | 0:8336c89b41ab | 77 | D=Kd*Dif; |
arthurdelange | 0:8336c89b41ab | 78 | |
arthurdelange | 0:8336c89b41ab | 79 | //PID sum |
arthurdelange | 0:8336c89b41ab | 80 | PID=P+D; |
arthurdelange | 0:8336c89b41ab | 81 | er2=er; |
arthurdelange | 0:8336c89b41ab | 82 | |
arthurdelange | 0:8336c89b41ab | 83 | //Motor control |
arthurdelange | 0:8336c89b41ab | 84 | if (P<0) |
arthurdelange | 0:8336c89b41ab | 85 | { |
arthurdelange | 0:8336c89b41ab | 86 | motor1MagnitudePin = fabs(P); |
arthurdelange | 0:8336c89b41ab | 87 | motor1DirectionPin = 1; |
arthurdelange | 0:8336c89b41ab | 88 | } |
arthurdelange | 0:8336c89b41ab | 89 | else if (P>0) |
arthurdelange | 0:8336c89b41ab | 90 | { |
arthurdelange | 0:8336c89b41ab | 91 | motor1MagnitudePin = fabs(P); |
arthurdelange | 0:8336c89b41ab | 92 | motor1DirectionPin = 0; |
arthurdelange | 0:8336c89b41ab | 93 | } |
arthurdelange | 0:8336c89b41ab | 94 | |
arthurdelange | 0:8336c89b41ab | 95 | } |
arthurdelange | 0:8336c89b41ab | 96 | |
arthurdelange | 0:8336c89b41ab | 97 | // Calculating q_set_k |
arthurdelange | 0:8336c89b41ab | 98 | void Kinematics() |
arthurdelange | 0:8336c89b41ab | 99 | { |
arthurdelange | 0:8336c89b41ab | 100 | q1_1 = q1_2; |
arthurdelange | 0:8336c89b41ab | 101 | q2_1 = q2_2; |
arthurdelange | 0:8336c89b41ab | 102 | |
arthurdelange | 0:8336c89b41ab | 103 | q1_2 = ((-(L3 + L2*cos(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vx + (-(L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q1_enc; |
arthurdelange | 0:8336c89b41ab | 104 | q2_2 = (((L3 + L2*cos(q2_1) - L1*sin(q1_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1)) * vx) + ((L1*cos(q1_1) + L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q2_enc; |
arthurdelange | 0:8336c89b41ab | 105 | } |
arthurdelange | 0:8336c89b41ab | 106 | |
arthurdelange | 0:8336c89b41ab | 107 | int main() |
arthurdelange | 0:8336c89b41ab | 108 | { |
arthurdelange | 0:8336c89b41ab | 109 | controller.attach_us(&PD, 10000); |
arthurdelange | 0:8336c89b41ab | 110 | |
arthurdelange | 0:8336c89b41ab | 111 | while(true) |
arthurdelange | 0:8336c89b41ab | 112 | { |
arthurdelange | 0:8336c89b41ab | 113 | } |
arthurdelange | 0:8336c89b41ab | 114 | |
arthurdelange | 0:8336c89b41ab | 115 | |
arthurdelange | 0:8336c89b41ab | 116 | } |