File content as of revision 12:221c9e02ea96:

#include <mbed.h>
#include "rtos.h"
#include "Periphery/SupportSystem.h"
#include "Misc/SystemTimer.h"
#include "Controller/QuadratureController.h"

#define PI 3.14159265

Serial serialXbee(p28,p27);
Serial serialMinnow(p13, p14);
PwmOut drivePWM(p22);
PwmOut steerPWM(p21);
I2C i2c(p9, p10);

DigitalOut heartbeatLED(LED1);
DigitalOut buttonLED(LED2);
DigitalOut redlightLED(LED3);
DigitalOut led4(LED4);

DigitalIn buttonOne(p25);
DigitalIn buttonTwo(p26);
DigitalIn buttonThree(p29);
DigitalIn buttonFour(p30);

IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;

QuadratureController *quadratureController;

// Queues von der Bahnplanung
Queue<float, 2> quadrature_queue;
Queue<float, 2> machine_direction_queue;
float steering_angle_minnow_queue;
float velocity_minnow_queue;


// Queues von dem Maussensor
Queue<float, 2> imu_queue_velocity;
Queue<float, 2> imu_queue_steering_angle;
float steering_angle_imu_queue;
float velocity_imu_queue;


// Variablen von der Trajektorienplanung

float velocity_set = 0;

// Variablen für die Längsregelung
float velocity_current = 0, velocity_last = 0;

uint8_t timer_velocity_sampling_time=0.01;
float l_esum, Vorsteuerung, PI_Regler, l_output, l_PWM, l_e,l_Kp;
float l_Ki=0.1*timer_velocity_sampling_time;

// Querregelung Ende

void hearbeat_thread(void const *args);

void serial_thread(void const *args) {
    size_t length = 20;
    char receive_buffer[length];
    char* ptr = (char*) receive_buffer;
    char* end = ptr + length;
    char c, x = 22;
    float f;
    int return_value = 666;
    while (true) {
        if (ptr != end) {
            c = serialMinnow.getc();
            *ptr++ = c;
        } else {
            ptr = (char*)receive_buffer;
        }
        heartbeatLED = !heartbeatLED;
        
        if (c == '\r') {
            return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
            ptr = (char*)receive_buffer;
        }
        
        if (x == 'v') {
            machine_direction_queue.put(&f);
            serialXbee.printf("v:%f\r\n", f);
            
            buttonLED = true;
            // Buffer wieder auf Anfang setzen
            ptr = (char*)receive_buffer;
            x = 22;
        } else if (x == 'g') {
            quadrature_queue.put(&f);
            serialXbee.printf("g:%f\r\n\r\n", f);
            redlightLED = true;
            // Buffer wieder auf Anfang setzen
            ptr = (char*)receive_buffer;
            x = 22;
        }
        
        //Thread::wait(100);
    }
}

void machine_direction_control(void const *args) {
    osEvent velocity_set_event = machine_direction_queue.get(0);
    if (velocity_set_event.status == osEventMessage) {
        velocity_set = *(float*)velocity_set_event.value.p;
    }
    
    osEvent velocity_current_event = imu_queue_velocity.get(0);
    if (velocity_current_event.status == osEventMessage) {
        velocity_current = *(float *)velocity_current_event.value.p;
    }
    
    Vorsteuerung=88.316*velocity_set+175.17;
    l_e = velocity_set-velocity_current;
    l_esum = l_esum + l_e;

    PI_Regler =l_Kp*l_e+l_Ki * l_esum;

    l_output=Vorsteuerung+PI_Regler;

    l_PWM = 1500+l_output;

    if(l_PWM<1500) {
        l_PWM = 1500;
        l_esum = l_esum-2*l_e;
    } else if(l_PWM>2000) {
        l_PWM = 2000;
        l_esum = l_esum-2*l_e;
    } else if(velocity_set < 0.1) {
        l_PWM = 1500;
    }
    
    drivePWM.pulsewidth_us(1700);
    //drivePWM.pulsewidth_us(l_PWM);
}

void redirect_quadrature_controller(void const *args) {
    quadratureController->cylic_control();
}

