Diff: distanceRegulation.h

Branch:

DistanceRegulation

Revision:

26:11539036f0fb

Parent:

25:69190c222dbf

Child:

27:5956d5e3ff63

--- a/distanceRegulation.h	Sun Nov 26 17:27:39 2017 +0000
+++ b/distanceRegulation.h	Fri Dec 01 11:11:23 2017 +0000
@@ -3,32 +3,51 @@
 #include "hardware.h"
 #include "definitions.h"
 
-void distanceRegulationThread(){
+
+//RtosTimer *_groudRegulationUpdateTimer;
+
+/*
+void distanceRegulationThread(void const *args){
+    
+    _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);   
+    int rate = 2; 
+    _groudRegulationUpdateTimer->start(rate);
+    
+    Thread::wait(osWaitForever);
+    
+}
+*/
+void distanceRegulationTask(){
     uint16_t channels[CHANNELS];
     int distance=0;
     float groundDistancePIDValue;
     int regulatedThrottle;
     
     //setup PID
-    _groundDistance->setInputLimits(0, 100);
+    _groundDistance->setInputLimits(0, 300);
     //_groundDistance->setOutputLimits(100, 300);
     _groundDistance->setMode(AUTO_MODE);
     //_groundDistance->setSetPoint(0.0);
     //_groundDistance->setBias(0);
     _groundDistance->setTunings(_P, _I, _D);
     
+    //Timer timer;
+    
     while(1){
         if(_groundRegulation){
+            //timer.reset();
+            //timer.start();
+            
             _ppmRegen->getAllChannels(channels);
             //PID SETUP
             if(_configChanges){
                 _configChanges = false;
-                _groundDistance->reset();
+                _groundDistance->resetError();
                 //pc.printf("PID tunings changed \n\r");    
                 _groundDistance->setTunings(_P, _I, _D);
                 _groundDistance->setSetPoint(_groundSetPoint);
                 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
-                //_groundDistance->setBias(_bias);
+                _groundDistance->setBias(_bias);
             }
             distance = _sonic->getDistan();
             _groundDistance->setProcessValue(distance);
@@ -59,6 +78,9 @@
             _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 {
             
@@ -76,7 +98,7 @@
             _aux2->pulsewidth_us(    channels[AUX2]);
         }
         
-        Thread::wait(1);
+        Thread::wait(2);
             
         
     }