Diff: distanceRegulation.h

Branch:

DistanceRegulation

Revision:

18:e0c5ab76f337

Parent:

17:1896d242945b

Child:

19:9ceec179b140

diff -r 1896d242945b -r e0c5ab76f337 distanceRegulation.h
--- a/distanceRegulation.h	Sun Oct 29 10:25:58 2017 +0000
+++ b/distanceRegulation.h	Sun Oct 29 10:44:10 2017 +0000
@@ -6,7 +6,7 @@
 void distanceRegulationThread(){
     uint16_t channels[CHANNELS];
     int distance=0;
-    float groundDistancePidValue;
+    float groundDistancePIDValue;
     int newThrottleValue;
     
     //setup PID
@@ -18,33 +18,36 @@
     _groundDistance->setTunings(_P, _I, _D);
     
     while(1){
-        
-        _ppmRegen->getAllChannels(channels);
-        distance = _sonic->getDistan();
-        
-        //calculate PID for given channel
+        //if config has changed
         if(_configChanges){
             _configChanges = false;
             pc.printf("PID tunings changed \n\r");    
             _groundDistance->setTunings(_P, _I, _D);
             _groundDistance->setSetPoint(_groundSetPoint);
         }
+        distance = _sonic->getDistan();
         _groundDistance->setProcessValue(distance);
-        groundDistancePidValue = _groundDistance->compute();
+        groundDistancePIDValue = _groundDistance->compute();
         
         //Update PWM values
-        newThrottleValue = channels[THROTTLE] + groundDistancePidValue;
+        _ppmRegen->getAllChannels(channels);
+        regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
+        if(regulatedThrottle > 2000)
+            regulatedThrottle = 2000
+        else if(regulatedThrottle < 1100)
+            regulatedThrottle = 1100
+        
         
         pc.printf("Sonic distance: %d \n\r", distance);
-        pc.printf("channel throttle pid value: %f \n\r", groundDistancePidValue); 
+        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", newThrottleValue);
+        pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
         
         // Generate PWM
         _roll->pulsewidth_us(    channels[ROLL]);
         _pitch->pulsewidth_us(   channels[PITCH]);
         _yaw->pulsewidth_us(     channels[YAW]);
-        _throttle->pulsewidth_us(channels[THROTTLE]);
+        _throttle->pulsewidth_us(regulatedThrottle);
         _aux1->pulsewidth_us(    channels[AUX1]);
         _aux2->pulsewidth_us(    channels[AUX2]);