Diff: distanceRegulation.h

Branch:

DistanceRegulation

Revision:

25:69190c222dbf

Parent:

23:46ae5b0464da

Child:

26:11539036f0fb

--- a/distanceRegulation.h	Sun Nov 05 09:41:25 2017 +0000
+++ b/distanceRegulation.h	Sun Nov 26 17:27:39 2017 +0000
@@ -10,39 +10,47 @@
     int regulatedThrottle;
     
     //setup PID
-    _groundDistance->setInputLimits(0, 500);
-    _groundDistance->setOutputLimits(-600, 600);
+    _groundDistance->setInputLimits(0, 100);
+    //_groundDistance->setOutputLimits(100, 300);
     _groundDistance->setMode(AUTO_MODE);
-    _groundDistance->setSetPoint(0.0);
-    _groundDistance->setBias(0);
+    //_groundDistance->setSetPoint(0.0);
+    //_groundDistance->setBias(0);
     _groundDistance->setTunings(_P, _I, _D);
     
     while(1){
         if(_groundRegulation){
-            //if config has changed
+            _ppmRegen->getAllChannels(channels);
+            //PID SETUP
             if(_configChanges){
                 _configChanges = false;
+                _groundDistance->reset();
                 //pc.printf("PID tunings changed \n\r");    
                 _groundDistance->setTunings(_P, _I, _D);
                 _groundDistance->setSetPoint(_groundSetPoint);
+                _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
+                //_groundDistance->setBias(_bias);
             }
             distance = _sonic->getDistan();
             _groundDistance->setProcessValue(distance);
             groundDistancePIDValue = _groundDistance->compute();
+            groundDistancePIDValue += _bias;
+            
             
             //Update PWM values
-            _ppmRegen->getAllChannels(channels);
             regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
+            if(channels[AUX2] < 1500){
+                channels[THROTTLE] = 1010;    
+            }
             if(regulatedThrottle > 2000)
                 regulatedThrottle = 2000;
-            else if(regulatedThrottle < 1100)
-                regulatedThrottle = 1100;
+            if(regulatedThrottle < 1010)
+                regulatedThrottle = 1010;
             
             
             pc.printf("Sonic distance: %d \n\r", distance);
-            pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
-            pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
-            pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
+            //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
+            //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
+            //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
             
             // Generate  regulated PWM
             _roll->pulsewidth_us(    channels[ROLL]);
@@ -54,10 +62,10 @@
         }
         else {
             
-            pc.printf("Sonic distance: %d \n\r", distance);
-            pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
-            pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
-            pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
+            //pc.printf("Sonic distance: %d \n\r", distance);
+            //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
+            //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
+            //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
             // Generate PWM
             _ppmRegen->getAllChannels(channels);
             _roll->pulsewidth_us(    channels[ROLL]);
@@ -68,7 +76,7 @@
             _aux2->pulsewidth_us(    channels[AUX2]);
         }
         
-        Thread::wait(20);
+        Thread::wait(1);
             
         
     }