Revision 18:0e8ad413a840, committed 2016-02-08

Comitter:

mborchers

Date:

Mon Feb 08 15:37:06 2016 +0000

Parent:

17:76636aaf80de

Commit message:

Support System Test

Changed in this revision

--- a/Controller/MachineDirectionController.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,60 +0,0 @@
-#include "Controller/MachineDirectionController.h"
-
-MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity) {
-    this->pwmOut = pwmOut;
-    this->machine_direction_queue = machine_direction_queue;
-    this->imu_queue_velocity = imu_queue_velocity;
-    init();
-}
-
-void MachineDirectionController::init() {
-    timer_velocity_sampling_time=0.01;
-    max_velocity = 2.0;
-    l_Ki=0.1*timer_velocity_sampling_time;
-}
-
-void MachineDirectionController::check_queues() {
-    velocity_set_event = machine_direction_queue->get(0);
-    if (velocity_set_event.status == osEventMessage) {
-        velocity_set = *(float*)velocity_set_event.value.p;
-    }
-    
-   /* velocity_current_event = imu_queue_velocity->get(0);
-    if (velocity_current_event.status == osEventMessage) {
-        velocity_current = *(float *)velocity_current_event.value.p;
-    }*/
-}
-
-void MachineDirectionController::cylic_control() {
-    check_queues();
-    if( velocity_set < 0.1) {
-        velocity_set = velocity_set_alt;
-    } else if(velocity_set > max_velocity){
-        velocity_set = max_velocity;
-    }
-    
-    velocity_set = 0.5;
-    
-    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;
-    l_PWM=1500+Vorsteuerung;
-    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;
-    }
-    velocity_set_alt=velocity_set;
-    
-    //pwmOut->pulsewidth_us(1700);
-    pwmOut->pulsewidth_us(l_PWM);
-}
\ No newline at end of file

--- a/Controller/MachineDirectionController.h	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef MACHINE_DIRECTION_CONTROLLER_H
-#define MACHINE_DIRECTION_CONTROLLER_H
-
-#include <mbed.h>
-#include <I2C.h>
-#include "rtos.h"
-
-/*
- * Necessary for strcut sizes
- */
-#pragma pack (1)
-
-class MachineDirectionController{
-
-private:
-    PwmOut *pwmOut;
-    Queue<float, 2> *machine_direction_queue;
-    Queue<float, 2> *imu_queue_velocity;
-    osEvent velocity_set_event, velocity_current_event;
-    
-    uint8_t timer_velocity_sampling_time;
-    
-    float velocity_set;
-    float velocity_current;
-    float l_esum, Vorsteuerung, PI_Regler, l_output, l_PWM, l_e, l_Kp, l_Ki, max_velocity;
-    float velocity_set_alt;
-    void init();
-    void check_queues();
-public:
-    MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity);
-    void cylic_control();
-};
-
-#endif
\ No newline at end of file

--- a/Controller/QuadratureController.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "Controller/QuadratureController.h"
-
-QuadratureController::QuadratureController(PwmOut *pwmOut, Queue<float, 2> *quadrature_queue, Queue<float, 2> *imu_queue_steering_angle) {
-    this->pwmOut = pwmOut;
-    this->quadrature_queue = quadrature_queue;
-    this->imu_queue_steering_angle = imu_queue_steering_angle;
-    init();
-}
-
-void QuadratureController::init() {
-    timer_steering_angle_sampling_time = 0.01;
-    
-    q_Kp = 0;
-    q_Ki = 18.6661236;
-    feed_forward_control_factor = 11.2921;
-    q_esum = 0;
-    feed_forward = 0;
-    q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time;
-}
-
-void QuadratureController::check_queues() {
-    steering_angle_set_event = quadrature_queue->get(0);
-    if (steering_angle_set_event.status == osEventMessage) {
-        steering_angle_set = *(float *)steering_angle_set_event.value.p;
-    }
-    
-    steering_angle_current_event = imu_queue_steering_angle->get(0);
-    if (steering_angle_current_event.status == osEventMessage) {
-        steering_angle_current = *(float *)steering_angle_current_event.value.p;
-    }
-}
-
-void QuadratureController::cylic_control() {
-    check_queues();
-    
-    q_e = steering_angle_set - steering_angle_current;
-    q_esum = q_esum + q_e;
-    
-    feed_forward = steering_angle_set * feed_forward_control_factor;
-    q_PI_controller = q_Kp*q_e + q_Ki_sampling_time * q_esum;
-
-    q_output = feed_forward + q_PI_controller;
-
-    if(q_output > 500){q_output = 500;}           // evtl Begrenzung schon auf z.b. 300/ -300 stellen (wegen Linearität)
-    else if(q_output < -500){q_output = - 500;}
-
-    q_PWM = 1500 + q_output;
-    
-    pwmOut->pulsewidth_us(q_PWM);
-}
\ No newline at end of file

