File content as of revision 90:e4164bb8c60e:

////////////////////////////////////////////////////////////////////////////////
// Motion control unit
////////////////////////////////////////////////////////////////////////////////
// Takes current state and motion command (set via accessors) and actuates it via motor layer calls
////////////////////////////////////////////////////////////////////////////////

//todo: check everything is properly initialised and not a race condition

#include "motion.h"
#include "math.h"
#include "Kalman.h"
#include "MotorControl.h"
#include "supportfuncs.h"
#include "AvoidDstSensor.h"

namespace motion
{
    // private function prototypes
    
    void setWaypointReached();
    void clearMotionCmd();
    void clearWaypoint();
}

namespace motion
{

volatile int collavoiden = 0; // TODO: kill oskar's code
AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR);  //TODO: kill oskar's hack

// motion commands supported
enum motion_type_t { motion_waypoint };

struct motion_cmd
{
    motion_type_t motion_type;    
    osThreadId setter_tid;    
    
    bool motion_done;
    
    union 
    {
        Waypoint *wp_ptr;
    };

};

// local copy of the current active motion
motion_cmd current_motion = {motion_waypoint, 0, false, NULL};

Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to

Mutex waypoint_flag_mutex;

// local to waypoint motion handler
bool wp_d_reached = false;
bool wp_a_reached = false;


void motionlayer(void const *dummy)
{   
    switch (current_motion.motion_type)
    {
        case motion_waypoint:
            waypoint_motion_handler();
        break;
        default:
        break;
    }
}

void waypoint_motion_handler()
{
    // save target waypoint
    //Waypoint target_waypoint = *current_motion.wp_ptr;
    
    
    // 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);
    
    // reversing if close to target and more optimal than forward movement
    bool reversing = false;
    if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded
    {
        reversing = true;
        angle_err = constrainAngle(angle_err + PI);
        distance_err = -distance_err;
    }
    
    float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle
    
    // is the waypoint reached
    waypoint_flag_mutex.lock(); //TODO: consider refactoring, mutexes suck, switch to semaphore style or more contained mutexes (think of several producers as well); race conditions not checked atm either
    if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
    {
        wp_d_reached = true;
        
        angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
        
        if (abs(angle_err) < target_waypoint.angle_threshold)
        {
            wp_a_reached = true;
        }
    } 
     // 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 (wp_d_reached && wp_a_reached)
    {
        setWaypointReached();
    }
    waypoint_flag_mutex.unlock();
    
    // angular velocity controller
    const float p_gain_av = 1;//0.7; //TODO: tune
    
    const float max_av = 1;//0.5; // 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.95;//0.7; //TODO: tune
    
    float max_fv = 0.3;//0.2; // meters per sec //TODO: tune
    float max_fv_reverse = 0.03; //TODO: tune
    const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune
    
    // control, distance_err in meters
    float forward_v = p_gain_fv * distance_err;
    
    // if reversing, robot is less stable hence a different cap is used
    if (reversing)
        max_fv = max_fv_reverse;
    
    // control the forward velocity envelope based on angular error
    max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack
    
    // 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);
    
    //TODO: remove oskar's avoidance hack
    if(collavoiden)
    {
        float d = ADS.Distanceincm();
        if(d > 10)
        {
            forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f);
        }
    }
    // end of Oskar hack
    
    // pass values to the motor control
    MotorControl::set_fwdcmd(forward_v);
    MotorControl::set_omegacmd(angular_v);
}


bool checkMotionStatus()
{
    return current_motion.motion_done;
}


void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp)
{
    clearMotionCmd();

    current_motion.setter_tid = setter_tid_in;
    current_motion.motion_type = motion_waypoint;

    target_waypoint = *new_wp;
}


void setWaypointReached()
{
    current_motion.motion_done = true;
    osSignalSet(current_motion.setter_tid,0x1);
}


void clearMotionCmd()
{
    current_motion.motion_done = false;
    osSignalClear(current_motion.setter_tid, 0x1);

    switch (current_motion.motion_type)
    {
        case motion_waypoint:
            clearWaypoint();
        break;
        
        default:
        break;   
    }
}


void clearWaypoint()
{
    wp_d_reached = false;
    wp_a_reached = false;
}

} //namespace