int main() {
    serialXbee.baud(9600);
    serialMinnow.baud(115200);

    drivePWM.period_ms(20);
    steerPWM.period_ms(20);

    SystemTimer *millis = new SystemTimer();

    SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
    
    quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);

    Thread machineDirectionControl(serial_thread);
    Thread hearbeatThread(hearbeat_thread);
    
    RtosTimer machine_direction_control_timer(machine_direction_control);
    RtosTimer quadrature_control_timer(redirect_quadrature_controller);

    // Konfiguration AMF-IMU
    // [0]: Conversation Factor
    // [1]: Sensor Position X
    // [2]: Sensor Position Y
    // [3]: Sensor Position Angle
    float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};

    supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, configData, sizeof(float)*4);

    // Flag setzen
    uint8_t command = 1<<2;
    supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
    
    bool timer_started = false;

    wait(0.1);
    
    steering_angle_minnow_queue = 0;
    quadrature_queue.put(&steering_angle_minnow_queue);
    
    velocity_minnow_queue = 0;
    machine_direction_queue.put(&velocity_minnow_queue);
    
    machine_direction_control_timer.start(10);
    quadrature_control_timer.start(10);

    while(true) {
        IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
        RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();

        //for(uint8_t i=0; i<3; i++) {
        //    serialMinnow.printf("RadioDecoder - Ch[%d] Valid:      %d\r\n",i,RadioDecoder_registerDataBuffer->channelValid[i]);
        //    serialMinnow.printf("RadioDecoder - Ch[%d] ActiveTime: %d\r\n",i,RadioDecoder_registerDataBuffer->channelActiveTime[i]);
        //    serialMinnow.printf("RadioDecoder - Ch[%d] Percentage: %d\r\n\r\n",i,RadioDecoder_registerDataBuffer->channelPercent[i]);
        //}

        uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
        uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
        
        uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
        uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];

        uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
        uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];


        if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
            // oben => Wettbewerb
            //heartbeatLED = true;
            //buttonLED = false;
            //redlightLED = false;
            supportSystem->setLightManagerRemoteLight(false, true);
            if (!timer_started) {
                timer_started = true;
                machine_direction_control_timer.start(10);
                quadrature_control_timer.start(10);
            }
        } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
            // unten => RC-Wettbewerb
            //heartbeatLED = false;
            //buttonLED = false;
            //redlightLED = true;
            supportSystem->setLightManagerRemoteLight(true, true);
            if (drive_valid) {
                drivePWM.pulsewidth_us(drive_percentage);
            }
            if (steer_valid) {
                steerPWM.pulsewidth_us(steer_percentage);
            }
            if (timer_started) {
                timer_started = false;
                machine_direction_control_timer.stop();
                quadrature_control_timer.stop();
            }
        } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
            // mitte => RC-Training
            //heartbeatLED = false;
            //buttonLED = true;
            //redlightLED = false;
            supportSystem->setLightManagerRemoteLight(true, true);
            if (drive_valid) {
                drivePWM.pulsewidth_us(drive_percentage);
            }
            if (steer_valid) {
                steerPWM.pulsewidth_us(steer_percentage);
            }
            if (timer_started) {
                timer_started = false;
                machine_direction_control_timer.stop();
                quadrature_control_timer.stop();
            }
        }
        
        velocity_imu_queue = IMU_registerDataBuffer->velocityXFilteredRegister;
        imu_queue_velocity.put(&velocity_imu_queue);
        
        float radius = IMU_registerDataBuffer->velocityXFilteredRegister/IMU_registerDataBuffer->velocityAngularFilteredRegister;
        
        steering_angle_imu_queue = atan(0.205/radius)*180/PI;
        imu_queue_steering_angle.put(&steering_angle_imu_queue);
        

        //serialMinnow.printf("%ld, ", difference_millis);
        //serialMinnow.printf("%f, ", velocity_minnow_queue);
        //serialMinnow.printf("%f, ", steering_angle_minnow_queue);
        //serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
        //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
        Thread::wait(50);
    }
}

void hearbeat_thread(void const *args) {
    while(true) {
        led4 = !led4;
        Thread::wait(100);
    }
}