Diff: main.cpp

Revision:

0:c4bd1a2d961e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 26 19:40:10 2022 +0000
@@ -0,0 +1,143 @@
+//Ben Katz, September 2013
+
+#include "mbed.h"
+#include "QEI.h"
+#include <fstream>
+#include <iomanip>
+//Declaring pins
+LocalFileSystem local("local");
+Timer t;
+float analogSignal;
+float Angle1;
+float preAngle1=0;
+float Angle2;
+float preAngle2=0;
+
+float mCurrent1;
+float mCurrent2;
+float dCurrent1;
+float dCurrent2;
+float dAngle1;
+float dAngle2;
+float dAngularVelocity1=0;
+float dAngularVelocity2=0;
+float AngularVelocity1;
+float AngularVelocity2;
+float duty1;
+float duty2;
+float alpha;
+float CurrentControlPeriod;
+float DataSavingPeriod=0.005;
+
+float Kp=5;
+float Kd=0.04;
+float Kv=0.35;
+float R=2.2;
+float Ki=5;
+DigitalOut dOut1(p5);
+DigitalOut dOut2(p7);  //pin 5 set as a digital output  Pins 5-30 can be used as digital I/O.
+DigitalOut dOut3(p8);
+DigitalOut dOut4(p9);
+//pin 6 set as digital input.
+AnalogIn aIn1(p16);
+AnalogIn aIn2(p17);  //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
+PwmOut pwmOut1(p21);
+PwmOut pwmOut2(p22);   //pin 21 set as PWM output.  Pins 21-26 can be used as PWM outputs.
+QEI motor1(p27, p28, NC, 1200, QEI::X4_ENCODING);
+QEI motor2(p23, p24, NC, 1200, QEI::X4_ENCODING);
+//ofstream out;
+FILE *fp= fopen("/local/data.csv","w");
+
+//Serial pc(USBTX, USBRX);  //create a serial connection to the computer over the tx/rx pins
+
+void Control()
+{
+  dAngle1 =motor2.getPulses()/1200.0*6.283;;
+    dAngularVelocity1=AngularVelocity2;
+    preAngle1=Angle1;
+    Angle1 = motor1.getPulses();
+    Angle1 =Angle1/1200.0*6.283;
+    AngularVelocity1 =(Angle1-preAngle1)/CurrentControlPeriod;
+    mCurrent1  = aIn1.read()*3.3/.14;
+    
+     dCurrent1 = 3*(dAngle1-Angle1)+0.04*(dAngularVelocity1-AngularVelocity1)
+
+    if (dCurrent1>0) {
+        dOut1=1;
+        dOut2=0;
+        duty1= (abs(dCurrent1)*2.10+0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
+    } else if (dCurrent1<0) {
+        dOut1=0;
+        dOut2=1;
+        duty1= (abs(dCurrent1)*2.10-0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
+    }
+
+    if (duty1>0.99) {
+        duty1 =1;
+    }
+
+    pwmOut1.write(duty1);
+        // Convert to Radians
+ 
+
+
+
+ 
+     dAngle2 =motor1.getPulses()/1200.0*6.283;;
+    dAngularVelocity1=AngularVelocity2;
+    preAngle2=Angle2;
+    Angle2 = motor2.getPulses();
+    Angle2 =Angle2/1200.0*6.283;
+    AngularVelocity2 =(Angle2-preAngle2)/CurrentControlPeriod;
+    mCurrent2  = aIn2.read()*3.3/.14;
+    
+    dCurrent2 = 3*(dAngle2-Angle2)+0.04*(dAngularVelocity2-AngularVelocity2); //0.016-p  gain 0.04- D gain
+
+    if (dCurrent2>0) {
+        dOut3=1;
+        dOut4=0;
+        duty2= (abs(dCurrent2)*2.10+0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
+    } else if (dCurrent2<0) {
+        dOut3=0;
+        dOut4=1;
+        duty2= (abs(dCurrent2)*2.10-0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
+    }
+
+    if (duty2>0.99) {
+        duty2 =1;
+    }
+
+    pwmOut2.write(duty2);
+
+
+
+}
+
+void Save()
+{
+     fprintf(fp, "%F,%F\n",mCurrent1,Angle1);
+}
+
+
+
+Ticker control;
+Ticker save;
+int main()
+{
+    alpha=0.0;
+    CurrentControlPeriod=0.001;
+    DataSavingPeriod=0.005;
+
+    pwmOut1.period(0.0001);
+    pwmOut2.period(0.0001);
+
+    //Set PWM output freqency to 20 kHz
+    control.attach(&Control,CurrentControlPeriod);
+
+    save.attach(&Save,DataSavingPeriod);
+    wait (1);
+
+
+    //  fclose(fp);
+
+}