Debugging controller.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1658 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_Controller/AP_Controller.h

@@ -52,7 +52,7 @@ protected:
 class ToServo: public Block
 {
 public:
-    ToServo(AP_RcChannel & ch) : _ch(ch)
+    ToServo(AP_RcChannel * ch) : _ch(ch)
     {
     }
     virtual void connect(Block * block)
@@ -63,11 +63,11 @@ public:
     virtual void update(const float & dt = 0)
     {
         if (_input.getSize() > 0)
-            _ch.setPosition(APVarPtr2Float(_output[0]));
+            _ch->setNormalized(APVarPtr2Float(_input[0]));
     }
 private:
     float _position;
-    AP_RcChannel & _ch;
+    AP_RcChannel * _ch;
 };
 
 /// SumGain
@@ -94,17 +94,15 @@ public:
         if ( (&var6 == &AP_Float_zero) || (&gain6 == &AP_Float_zero) ) add(var6,gain6);
         if ( (&var7 == &AP_Float_zero) || (&gain7 == &AP_Float_zero) ) add(var7,gain7);
         if ( (&var8 == &AP_Float_zero) || (&gain8 == &AP_Float_zero) ) add(var8,gain8);
+
+        // create output
+        _output.push_back(new AP_Float(0));
     }
     void add(AP_Var & var, AP_Var & gain)
     {
         _input.push_back(&var);
         _gain.push_back(&gain);
     }
-    virtual void connect(Block * block)
-    {
-        if (block->getOutput().getSize() > 0)
-            _input.push_back(block->getOutput()[0]);
-    }
     virtual void update(const float & dt = 0)
     {
         if (_output.getSize() < 1) return;
@@ -140,12 +138,13 @@ public:
     }
     virtual void connect(Block * block)
     {
+        if (!block) return;
         if (block->getOutput().getSize() > 0)
             _input.push_back(block->getOutput()[0]);
     }
     virtual void update(const float & dt = 0)
     {
-        if (_output.getSize() < 1) return;
+        if (_output.getSize() < 1 || !_input[0] || !_output[0]) return;
 
         // derivative
         float RC = 1/(2*M_PI*_fCut); // low pass filter
@@ -160,6 +159,18 @@ public:
 
         // pid sum
         *_output[0] = _kP*_e + _kI*_eI +  _kD*_eD;
+
+
+        Serial.println("debug");
+        Serial.println(_kP);
+        Serial.println(_kI);
+        Serial.println(_kD);
+        Serial.println(_e);
+        Serial.println(_eI);
+        Serial.println(_eD);
+        Serial.println(APVarPtr2Float(_output[0]));
+
+
     }
 private:
     float _e; /// input
@@ -179,25 +190,19 @@ public:
     void addBlock(Block * block)
     {
         if (_blocks.getSize() > 0) 
-            _blocks[_blocks.getSize()]->connect(block);
+            _blocks[_blocks.getSize()-1]->connect(block);
         _blocks.push_back(block);
     }
-    void addCh(AP_RcChannel * ch)
+    void update(const double dt=0)
-	{
-		_rc.push_back(ch);
-	}
-    AP_RcChannel & getRc(int i)
-	{
-		return *(_rc[i]);
-	}
-    void update()
     {
         for (int i=0;i<_blocks.getSize();i++)
-            _blocks[i]->update();
+        {
+            if (!_blocks[i]) continue;
+            _blocks[i]->update(dt);
+        }
     }
 private:
     Vector<Block * > _blocks;   
-    Vector<AP_RcChannel * > _rc;
 };
 
 #endif // AP_Controller_H

libraries/AP_Controller/examples/Car/Car.pde

@@ -1,7 +1,6 @@
 #include <FastSerial.h>
 #include <AP_Controller.h>
 #include <AP_Var.h>
-#include <AP_EEProm.h>
 #include <AP_RcChannel.h>
 #include <APM_RC.h>
 
