Diff: main.cpp

Revision:

12:221c9e02ea96

Parent:

11:8b79f6545628

Child:

13:34f7f783ad24

--- a/main.cpp	Thu Feb 04 19:39:56 2016 +0000
+++ b/main.cpp	Thu Feb 04 21:43:08 2016 +0000
@@ -6,6 +6,7 @@
 
 #define PI 3.14159265
 
+Serial serialXbee(p28,p27);
 Serial serialMinnow(p13, p14);
 PwmOut drivePWM(p22);
 PwmOut steerPWM(p21);
@@ -48,10 +49,8 @@
 float velocity_current = 0, velocity_last = 0;
 
 uint8_t timer_velocity_sampling_time=0.01;
-float Vorsteuerungsfaktor;
-float l_esum, Vorsteuerung, I_Regler, l_output, 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};
+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
 
@@ -74,18 +73,22 @@
         }
         heartbeatLED = !heartbeatLED;
         
-        if (c == '\r') return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
-        
-        serialMinnow.printf("%d\r\n", return_value);
+        if (c == '\r') {
+            return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
+            ptr = (char*)receive_buffer;
+        }
         
         if (x == 'v') {
-            serialMinnow.printf("v erhalten: %f\r\n", f);
+            machine_direction_queue.put(&f);
+            serialXbee.printf("v:%f\r\n", f);
+            
             buttonLED = true;
             // Buffer wieder auf Anfang setzen
             ptr = (char*)receive_buffer;
             x = 22;
         } else if (x == 'g') {
-            serialMinnow.printf("g erhalten: %f\r\n", f);
+            quadrature_queue.put(&f);
+            serialXbee.printf("g:%f\r\n\r\n", f);
             redlightLED = true;
             // Buffer wieder auf Anfang setzen
             ptr = (char*)receive_buffer;
@@ -107,27 +110,28 @@
         velocity_current = *(float *)velocity_current_event.value.p;
     }
     
-    Vorsteuerungsfaktor = a[(uint8_t)velocity_set*4];
+    Vorsteuerung=88.316*velocity_set+175.17;
     l_e = velocity_set-velocity_current;
     l_esum = l_esum + l_e;
 
-    Vorsteuerung=Vorsteuerungsfaktor*velocity_set;
-    I_Regler = l_Ki * l_esum;
+    PI_Regler =l_Kp*l_e+l_Ki * l_esum;
 
-    l_output=Vorsteuerung;//+I_Regler;
+    l_output=Vorsteuerung+PI_Regler;
 
     l_PWM = 1500+l_output;
-    
-    if(l_PWM<1500)
-    {
-        l_PWM=1500;
-    }
-    else if(l_PWM>2000)
-    {
-        l_PWM=2000;
+
+    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_quadrature_controller(void const *args) {
@@ -135,6 +139,7 @@
 }
 
 int main() {
+    serialXbee.baud(9600);
     serialMinnow.baud(115200);
 
     drivePWM.period_ms(20);
@@ -169,11 +174,14 @@
 
     wait(0.1);
     
-    steering_angle_minnow_queue = 10.0;
+    steering_angle_minnow_queue = 0;
     quadrature_queue.put(&steering_angle_minnow_queue);
     
-    velocity_minnow_queue = 1.5;
+    velocity_minnow_queue = 0;
     machine_direction_queue.put(&velocity_minnow_queue);
+    
+    machine_direction_control_timer.start(10);
+    quadrature_control_timer.start(10);
 
     while(true) {
         IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();