Diff: main.cpp

Revision:

0:937360eb9f8c

Child:

1:e79de7ffd211

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 04 10:24:29 2019 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "PT1_Controller.h"
+#include "Target_Planer.h"
+#include <vector>
+ 
+using namespace  std;
+
+#define   pi 3.141592653589793
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+double kpv = 0.6412;        // proportional controller for distance error
+double kpw = 0.6801;        // proportional controller for angle error
+double Ts = 1/30.0;         // samplingrate 30 Hz
+double Tf = 1/(2.0*pi*4.0); // fg = 4 Hz
+PT1_Controller distance_cntr(kpv, Ts, Tf);
+PT1_Controller angle_cntr(kpw, Ts, Tf);
+
+// (vRx (m/s), wR (rad/s)) -> (vRxRB (unit unknown), wRRB (unit unknown))
+// Cu2uRB = 2.8070         0
+//               0    0.9263
+double kv2RB = 2.8070; // maps desired forward velocity from m/s 2 RB unit
+double kw2RB = 0.9263; // maps desired turn speed from rad/s 2 RB unit
+
+Timer timer; // timer for time measurement
+float dt = 0.0f;
+
+uint32_t i;
+
+// user defined functions
+double quat2psi(double qw, double qx, double qy, double qz);
+double getShortRotation(double ang);
+
+const int N = 4;
+double Pathx[N] = {0, 2, 1, 4};
+double Pathy[N] = {0, 3, 0 ,1};
+double R = 0.16;
+Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
+double RobotPosx = 10.1;
+double RobotPosy = 10.1;
+double TargetPosx = 37;
+double TargetPosy = 23;
+
+int main()
+{
+    pc.baud(2000000);
+
+    timer.start();
+
+    i = 0;
+    
+    // reset internal filter storage to zero
+    distance_cntr.reset();
+    angle_cntr.reset();
+        
+    while(1) {
+        
+        dt = timer.read();
+        timer.reset();
+                
+        /*
+        double phiR = quat2psi(double qw, double qx, double qy, double qz); // transform actual robot orientation (quaternioni) into turning angle
+        // it is assumed that target_ang and phiR both lie between -pi and pi
+        double e_ang = getShortRotation(target_angle - phiR);
+        // update angle controller
+        */
+        // double qw = 0.984005578673161;
+        // double qx = 0.009063618716137;
+        // double qy = 0.177899336159591;
+        // double qz = 0.001629344754243;
+        
+        if(i < 4) {
+            // pc.printf("%i; %.12f; %.12f; %0.12f; %.6f;\r\n", i, quat2psi(qw, qx, qy, qz), qx*qy + qw*qz, 0.5 - (qy*qy + qz*qz), dt);
+            // pc.printf("%i; %i; %.6f;\r\n", i, target_planer.returnPathLength(), dt);
+            pc.printf("%i; %.12f; %.12f; %i; %i; %.6f;\r\n", i, target_planer.returnPathxAtIndex(i), target_planer.returnPathyAtIndex(i), target_planer.returnPathLength(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
+            pc.printf("%i; %.12f; %.12f;;\r\n", i, TargetPosx, TargetPosy);
+            target_planer.update(RobotPosx, RobotPosx);
+            target_planer.readTargetPos(&TargetPosx, &TargetPosy);
+        }
+        
+        i++;        
+        wait_us(33333);
+    }
+}
+
+/*
+function psi = quat2psi(q)
+% q = [qw qx qy qz] (= [q0, q1, q2, q3] = [qr, qi, qj, qk])
+    for i = 1:length(q) % restore norm
+        q(i,:) = q(i,:) / sqrt(q(i,1)^2 + q(i,2)^2 + q(i,3)^2 + q(i,4)^2);
+    end
+    psi   = atan2( (q(:,2).*q(:,3) + q(:,1).*q(:,4) ), 1/2 - (q(:,3).^2 + q(:,4).^2) );
+end
+*/
+double quat2psi(double qw, double qx, double qy, double qz)
+{
+    double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);    
+    return atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
+}
+
+double getShortRotation(double ang)
+{   
+    // we maybe need to introduce a threshold in case the angle is noisy and around +/-pi (180° rotation)
+    return atan2(sin(ang), cos(ang));
+}
+