movement.cpp

movement.cpp

#include "movement.h"

// forward declared dependencies
//===================================
#include "strategy.h"
#include "detection.h"

// constructors
//===================================
Movement::Movement() {
    status = 0;
    vmax = 0.5;
    vmag = 0.5;
    turn.memory = false;

    // velp
    dr = 0.0;
    dtheta = 0.0;
    dtheta2 = 0.0;
    dr_arrived = 0.05;
    kw = 3.0;
}

// getters
//===================================
Point Movement::getVelocity(){
    return current_velocity;
}

// setters
//===================================


// monitors
//===================================
int Movement::monitor() {
    // Set tolerances:
    float arrived_tol = 0.05;          // tollerance for arrived condition [m].
    float arrived_tol_theta = 2*PI/360;// angular tollerance for arrived condition [rad].
    float v_standing_still = 0.04;     // standing still velocity tollerance [m/s].

    // Determine output status based on robot position/movement:
    if(type == 0){
        if(std::abs(A.x-B.x) < arrived_tol && std::abs(A.y-B.y) < arrived_tol && std::abs(A.a-B.a) < arrived_tol_theta ){
            status = 1;     // A == B, robot has arrived at location.
        }
        else if(current_velocity.x < v_standing_still && std::abs(A.x-B.x) > arrived_tol && std::abs(A.y-B.y) > arrived_tol){
            status = 2;     // Robot is driving backwards (due to collision avoidance), ask for new set-point.
        }
        else{
            status = 0;     // everything is OK, robot is moving towards B.
        }
    }
    else if(type == 2){

        if(move == 0){
            status = 0;
        }
        else if(move == 1){
            status = 1;
            std::cout << "turn complete" << std::endl;
        }

    }
    else if(type == 1){
        status = 0;
    }

    return status;
}

//int Movement::getStatus(){
//    return status;
//}

// configurators
//===================================
void Movement::setMaxvelocity(float maxvelocity) {
    this->vmax = maxvelocity;
}

// coordinator & composer
//===================================
void Movement::movement(emc::IO &io, emc::LaserData scan, Detection &det, Strategy &strat) {
    /* Function: Movement.cpp

    This function receives the current position of the robot A and the destination of the robot B in
    the global coordinate system, and adjusts the velocity of the robot so it moves in the direction
    of B. This is done either by using a non-linear controller.

    Inputs:
    emc::IO            &io : PICO oi-layer
    emc::LaserData     scan: PICO laserdata
    Coordinate         A   : current position of the robot (x,y,theta) in the global frame [m]/[rad].
    vector<Coordinate> B   : vector containing wanted positions of the robot (x,y,theta) in the global frame [m]/[rad].
    double&            v0  : current velocity magnitude [m/s]. This velocity is passed by reference and changed within
                             this funciton.

    Outputs:
    int status: integer indicating the movement status of the robot:
                0 = on its way
                1 = arrived at location
                2 = can't reach location due to obstacle in the way, need new set-point (B).

    */

    //-----------------------------------------------------------------------------//

   // Recieve movement type: point-to-point or homing, this is decided by strategy.
    type = strat.getMovementType(); //indicates type of movement (p2p,homing,oneeithy)
    command = strat.getHomingType(); // indicates the direction of homing (left, right, straight)

    // Receive current location:
    A = det.getCurrentLocation();

    // Get set-point location (only used for point2point)
    B = strat.getPoint();
    Point Bp = B;
    Point Ap = A;

    if(type == 0){// Add velocity vectors of point2point function and colision avoidance together:
        // Potential field for collision avoidance
        Movement::potf(io, scan);

        // Velocity profile for smooth movement with PICO
        Movement::velp(Ap, Bp);

        // Nonlinear controller for point to point movement
        Movement::nlc();

        current_velocity.x = velocity_p2p.x + velocity_colavoid.x;
        current_velocity.y = velocity_p2p.y + velocity_colavoid.y;
        current_velocity.a = velocity_p2p.a + velocity_colavoid.a;
    }
    else if(type == 1){ // Add velocity vectors of homing,
        // Homing for autonomous driving, without point-based nagivation
        Movement::homing(io, scan);

        current_velocity.x = velocity_homing.x;
        current_velocity.y = velocity_homing.y;
        current_velocity.a = velocity_homing.a;
    }
    else if(type == 2){ // Stops pico and turns 180 degrees

        Movement::oneeighty(io, scan);

        current_velocity.x = velocity_oneeighty.x;
        current_velocity.y = velocity_oneeighty.y;
        current_velocity.a = velocity_oneeighty.a;
    }
    else if(type == 3){ // set robot velocity to zero
        current_velocity.x = 0.0;
        current_velocity.y = 0.0;
        current_velocity.a = 0.0;
    }

    // Set robot velocity:
    io.sendBaseReference(current_velocity.x, current_velocity.y, current_velocity.a);
}


// computational entities
//===================================