Diff: main.cpp

Revision:

1:7eddde9fba60

Parent:

0:8a6003b8bb5b

Child:

2:bf739d7d9f8f

--- a/main.cpp	Wed Feb 03 17:19:21 2016 +0000
+++ b/main.cpp	Wed Feb 03 18:34:34 2016 +0000
@@ -23,8 +23,6 @@
 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;
@@ -46,6 +44,12 @@
 // Variablen für die Längsregelung
 float velocity_current = 0, velocity_last = 0;
 
+uint8_t timer_velocity_sampling_time=0.01;
+float Vorsteuerungsfaktor;
+float l_esum, Vorsteuerung, I_Regler, Ausgabe, l_PWM, l_e;
+float l_Ki=30*timer_velocity_sampling_time;
+uint16_t a[19]={0,600,400,325,250,237,225,212,200,177,144,140,136,132,128,124,120,115,111};
+
 
 // Variablen für die Querregelung
 float steering_angle_current = 0, steering_angle_last = 0;
@@ -60,7 +64,7 @@
 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
+// Querregelung Ende
 
 void serial_thread(void const *args) {
     while (true) {
@@ -79,7 +83,18 @@
         velocity_current = *(float *)velocity_current_event.value.p;
     }
     
-    drivePWM.pulsewidth_us(1800);
+    Vorsteuerungsfaktor = a[(uint8_t)velocity_set*4];
+    l_e = velocity_set-velocity_current;
+    l_esum = l_esum + l_e;
+
+    Vorsteuerung=Vorsteuerungsfaktor*velocity_set;
+    I_Regler = q_Ki * l_esum;
+
+    Ausgabe=Vorsteuerung+I_Regler;
+
+    l_PWM = 1500+Ausgabe;
+    
+    drivePWM.pulsewidth_us(l_PWM);
 }
 
 void quadrature_control(void const *args) {
@@ -119,7 +134,7 @@
 
     SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
 
-    Thread machineDirectionControl(test_thread);
+    Thread machineDirectionControl(serial_thread);
     
     RtosTimer machine_direction_control_timer(machine_direction_control);
     RtosTimer quadrature_control_timer(quadrature_control);