Diff: main.cpp

Revision:

3:391c4639bc7d

Parent:

2:bf739d7d9f8f

Child:

4:f0be27a5a83a

--- a/main.cpp	Wed Feb 03 20:09:14 2016 +0000
+++ b/main.cpp	Thu Feb 04 08:54:06 2016 +0000
@@ -2,7 +2,7 @@
 #include "rtos.h"
 #include "Periphery/SupportSystem.h"
 #include "Misc/SystemTimer.h"
-#include "Threads/MachineDirectionControl.h"
+#include "Controller/QuadratureController.h"
 
 #define PI 3.14159265
 
@@ -39,7 +39,7 @@
 
 // Variablen von der Trajektorienplanung
 
-float velocity_set = 0, steering_angle_set = 0;
+float velocity_set = 0;
 
 // Variablen für die Längsregelung
 float velocity_current = 0, velocity_last = 0;
@@ -50,20 +50,6 @@
 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;
-
-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;
-
 // Querregelung Ende
 
 void serial_thread(void const *args) {
@@ -88,7 +74,7 @@
     l_esum = l_esum + l_e;
 
     Vorsteuerung=Vorsteuerungsfaktor*velocity_set;
-    I_Regler = q_Ki * l_esum;
+    I_Regler = l_Ki * l_esum;
 
     l_output=Vorsteuerung;//+I_Regler;
 
@@ -106,33 +92,6 @@
     drivePWM.pulsewidth_us(l_PWM);
 }
 
-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);
 
@@ -142,11 +101,13 @@
     SystemTimer *millis = new SystemTimer();
 
     SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
+    
+    QuadratureController *quadratureController = new QuadratureController(&steerPWM);
 
     Thread machineDirectionControl(serial_thread);
     
     RtosTimer machine_direction_control_timer(machine_direction_control);
-    RtosTimer quadrature_control_timer(quadrature_control);
+    RtosTimer quadrature_control_timer(quadratureController->cylic_control);
 
     // Konfiguration AMF-IMU
     // [0]: Conversation Factor