Diff: main.cpp

Revision:

2:bf739d7d9f8f

Parent:

1:7eddde9fba60

Child:

3:391c4639bc7d

--- a/main.cpp	Wed Feb 03 18:34:34 2016 +0000
+++ b/main.cpp	Wed Feb 03 20:09:14 2016 +0000
@@ -46,7 +46,7 @@
 
 uint8_t timer_velocity_sampling_time=0.01;
 float Vorsteuerungsfaktor;
-float l_esum, Vorsteuerung, I_Regler, Ausgabe, l_PWM, l_e;
+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};
 
@@ -90,9 +90,18 @@
     Vorsteuerung=Vorsteuerungsfaktor*velocity_set;
     I_Regler = q_Ki * l_esum;
 
-    Ausgabe=Vorsteuerung+I_Regler;
+    l_output=Vorsteuerung;//+I_Regler;
 
-    l_PWM = 1500+Ausgabe;
+    l_PWM = 1500+l_output;
+    
+    if(l_PWM<1500)
+    {
+        l_PWM=1500;
+    }
+    else if(l_PWM>2000)
+    {
+        l_PWM=2000;
+    }
     
     drivePWM.pulsewidth_us(l_PWM);
 }
@@ -156,8 +165,11 @@
 
     wait(0.1);
     
-    velocity_minnow_queue = 12.0;
-    quadrature_queue.put(&velocity_minnow_queue);
+    steering_angle_minnow_queue = 0.0;
+    quadrature_queue.put(&steering_angle_minnow_queue);
+    
+    velocity_minnow_queue = 1.5;
+    machine_direction_queue.put(&velocity_minnow_queue);
 
     while(true) {
         IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
@@ -235,7 +247,14 @@
         imu_queue_steering_angle.put(&steering_angle_imu_queue);
         
 
-        serialMinnow.printf("%f\r\n\r\n", IMU_registerDataBuffer->sensorPositionAngleRegister);
+        //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(50);
     }
 }