File content as of revision 136:b35f2d9b7402:

/**
 * 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
Timer t9;

int search_state = 0;
int coord_move_state = 0;
int list_step = 0;

float left = 0;
float right = 0;

bool devider = true;

int random_state = 0;
int swerve_state = 0;


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();
        printf("move for %f m\r\n", distance);

    } 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();
        printf("turn %f deg\r\n", deg);

    } 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];
            printf("next position: %d || %d\r\n",  next_position.x, next_position.y);
            next_coord = pos_to_coord(next_position);
            printf("next coordinate: %f || %f\r\n",  next_coord.x, next_coord.y);
            printf("list step: %d\r\n", list_step);
            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) {
                printf("target reached\r\n");
                coord_move_state = 0;
                return 47;
            }
            if(obstacle_list[next_position.x][next_position.y] != 0) {
                printf("path obstructed\r\n");
                coord_move_state = 0;
                return 35;
            }
            list_step += 1;
            coord_move_state = 3;
            break;
        case 3:
            // calc new heading and distance
            float x = (next_coord.x - current_coordinates.x) / 25.0f;
            float y = (next_coord.y - current_coordinates.y) / 25.0f;
            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;
            printf("current heading %f\r\n", current_head);
            printf("needed heading %f\r\n", needed_heading);
            if(needed_heading != current_head) {
                coord_move_state = 5;
            } else {
                coord_move_state = 8;
            }
            break;

        case 5:
            // turn init with new heading
            float turn_deg = needed_heading-current_head;
            if( turn_deg > 180.0f) turn_deg -= 360.0f;
            turn_for_deg(turn_deg,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
**/







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();
                    t8.reset();
                    t8.start();                                                 // start timer for enough measurements
                    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 = 2;                                           // brick found
                    printf("Brick first detetection lower: %f upper:%f",lower,upper);
                }
            } else {
                search_state = 1;                                               // go to same state
                if(turn_for_deg(0, 1) < 0) {                                    // when first 60degree rotation finished
                    stop_turn();
                    search_state = 4;                                           // go to init turn other direction
                }
            }
            break;



        case 2: // Check if Sensor after waiting still the same value
            if(t8.read() > 0.1f) {
                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
                        search_state = 10;                                          // When still the same go to move forward
                    } else {
                        search_state=3;                                             // When afterwait not the same go to continue turning
                    }
                }
            }
            break;


        case 3: // 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();
                    t8.reset();
                    t8.start();                                                 // start timer for enough measurements
                    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 = 6;                                            // brick found
                    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 = 20;                                          // error go to default state, bc nothing found
                }
            }
            break;

        case 6: // Check if Sensor after waiting still detect brick
            if(t8.read() > 0.1f) {
                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
                        search_state = 10;                                          // When still the same go to move forward
                    } else {
                        search_state=7;                                             // When afterwait not the same go to continue turning
                    }
                }
            }
            break;

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

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

        case 11: // 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 = 12;
            }
            break;

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

        default:
            printf("default State - move in search for brick\r\n");
            // error
            //if nothing found or something got wrong move random
            return 60;
    }
    return 47;                                                                  //called until function is done
}


int move_random()
{
    t9.start();
    float sign = 1.0f;
    float rad = 0.0f;
    float dist = 0.0f;
    float time = t9.read();
    time *=100;
    int time_sub = (int)time;
    time -= (float)time_sub;
    time /= 100;
    srand(time);
    switch (random_state) {
        case 0:
            if ((rand()%1 + 1.0f) < 0.5f) {
                sign = -1.0f;
            } else {
                sign = 1.0f;
            }
            rad = (rand()%1 + 1.0f) * 0.7f + 0.3f;
            rad *= sign;
            dist = (rand()%1 + 1.0f) * 0.5f + 0.2f;
            printf("radius %f || distance %f\r\n", rad, dist);
            move_for_distance_with_radius(dist, rad);
            //set sensor servos only once because wait is needed
            set_servo_position(2, 45);
            set_servo_position(0, -45);
            wait(0.5f);
            random_state = 1;
            break;
        case 1:
            float dist_ir = getDistanceIR(3);
            float dist_left = 1.0f;
            float dist_right = 1.0f;
            dist_left = getDistanceIR(4);
            dist_right = getDistanceIR(2);
            printf("Distance left %f || center %f || right %f\n\r",dist_left, dist_ir, dist_right);
            if (dist_ir < 0.1f || dist_left < 0.1f || dist_right <0.1f) {
                /*move backwards*/
                random_state = 2;
            }

            if(move_for_distance_with_radius(0,0) < 0 ) {
                random_state = 0;
                return 47;
            }
            break;
        case 2:
            move_for_distance(-0.10f);
            random_state = 3;
            break;
        case 3:
            if(move_for_distance(0) < 0) {
                random_state = 0;
                return 61;
            }
            break;
    }
    return 60;  //recall
}


int swerve_go()
{
    int dist_left = 0;
    int dist_right = 0;
    switch (swerve_state) {
        case 0:
            set_servo_position(2, 45);
            set_servo_position(0, -45);
            wait(0.5f);
            dist_left = getDistanceIR(4);
            dist_right = getDistanceIR(2);
            if (dist_left < dist_right) {
                swerve_state = 1;
            } else {
                swerve_state = 2;
            }
            break;
        case 1:
            turn_for_deg(45,1);
            swerve_state = 3;
            break;
        case 2:
            turn_for_deg(-45,1);
            swerve_state = 3;
            break;
        case 3:
            if(turn_for_deg(0,0) < 0) {
                swerve_state = 4;
            }
            break;
        case 4:
            move_for_distance(0.2f);
            swerve_state = 5;
            break;
        case 5:
            if (move_for_distance(0) < 0) {
                swerve_state = 0;
                return 60;
            }
            break;
    }
    return 61;  //recall
}