Diff: main2.cpp

Revision:

0:44e787b4e1d0

Child:

1:d2ed8dcf965a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main2.cpp	Sat Dec 12 12:19:03 2020 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "dsp.h"
+ 
+Ticker ticker;
+AnalogIn    inX(PC_2);
+AnalogIn    inY(PC_0);
+AnalogOut   outX(PA_4);
+AnalogOut   outY(PA_5);
+DigitalOut  out_led(D4);
+arm_pid_instance_f32 pidX;
+arm_pid_instance_f32 pidY;
+
+double Kp = 1.0;
+//
+void controlLop(void){
+    out_led = 1;
+    double outxx, outyy;
+    
+    outxx = inX.read()-0.5;
+    outyy = inY.read()-0.5;
+    
+    outX.write(Kp*outxx);
+    outY.write(Kp*outyy);
+    out_led = 0;
+}
+//
+void controlLoop(void)
+{
+    out_led = 1;
+    
+    //Process the PID controller
+    double outxx = arm_pid_f32(&pidX, inX.read()-0.5);
+    double outyy = arm_pid_f32(&pidY, inY.read()-0.5);
+    
+    //Range limit the output
+    if (outxx < -0.5)
+        outxx = -0.5;
+    else if (outxx > 0.5)
+        outxx = 0.5;
+    if (outyy < -0.5)
+        outyy = -0.5;
+    else if (outyy > 0.5)
+        outyy = 0.5;  
+    //Set the new output duty cycle
+    outX.write(outxx+0.5);
+    outY.write(outyy+0.5);
+    /*
+    outX.write(inX.read());
+    outY.write(inY.read());
+    */
+    out_led = 0;
+}
+ 
+int main()
+{
+    //Initialize the PID instance structure
+    pidX.Kp = 1.0;
+    pidX.Ki = 0.001;
+    pidX.Kd = 0.0;
+    arm_pid_init_f32(&pidX, 1);
+    pidY.Kp = 1.0;
+    pidY.Ki = 0.001;
+    pidY.Kd = 0.0;
+    arm_pid_init_f32(&pidY, 1);
+ 
+    //Run the PID control loop every 1ms
+    ticker.attach(&controlLoop, 0.0001);
+    //ticker.attach(&controlLop, 0.0001);
+ 
+    while(1) {
+        //Nothing to do here (except sleep maybe)
+    }
+}
\ No newline at end of file