diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde index f86cfaca6c..eac8000d6d 100644 --- a/libraries/AP_Controller/examples/Car/Car.pde +++ b/libraries/AP_Controller/examples/Car/Car.pde @@ -3,51 +3,61 @@ #include #include #include +#include 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), + 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 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; @@ -58,7 +68,7 @@ enum keys chThrKey, pidStrKey, pidThrKey -}; +}; void setup() { @@ -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() @@ -89,7 +99,7 @@ void loop() { value = 1; sign = -1; - } + } else if (value < -1) { value = -1; @@ -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