Begin Final script
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "Serial.h" 00003 #include "QEI.h" 00004 #include "math.h" 00005 00006 00007 Serial pc(USBTX, USBRX); //Serial PC connectie 00008 QEI encoder(D13, D12, NC, 32); //encoder instellen 00009 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) 00010 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) 00011 Ticker controller; //toets sample tijd 00012 double fs = 10000; // sample frequentie 00013 double ts = 1/fs; 00014 00015 // variablen voor void 00016 int pulses; 00017 int ref; 00018 int er; 00019 float Kp=0.0005; 00020 float P; 00021 float D; 00022 float Dif; 00023 float Kd=0.00012; 00024 float PID; 00025 int er2=0; 00026 00027 // constantes_k 00028 float L1 = 0.250; 00029 float L2 = 0.355; 00030 float L3 = 0.150; 00031 00032 00033 // Reference position_k 00034 float q1_1 = 0; 00035 float q2_1 = 0; 00036 float q1_2; 00037 float q2_2; 00038 00039 // EMG Input_k 00040 float vx ; 00041 float vy ; 00042 00043 00044 // Encoder input_k 00045 float q1_enc; //encoder actuator 1 00046 float q2_enc; //encoder actuator 2 00047 00048 00049 void PD() 00050 { 00051 char key; 00052 if(pc.readable()==true) 00053 { key = pc.getc(); 00054 if (key=='q') 00055 { 00056 ref=-500; //reference wordt 500 pulses 00057 } 00058 else if(key=='w') 00059 { 00060 ref=0; 00061 } 00062 else if(key=='e') 00063 { 00064 ref=500; //reference wordt 0 pulses 00065 } 00066 } 00067 //error bepalen 00068 pulses=encoder.getPulses(); 00069 er=ref-pulses; 00070 00071 //PID 00072 //Proportional part 00073 P = Kp*er; 00074 00075 //Differential part 00076 Dif=(er2-er)/ts; 00077 D=Kd*Dif; 00078 00079 //PID sum 00080 PID=P+D; 00081 er2=er; 00082 00083 //Motor control 00084 if (P<0) 00085 { 00086 motor1MagnitudePin = fabs(P); 00087 motor1DirectionPin = 1; 00088 } 00089 else if (P>0) 00090 { 00091 motor1MagnitudePin = fabs(P); 00092 motor1DirectionPin = 0; 00093 } 00094 00095 } 00096 00097 // Calculating q_set_k 00098 void Kinematics() 00099 { 00100 q1_1 = q1_2; 00101 q2_1 = q2_2; 00102 00103 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; 00104 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; 00105 } 00106 00107 int main() 00108 { 00109 controller.attach_us(&PD, 10000); 00110 00111 while(true) 00112 { 00113 } 00114 00115 00116 }
Generated on Mon Jul 18 2022 01:48:12 by
1.7.2
