main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "QEI.h"
00004 #include <fstream>
00005 #include <iomanip>
00006 #include "stdlib.h"
00007 
00008 #define MOTOR_PPR 300
00009 #define ENCODER_PPR 1024
00010 
00011 #define ENC_QUADRATURE_TYPE  4
00012 #define MOT_QUADRATURE_TYPE  2
00013 #define OUR_PI  3.141592653589793
00014 #define DATA_COLS 7
00015 #define BUFFER_SIZE 4200
00016 #define MAX_VOLTAGE 3.3
00017 #define VOLTS_PER_AMP 0.14
00018 #define PROGRAM_RUNTIME 30.0
00019 
00020 Serial pc(USBTX, USBRX);
00021 
00022 QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
00023 QEI motor(p25, p26, NC, MOTOR_PPR);
00024 Timer T;
00025 
00026 //Curent Measurement
00027 AnalogIn aIn(p16);    //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
00028 
00029 //Motor direction and PWM
00030 DigitalOut dOut1(p5);
00031 DigitalOut dOut2(p7);
00032 PwmOut pwmOut(p21);
00033 
00034 // open a file for data logger
00035 LocalFileSystem local("local");
00036 float theta1, theta2, dtheta1, dtheta2, dtheta2MvFil;
00037 float mCurrent = 0.0;
00038 float inputVoltage = 0.0;
00039 //int pulses0 = 0;
00040 //int deltaPulses;
00041 float t0 = 0.0;
00042 float t = 0.0, dt;
00043 
00044 //Controller gains - Full-state Feedback
00045 //float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
00046 //float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;
00047 float k1 = -0.1000 , k2 = 12.0293, k3 = -0.5357, k4 = 1.5522, k5 = 0.1684;
00048 
00049 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(ENC_QUADRATURE_TYPE));
00050 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(MOT_QUADRATURE_TYPE));
00051 
00052 float* buffer;
00053 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
00054 int index;
00055 int pulsesPend, pulsesMot;
00056 bool flag = 1;
00057 
00058 void saving(void const *args) {
00059     index = 0;
00060     while ((index < BUFFER_SIZE)&&(flag == 1))  {
00061         buffer[index] = theta1;
00062         buffer[index+1] = theta2;
00063         buffer[index+2] = dtheta1;
00064         buffer[index+3] = dtheta2;
00065         buffer[index+4] = mCurrent;
00066         buffer[index+5] = inputVoltage;
00067         buffer[index+6] = t;
00068         index = index + DATA_COLS;
00069         Thread::wait(20);
00070     }
00071 }
00072 
00073 float c2,
00074 g = 9.8,
00075 m2 = 0.0391 + 0.0259,
00076 L1 = (51+44.55)*0.001,
00077 l1 = -0.03478,
00078 L2 = 0.2983,
00079 l2 = 0.05847,
00080 I2x = 0.000534,
00081 I2y = 0.000841,
00082 I2z = 0.00031, 
00083 Ixz2 = -0.00024;
00084 
00085 float currentEnergy;
00086 
00087 float calcEnergy(void)
00088 {
00089 c2 = cos(theta2);
00090 //return (I2x*dtheta2*dtheta2)/2.0 + (I2y*dtheta1*dtheta1)/2.0 + (L1*L1*dtheta1*dtheta1*m2)/2.0 + (dtheta1*dtheta1*l2*l2*m2)/2.0 + (dtheta2*dtheta2*l2*l2*m2)/2.0 
00091 //- (I2y*dtheta1*dtheta1*c2*c2)/2.0 + (I2z*dtheta1*dtheta1*c2*c2)/2.0 + Ixz2*dtheta1*dtheta2*c2 + g*l2*m2*c2 - (dtheta1*dtheta1*l2*l2*m2*c2*c2)/2.0 
00092 //- L1*dtheta1*dtheta2*l2*m2*c2;
00093 return (I2x*dtheta2*dtheta2)/2.0 + g*l2*m2*c2;
00094 }
00095 
00096 void setVoltage(float inputVoltage)
00097 {
00098     if(inputVoltage<0.0) {
00099         inputVoltage = -inputVoltage;
00100         dOut1=0;
00101         dOut2=1;
00102     } else {
00103         dOut1=1;
00104         dOut2=0;
00105     }
00106     float dutyCycle = inputVoltage/MAX_VOLTAGE;
00107     dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
00108     pwmOut.write(dutyCycle);
00109 }
00110 //
00111 //#define MOVING_AVERAGE_SIZE 10
00112 //
00113 //float movingAverage(float unfiltered)
00114 //{
00115 //    static std::deque<float> movAvgBuffer;              // empty deque of floats
00116 //    static float sum = 0.0;
00117 //    
00118 //    movAvgBuffer.push_front(unfiltered);
00119 //    sum += unfiltered;
00120 //    
00121 //    if( movAvgBuffer.size() <= MOVING_AVERAGE_SIZE)
00122 //    {
00123 //        return (sum/float(movAvgBuffer.size()));
00124 //    }
00125 //    else
00126 //    {
00127 //        sum -= movAvgBuffer.back();
00128 //        movAvgBuffer.pop_back();
00129 //    
00130 //        return (sum/float(MOVING_AVERAGE_SIZE));
00131 //    }
00132 //}
00133 
00134 void computing(void const *args) {
00135     float z1=0.0, z2=0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
00136     bool firstTime = true;
00137     bool controller = 0;
00138     while (true ) {
00139         t = T.read();
00140         
00141        
00142         //read current
00143         mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
00144         if(dOut1 == 0)
00145             mCurrent = -mCurrent;
00146         pulsesPend = -encoder.getPulses();
00147         pulsesMot = motor.getPulses();
00148         
00149         dt = t - t0; //time difference
00150         theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
00151         theta1 = float(pulsesMot)*motor_conv;
00152         if(firstTime)
00153         {
00154             // z1 and z2 are in the beginning the same as the angle so that dtheta1 and dtheta2 are zero
00155             z1 = theta1;
00156             z2 = 1.002*theta2;
00157             firstTime = false;
00158         }
00159         //calculate dtheta1 
00160         dz1 = - lambda1 * z1 + lambda1 * theta1;
00161         z1 = z1 + dz1 * dt;
00162         dtheta1 = dz1;
00163         
00164         //calculate dtheta2 
00165         dz2 = - lambda2 * z2 + lambda2 * theta2;
00166         z2 = z2 + dz2 * dt;
00167         dtheta2 = dz2;
00168         
00169         //dtheta2MvFil = movingAverage(dtheta2);
00170         
00171         //filter current 
00172         dz3 = -lambda3 * z3 + lambda3 * mCurrent;
00173         z3 = z3 + dz3 * dt;
00174         mCurrent = z3;
00175         
00176         if (sin(theta2) < 0.0)
00177         controller = 1;
00178         if (cos(theta2) < 0.96)
00179         controller = 0;
00180         
00181         //set pwm 
00182         if ( controller == 0) {
00183             
00184             flag = 1;
00185             currentEnergy = calcEnergy();
00186             inputVoltage = -2.3*dtheta2*(0.0372 - currentEnergy); 
00187             //inputVoltage = 0.0;                  
00188             
00189         } else {
00190             flag = 1;
00191             inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
00192         }
00193         
00194         //Swing-Down
00195         if (t > PROGRAM_RUNTIME - 5.0)
00196             inputVoltage = 1.8*dtheta2*(0.0372 - currentEnergy); 
00197         
00198         setVoltage(inputVoltage);
00199         
00200         t0 = t;
00201         Thread::wait(1);
00202     }
00203 }
00204 
00205 void saveToFile ()
00206 {
00207     FILE *fp = fopen("/local/data.csv", "w");
00208     if (!fp) {
00209         fprintf(stderr, "File could not be openend \n\r");
00210         exit(1);
00211     }
00212  
00213     wait(2.0);
00214  
00215     for (int i=0; i < index; i = i + DATA_COLS) 
00216     {
00217         for (int j = 0; j < DATA_COLS; j++)
00218         {
00219             fprintf(fp,"%f,", buffer[i+j]);
00220         }
00221         fprintf(fp,"\n");
00222     }
00223     pc.printf("closing file\n\r");
00224     fclose(fp);
00225     wait(2.0);;
00226 }
00227 
00228 int main() {
00229     //allocate memory for the buffer
00230     pc.printf("creating buffer!\r\n");
00231     buffer = new float[BUFFER_SIZE];
00232     pc.printf("done creating buffer!\r\n");
00233     T.start();
00234     pwmOut.period(0.0001); 
00235     
00236     Thread::wait(2000);
00237     
00238     Thread thrd2(computing,NULL,osPriorityRealtime);
00239     pc.printf("started computing thread!\r\n");
00240     Thread thrd3(saving,NULL,osPriorityNormal);
00241     pc.printf("started saving thread!\r\n");
00242    
00243     
00244     pc.printf("Start!\r\n");
00245     pc.printf("Time: %f\r\n", t);
00246     
00247     while (t < PROGRAM_RUNTIME) 
00248     {
00249         //pc.printf("at time: %f energy: %f\n",t, currentEnergy);
00250         
00251         Thread::wait(200);
00252     }
00253     pc.printf("Done at Index: %d\r\n",index);
00254     pwmOut.write(0.0);
00255     thrd2.terminate();
00256     thrd3.terminate();
00257     saveToFile();
00258 }