main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "math.h"
00004 #include "BiQuad.h"
00005 #include "QEI.h"
00006 
00007 //Inputs outputs e.t.c.
00008 
00009 //Serial
00010 MODSERIAL pc(USBTX, USBRX);
00011 
00012 //Leds
00013 DigitalOut led1(LED_GREEN);
00014 DigitalOut led2(LED_BLUE);
00015 DigitalOut led3(LED_RED);
00016 
00017 //Timing
00018 Ticker LedTimer;
00019 
00020 Ticker ScopeTimer;
00021 Timeout SignalTimeout;
00022 
00023 Ticker ControlTicker;
00024 Ticker ControlTicker1;
00025 
00026 //Buttons
00027 DigitalIn button(SW2);
00028 
00029 //Emg
00030 AnalogIn emg1(A0);
00031 AnalogIn emg2(A1);
00032 AnalogIn emg3(A2);
00033 
00034 
00035 //Control
00036 PwmOut Motor2PWR(D5);
00037 DigitalOut Motor2DIR(D4);
00038 PwmOut Motor1PWR(D6);
00039 DigitalOut Motor1DIR(D7);
00040 QEI Encoder1(D12,D13,NC,32);
00041 QEI Encoder2(D10,D11,NC,32);
00042 
00043 //Filter
00044 BiQuadChain bqc;
00045 
00046 
00047 
00048 //Declare variables
00049 double fsample = 500; //Hz
00050 double fcontrol = 1000; //Hz
00051 int max_n;  // length of measured arrays
00052 double pi = 3.14159265;
00053 
00054 // variables that depend on fsample
00055 double Ts = 1/500; // Sample time in seconds
00056 double emg1_log[500*6];
00057 double emg2_log[500*6];
00058 double emg3_log[500*6];
00059     
00060 double RadPerCount = 131.25*2*pi/4200;
00061 
00062 //Encoders
00063 int counts1;
00064 int counts2;
00065 
00066 //PID
00067 double Kp = 10;
00068 double Ki = 1.02;
00069 double Kd = 15.2;
00070 
00071 //Movavg
00072 int avgn;
00073 double avgh;
00074 double ms = 100; // number of datapoints used in moving average filter
00075 double highnum;
00076 
00077 //Bools
00078 bool MeasureBool = true;
00079 bool MoveBool;
00080 bool Calibrated = false;
00081 volatile bool pause_loop = false;
00082 
00083 //States
00084 bool statechanged = false;
00085 enum Colors {Green, Blue, Red, Off};
00086 enum states {CalibrateM, CalibrateE, MandM, Done};
00087 enum Directions {Up, Down, Left, Right};
00088 Directions CurrentDir = Down;
00089 Colors LedState = Off;
00090 states ProgramState = CalibrateM;
00091 
00092 //Systeem eigenschappen
00093 double L0 = 40; //cm
00094 double L1 = 45; //dm
00095 
00096 //Speeds and Positions
00097 double setpoint;
00098 double MaxV = 6.3; //rad/s
00099 double qv[2]; //setpoint velocity
00100 double qr[2]; //setpoint pos
00101 double q1;
00102 double q2;
00103 
00104 //Maak filters
00105 BiQuad bq1(0.9475454809720195,-1.895090961944039,0.9475454809720195,-1.8932193507849706,0.8969625731031077); //highpass 5Hz
00106 BiQuad bq2(0.843582000346111,-1.3649443488576334,0.843582000346111,-1.3649443488576334,0.6871640006922219); //notch 50Hz
00107 BiQuad bq3(0.9824,-0.6071,0.9824,-0.6071,0.9647); //notch 100Hz
00108 BiQuad bq4(0.9738  ,  0.6018 ,   0.9738,0.6018  ,  0.9475); //notch 150Hz
00109 BiQuad bq5(0.9653  ,  1.5619  ,  0.9653,1.5619  ,  0.9307); //notch 200Hz
00110 
00111 //Programma
00112 
00113 void CancelPause(){
00114     pause_loop = false;
00115 }
00116 
00117 void ClearEmg(){
00118     int n;
00119     for(n = 0; n <= max_n; n++){
00120         emg1_log[n] = 0;
00121     }
00122     avgh = 0;
00123     avgn = 0;
00124     max_n = 0;
00125 }
00126 
00127 void MeasurePos(){
00128     counts1 = Encoder1.getPulses();
00129     counts2 = Encoder2.getPulses();
00130     q1 = RadPerCount*counts1;
00131     q2 = RadPerCount*counts2;
00132 }
00133 
00134 void ReferencePos(){
00135     qr[0] += qv[0]*1/fcontrol;
00136     qr[1] += qv[1]*1/fcontrol;
00137 }
00138 
00139 void fmovavg(){
00140     double avgm;
00141     avgh += emg1_log[max_n];
00142     if(max_n >= ms){
00143         avgm = emg1_log[avgn];
00144         emg1_log[avgn] = avgh/ms;
00145         avgh -= avgm;
00146         avgn++;
00147     }
00148 }
00149 
00150 void EmgMeasure(){    //meet de EMG waarden en stop ze in een array wel filters maar geen moving average
00151     double n = max_n;
00152     if(MeasureBool && Calibrated){
00153     emg1_log[max_n] =  fabs(bqc.step(emg1.read()))/highnum;
00154     fmovavg();
00155     max_n ++;
00156     }
00157     else if(MeasureBool){
00158     emg1_log[max_n] =  fabs(bqc.step(emg1.read()));
00159     fmovavg();
00160     max_n++;
00161     }
00162 }
00163 
00164 void MakeEmg(){
00165     int n;
00166     for(n =0 ;n<=max_n;n++){
00167         if((n >= 500) && (n <= 600)){
00168         emg1_log[n] = 0.3;
00169         }
00170         else{
00171             emg1_log[n] = 0;
00172         }
00173     }
00174 }
00175 
00176 void LedBlink(){
00177     switch(LedState){
00178         case Green:
00179             led1.write(!led1.read());
00180             led2.write(1);
00181             led3.write(1);
00182             break;
00183         case Blue:
00184             led2.write(!led2.read());
00185             led1.write(1);
00186             led3.write(1);
00187             break;
00188         case Red:
00189             led3.write(!led3.read());
00190             led1.write(1);
00191             led2.write(1);
00192             break;
00193         default:
00194             led1.write(1);
00195             led2.write(1);
00196             led3.write(1);
00197             break;
00198     }
00199 }
00200 
00201 void ChangeDIR(){
00202     switch(CurrentDir){
00203         case Up:
00204             CurrentDir = Down;
00205             break;
00206         case Down:
00207             CurrentDir = Left;
00208             break;
00209         case Left:
00210             CurrentDir = Right;
00211             break;
00212         case Right:
00213             CurrentDir = Up;
00214             break;
00215         default:
00216             break;
00217         }
00218 }
00219 
00220 void CheckValue(){
00221     setpoint = 0;
00222     int n;
00223     for(n = 0;n<=max_n;n++){
00224         if(emg1_log[n] >= setpoint){
00225             setpoint = emg1_log[n];
00226         }
00227         emg1_log[n] = 0;
00228         if(setpoint>1){     //Saturaion
00229             setpoint = 1;
00230         }
00231         if(setpoint<0.2){   //Rest
00232             setpoint = 0;
00233         }
00234     }
00235 }
00236 
00237 void InvKin(){
00238     double Inverse_J[2][2] = {{(-cos(q1) + sin(q2))/((cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2)))     ,       (L1 + L0*cos(q2) - L1*sin(q1))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2)))}     ,       {sin(q2)/(-cos(q1)*(L1 + L0*cos(q2)) + L1*sin(q1)*sin(q2))     ,       -((L1 + L0*cos(q2))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2))))}};
00239     if(button.read() == 0){
00240         ChangeDIR();
00241     }
00242     switch(CurrentDir){
00243         case Up:
00244             qv[0] = Inverse_J[0][1]*setpoint;
00245             qv[1] = Inverse_J[1][1]*setpoint;
00246             break;
00247         case Down:
00248             qv[0] = Inverse_J[0][1]*(-setpoint);
00249             qv[1] = Inverse_J[1][1]*(-setpoint);
00250             break;
00251         case Left:
00252             qv[0] = Inverse_J[0][0]*(-setpoint);
00253             qv[1] = Inverse_J[1][0]*(-setpoint);
00254             break;
00255         case Right:
00256             qv[0] = Inverse_J[0][0]*setpoint;
00257             qv[1] = Inverse_J[1][0]*setpoint;
00258             break;
00259         default:
00260             break;
00261         }
00262     qv[0] = qv[0]*MaxV;
00263     qv[1] = qv[1]*MaxV;
00264 }
00265 
00266 void CalibrateEmg(){
00267     MeasureBool = false;
00268     LedState = Off;
00269     int n;
00270     for(n = 0;n<=max_n;n++){
00271         if(emg1_log[n] >= highnum){
00272             highnum = emg1_log[n];
00273         }
00274         emg1_log[n] = 0;
00275     }
00276     if(highnum>1){
00277         highnum = 1;
00278     }
00279     max_n = 0;
00280     if(highnum <= 0.2){
00281         pc.printf("Done calibrating \r\n%.2f Oei! Dat is jammer \r\n",highnum);
00282     }
00283     else if(highnum <= 0.4){
00284         pc.printf("Done calibrating \r\n%.2f Kan beter he \r\n",highnum);
00285     }
00286     else if(highnum <= 0.6){
00287         pc.printf("Done calibrating \r\n%.2f Komt in de buurt \r\n",highnum);
00288     }
00289     else if(highnum <= 0.8){
00290         pc.printf("Done calibrating \r\n%.2f Prima! \r\n",highnum);
00291     }
00292     else if(highnum <= 1){
00293         pc.printf("Done calibrating \r\n%.2f Lekker Bezig man!\r\n",highnum);
00294     }
00295     Calibrated = true;
00296     MeasureBool = true;
00297     wait(0.1f);
00298     pause_loop = false;
00299 }
00300 
00301 void CalibrateMotors(){
00302     Motor1DIR = 0;
00303     Motor2DIR = 0;
00304     Motor1PWR = 0.52;
00305     Motor2PWR = 0.52;
00306     ProgramState = CalibrateE;
00307 }
00308 
00309 double PID_Controller(double error){
00310     static double error_integral = 0;
00311     static double error_prev = error; // initialization with this value only done once!
00312     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);  
00313     
00314     // Proportional part:
00315     double u_k = Kp * error;
00316 
00317     // Integral part
00318     error_integral = error_integral + error * Ts;
00319     double u_i = Ki * error_integral;
00320 
00321     // Derivative part
00322     double error_derivative = (error - error_prev)/Ts;
00323     double filtered_error_derivative = LowPassFilter.step(error_derivative);
00324     double u_d = Kd * filtered_error_derivative;
00325     error_prev = error;
00326     
00327     // Sum all parts and return it
00328     return u_k + u_i + u_d;  
00329 }
00330 
00331 void LogSignals(){     //stuur het signaal naar de pc via serial
00332     MeasureBool = false;
00333     int n;
00334     for(n = 0;n<=max_n;n++){
00335         emg1_log[n] = emg1_log[n]/highnum;
00336         //pc.printf("%f, ",emg1_log[n]);
00337         //wait(0.01f);
00338     }
00339     pc.printf("\r\n\r\n%i", max_n);
00340     ClearEmg();
00341     pause_loop = false;
00342     wait(0.1f);
00343 }
00344 
00345 void SetPointJoint(){
00346     if(MoveBool){
00347         MeasureBool = false;
00348         wait(0.01f);
00349         pc.printf("\r\nHO");
00350         CheckValue();
00351         ClearEmg();
00352         MeasureBool = true;
00353     }
00354 }
00355 
00356 
00357 void MeasureAndControl(){
00358     InvKin();
00359     ReferencePos();
00360     MeasurePos();
00361     double motorinput1 = PID_Controller(qr[0] - q1);
00362     double motorinput2 = PID_Controller(qr[1] - q2);
00363 }
00364 
00365 void Movemotors(double input1, double input2){
00366     if(input1>0){
00367         Motor1DIR = 1;
00368     }
00369     else{
00370         Motor1DIR = 0;
00371     }
00372     if(fabs(input1)>1){
00373         Motor1PWR = 1;
00374     }
00375     else{
00376         Motor1PWR = fabs(input1);
00377     }
00378     if(input2>0){
00379         Motor2DIR = 1;
00380     }
00381     else{
00382         Motor2DIR = 0;
00383     }
00384     if(fabs(input2)>1){
00385         Motor2PWR = 1;
00386     }
00387     else{
00388         Motor2PWR = fabs(input2);
00389     }
00390 }
00391 
00392 
00393 void ProcessStateMachine(){
00394     switch(ProgramState){
00395     case CalibrateM:
00396         CalibrateMotors();
00397         break;
00398     case CalibrateE:
00399         LedState = Green;
00400         pause_loop = true;
00401         LedTimer.attach(&LedBlink,0.2);
00402         ScopeTimer.attach(&EmgMeasure,1/fsample);
00403         //MakeEmg();
00404         SignalTimeout.attach(&CalibrateEmg,5);
00405         while(pause_loop){}
00406         ProgramState = MandM; 
00407         break;
00408     case MandM:
00409         LedState = Blue;
00410         pause_loop = true;
00411         MoveBool = true;
00412         ScopeTimer.attach(&EmgMeasure,1/fsample);
00413         //SignalTimeout.attach(CancelPause,0.5);
00414         //SignalTimeout.attach(&LogSignals,5);
00415         ControlTicker.attach(&SetPointJoint,0.5);
00416         ControlTicker1.attach(&MeasureAndControl,1/fcontrol);
00417         while(pause_loop){}
00418         ProgramState = Done;
00419         break;
00420     default:
00421         LedState = Off;
00422         if(statechanged){
00423         pc.printf("\r\nyeeee\r\n");
00424         statechanged = false;
00425         }
00426         break;
00427     }
00428 }
00429     
00430 int main(){
00431     led1.write(1);
00432     led2.write(1);
00433     led3.write(1);
00434     
00435     pc.baud(115200);
00436     Motor1PWR.period_us(60);
00437     Motor2PWR.period_us(60);
00438     bqc.add( &bq1).add( &bq2 ).add( &bq3 ).add( &bq4 ).add( &bq5 );
00439     
00440     while(1){
00441         ProcessStateMachine();
00442      }
00443 }