Diff: main.cpp

Revision:

0:0b04f8ba31ca

Child:

1:693bacef296b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 19 11:44:56 2018 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+DigitalOut motor_dir(D7);
+
+AnalogIn pot1(A2);
+AnalogIn pot2(A3);
+
+double Ts = 0.01; //Sample time in seconds
+Serial pc(USBTX,USBRX);    
+pc.baud(115200); 
+pi = 3.14159261;
+QEI Encoder(D12,D13,NC,32);
+double countsrotation = 32;
+double gear_ration = 131;
+int main()
+{
+    
+//Determine reference position in radians between 0 and 2pi
+    
+        double pos_ref = pot1 * 2pi;
+        pc.printf("%f \r\n",pos_ref);
+        
+// get actual position(measure plant output)
+    
+        int counts;       
+        
+        pos_counts = Encoder.getPulses();
+        double pos_offset_counts = 0;
+        double counts_per_rad = countsrotation / gear_ratio;
+        
+        double pos_rad = - (pos_counts - pos_offset_counts) / counts_per_rad;
+        
+// P controller    
+    
+        double e = pos_ref - pos_rad;
+        
+        double Kp = potmeter2.read();
+        double u_k = Kp * e;
+        
+        }
+   
\ No newline at end of file