File content as of revision 44:7118b23b0fd7:

// Roboter file


#include "mbed.h"
#include "Robot.h"
#include "EncoderCounter.h"
#include "LowpassFilter.h"
#include "IMU.h"

#include "IRSensor.h"


const float PERIOD = 0.001f;                    // period of control task, given in [s]
const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
const float correction_value = 2.45f;           // correction value for desired speed

LowpassFilter speedLeftFilter;
LowpassFilter speedRightFilter;

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

DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);

DigitalOut my_led(LED1);

//Periphery for distance sensors
AnalogIn distance(PB_1);
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);

IRSensor sensors[6];

//indicator leds arround robot
DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };


//Periphery for the IMU
SPI spi(PC_12, PC_11, PC_10);
DigitalOut csG(PA_15);
DigitalOut csXM(PD_2);

IMU imu(spi, csG, csXM);

//Periphery for I2C
I2C i2c(PB_9, PB_8);
DigitalOut Servo_enable(PA_10);


short previousValueCounterRight = 0;
short previousValueCounterLeft = 0;

float desiredSpeedLeft = 0;
float desiredSpeedRight = 0;

float actualSpeedLeft;
float actualSpeedRight;




void Robot_init_all()
{
    Speedcontroller_init();
    Sensor_init();
}

//speed controll
void Speedcontroller_init()
{
    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
    pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
    pwmRight.period(0.00005f);
    pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
    pwmRight = 0.5f; // Duty-Cycle von 50%

    // Initialisieren von lokalen Variabeln
    previousValueCounterLeft = counterLeft.read();
    previousValueCounterRight = counterRight.read();
    speedLeftFilter.setPeriod(PERIOD);
    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
    speedRightFilter.setPeriod(PERIOD);
    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);

    desiredSpeedLeft = 0.0f;
    desiredSpeedRight = 0.0f;
    actualSpeedLeft = 0.0f;
    actualSpeedRight = 0.0f;

    Ticker t1;
    t1.attach( &speedCtrl, PERIOD);

    //desiredSpeedLeft = 50.0f+correction_value; //50 RPM
    //desiredSpeedRight = -50.0f; //50 RPM
    enableMotorDriver = 1;
}

void set_speed(float left, float right)
{
    desiredSpeedLeft = left+correction_value; //50 RPM
    desiredSpeedRight = -right; //50 RPM
}


void speedCtrl()
{
    // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
    short valueCounterLeft = counterLeft.read();
    short valueCounterRight = counterRight.read();
    short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
    short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;

    previousValueCounterLeft = valueCounterLeft;
    previousValueCounterRight = valueCounterRight;
    actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
    actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);

    // Berechnen der Motorspannungen Uout
    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;

    // Berechnen, Limitieren und Setzen der Duty-Cycle
    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
    if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
    else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;

    pwmLeft = dutyCycleLeft;
    float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
    if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
    else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;

    pwmRight = dutyCycleRight;
}

//Sensors
void Sensor_init()
{
    for( int ii = 0; ii<6; ++ii)
        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
}

float getDistanceIR(int number)
{
    return sensors[number];
}

//IMU
float read_acc_x()
{
    return imu.readAccelerationX();
}

float read_acc_y()
{
    return imu.readAccelerationY();
}

float read_acc_z()
{
    return imu.readAccelerationZ();
}

float read_gyr_x()
{
    return imu.readGyroX();
}

float read_gyr_y()
{
    return imu.readGyroY();
}

float read_gyr_z()
{
    return imu.readGyroZ();
}

float read_heading()
{
    double deg = (double)(180.0f/(double)M_PI*imu.readHeading());
    return deg;
}


// Servo I2C
void set_servo_position(int servo, uint16_t data){
    i2c.write((0x40), &data, 2);
    }
    
void init_i2c()
    
void enable_servos(){
    Servo_enable = 1;
    }
    
void disable_servos(){
    Servo_enable = 0;
    }