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 }