File content as of revision 124:6ef4d9ed2e28:

/**
 * Movement function library
 * Handels Movement of the Robot
**/

#include "Movement.h"
#define OFFSET_GREIFER_TO_IRSENSOR 0.2                 // Constant for distance between front IR Sensor and the postion where the Greifer is in grabbing Position
#define OFFSET_WHEELS 0.09                              // Offset of the wheels from the max pos

bool is_moving = false;
float wanted_dist = 0;
bool is_turning = false;
float wanted_deg = 0;
bool direction = false;
float restdegAfterstop = 0;                                                              // Variable for Rest degree we still have to cover after e.g. 1 brick found but its not really a brick so we turn further until e.g 60 degrees covered.
float needed_heading = 0;
float distance_to_next_coord = 0;
float current_head = 0;
coordinates current_coordinates = {0};
coordinates next_coord = {0};

float TOLERANCE_BRICK_OR_OBSTACLE = 0.08f;                                       // Variable for Brick detection it sets how much upper sensor and lower can differ that its still detected as an obstacle and not a brick

Timer t;
Timer t8;                                                                       // timer used for waiting enough distance measurements

int search_state = 0;                                                           // state for Move in search for brick statemachine
int coord_move_state = 0;
int list_step = 0;
int sigma_brick = 0;                                                            // Count how many times a brick is detected in fine postioning in statemachine move in search for brick

float left = 0;
float right = 0;

bool devider = true;


int moving()
{

    return 0;
}

/**
 * Stops current movement immediately
**/
void stop_move()
{
    set_speed(0,0);
    wanted_dist = 0;
    is_moving = false;
}

/**
 * Stops current turn immediately
**/
void stop_turn()
{
    set_speed(0,0);
    wanted_deg = 0;
    is_turning = false;
}


