mirror of https://github.com/ArduPilot/ardupilot
Updated Car controller demo.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1846 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
b646cdd61a
commit
2868181322
|
@ -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),
|
||||
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;
|
||||
|
||||
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue