Arthur de Lange / Mbed 2 deprecated Final_Project_G5

Begin Final script

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }