Updated Car controller demo.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1846 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-04-04 02:23:32 +00:00
parent b646cdd61a
commit 2868181322
1 changed files with 28 additions and 16 deletions

View File

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