/**
 * move for wanted distance on circle with a given radius
 * needs to be called until return < 0
 * if calling distance not 0: distance and radius initilisation.
 * by Claudio Citterio
**/
float move_for_distance_with_radius(float distance, float r)
{

    if(distance != 0) {

        is_moving = true;
        wanted_dist = fabsf(distance);

        float circumference = r*2*(float)M_PI;
        float circumference_inner = ((r-(float)OFFSET_WHEELS)*2*(float)M_PI);
        float circumference_outer = ((r+(float)OFFSET_WHEELS)*2*(float)M_PI);

        float max_speed = 50;
        float inner_speed = max_speed/circumference*circumference_inner;
        float outer_speed = max_speed/circumference*circumference_outer;

        //reduce outer speed to max speed
        float multiplier = 1.0f/inner_speed*max_speed;
        inner_speed *= multiplier;
        outer_speed *= multiplier;

        if(r < 0.21f) {
            outer_speed *= 0.8f;
            inner_speed *= 0.8f;
        }

        if(r != 0) {
            //move with turn
            if(distance > 0) {  //move forward
                direction = 1;
                left = outer_speed;
                right = inner_speed;
            } else {            //move backward
                direction = 0;
                left = -outer_speed;
                right = -inner_speed;
            }
        } else {
            //normal straight movement
            printf("move straight\r\n");
            if(distance > 0) {  //move forward
                direction = 1;
                left = max_speed;
                right = max_speed;
            } else {            //move backward
                direction = 0;
                left = -max_speed;
                right = -max_speed;
            }
        }

        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();
    } else {
        float speed_multiplier = 0.6f;
        if(wanted_dist < 0.10f && devider == true) {
            //printf("devided\r\n");
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            //printf("left: %f || right: %f\r\n", left, right);
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        float speed_right = get_speed_right();
        wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * ((fabsf(speed_left)+fabsf(speed_right))/2) * 0.1f;
        t.reset();

        if(wanted_dist <= 0) { //distance covered, Stop function
            set_speed(0,0);
            is_moving = false;
            t.stop();
        }
    }
    printf("remaining distance to cover: %f\r\n", wanted_dist);
    return wanted_dist;
}

/**
 * move for wanted distance
 * needs to be called until return < 0
 * if calling distance not 0: distance initilisation.
 * by Claudio Citterio
**/
float move_for_distance(float distance)
{
    printf("move for distance\r\n");
    if(distance != 0) {

        is_moving = true;

        wanted_dist = fabsf(distance);

        if(distance > 0) {  //move forward
            direction = 1;
            left = 50.0f;
            right = 50.0f;
        } else {            //move backward
            direction = 0;
            left = -50.0f;
            right = -50.0f;
        }
        printf("set speed %f\r\n", left);
        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();

    } else {
        float speed_multiplier = 0.6f;
        if(wanted_dist < 0.10f && devider == true) {
            //printf("devided\r\n");
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            //printf("left: %f || right: %f\r\n", left, right);
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        printf("speed left: %f\r\n", speed_left);
        wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
        t.reset();

        if(wanted_dist <= 0) { //distance covered, Stop function
            set_speed(0,0);
            is_moving = false;
            t.stop();
        }
    }
    printf("remaining distance to cover: %f\r\n", wanted_dist);
    return wanted_dist;
}

/**
 * turn for wanted degree
 * needs to be called until return < 0
 * if deg not 0: turn initilisation.
 * Claudio Citterio
**/
float turn_for_deg(float deg, float multiplier)
{

    if(deg != 0) {

        is_turning = true;
        wanted_deg = fabsf(deg);

        if(deg < 0) { // turn left
            direction = 1;
            left = -20.0f*multiplier;
            right = 20.0f*multiplier;
        } else { // turn right
            direction = 0;
            left = 20.0f*multiplier;
            right = -20.0f*multiplier;
        }
        set_speed(left, right);
        devider = true;
        t.reset();
        t.start();

    } else {
        float speed_multiplier = 0.6f;
        if(wanted_deg < 10.0f && devider == true) {
            devider = false;
            left = left * speed_multiplier;
            right = right * speed_multiplier;
            set_speed(left, right);
        }

        float speed_left = get_speed_left();
        wanted_deg -= 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f);
        t.reset();
        if(wanted_deg <= 0) {
            set_speed(0,0);
            is_turning = false;
            t.stop();
        }
    }
    printf("remaining deg %f\r\n", wanted_deg);
    return (wanted_deg);
}

/** has errors
 * moves to next coordinate from coordinate list
 * by Claudio Citterio
**/

int move_to_brick_by_coordlist()
{
    switch(coord_move_state) {
        case 0:
            // init move to brick by list
            list_step = 1;

            coord_move_state = 1;
            break;
        case 1:
            // get positions, coords, heading and distances
            current_head = get_current_heading();
            current_coordinates = get_current_coord();
            position next_position = walkpath[list_step];
            next_coord = pos_to_coord(next_position);

            coord_move_state = 2;
            break;
        case 2:
            // check if path is still possible with updated map or target reached

            if(next_position.x == 0 && next_position.y == 0) {
                // target reached
                coord_move_state = 0;
                return 47;
            }
            if(obstacle_list[next_position.x][next_position.y] != 0) {
                // path obstructed
                coord_move_state = 0;
                return 35;
            }
            list_step += 1;
            coord_move_state = 3;
            break;
        case 3:
            // calc new headings
            float x = next_coord.x - current_coordinates.x;
            float y = next_coord.y - current_coordinates.y;
            distance_to_next_coord = sqrt(x*x + y*y);

            needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;
            if (x < 0) needed_heading += 180;

            if(needed_heading != current_head) {
                coord_move_state = 5;
            } else {
                coord_move_state = 8;
            }
            break;

        case 5:
            // turn init with new heading
            turn_for_deg(needed_heading-current_head,1);
            coord_move_state = 6;
            break;
        case 6:
            //turn until new heading == heading
            if(turn_for_deg(0,1)<0) {
                stop_turn();
                coord_move_state = 8;
            }
            break;

        case 8:
            // move init with distance
            move_for_distance(distance_to_next_coord);
            coord_move_state = 9;
            break;
        case 9:
            // move until distance is covered
            if(move_for_distance(0)<0) {
                stop_move();
                coord_move_state = 1;
            }
            break;
    }
    return 45;
}


/**
 * this function searchs a nearby brick, moves towards it and grabbs it
 * by Tobias Berger, state machine by Claudio Citterio
 * returns 1 if brick found or 0 if nothing found
**/
int move_in_search_for_brick()
{

    float upper = getDistanceIR(2);                                             // get distance from upper max Sensor
    float lower = getDistanceIR(3);                                             // get distance from Lower max Sensor
    //printf("Current Search State: >%d<\r\n",search_state);
    switch (search_state) {
        case 0: //first cycle right
            turn_for_deg(60.0f,0.8f);                                           // call function and start turning
            search_state = 1;
            break;

        case 1: // turn right and check for obstacles
            if((lower<0.45f)&&(lower>0.08f)) {                                  // if something is in the range of 10 to 80cm at the lower Sensor
                if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) {          // and nothing is detected with the upper Sensor
                    stop_turn();
                    restdegAfterstop = turn_for_deg(0,1);                       // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
                    search_state = 30;                                          // go in Fine Positioning statemachine
                    printf("Brick first detetection lower: %f upper:%f",lower,upper);
                }
            } else {
                search_state = 1;                                               // go to same state continue turning
                if(turn_for_deg(0, 1) < 0) {                                    // when first 60degree rotation finished
                    stop_turn();
                    search_state = 4;                                           // go to init turn other direction bc nothing found
                }
            }
            break;


        case 2: // init continue turning for restdeg
            turn_for_deg(restdegAfterstop,0.8f);                                // call function and start turning for restdegrees after stop
            search_state = 1;                                                   // go back to turn and search
            break;

        case 4: // init turn left 120 deg
            turn_for_deg(-120.0f,0.8f);
            search_state = 5;
            break;

        case 5: // turn and search opposite direction
            if((lower<0.45f)&&(lower>0.05f)) {                                  // if something is in the range of 10 to 80cm at the lower Sensor
                if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) {          // and nothing is detected with the upper Sensor
                    stop_turn();
                    restdegAfterstop = (-1*turn_for_deg(0,1));                         // get restdegrees from turn function. if a brick is falsly detected we turn restdegAfterstop to finisch search turn
                    search_state = 30;                                            // goto fine postioning stateamchine
                    printf("Brick first detetection lower: %f upper:%f",lower,upper);
                }
            } else {
                search_state = 5;                                               // go to same state
                if(turn_for_deg(0, 1) < 0) {                                    // when 60degree rotation finished
                    stop_turn();
                    search_state = 100;                                          // error go to default state, bc nothing found
                }
            }
            break;

        case 6:// init continue turning for restdeg
            turn_for_deg((-1*restdegAfterstop),0.8f);                           // call function and start turning for restdegrees after stop
            search_state = 5;                                                   // go back to turn and search
            break;

//-finepositioning-------------------------------------------------------------------------------------------------------------------------------------------------------------
        case 30: // Start Statemachine fine positioning Init turn right for 3 degres
            turn_for_deg(0.5f,0.8f);
            search_state=31;
            break;

        case 31: // Wait until rotation finished
            if(turn_for_deg(0,0.8f)<0) {
                stop_turn();
                search_state=32;
            } else {
                search_state=31;                                       // As long as turn not finisched call case 1 wait until finished
            }
            break;


        case 32: // Measure if Brick or Obstacle
            if((lower<0.45f)&&(lower>0.08f)) {                                  // if something is in the range of 10 to 80cm at the lower Sensor
                if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) {         // and nothing is detected with the upper Sensor
                    sigma_brick+=1;                                                 // increment sigma brick

                }
            }
            search_state=33;
            break;

        case 33: // Init turn left direction for 3 degres
            turn_for_deg(-1.0f,0.8f);
            search_state=34;
            break;

        case 34: // Wait until rotation left finished
            if(turn_for_deg(0,0.8f)<0) {
                stop_turn();
                printf("turn left after STOP");
                search_state=35;
            } else {
                search_state=34;                                                // As long as turn not finisched call case 1 wait until finished
            }
            break;

        case 35: // Measure if Brick or Obstacle
            if((lower<0.45f)&&(lower>0.08f)) {                                  // if something is in the range of 10 to 80cm at the lower Sensor
                if(fabsf((upper-lower))>TOLERANCE_BRICK_OR_OBSTACLE) {         // and nothing is detected with the upper Sensor
                    sigma_brick+=1;                                                 // increment sigma brick

                }
            }
            search_state=36;
            break;


        case 36: //  Init Turn Back right in Middle Position
            turn_for_deg(0.5f,0.8f);
            search_state = 37;
            break;

        case 37: // Turn until rotation finished
            if(turn_for_deg(0,0.8f)<0) {
                stop_turn();
                search_state=38;
            } else {
                search_state=37;                                       // As long as turn not finisched call case 1 wait until finished
            }

            break;


        case 38: // Evaluation
            if(sigma_brick > 1) {                                               // If a brick is detected more than once got to move
                sigma_brick = 0;                                                // reset sigma brick
                search_state = 50;                                              // go to move forward bc brick found
                printf("brick found in finepositioning");

            } else {
                printf("Nothing found in fine positioning");
                if(restdegAfterstop>0) {
                    sigma_brick = 0;                                                // reset sigma brick
                    search_state = 2;                                           // continue turning right bc nothing found
                } else {
                    sigma_brick = 0;                                                // reset sigma brick
                    search_state = 6;                                           // continue turning left bc nothing found
                }
            }
            break;

printf("SIGMA BRICK ---------------------------%d",sigma_brick);                  



// Move to found brick-----------------------------------------------------------------------------------------------

        case 50: // first cycle move forward
            float distance_to_Brick = lower-(float)OFFSET_GREIFER_TO_IRSENSOR;  // calculate
            move_for_distance(distance_to_Brick);
            search_state = 51;
            break;

        case 51: // move forward
            if(move_for_distance(0) < 0) {
                //Safety Function:
                if (getDistanceIR(2)<0.08f) {
                    stop_move();
                    //move_for_distance(-0.10f);
                    search_state = 0;
                }
                stop_move();
                search_state = 52;
            }
            break;

        case 52: // Grabbing
            return 50;
        break;                                                                  // main state machine set as Grabbing

        default:
            printf("default State - move in search for brick\r\n");
            return 60;                                                          // error nothing found go to Aeschlimans state
            // error
            break;
    }
    return 47;                                                                  //called until function is done
}