Teleoperation code
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Mon Sep 26 2022 19:40:31 by
1.7.2