main.cpp

00001 //Ben Katz, September 2013
00002 
00003 #include "mbed.h"
00004 #include "QEI.h"
00005 #include <fstream>
00006 #include <iomanip>
00007 //Declaring pins
00008 LocalFileSystem local("local");
00009 Timer t;
00010 float analogSignal;
00011 float Angle1;
00012 float preAngle1=0;
00013 float Angle2;
00014 float preAngle2=0;
00015 
00016 float mCurrent1;
00017 float mCurrent2;
00018 float dCurrent1;
00019 float dCurrent2;
00020 float dAngle1;
00021 float dAngle2;
00022 float dAngularVelocity1=0;
00023 float dAngularVelocity2=0;
00024 float AngularVelocity1;
00025 float AngularVelocity2;
00026 float duty1;
00027 float duty2;
00028 float alpha;
00029 float CurrentControlPeriod;
00030 float DataSavingPeriod=0.005;
00031 
00032 float Kp=5;
00033 float Kd=0.04;
00034 float Kv=0.35;
00035 float R=2.2;
00036 float Ki=5;
00037 DigitalOut dOut1(p5);
00038 DigitalOut dOut2(p7);  //pin 5 set as a digital output  Pins 5-30 can be used as digital I/O.
00039 DigitalOut dOut3(p8);
00040 DigitalOut dOut4(p9);
00041 //pin 6 set as digital input.
00042 AnalogIn aIn1(p16);
00043 AnalogIn aIn2(p17);  //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
00044 PwmOut pwmOut1(p21);
00045 PwmOut pwmOut2(p22);   //pin 21 set as PWM output.  Pins 21-26 can be used as PWM outputs.
00046 QEI motor1(p27, p28, NC, 1200, QEI::X4_ENCODING);
00047 QEI motor2(p23, p24, NC, 1200, QEI::X4_ENCODING);
00048 //ofstream out;
00049 FILE *fp= fopen("/local/data.csv","w");
00050 
00051 //Serial pc(USBTX, USBRX);  //create a serial connection to the computer over the tx/rx pins
00052 
00053 void Control()
00054 {
00055   dAngle1 =motor2.getPulses()/1200.0*6.283;;
00056     dAngularVelocity1=AngularVelocity2;
00057     preAngle1=Angle1;
00058     Angle1 = motor1.getPulses();
00059     Angle1 =Angle1/1200.0*6.283;
00060     AngularVelocity1 =(Angle1-preAngle1)/CurrentControlPeriod;
00061     mCurrent1  = aIn1.read()*3.3/.14;
00062     
00063      dCurrent1 = 3*(dAngle1-Angle1)+0.04*(dAngularVelocity1-AngularVelocity1)
00064 
00065     if (dCurrent1>0) {
00066         dOut1=1;
00067         dOut2=0;
00068         duty1= (abs(dCurrent1)*2.10+0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
00069     } else if (dCurrent1<0) {
00070         dOut1=0;
00071         dOut2=1;
00072         duty1= (abs(dCurrent1)*2.10-0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
00073     }
00074 
00075     if (duty1>0.99) {
00076         duty1 =1;
00077     }
00078 
00079     pwmOut1.write(duty1);
00080         // Convert to Radians
00081  
00082 
00083 
00084 
00085  
00086      dAngle2 =motor1.getPulses()/1200.0*6.283;;
00087     dAngularVelocity1=AngularVelocity2;
00088     preAngle2=Angle2;
00089     Angle2 = motor2.getPulses();
00090     Angle2 =Angle2/1200.0*6.283;
00091     AngularVelocity2 =(Angle2-preAngle2)/CurrentControlPeriod;
00092     mCurrent2  = aIn2.read()*3.3/.14;
00093     
00094     dCurrent2 = 3*(dAngle2-Angle2)+0.04*(dAngularVelocity2-AngularVelocity2); //0.016-p  gain 0.04- D gain
00095 
00096     if (dCurrent2>0) {
00097         dOut3=1;
00098         dOut4=0;
00099         duty2= (abs(dCurrent2)*2.10+0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
00100     } else if (dCurrent2<0) {
00101         dOut3=0;
00102         dOut4=1;
00103         duty2= (abs(dCurrent2)*2.10-0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
00104     }
00105 
00106     if (duty2>0.99) {
00107         duty2 =1;
00108     }
00109 
00110     pwmOut2.write(duty2);
00111 
00112 
00113 
00114 }
00115 
00116 void Save()
00117 {
00118      fprintf(fp, "%F,%F\n",mCurrent1,Angle1);
00119 }
00120 
00121 
00122 
00123 Ticker control;
00124 Ticker save;
00125 int main()
00126 {
00127     alpha=0.0;
00128     CurrentControlPeriod=0.001;
00129     DataSavingPeriod=0.005;
00130 
00131     pwmOut1.period(0.0001);
00132     pwmOut2.period(0.0001);
00133 
00134     //Set PWM output freqency to 20 kHz
00135     control.attach(&Control,CurrentControlPeriod);
00136 
00137     save.attach(&Save,DataSavingPeriod);
00138     wait (1);
00139 
00140 
00141     //  fclose(fp);
00142 
00143 }