Diff: main.cpp

Revision:

17:76636aaf80de

Parent:

16:a387f2e275c8

Child:

18:0e8ad413a840

--- a/main.cpp	Sun Feb 07 20:54:58 2016 +0000
+++ b/main.cpp	Mon Feb 08 14:56:34 2016 +0000
@@ -4,16 +4,24 @@
 #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), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
+DigitalIn buttonOne(p25), buttonDrivingObstacle(p26), buttonDriving(p29), buttonParking(p30);
 
 IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
 RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
@@ -34,7 +42,7 @@
 float steering_angle_imu_queue;
 float velocity_imu_queue;
 
-// Queue 
+// Queue
 typedef struct {
     char identifier;
     uint8_t payload;
@@ -44,18 +52,33 @@
 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) {
-    serialMinnow.printf("Serial Thread erstellt");
+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) {
-            //serialMinnow.printf("in der if\r\n");
-            //serialMinnow.printf("[%c]=%d\r", ((serial_output_t *)modus_event.value.p)->identifier, ((serial_output_t *)modus_event.value.p)->payload);
-            serialMinnow.printf("[m]=%d\r", *(uint8_t *)modus_event.value.p);
+            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");
+            }
         }
     }
 }
@@ -67,6 +90,8 @@
     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) {
@@ -76,42 +101,53 @@
                 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') {
-                machine_direction_queue.put(&f);
-            
+                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') {
-                f = -1*f;
-                quadrature_queue.put(&f);
+                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) {
+void redirect_machine_direction_controller(void const *args)
+{
     machineDirectionController->cylic_control();
 }
 
-void redirect_quadrature_controller(void const *args) {
+void redirect_quadrature_controller(void const *args)
+{
     quadratureController->cylic_control();
 }
 
-int main() {
+int main()
+{
     serialMinnow.baud(115200);
 
     drivePWM.period_ms(20);
@@ -120,16 +156,17 @@
     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);
 
@@ -139,104 +176,99 @@
     // [2]: Sensor Position Y
     // [3]: Sensor Position Angle
     float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
-    
+
     supportSystem->writeImuConfig(configData);
-    
-    bool timer_started = false;
-    
+
     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) {
-        //modus_struct.identifier = 'm';
-        //modus_struct.payload = 1;
-        //serial_output_modus_queue.put(&modus_struct);
-        
-        modus = 1;
-        serial_output_modus_queue.put(&modus);
-        
+
         IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
         RadioDecoder_registerDataBuffer = supportSystem->getRadioDecoderRegisterDataBuffer();
 
-        uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
+        uint16_t rc_active_time = RadioDecoder_registerDataBuffer->channelActiveTime[0];
         uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
-        
-        uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
+
+        int8_t drive_percentage = RadioDecoder_registerDataBuffer->channelPercent[1];
         uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
 
-        uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
+        int8_t steer_percentage = RadioDecoder_registerDataBuffer->channelPercent[2];
         uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
 
 
-        if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
+        if (rc_active_time > (uint16_t) 1800 && rc_valid != 0) {
             // oben => Wettbewerb
             supportSystem->setLightManagerRemoteLight(false, true);
-            //serialMinnow.putc('w');
-            if (!timer_started) {
-                timer_started = true;
-                machine_direction_control_timer.start(10);
-                quadrature_control_timer.start(10);
-            }
-        } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
+            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);
-            //serialMinnow.putc('r');
+            Thread::wait(1000);
+            modus = MODUS_RC;
+            serial_output_modus_queue.put(&modus);
             if (drive_valid) {
-                drivePWM.pulsewidth_us(drive_percentage);
+                velocity_minnow_queue = 0.3 * drive_percentage;
+                machine_direction_queue.put(&velocity_minnow_queue);
             }
             if (steer_valid) {
                 steerPWM.pulsewidth_us(steer_percentage);
             }
-            if (timer_started) {
-                timer_started = false;
-                machine_direction_control_timer.stop();
-                quadrature_control_timer.stop();
-            }
-        } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
+        } else if (rc_active_time > (uint16_t) 800 && rc_valid != 0) {
             // mitte => RC-Training
             supportSystem->setLightManagerRemoteLight(true, true);
-            //serialMinnow.putc('r');
+            modus = MODUS_RC;
+            serial_output_modus_queue.put(&modus);
             if (drive_valid) {
-                drivePWM.pulsewidth_us(drive_percentage);
+                velocity_minnow_queue = 2.0 * drive_percentage;
+                machine_direction_queue.put(&velocity_minnow_queue);
             }
             if (steer_valid) {
                 steerPWM.pulsewidth_us(steer_percentage);
             }
-            if (timer_started) {
-                timer_started = false;
-                machine_direction_control_timer.stop();
-                quadrature_control_timer.stop();
+        }
+
+        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);
-        
 
-        //serialMinnow.printf("%ld, ", difference_millis);
-        //serialMinnow.printf("%f, ", velocity_minnow_queue);
-        //serialMinnow.printf("%f, ", steering_angle_minnow_queue);
-        //serialMinnow.printf("%d, ", IMU_registerDataBuffer->curveRadiusRegister);
-        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularRegister);
-        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityAngularFilteredRegister);
-        //serialMinnow.printf("%f, ", IMU_registerDataBuffer->velocityXRegister);
-        //serialMinnow.printf("%f\r\n", IMU_registerDataBuffer->velocityXFilteredRegister);
         Thread::wait(10);
     }
 }
 
-void hearbeat_thread(void const *args) {
+void hearbeat_thread(void const *args)
+{
     while(true) {
         heartbeatLED = !heartbeatLED;
         Thread::wait(100);