Updated Car controller demo.

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

libraries/AP_Controller/examples/Car/Car.pde

@@ -3,51 +3,61 @@
 #include <AP_Var.h>
 #include <AP_RcChannel.h>
 #include <APM_RC.h>
+#include <AP_Common.h>
 
 FastSerialPort0(Serial);
+float test1;
+float test2;
 
 class CarController : public AP_Controller
 {
 private:
     // state
-    AP_Float & velocity;
-    AP_Float & heading;
+    float * velocity;
+    float *  heading;
 
     // control variables
-    AP_Float & headingCommand;
-    AP_Float & velocityCommand;
+    float *  headingCommand;
+    float *  velocityCommand;
 
     // channels
     AP_RcChannel * steeringCh;
     AP_RcChannel * throttleCh;
 
+    static float one;
+    static float negOne;
+
 public:
     CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
-            AP_Float & heading, AP_Float & velocity,
-            AP_Float & headingCommand, AP_Float & velocityCommand,
+    		float *  heading, float *  velocity,
+    		float *  headingCommand, float *  velocityCommand,
             AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) :
         heading(heading), velocity(velocity),
         headingCommand(headingCommand), velocityCommand(velocityCommand),
         steeringCh(steeringCh), throttleCh(throttleCh)
     {
         // steering control loop
-        addBlock(new SumGain(headingCommand,AP_Float_unity,heading,AP_Float_negative_unity));
+        addBlock(new SumGain(headingCommand,&one,heading,&negOne));
         addBlock(new Pid(pidStrKey,PSTR("STR"),1,1,1,1,20));
+        //addBlock(new Sink(test1));
         addBlock(new ToServo(steeringCh));
 
 
         // throttle control loop
-        addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity));
+        addBlock(new SumGain(velocityCommand,&one,velocity,&negOne));
         addBlock(new Pid(pidThrKey,PSTR("THR"),1,1,1,1,20));
+        //addBlock(new Sink(test2));
         addBlock(new ToServo(throttleCh));
     }
 };
+float CarController::negOne = -1;
+float CarController::one = 1;
 
 AP_Controller * controller;
 Vector<AP_RcChannel * > ch;
 
-AP_Float heading, velocity; // measurements
-AP_Float headingCommand, velocityCommand; // guidance data
+float heading, velocity; // measurements
+float headingCommand, velocityCommand; // guidance data
 int8_t sign = 1;
 float value = 0;
 
@@ -76,7 +86,7 @@ void setup()
     ch.push_back(new AP_RcChannel(chThrKey,PSTR("THR"),APM_RC,2,45));
 
     // controller
-    controller = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]);
+    controller = new CarController(pidStrKey,pidThrKey,&heading,&velocity,&headingCommand,&velocityCommand,ch[0],ch[1]);
 }
 
 void loop()
@@ -103,8 +113,10 @@ void loop()
     controller->update(1./20);
 
     // output
-    Serial.printf("hdng:%f\tcmd:(%f)\tservo:%f\t", heading.get(),headingCommand.get(), ch[0]->getNormalized());
-    Serial.printf("| thr:%f\tcmd:(%f)\tservo:%f\n", velocity.get(), velocityCommand.get(), ch[1]->getNormalized());
+    Serial.printf("hdng:%f\tcmd:(%f)\tservo:%f\t", heading,headingCommand, ch[0]->getNormalized());
+    Serial.printf("| thr:%f\tcmd:(%f)\tservo:%f\n", velocity, velocityCommand, ch[1]->getNormalized());
+
+    //Serial.printf("test 1: %f test 2: %f\n", test1, test2);
 }
 
 // vim:ts=4:sw=4:expandtab