Revision 13:34f7f783ad24, committed 2016-02-05

Comitter:

mborchers

Date:

Fri Feb 05 16:06:44 2016 +0000

Parent:

12:221c9e02ea96

Commit message:

L?ngsregelung ausgelagert

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/MachineDirectionController.cpp	Fri Feb 05 16:06:44 2016 +0000
@@ -0,0 +1,53 @@
+#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;
+    
+    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();
+    
+    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;
+    }
+    
+    pwmOut->pulsewidth_us(1700);
+    //pwmOut.pulsewidth_us(l_PWM);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/MachineDirectionController.h	Fri Feb 05 16:06:44 2016 +0000
@@ -0,0 +1,34 @@
+#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;
+    
+    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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SerialOutputLogger.cpp	Fri Feb 05 16:06:44 2016 +0000
@@ -0,0 +1,34 @@
+#include "Misc/SerialOutputLogger.h"
+
+SerialOutputLogger::SerialOutputLogger(Serial *serialPort, IMU_RegisterDataBuffer_t *imuRegisterDataBuffer, RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer) {
+    this->serialPort = serialPort;
+    this->imuRegisterDataBuffer = imuRegisterDataBuffer;
+    this->radioDecoderRegisterDataBuffer = radioDecoderRegisterDataBuffer;
+}
+
+void SerialOutputLogger::writeRadioDecoderDataLog(bool csv) {
+    if (csv) {
+        for(uint8_t i=0; i<3; i++) {
+            serialPort->printf("%d, ", radioDecoderRegisterDataBuffer->channelValid[i]);
+            serialPort->printf("%d, ", radioDecoderRegisterDataBuffer->channelActiveTime[i]);
+            if (i == 2) {
+                serialPort->printf("%d\r\n", radioDecoderRegisterDataBuffer->channelPercent[i]);
+            }
+        }
+    } else {
+        for(uint8_t i=0; i<3; i++) {
+            serialPort->printf("RadioDecoder - Ch[%d] Valid:      %d\r\n",i,radioDecoderRegisterDataBuffer->channelValid[i]);
+            serialPort->printf("RadioDecoder - Ch[%d] ActiveTime: %d\r\n",i,radioDecoderRegisterDataBuffer->channelActiveTime[i]);
+            serialPort->printf("RadioDecoder - Ch[%d] Percentage: %d\r\n\r\n",i,radioDecoderRegisterDataBuffer->channelPercent[i]);
+        }
+    }
+}
+
+
+void SerialOutputLogger::writeImuDataLog(bool csv) {
+    if (csv) {
+        
+    } else {
+        
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SerialOutputLogger.h	Fri Feb 05 16:06:44 2016 +0000
@@ -0,0 +1,19 @@
+#ifndef SERIAL_OUTPUT_LOGGER_H_
+#define SERIAL_OUTPUT_LOGGER_H_
+
+#include <mbed.h>
+#include "Periphery/SupportSystem.h"
+
+class SerialOutputLogger {
+
+private:
+    Serial *serialPort;
+    IMU_RegisterDataBuffer_t *imuRegisterDataBuffer;
+    RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer;
+public:
+    SerialOutputLogger(Serial *serialPort, IMU_RegisterDataBuffer_t *imuRegisterDataBuffer, RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer);
+    void writeRadioDecoderDataLog(bool csv);    
+    void writeImuDataLog(bool csv);
+};
+
+#endif
\ No newline at end of file

--- a/Periphery/SupportSystem.cpp	Thu Feb 04 21:43:08 2016 +0000
+++ b/Periphery/SupportSystem.cpp	Fri Feb 05 16:06:44 2016 +0000
@@ -137,6 +137,15 @@
 //
 //}
 
+void SupportSystem::writeImuConfig(float *config_register, size_t length) {
+    writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, config_register, sizeof(float)*length);
+    
+    uint8_t command = 1<<2;
+    writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
+    
+    wait(0.1);
+}
+
 
 IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){
 

--- a/Periphery/SupportSystem.h	Thu Feb 04 21:43:08 2016 +0000
+++ b/Periphery/SupportSystem.h	Fri Feb 05 16:06:44 2016 +0000
@@ -193,6 +193,7 @@
      * IMU
      */
     IMU_RegisterDataBuffer_t *getImuRegisterDataBuffer();
+    void writeImuConfig(float *config_register, size_t length=4);
 
 
 

--- a/main.cpp	Thu Feb 04 21:43:08 2016 +0000
+++ b/main.cpp	Fri Feb 05 16:06:44 2016 +0000
@@ -3,29 +3,23 @@
 #include "Periphery/SupportSystem.h"
 #include "Misc/SystemTimer.h"
 #include "Controller/QuadratureController.h"
+#include "Controller/MachineDirectionController.h"
 
 #define PI 3.14159265
 
-Serial serialXbee(p28,p27);
-Serial serialMinnow(p13, p14);
-PwmOut drivePWM(p22);
-PwmOut steerPWM(p21);
+Serial serialXbee(p28,p27), serialMinnow(p13, p14);
+PwmOut drivePWM(p22), steerPWM(p21);
 I2C i2c(p9, p10);
 
-DigitalOut heartbeatLED(LED1);
-DigitalOut buttonLED(LED2);
-DigitalOut redlightLED(LED3);
-DigitalOut led4(LED4);
+DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);
 
