Diff: distanceRegulation.h

Branch:

DistanceRegulation

Revision:

41:5fe200d20022

Parent:

40:0aa1cefe80ab

--- a/distanceRegulation.h	Tue May 15 10:34:35 2018 +0000
+++ b/distanceRegulation.h	Tue May 22 19:43:09 2018 +0000
@@ -11,18 +11,24 @@
     
     pc.printf("Flight controller thread started\r\n");
     
-    _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);   
+    _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void*)0);   
     int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; 
     _groudRegulationUpdateTimer->start(rate);
     
+    osThreadId id;
+    id = Thread::gettid();
+    pc.printf("regulation  gettid 0x%08X \n\r", id);
+    
     Thread::wait(osWaitForever);
     
 }
 
 void distanceRegulationTask(void const *args){
     
-    //pc.printf("Flight controller task started\r\n");
-    
+    //osThreadId id;
+    //id = Thread::gettid();
+    //pc.printf("regulation task gettid 0x%08X \n\r", id);
+   
     uint16_t channels[CHANNELS];
     float distance1=0;
     float groundDistancePIDValue;
@@ -44,98 +50,86 @@
             pc.printf("setpoint %f \n\r", _groundSetPoint);
             pc.printf("bias %f \n\r", _bias);
             _groundDistance->resetError();
-            //pc.printf("PID tunings changed \n\r");    
             _groundDistance->setTunings(_P, _I, _D);
             _groundDistance->setSetPoint(_groundSetPoint);
             _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
-            //_groundDistance->setBias(_bias);
         }
     }
     
     //distance1 = _sonic->getDistan1();
-    //pc.printf("%f \n\r", distance1);
+    //pc.printf("        distance1 %f \n\r", distance1);
     
     if(_groundRegulation){
-        //timer.reset();
-        //timer.start();
-        
+        distance1 = _sonic->getDistan1();
         _ppmRegen->getAllChannels(channels);
+        _groundDistance->setProcessValue(distance1);
         
-        distance1 = _sonic->getDistan1();
-        //distance2 = _sonic->getDistan2();
-        //pc.printf("%f \n\r", distance1);
-        //float p = _groundDistance->getPParam();
-        //float i = _groundDistance->getIParam();
-        //float d = _groundDistance->getDParam();
-        //float max = _groundDistance->getInMax();
-        //pc.printf("%f \n\r", max);
-        //pc.printf("%f \n\r", p);
-        //pc.printf("%f \n\r", i);
-        //pc.printf("%f \n\r", d);
-        //pc.printf("distance2: %d \n\r", distance2);
-        _groundDistance->setProcessValue(distance1);
+        //landing
+        if(channels[AUX1] >= 1300){
+            //pc.printf("landing \n\r");
+            if(distance1 <= LANDING_HEIGHT)
+                channels[AUX2] = 1000;
+            _groundDistance->setLandingPoint((float)LANDING_HEIGHT);                
+        }
+        
         groundDistancePIDValue = _groundDistance->compute();
-        //pc.printf("pid value %f \n\r", groundDistancePIDValue);
         
         //Update PWM values
         regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
         if(channels[AUX2] < 1500){
             channels[THROTTLE] = 1010;    
         }
+        
+        // limits of throttle
         if(regulatedThrottle > THROTTLE_LIMIT)
             regulatedThrottle = THROTTLE_LIMIT;
         if(regulatedThrottle < 1010)
             regulatedThrottle = 1010;
         
-        
-        //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);
-        
-        // goAhead for ROLL
-        if(_goAhead == true && _groundDistance->quadStabilized() && _frontDistance >= 40){
-            _pitch->pulsewidth_us(    PITCH_GO_AHEAD);
-        }
-        if(_frontDistance < 40){
-            _goAhead = false;
-            _frontWall = true;
-            _pitch->pulsewidth_us(    PITCH_GO_BACK);    
+        // goAhead and back
+        if(_groundDistance->quadStabilized() && _goAhead){
+            //pc.printf("going agead \n\r");
+            if(_frontDistance <= 40){
+                _frontWall = true;
+                _backWall = false;
+            }
+            if(_backDistance <= 40){
+                _frontWall = false;
+                _backWall = true;
+            }
+            if(_frontWall){
+                //pc.printf("     going backward \n\r");
+                _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD);   
+            }
+            if(_backWall){
+                //pc.printf("     going forward \n\r");
+                _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD);  
+            }
+            
+            
+            //if(_frontDistance > 40 && _backDistance > 40)
+            //    _pitch->pulsewidth_us(channels[PITCH]);
+            
         }
         else{
-            _pitch->pulsewidth_us(   channels[PITCH]);
+            _pitch->pulsewidth_us(channels[PITCH]);        
         }
-        //pc.printf("front distance %d \n\r", _frontDistance);
-        
-        //if(_frontWall){
-        //        
-        //}
-        
-        
-        
         
         _roll->pulsewidth_us(    channels[ROLL]);
         _yaw->pulsewidth_us(     channels[YAW]);
         _throttle->pulsewidth_us(regulatedThrottle);
         _aux1->pulsewidth_us(    channels[AUX1]);
         _aux2->pulsewidth_us(    channels[AUX2]);
-        //timer.stop();
-        //pc.printf("Timer: %d \n\r", timer.read_us());
         
     }
+    
     else {
-        //distance = _sonic->getDistan();
-        //pc.printf("Sonic distance: %d \n\r", distance);
-        //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
+        if(_groundDistance->quadStabilized())
+            _groundDistance->setNotStabelized();
         
-        //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
+        
         // Generate PWM
         _ppmRegen->getAllChannels(channels);
-        //pc.printf("channel value: %d \n\r", channels[ROLL]);
-        //pc.printf("channel value: %d \n\r", channels[PITCH]);
-        //pc.printf("channel value: %d \n\r", channels[YAW]);
-        //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
-        //pc.printf("channel value: %d \n\r", channels[AUX1]);
-        //pc.printf("channel value: %d \n\r", channels[AUX2]);
         
         _roll->pulsewidth_us(    channels[ROLL]);
         _pitch->pulsewidth_us(   channels[PITCH]);
@@ -145,4 +139,5 @@
         _aux2->pulsewidth_us(    channels[AUX2]);
     }
     
+    
 }
\ No newline at end of file