@@ -11,53 +10,82 @@ class CarController : public AP_Controller
 {
 private:
     // state
-    AP_Float * roll;
+    AP_Var & velocity;
-    AP_Float * airspeed;
+    AP_Var & heading;
-    AP_Float * velocity;
-    AP_Float * heading;
-
-    // servo positions
-    AP_Float * steering;
-    AP_Float * throttle;
 
     // control variables
-    AP_Float * headingCommand;
+    AP_Var & headingCommand;
-    AP_Float * velocityCommand;
+    AP_Var & velocityCommand;
 
     // channels
-    uint8_t chStr;
+    AP_RcChannel * steeringCh;
-    uint8_t chThr;
+    AP_RcChannel * throttleCh;
+
 public:
-    CarController() :
+    CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
-        chStr(0), chThr(1)
+            AP_Var & heading, AP_Var & velocity,
+            AP_Var & headingCommand, AP_Var & velocityCommand,
+            AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) :
+        heading(heading), velocity(velocity),
+        headingCommand(headingCommand), velocityCommand(velocityCommand), 
+        steeringCh(steeringCh), throttleCh(throttleCh)
     {
-        // rc channels
-        addCh(new AP_RcChannel("STR",APM_RC,chStr,45));
-        addCh(new AP_RcChannel("THR",APM_RC,chThr,100));
       
         // steering control loop
-        addBlock(new SumGain(headingCommand,&AP_unity,heading,&AP_negativeUnity));
+        addBlock(new SumGain(headingCommand,AP_Float_unity,heading,AP_Float_negative_unity));
-        addBlock(new Pid("HDNG",0.1,0,0,1,20));
+        addBlock(new Pid(pidStrKey,PSTR("STR"),0.1,0,0,1,20));
-        addBlock(new ToServo(getRc(chStr)));
+        addBlock(new ToServo(steeringCh));
 
         // throttle control loop
-        addBlock(new SumGain(velocityCommand,&AP_unity,velocity,&AP_negativeUnity));
+        addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity));
-        addBlock(new Pid("THR",0.1,0,0,1,20));
+        addBlock(new Pid(pidThrKey,PSTR("THR"),0.1,0,0,1,20));
-        addBlock(new ToServo(getRc(chThr)));
+        addBlock(new ToServo(throttleCh));
     }
 };
 
 AP_Controller * controller;
+Vector<AP_RcChannel * > ch;
+
+AP_Float heading, velocity; // measurements
+AP_Float headingCommand, velocityCommand; // guidance data
+
+enum keys
+{
+    config,
+    chStrKey,
+    chThrKey,
+    pidStrKey,
+    pidThrKey
+}; 
 
 void setup()
 {
     Serial.begin(115200);
-	//controller = new CarController;
+
+    // variables
+    heading = 0;
+    velocity = 0;
+    headingCommand = 1;
+    velocityCommand = 1;
+
+    // rc channels
+    APM_RC.Init();
+    ch.push_back(new AP_RcChannel(chStrKey,PSTR("STR"),APM_RC,1,45));
+    ch.push_back(new AP_RcChannel(chThrKey,PSTR("THR"),APM_RC,2,100));
+
+    // controller
+    controller = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]);
 }
 
 void loop()
 {
     Serial.println("Looping");
+    heading += 0.1;
+    velocity += 0.1;
     delay(1000);
-	//controller->update();
+    controller->update(.1);
+    Serial.printf("heading:\t%f\tcommand(%f)\tservo(%f)\n", (float)heading,(float) headingCommand, ch[0]->getNormalized());
+    Serial.printf("velocity:\t%f\tcommand(%f)\tservo(%f)\n", (float)velocity, (float) velocityCommand, ch[1]->getNormalized());
 }
+
+// vim:ts=4:sw=4:expandtab

libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde

@@ -109,3 +109,5 @@ void loop()
 {
     test->update();
 }
+
+// vim:ts=4:sw=4:expandtab