Diff: main.cpp

Revision:

13:34f7f783ad24

Parent:

12:221c9e02ea96

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