File content as of revision 89:7f9d6e641a01:

// Roboter file


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

#include "IRSensor.h"

//Define I2C-Servoboard Consts
#define ADRESS          0x40
#define MODE1           0x00
#define MODE2           0x01
#define PRESCALE        0xFE
#define LED0_ON_L       0x06
#define LED0_ON_H       0x07
#define LED0_OFF_L      0x08
#define LED0_OFF_H      0x09

#define RESTART         0x80
#define SLEEP           0x10

#define servo0center    300
#define servo2center    300

DigitalIn user(USER_BUTTON);

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
Ticker t5;
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);

short previousValueCounterRight = 0;
short previousValueCounterLeft = 0;

float desiredSpeedLeft = 0;
float desiredSpeedRight = 0;

float actualSpeedLeft;
float actualSpeedRight;


//Periphery for distance sensors
AnalogIn distance(PB_1);
AnalogIn distance2(PA_1);
DigitalOut IRenable(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);

//Color sensor
DigitalIn red(PB_13);
DigitalIn green(PC_4);



bool get_user()
{
    return user;
}


void Robot_init_all()
{
    Speedcontroller_init();
    //Sensor_init();
    //init_servo(60);
}

//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;


    t5.attach( &speedCtrl, PERIOD);

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

void set_speed(float left, float right)
{
    enableMotorDriver = 0;    
    desiredSpeedLeft = left+correction_value; //50 RPM
    desiredSpeedRight = -right; //50 RPM
    enableMotorDriver = 1;
    printf("real speed set as: %f\r\n",desiredSpeedLeft);
}

float get_speed_left()
{
    return actualSpeedLeft;
}

float get_speed_right()
{
    return actualSpeedRight;
}


void speedCtrl()
{
    // printf("speed Ctrl called\r\n");
    // 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,&distance2, &bit0, &bit1, &bit2, ii);
    
    IRenable = 1;
}

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

//Color
bool get_color()
{
    if (red == 1 && green == 0) {   //Active - LOW
        return 1;
    } else {
        return 0;
    }
}

//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 init_servo(int freq)
{
    char data[2];
    //Reset
    data[0] = (char) MODE1;
    data[1] = (char) SLEEP;
    i2c.write((ADRESS<<1), data,2);

    wait_ms(1);

    float prescaleval = 25000000.0;     //25MHz
    prescaleval /= 4096.0f;             //12-Bit
    prescaleval /= (float)freq;
    prescaleval -= 1.0f;
    char prescale = (char)(prescaleval);    //0x64 bei 50Hz

    data[0] = (char) PRESCALE;
    data[1] = prescale;
    i2c.write((ADRESS<<1), data,2);

    data[0] = (char) MODE1;
    data[1] = 0x00;             //wake up
    i2c.write((ADRESS<<1), data,2);

    wait_ms(1);

    data[0] = (char) MODE1;
    data[1] = 0x80;             //do restart
    i2c.write((ADRESS<<1), data,2);

    wait_ms(1);

    data[0] = (char) MODE2;
    data[1] = (char) 0x04;
    i2c.write((ADRESS<<1), data,2);

    wait_ms(1);

    printf("init done...\r\n");
}

void set_servo_position(int servo, int deg)
{
    // Servo 0 = IR_left
    // Servo 2 = IR_right
    // Servo 4 = Arm_1
    // Servo 6 = Arm_2
    // Servo 8 = Grabber
    if (servo == 0) {
        deg = servo0center + 3*deg;
    }
    if (servo == 2) {
        deg = servo2center + 3*deg;
    }
    
    //only for tests with putty
    if (deg < 0 || deg > 4095) {
        deg = 300;
    }  
    char data[2];
    int16_t wert1 = (deg>>8);
    int16_t wert2 = (deg & 0xFF);
    printf("%x %x servo deg\r\n",wert1,wert2);

    data[0] = (char)LED0_ON_L+(4*servo);
    data[1] = 0x00;
    i2c.write((ADRESS<<1), data,2);

    data[0] = (char)LED0_ON_H+(4*servo);
    data[1] = 0x00;
    i2c.write((ADRESS<<1), data,2);

    data[0] = (char)LED0_OFF_L+(4*servo);
    data[1] = (char)wert2;
    i2c.write((ADRESS<<1), data,2);

    data[0] = (char)LED0_OFF_H+(4*servo);
    data[1] = (char)wert1;
    i2c.write((ADRESS<<1), data,2);
}

int getPWM(uint8_t servo)
{
    char read = 255;
    char secondread = 255;
    char data;
    data = (char)(LED0_ON_L+(4*servo));
    i2c.write((ADRESS<<1), &data, 1, 1);
    i2c.read((ADRESS<<1), &read, 1, 0);
    data = (char)(LED0_ON_H+(4*servo));
    i2c.write((ADRESS<<1), &data, 1, 1);
    i2c.read((ADRESS<<1), &secondread, 1, 0);
    printf("LED %d ON_L: %x ON_H: %x\r\n",servo, read, secondread);
    return (int)read;
}

int getPRESCALE(void)
{
    char read = 255;
    char data;
    data = (char)(PRESCALE);
    i2c.write((ADRESS<<1), &data, 1, 1);
    i2c.read((ADRESS<<1), &read, 1, 0);
    printf("read Prescale value: >>%x<<\r\n",read);
    return (int)read;
}

void enable_servos()
{
    Servo_enable = 0;
}

void disable_servos()
{
    Servo_enable = 1;
}