File content as of revision 53:453f24775644:

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

#include "Movement.h"

int moving()
{

    return 0;
}

int move_forward_for_distance(float distance)
{

    return 0;
}

int move_backward_for_distance()
{

    return 0;
}

int turn_for_deg(float deg)
{

    return 0;
}


int move_to_next_coord()
{

    float current_heading = get_current_heading();
    position current_pos = get_current_pos();
    position next_pos = get_next_pos();
    
    float needed_heading = 0;
    float distance = 0;
    
    // nord(-y) = 0 grad
    if(current_pos.y > next_pos.y){
        if(current_pos.x > next_pos.x) needed_heading = 315; distance = sqrt2;
        if(current_pos.x == next_pos.x) needed_heading = 0; distance = 1;
        if(current_pos.x < next_pos.x) needed_heading = 45; distance = sqrt2;
        }
    if(current_pos.y == next_pos.y){
        if(current_pos.x > next_pos.x) needed_heading = 270; distance = 1;
        if(current_pos.x == next_pos.x) //error same position;
        if(current_pos.x < next_pos.x) needed_heading = 90; distance = 1;
        }
    if(current_pos.y < next_pos.y){
        if(current_pos.x > next_pos.x) needed_heading = 225; distance = sqrt2;
        if(current_pos.x == next_pos.x) needed_heading = 180; distance = 1;
        if(current_pos.x < next_pos.x) needed_heading = 135; distance = sqrt2;
        }

    if(needed_heading != current_heading) {
        turn_for_deg((needed_heading-current_heading));
    } else {  
        move_forward_for_distance(distance);
    }
    return 0;
}

int move_in_search_for_brick()
{

    return 0;
}





















/*

#include "mbed.h"
#include "movement.h"
#include "EncoderCounter.h"

static double time_counter = 0.0f;
static double timer0 = 0.0f;
static float PID_correction_value = 1.0f;

static float power_value_slow = 0.6f;
static float power_value_medium = 0.7f;
static float power_value_fast = 0.8f;
static float ludicrous_value = 1.0f;

//Motor Encoders
EncoderCounter counterLeft(PB_6, PB_7);
EncoderCounter counterRight(PA_6, PC_7);

//motor stuff
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmL(PA_8);
PwmOut pwmR(PA_9);
DigitalIn motorDriverFault(PB_14);
DigitalIn motorDriverWarning(PB_15);

//DigitalOut led(LED1); // Board LED

DigitalOut led(LED1); // Board LED


// ******************************************************************************

void move_init()
{
    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
    pwmR.period(0.00005f);

    pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
    pwmR.write(0.5f);
    enableMotorDriver = 1;

    PID_correction_value = 1.0f;

}
// ******************************************************************************
void move_forward_slow(float correction_value)
{
    pwmL.write(power_value_slow);
    pwmR.write(correction_value);
    printf("Left: %f || Right: %f    value:%f \r\n",pwmL.read(), pwmR.read(), correction_value);

}

void move_forward_medium(float correction_value)
{
    pwmL = power_value_medium;
    pwmR = 1-power_value_medium*correction_value;
}

void move_backward_slow(float correction_value)
{
    pwmL = 1-power_value_slow*correction_value;
    pwmR = power_value_slow;
}

void move_backward_medium(float correction_value)
{
    pwmL = 1-power_value_slow*correction_value;
    pwmR = power_value_slow;

}
// ******************************************************************************

void stop_movement()
{
    pwmL = 0.5f;
    pwmR = 0.5f;
    counterLeft.reset();
    counterRight.reset();
}

void sync_movement(bool speed, bool direction)
{
 if(counterLeft.read() > 30000 || -counterRight > 30000){

     }
 printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
    if(counterLeft.read() > -counterRight.read()) {
        PID_correction_value += 0.001f;
    } else {
        if(counterLeft.read() < -counterRight.read()) {
            PID_correction_value -= 0.001f;
        } else {
            // even
        }
    }

    if(PID_correction_value < 0.0f) {
        PID_correction_value = 0;
    }
    if(PID_correction_value > 2.0f) {
        PID_correction_value = 2;
    }

// Call movement:
// direction 0 = backward, direction 1 = forward
// speed 0 = slow, speed 1 = medium

    if(direction && speed) {
        move_forward_medium(PID_correction_value);
    }
    if(direction && !speed) {
        float value = 1.0f-power_value_slow*PID_correction_value;
        move_forward_slow(value);
    }
    if(!direction && speed) {
        move_backward_medium(PID_correction_value);
    }
    if(!direction && !speed) {
        move_backward_slow(PID_correction_value);
    }
}

void terminate_movement()
{
    PID_correction_value = 1.0f;
    pwmL.write(0.5f);
    pwmR.write(0.5f);
}
*/