File content as of revision 37:34f4b38039bb:

////////////////////////////////////////////////////////////////////////////////
// Motion control unit
////////////////////////////////////////////////////////////////////////////////
// Takes current state of the robot and target waypoint,
// calculates desired forward and angular velocities and requests those from the motor control layer.
////////////////////////////////////////////////////////////////////////////////

#include "motion.h"
DigitalOut OLED4(LED4);
namespace motion
{

void motionlayer(void const *dummy)
{    
    // get target waypoint from AI
    Waypoint target_waypoint = *AI::current_waypoint;
    
    // get current state from Kalman
    State current_state = Kalman::getState();
    
    float delta_x = target_waypoint.x - current_state.x;
    float delta_y = target_waypoint.y - current_state.y;
    
    //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
    
    float distance_err = hypot(delta_x, delta_y);
    
    float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
    
    // is the waypoint reached
    if (distance_err < target_waypoint.pos_threshold)
    {
        distance_err = 0;
        
        if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < target_waypoint.angle_threshold)
        {
            angle_err = 0;
        }
    } 
        
    AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
    if (distance_err == 0 && angle_err == 0)
    {
        OLED4 = 1;
        AI::setWaypointReached();
        return;
    }
    AI::waypoint_flag_mutex.unlock();
    
    // angular velocity controller
    const float p_gain_av = 0.5; //TODO: tune
    
    const float max_av = 0.5*PI; // radians per sec //TODO: tune
    
    // angle error [-pi, pi]
    float angular_v = p_gain_av * angle_err;
    
    // constrain range
    if (angular_v > max_av)
        angular_v = max_av;
    else if (angular_v < -max_av)
        angular_v = -max_av;
    
    
    // forward velocity controller
    const float p_gain_fv = 0.5; //TODO: tune
    
    float max_fv = 0.2; // meters per sec //TODO: tune
    const float angle_envelope_exponent = 8.0; //TODO: tune
    
    // control, distance_err in meters
    float forward_v = p_gain_fv * distance_err;
    
    // control the forward velocity envelope based on angular error
    max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
    
    // constrain range
    if (forward_v > max_fv)
        forward_v = max_fv;
    else if (forward_v < -max_fv)
        forward_v = -max_fv;
        
    //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
    
    // pass values to the motor control
    MotorControl::set_fwdcmd(forward_v);
    MotorControl::set_omegacmd(angular_v);
}

} //namespace