Diff: main.cpp

Revision:

0:8a6003b8bb5b

Child:

1:7eddde9fba60

diff -r 000000000000 -r 8a6003b8bb5b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 03 17:19:21 2016 +0000
@@ -0,0 +1,226 @@
+#include <mbed.h>
+#include "rtos.h"
+#include "Periphery/SupportSystem.h"
+#include "Misc/SystemTimer.h"
+#include "Threads/MachineDirectionControl.h"
+
+#define PI 3.14159265
+
+Serial serialMinnow(p13, p14);
+PwmOut drivePWM(p22);
+PwmOut steerPWM(p21);
+I2C i2c(p9, p10);
+
+DigitalOut heartbeatLED(LED1);
+DigitalOut buttonLED(LED2);
+DigitalOut redlightLED(LED3);
+
+DigitalIn buttonOne(p25);
+DigitalIn buttonTwo(p26);
+DigitalIn buttonThree(p29);
+DigitalIn buttonFour(p30);
+
+IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;
+RadioDecoder_RegisterDataBuffer_t *RadioDecoder_registerDataBuffer;
+
+uint32_t pulse_duration_drive_pwm_current, pulse_duration_drive_pwm_last;
+
+// 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;
+
+
+// Variablen von der Trajektorienplanung
+
+float velocity_set = 0, steering_angle_set = 0;
+
+// Variablen für die Längsregelung
+float velocity_current = 0, velocity_last = 0;
+
+
+// Variablen für die Querregelung
+float steering_angle_current = 0, steering_angle_last = 0;
+
+uint8_t timer_steering_angle_sampling_time = 0.01;
+
+float q_Kp = 8.166343211;
+float q_Ki = 18.6661236;
+float feed_forward_control_factor = 13.37091452;
+float q_esum = 0;
+float feed_forward = 0;
+float q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time;
+float q_PI_controller, q_PWM, q_e, q_output;
+
+// Variablen für die Querregelung Ende
+
+void serial_thread(void const *args) {
+    while (true) {
+        Thread::wait(100);
+    }
+}
+
+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;
+    }
+    
+    drivePWM.pulsewidth_us(1800);
+}
+
+void quadrature_control(void const *args) {
+    osEvent 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;
+    }
+    
+    osEvent 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;
+    }
+    
+    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)
+    if(q_output < -500){q_output = - 500;}
+
+    q_PWM = 1500 + q_output;
+    
+    steerPWM.pulsewidth_us(q_PWM);
+}
+
+int main() {
+    serialMinnow.baud(115200);
+
+    drivePWM.period_ms(20);
+    steerPWM.period_ms(20);
+
+    SystemTimer *millis = new SystemTimer();
+
+    SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
+
+    Thread machineDirectionControl(test_thread);
+    
+    RtosTimer machine_direction_control_timer(machine_direction_control);
+    RtosTimer quadrature_control_timer(quadrature_control);
+
+    // 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, 269.0f};
+
+    supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, configData, sizeof(float)*4);
+
+    // Flag setzen
+    uint8_t command = 1<<3;
+    supportSystem->writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
+    
+    bool timer_started = false;
+
+    wait(0.1);
+    
+    velocity_minnow_queue = 12.0;
+    quadrature_queue.put(&velocity_minnow_queue);
+
+    while(true) {
+        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",i,RadioDecoder_registerDataBuffer->channelPercent[i]);
+        }
+
+        uint16_t rc_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[0];
+        uint8_t rc_valid = RadioDecoder_registerDataBuffer->channelValid[0];
+        
+        uint16_t drive_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[1];
+        uint8_t drive_valid = RadioDecoder_registerDataBuffer->channelValid[1];
+
+        uint16_t steer_percentage = RadioDecoder_registerDataBuffer->channelActiveTime[2];
+        uint8_t steer_valid = RadioDecoder_registerDataBuffer->channelValid[2];
+
+
+        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;
+                machine_direction_control_timer.start(10);
+                quadrature_control_timer.start(10);
+            }
+        } 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);
+            }
+            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) {
+            // mitte => RC-Training
+            heartbeatLED = false;
+            buttonLED = true;
+            redlightLED = false;
+            supportSystem->setLightManagerRemoteLight(true, true);
+            if (drive_valid) {
+                drivePWM.pulsewidth_us(drive_percentage);
+            }
+            if (steer_valid) {
+                steerPWM.pulsewidth_us(steer_percentage);
+            }
+            if (timer_started) {
+                timer_started = false;
+                machine_direction_control_timer.stop();
+                quadrature_control_timer.stop();
+            }
+        }
+        
+        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("%f\r\n\r\n", IMU_registerDataBuffer->sensorPositionAngleRegister);
+        Thread::wait(50);
+    }
+}