Jorn-Jan van de Beld / Mbed 2 deprecated PI_standaardsignalen

kom op

Dependencies:   QEI mbed

main.cpp@7:5d9907aa6dbc, 2017-11-02 (annotated)

Committer:
arthurdelange
Date:
Thu Nov 02 19:50:07 2017 +0000
Revision:
7:5d9907aa6dbc
Parent:
6:f40e26afc9aa 
Syndroom van down is bij M2 opgelost, hij heeft er een botvliesontsteking bijgekregen op M1 op de onderste pulley.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:921402a95675 1 #include "mbed.h"
JornJan 0:921402a95675 2 #include "Serial.h"
JornJan 0:921402a95675 3 #include "math.h"
JornJan 0:921402a95675 4 #include "QEI.h"
JornJan 0:921402a95675 5
JornJan 0:921402a95675 6 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 0:921402a95675 7 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:921402a95675 8 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 0:921402a95675 9 DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord)
JornJan 0:921402a95675 10 PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord)
JornJan 0:921402a95675 11 QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
JornJan 0:921402a95675 12 QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
JornJan 0:921402a95675 13 const double pi = 3.1415926535897; // waarde voor pi aanmaken
JornJan 0:921402a95675 14
JornJan 0:921402a95675 15 Timer t;
JornJan 0:921402a95675 16 Ticker aansturing;
JornJan 0:921402a95675 17 Ticker stepres;
JornJan 3:3b5b85a32c9a 18 Ticker kinmat;
JornJan 0:921402a95675 19
JornJan 0:921402a95675 20 double ref1 = 0;
JornJan 0:921402a95675 21 double refrad1;
JornJan 0:921402a95675 22 double refrad2;
JornJan 0:921402a95675 23 double ref2 = 0;
arthurdelange 7:5d9907aa6dbc 24 double Kp1 = 0.002; // 1 moet nog getweakt worden
JornJan 0:921402a95675 25 double Ki1 = 0;
arthurdelange 7:5d9907aa6dbc 26 double Kp2 = 0.00021;
arthurdelange 7:5d9907aa6dbc 27 double Ki2 = 0.00005;
JornJan 0:921402a95675 28 int q1_puls;
JornJan 0:921402a95675 29 int q2_puls;
JornJan 3:3b5b85a32c9a 30 double rad2pulses=(2100/pi);
JornJan 4:e98ad06f227c 31 double pulses2rad = (pi)/2100;
JornJan 0:921402a95675 32 double ts = 0.001;
JornJan 0:921402a95675 33 double PI1;
JornJan 0:921402a95675 34 int n;
JornJan 0:921402a95675 35
JornJan 0:921402a95675 36 double error1_1;
JornJan 0:921402a95675 37 double error2_1 = 0;
JornJan 0:921402a95675 38 double error_I_1;
JornJan 0:921402a95675 39 double error_I2_1 = 0;
JornJan 0:921402a95675 40 double error1_2;
JornJan 0:921402a95675 41 double error2_2 = 0;
JornJan 0:921402a95675 42 double error_I_2;
JornJan 0:921402a95675 43 double error_I2_2 = 0;
JornJan 0:921402a95675 44 double PI2;
JornJan 0:921402a95675 45
JornJan 2:0686b2132556 46 // kinematica gegevens
JornJan 2:0686b2132556 47 // lengte armen
JornJan 2:0686b2132556 48 double L1 = 0.250;
JornJan 2:0686b2132556 49 double L2 = 0.355;
JornJan 2:0686b2132556 50 double L3 = 0.150;
JornJan 2:0686b2132556 51
JornJan 2:0686b2132556 52 // reference position
JornJan 2:0686b2132556 53 double q1=0; // positie q1 in radialen
JornJan 2:0686b2132556 54 double q2=0; // positie q2 in radialen
JornJan 2:0686b2132556 55 double q1_pos;
JornJan 2:0686b2132556 56 double q2_pos;
JornJan 2:0686b2132556 57
JornJan 2:0686b2132556 58 // EMG Input_k
JornJan 2:0686b2132556 59 double vx;
JornJan 2:0686b2132556 60 double vy;
JornJan 2:0686b2132556 61
JornJan 2:0686b2132556 62
JornJan 2:0686b2132556 63 void kinematica()
JornJan 2:0686b2132556 64 {
JornJan 2:0686b2132556 65 // encoders uitlezen
JornJan 4:e98ad06f227c 66 q1_puls = q1_enc.getPulses();
JornJan 3:3b5b85a32c9a 67 q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
JornJan 2:0686b2132556 68 q2_puls = q2_enc.getPulses();
JornJan 3:3b5b85a32c9a 69 q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
JornJan 2:0686b2132556 70
arthurdelange 7:5d9907aa6dbc 71 vy= 0;
arthurdelange 7:5d9907aa6dbc 72 vx=0.012f*sin(t.read());
JornJan 3:3b5b85a32c9a 73
arthurdelange 6:f40e26afc9aa 74 q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 *ts + q1;
arthurdelange 6:f40e26afc9aa 75 q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) + ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 * ts + q2;
JornJan 4:e98ad06f227c 76
arthurdelange 6:f40e26afc9aa 77 ref1 = -q1*rad2pulses;
JornJan 4:e98ad06f227c 78 ref2 = q2*rad2pulses;
arthurdelange 6:f40e26afc9aa 79
arthurdelange 7:5d9907aa6dbc 80
arthurdelange 7:5d9907aa6dbc 81
arthurdelange 7:5d9907aa6dbc 82
JornJan 4:e98ad06f227c 83
JornJan 4:e98ad06f227c 84
JornJan 2:0686b2132556 85 }
JornJan 2:0686b2132556 86
JornJan 2:0686b2132556 87
JornJan 0:921402a95675 88 void controller()
JornJan 3:3b5b85a32c9a 89 {
JornJan 4:e98ad06f227c 90 kinematica();
JornJan 0:921402a95675 91 //PID 1
JornJan 0:921402a95675 92 error1_1 = ref1 - q1_puls;
JornJan 0:921402a95675 93 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
arthurdelange 7:5d9907aa6dbc 94 error_I2_1 = error_I_1;
arthurdelange 7:5d9907aa6dbc 95 error2_1 = error1_1;
JornJan 0:921402a95675 96
JornJan 0:921402a95675 97 PI1 = Kp1*error1_1 + Ki1*error_I_1;
JornJan 0:921402a95675 98
JornJan 0:921402a95675 99 error2_1 = error1_1; // opslaan variabelen voor integraal onderdeel
JornJan 0:921402a95675 100 error_I2_1 = error_I_1;
JornJan 0:921402a95675 101
JornJan 0:921402a95675 102 //PID 2
JornJan 0:921402a95675 103 error1_2 = ref2 - q2_puls;
JornJan 0:921402a95675 104 error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
JornJan 4:e98ad06f227c 105 error_I2_2 = error_I_2;
JornJan 4:e98ad06f227c 106 error2_2 = error1_2;
JornJan 0:921402a95675 107
JornJan 0:921402a95675 108 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
JornJan 0:921402a95675 109
JornJan 0:921402a95675 110
JornJan 0:921402a95675 111 if (PI1<=0)
JornJan 0:921402a95675 112 {
JornJan 0:921402a95675 113 motor1DirectionPin = 1;
arthurdelange 6:f40e26afc9aa 114 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 115 }
JornJan 0:921402a95675 116 else if (PI1>0)
JornJan 0:921402a95675 117 {
JornJan 0:921402a95675 118 motor1DirectionPin = 0;
arthurdelange 6:f40e26afc9aa 119 motor1MagnitudePin = fabs(PI1);
JornJan 0:921402a95675 120 }
JornJan 0:921402a95675 121
JornJan 0:921402a95675 122 if (PI2>=0)
JornJan 0:921402a95675 123 {
JornJan 0:921402a95675 124 motor2DirectionPin = 1;
JornJan 3:3b5b85a32c9a 125 motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 126 }
JornJan 0:921402a95675 127 else if(PI2<0)
JornJan 0:921402a95675 128 {
JornJan 0:921402a95675 129 motor2DirectionPin = 0;
JornJan 3:3b5b85a32c9a 130 motor2MagnitudePin = fabs(PI2);
JornJan 0:921402a95675 131 }
JornJan 0:921402a95675 132
arthurdelange 7:5d9907aa6dbc 133 if(n==100){
JornJan 4:e98ad06f227c 134 printf("q1_puls = %d q2_puls = %d ref1 = %f ref2 = %f\n\r", q1_puls, q2_puls, ref1, ref2);
JornJan 0:921402a95675 135 n=0;
JornJan 0:921402a95675 136 }
JornJan 0:921402a95675 137 else{
JornJan 0:921402a95675 138 n=n+1;
JornJan 0:921402a95675 139 }
JornJan 0:921402a95675 140
JornJan 0:921402a95675 141 }
JornJan 0:921402a95675 142
JornJan 0:921402a95675 143
JornJan 0:921402a95675 144 int main()
JornJan 0:921402a95675 145 {
JornJan 0:921402a95675 146 pc.baud(115200);
JornJan 0:921402a95675 147 t.start();
JornJan 0:921402a95675 148 aansturing.attach_us(&controller, 1000);
JornJan 4:e98ad06f227c 149 //kinmat.attach(&kinematica, 0.001);
JornJan 4:e98ad06f227c 150
JornJan 0:921402a95675 151
JornJan 0:921402a95675 152 while(true){
JornJan 0:921402a95675 153
JornJan 0:921402a95675 154 }
JornJan 0:921402a95675 155
JornJan 0:921402a95675 156
JornJan 0:921402a95675 157 }