File content as of revision 17:76636aaf80de:

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

#define PI 3.14159265


// Different Modes für the serial protocol
static const uint8_t MODUS_PARKING = 1, MODUS_DRIVING = 2, MODUS_DRIVING_OBSTACLE = 3, MODUS_RC = 4, MODUS_NO_RC = 5;

static const uint8_t INIT_STATE = 0, DRIVE_STATE = 1, PARKING_STATE = 2;
uint8_t current_state = INIT_STATE;

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

DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);

DigitalIn buttonOne(p25), buttonDrivingObstacle(p26), buttonDriving(p29), buttonParking(p30);

IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;

QuadratureController *quadratureController;
MachineDirectionController *machineDirectionController;

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

// Queue
typedef struct {
    char identifier;
    uint8_t payload;
} serial_output_t;
//serial_output_t modus_struct;
//Queue<serial_output_t, 2> serial_output_modus_queue;
uint8_t modus;
Queue<uint8_t, 2> serial_output_modus_queue;

uint8_t current_modus;
Queue<uint8_t, 2> serial_input_modus_queue;


InterruptIn lightSensor(p12);

void hearbeat_thread(void const *args);

void serial_transmit_thread(void const *args)
{
    osEvent modus_event;
    uint8_t received_modus;
    while (true) {
        modus_event = serial_output_modus_queue.get();
        if (modus_event.status == osEventMessage) {
            received_modus = *((uint8_t *)modus_event.value.p);
            if (received_modus == MODUS_PARKING) {
                serialMinnow.printf("[m]=%d\r", received_modus);
            } else if (received_modus == MODUS_DRIVING) {
                serialMinnow.printf("[m]=%d\r", received_modus);
            } else if (received_modus == MODUS_DRIVING_OBSTACLE) {
                serialMinnow.printf("[m]=%d\r", received_modus);
            } else if (received_modus == MODUS_RC) {
                serialMinnow.printf("[r]=1\r");
            } else if (received_modus == MODUS_NO_RC) {
                serialMinnow.printf("[r]=0\r");
            }
        }
    }
}

void serial_receive_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;
    osEvent modus_event;
    uint8_t received_modus = MODUS_NO_RC;
    while (true) {
        while (serialMinnow.readable()) {
            if (ptr != end) {
                c = serialMinnow.getc();
                *ptr++ = c;
            } else {
                ptr = (char*)receive_buffer;
            }
            serialLED = !serialLED;
            if (c == '\r') {
                sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
                ptr = (char*)receive_buffer;
            }
            
            modus_event = serial_output_modus_queue.get(0);
            if (modus_event.status == osEventMessage) {
                received_modus = *((uint8_t *)modus_event.value.p);
            }
            
            if (x == 'v') {
                if (received_modus != MODUS_NO_RC) {
                    machine_direction_queue.put(&f);
                }

                ledTwo = true;
                // Buffer wieder auf Anfang setzen
                ptr = (char*)receive_buffer;
                x = 22;
            } else if (x == 'g') {
                if (received_modus != MODUS_NO_RC) {
                    f = -1*f;
                    quadrature_queue.put(&f);
                }
                ledThree = true;
                // Buffer wieder auf Anfang setzen
                ptr = (char*)receive_buffer;
                x = 22;
            }
        }

        Thread::wait(10);
    }
}

void redirect_machine_direction_controller(void const *args)
{
    machineDirectionController->cylic_control();
}

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

int main()
{
    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);
    machineDirectionController = new MachineDirectionController(&drivePWM, &machine_direction_queue, &imu_queue_velocity);

    lightSensor.rise(&Parking::increaseLightimpulseCounter);

    Thread serialTransmitThread(serial_transmit_thread);
    Thread machineDirectionControl(serial_receive_thread);
    Thread hearbeatThread(hearbeat_thread);
    Thread testParkingThread(Parking::parking_thread);

    RtosTimer machine_direction_control_timer(redirect_machine_direction_controller);
    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->writeImuConfig(configData);

    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();

        uint16_t rc_active_time = RadioDecoder_registerDataBuffer->channelActiveTime[0];
        uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];

        int8_t drive_percentage = RadioDecoder_registerDataBuffer->channelPercent[1];
        uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];

        int8_t steer_percentage = RadioDecoder_registerDataBuffer->channelPercent[2];
        uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];


        if (rc_active_time > (uint16_t) 1800 && rc_valid != 0) {
            // oben => Wettbewerb
            supportSystem->setLightManagerRemoteLight(false, true);
            modus = MODUS_NO_RC;
            serial_output_modus_queue.put(&modus);

        } else if (rc_active_time > (uint16_t) 1200 && rc_valid != 0) {
            // unten => RC-Wettbewerb
            supportSystem->setLightManagerRemoteLight(true, true);
            Thread::wait(1000);
            modus = MODUS_RC;
            serial_output_modus_queue.put(&modus);
            if (drive_valid) {
                velocity_minnow_queue = 0.3 * drive_percentage;
                machine_direction_queue.put(&velocity_minnow_queue);
            }
            if (steer_valid) {
                steerPWM.pulsewidth_us(steer_percentage);
            }
        } else if (rc_active_time > (uint16_t) 800 && rc_valid != 0) {
            // mitte => RC-Training
            supportSystem->setLightManagerRemoteLight(true, true);
            modus = MODUS_RC;
            serial_output_modus_queue.put(&modus);
            if (drive_valid) {
                velocity_minnow_queue = 2.0 * drive_percentage;
                machine_direction_queue.put(&velocity_minnow_queue);
            }
            if (steer_valid) {
                steerPWM.pulsewidth_us(steer_percentage);
            }
        }

        if (current_state == INIT_STATE) {
            // Initializing State
            // TODO: Start and Initialize Controllers (velocity = 0), (gamma = 0)
            if (buttonDriving) {
                current_state = DRIVE_STATE;
            } else if (buttonDrivingObstacle) {
                // No Action until now
            } else if (buttonParking) {
                current_state = PARKING_STATE;
            }
        } else if (current_state == DRIVE_STATE) {
            // Driving State
            serial_output_modus_queue.put(&current_state);
        } else if (current_state == PARKING_STATE) {
            // Parking State
            // TODO: Start Parking Thread
            serial_output_modus_queue.put(&current_state);
        }

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

        Thread::wait(10);
    }
}

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