--- a/Controller/QuadratureController.h	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,32 +0,0 @@
-#ifndef QUADRATURE_CONTROLLER_H
-#define QUADRATURE_CONTROLLER_H
-
-#include <mbed.h>
-#include <I2C.h>
-#include "rtos.h"
-
-/*
- * Necessary for strcut sizes
- */
-#pragma pack (1)
-
-class QuadratureController{
-
-private:
-    PwmOut *pwmOut;
-    Queue<float, 2> *quadrature_queue;
-    Queue<float, 2> *imu_queue_steering_angle;
-    osEvent steering_angle_set_event, steering_angle_current_event;
-    
-    uint8_t timer_steering_angle_sampling_time;
-    float q_Kp, q_Ki, feed_forward_control_factor, q_esum, feed_forward, q_Ki_sampling_time;
-    float q_PI_controller, q_PWM, q_e, q_output, steering_angle_set, steering_angle_current;
-    
-    void init();
-    void check_queues();
-public:
-    QuadratureController(PwmOut *pwmOut, Queue<float, 2> *quadrature_queue, Queue<float, 2> *imu_queue_steering_angle);
-    void cylic_control();
-};
-
-#endif
\ No newline at end of file

--- a/Modus/Parking.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "Modus/Parking.h"
-
-int Parking::lightimpulseCounter;
-
-void Parking::increaseLightimpulseCounter(){                     
-    lightimpulseCounter++;
-}
-
-int Parking::getDistance(void){                                  
-    int distance = lightimpulseCounter * 4;
-    lightimpulseCounter = 0;
-    return distance;
-}
-
-void Parking::parking_thread(void const *args) {
-    float size_parking_space;
-    
-    if(Parking::redlight == 1){
-        while(true){
-            do{
-            }while(Parking::redlight == 1);
-                Parking::getDistance();
-            do{
-            }while(Parking::redlight == 0);
-            
-            size_parking_space = Parking::getDistance();
-            //serialMinnow.printf("Parklücke[mm]: %f\r\n", size_parking_space);
-                
-            if(size_parking_space < 600 && size_parking_space > 500){
-                    
-                drivePWM.pulsewidth_us(1700);   //Stück nach vorne fahren
-                wait_ms(400);
-                drivePWM.pulsewidth_us(1500);   //Stehen
-                wait_ms(200);
-                drivePWM.pulsewidth_us(1325);   //1. Einschlag voll rechts
-                steerPWM.pulsewidth_us(2000);
-                wait_ms(970);
-                steerPWM.pulsewidth_us(1000);   //2. Einschlag - voll links
-                wait_ms(950);
-                drivePWM.pulsewidth_us(1700);   //Korrektur nach vorne - leicht rechts
-                steerPWM.pulsewidth_us(1700);
-                wait_ms(420);
-                drivePWM.pulsewidth_us(1500);   //Ende
-                steerPWM.pulsewidth_us(1500);
-                //3mal aufblinken der Blinker
-                break;
-             }
-        }
-    }
-}
\ No newline at end of file

--- a/Modus/Parking.h	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-#ifndef PARKING_THREAD_H
-#define PARKING_THREAD_H
-
-#include <mbed.h>
-#include <I2C.h>
-#include "rtos.h"
-
-/*
- * Necessary for strcut sizes
- */
-#pragma pack (1)
-
-class Parking{
-public:
-    static DigitalIn redlight(p16);
-    static int lightimpulseCounter;
-    static int getDistance();
-    static void parking_thread(void const *args);
-    static void increaseLightimpulseCounter();
-};
-
-#endif
\ No newline at end of file

--- a/Starter.h	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,6 +0,0 @@
-#ifndef STARTER_H
-#define STARTER_H
-
-
-
-#endif
\ No newline at end of file

--- a/Threads/MachineDirectionControl.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,7 +0,0 @@
-#include "Threads/MachineDirectionControl.h"
-
-void machine_direction_control_thread(void const *args) {
-    while (true) {
-        Thread::wait(100);
-    }
-}
\ No newline at end of file

--- a/Threads/MachineDirectionControl.h	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,10 +0,0 @@
-#ifndef MACHINE_DIRECTION_CONTROL_H
-#define MACHINE_DIRECTION_CONTROL_H
-
-#include <mbed.h>
-#include <I2C.h>
-#include <rtos.h>
-
-void machine_direction_control_thread(void);
-
-#endif
\ No newline at end of file

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