-DigitalIn buttonOne(p25);
-DigitalIn buttonTwo(p26);
-DigitalIn buttonThree(p29);
-DigitalIn buttonFour(p30);
+DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
 
 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
 
 QuadratureController *quadratureController;
+MachineDirectionController *machineDirectionController;
 
 // Queues von der Bahnplanung
 Queue<float, 2> quadrature_queue;
@@ -40,20 +34,6 @@
 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) {
@@ -63,7 +43,6 @@
     char* end = ptr + length;
     char c, x = 22;
     float f;
-    int return_value = 666;
     while (true) {
         if (ptr != end) {
             c = serialMinnow.getc();
@@ -71,25 +50,24 @@
         } else {
             ptr = (char*)receive_buffer;
         }
-        heartbeatLED = !heartbeatLED;
+        serialLED = !serialLED;
         
         if (c == '\r') {
-            return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
+            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;
+            ledTwo = true;
             // Buffer wieder auf Anfang setzen
             ptr = (char*)receive_buffer;
             x = 22;
         } else if (x == 'g') {
+            f = -1*f;
             quadrature_queue.put(&f);
-            serialXbee.printf("g:%f\r\n\r\n", f);
-            redlightLED = true;
+            ledThree = true;
             // Buffer wieder auf Anfang setzen
             ptr = (char*)receive_buffer;
             x = 22;
@@ -99,39 +77,8 @@
     }
 }
 
-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_machine_direction_controller(void const *args) {
+    machineDirectionController->cylic_control();
 }
 
 void redirect_quadrature_controller(void const *args) {
@@ -139,7 +86,6 @@
 }
 
 int main() {
-    serialXbee.baud(9600);
     serialMinnow.baud(115200);
 
     drivePWM.period_ms(20);
@@ -150,11 +96,12 @@
     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);
 
     Thread machineDirectionControl(serial_thread);
     Thread hearbeatThread(hearbeat_thread);
     
-    RtosTimer machine_direction_control_timer(machine_direction_control);
+    RtosTimer machine_direction_control_timer(redirect_quadrature_controller);
     RtosTimer quadrature_control_timer(redirect_quadrature_controller);
 
     // Konfiguration AMF-IMU
@@ -163,16 +110,10 @@
     // [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));
+    
+    supportSystem->writeImuConfig(configData);
     
     bool timer_started = false;
-
-    wait(0.1);
     
     steering_angle_minnow_queue = 0;
     quadrature_queue.put(&steering_angle_minnow_queue);
@@ -187,12 +128,6 @@
         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];
         
@@ -205,9 +140,6 @@
 
         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;
@@ -216,9 +148,6 @@
             }
         } 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);
@@ -233,9 +162,6 @@
             }
         } 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);
@@ -267,13 +193,13 @@
         //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
         //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
         //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
-        Thread::wait(50);
+        Thread::wait(10);
     }
 }
 
 void hearbeat_thread(void const *args) {
     while(true) {
-        led4 = !led4;
+        heartbeatLED = !heartbeatLED;
         Thread::wait(100);
     }
 }