File content as of revision 3:e6d345973797:

#include "mbed.h"
#include "PT1_Controller.h"
#include "Target_Planer.h"
#include "Motion.h"
 
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 = 40;
double Pathx[N] = {7.3000, 7.2500, 7.2000, 7.1500, 7.1000, 7.0500, 7.0000, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500, 6.2000, 6.1500, 6.1000, 6.0500, 6.0000, 5.9500, 5.9000, 5.8500, 5.8000, 5.7500, 5.7000, 5.6500, 5.6000, 5.5500, 5.5000, 5.4500, 5.4000, 5.3500};
double Pathy[N] = {6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500};
double R = 0.116959064327485;
Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
double RobotPosx = 4.35;
double RobotPosy = 6.19;
double TargetPosx = -999.0;
double TargetPosy = -999.0;
double TargetAng  = -999.0;

double pos0   = 179.0*pi/180.0;
float  vel0   = 0.0f*(float)pi/180.0f;
float  velMax = 90.0f*(float)pi/180.0f;
float  accMax = 60.0f*(float)pi/180.0f;
Motion motion_planer(pos0, vel0, velMax, accMax);
double targetPos = 0*pi/180.0;
float  targetVel = 0*(float)pi/180.0f;

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();
        
        target_planer.initialize(RobotPosx, RobotPosy);
                
        /*
        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 < 200) {
            // 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);
            // pc.printf("%i; %.12f; %.12f; %.12f; %i; %i; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, target_planer.return_i_min_(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
            // target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
            
            pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f;\r\n", i, motion_planer.getTimeToPosition(targetPos), motion_planer.getDistanceToStop(), motion_planer.getPosition(), motion_planer.getVelocity(), motion_planer.getAcceleration(), dt);
            motion_planer.incrementToPosition(targetPos, Ts);
        }
        
        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));
}