File content as of revision 21:cb40c0533bc2:

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


#include "mbed.h"
#include "move.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;

DigitalOut led(LED1); // Board LED

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

InterruptIn EncoderLeftA(PB_6);
InterruptIn EncoderLeftB(PB_7);
InterruptIn EncoderRightA(PA_6);
InterruptIn EncoderRightB(PA_7);

static int EncoderCounterLeft = 0;
static int EncoderCounterRight = 0;


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

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

    EncoderCounterLeft = 0;
    EncoderCounterRight = 0;
    PID_correction_value = 1.0f;
}

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

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

void move_forward_fast(float correction_value)
{
    pwmL = power_value_fast;
    pwmR = 1-power_value_fast*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 move_backward_fast(float correction_value)
{
    pwmL = 1-power_value_slow*correction_value;
    pwmR = power_value_slow;
}

void stop_movement()
{
    pwmL = 0.5f;
    pwmR = 0.5f;
}

void highPulseDetectedL()
{
    EncoderCounterLeft += 1;
}

void highPulseDetectedR()
{
    EncoderCounterRight +=1;
}




void sync_movement(bool speed, bool direction)
{

    EncoderLeftA.rise(&highPulseDetectedL);
    EncoderLeftB.rise(&highPulseDetectedL);
    EncoderRightA.rise(&highPulseDetectedR);
    EncoderRightB.rise(&highPulseDetectedR);


// PID correction Value calcualtion

    if(EncoderCounterLeft > EncoderCounterRight) {
        PID_correction_value -= 0.0001f;
        led = 1;
    } else {
        led = 0;
        PID_correction_value += 0.0001f;
    }

    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) {
        move_forward_slow(PID_correction_value);
    }
    if(!direction && speed) {
        move_backward_medium(PID_correction_value);
    }
    if(!direction && !speed) {
    move_backward_slow(PID_correction_value);
    }
}

void termiante_sync_movement()
{
    PID_correction_value = 1.0f;
}