Diff: main.cpp

Revision:

18:0e8ad413a840

Parent:

17:76636aaf80de

--- a/main.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ b/main.cpp	Mon Feb 08 15:37:06 2016 +0000
@@ -1,276 +1,21 @@
 #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();
-
+int main() {
     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);
-
+    supportSystem->setLightManagerRemoteLight(true, false);
+    supportSystem->setLightManagerBrakeLight(true, false);
+    supportSystem->setLightManagerSignalBoth(true, true);
     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);
     }
 }
\ No newline at end of file