TankDrive::TankDrive(int speedPinLeft, int speedPinRight, int forwardPinRight, int reversePinRight, int forwardPinLeft, int reversePinLeft)
{
// connect motor controller pins to Arduino digital pins
// motor Left
_speedPinLeft = speedPinLeft;
_forwardPinLeft = forwardPinLeft;
_reversePinLeft = reversePinLeft;
// motor two
_speedPinRight = speedPinRight;
_forwardPinRight = forwardPinRight;
_reversePinRight = reversePinRight;
// set all the motor control pins to outputs
pinMode(speedPinLeft, OUTPUT);
pinMode(speedPinRight, OUTPUT);
pinMode(forwardPinLeft, OUTPUT);
pinMode(reversePinLeft, OUTPUT);
pinMode(forwardPinRight, OUTPUT);
pinMode(reversePinRight, OUTPUT);
_usePID=1; //Will not use PID by default.
PID MotorPID = PID();
Trapezoid TrapPath = Trapezoid();
}
static int16_t SumBucket(int16_t const * in, unsigned index) {
uint16_t lo = pitch_bounds[index];
uint16_t hi = pitch_bounds[index + 1];
unsigned lo_int = lo >> 8;
unsigned hi_int = hi >> 8;
uint8_t lo_frc = lo & 0xFF;
uint8_t hi_frc = hi & 0xFF;
unsigned count = hi_int - lo_int;
int16_t const * p = in + lo_int;
int32_t result = Sum(p, count);
result -= in[lo_int] / 2;
result -= Trapezoid(lo_frc, in[lo_int], in[lo_int + 1]);
result += in[hi_int] / 2;
result += Trapezoid(hi_frc, in[hi_int], in[hi_int + 1]);
return Sat(result);
}
std::vector<std::string> TermFactory::available() const {
std::vector<std::string> result;
result.push_back(Discrete().className());
result.push_back(Bell().className());
result.push_back(Gaussian().className());
result.push_back(GaussianProduct().className());
result.push_back(PiShape().className());
result.push_back(Ramp().className());
result.push_back(Rectangle().className());
result.push_back(SShape().className());
result.push_back(Sigmoid().className());
result.push_back(SigmoidDifference().className());
result.push_back(SigmoidProduct().className());
result.push_back(Trapezoid().className());
result.push_back(Triangle().className());
result.push_back(ZShape().className());
return result;
}
TermFactory::TermFactory() : ConstructionFactory<Term*>("Term") {
registerConstructor("", fl::null);
registerConstructor(Bell().className(), &(Bell::constructor));
registerConstructor(Binary().className(), &(Binary::constructor));
registerConstructor(Concave().className(), &(Concave::constructor));
registerConstructor(Constant().className(), &(Constant::constructor));
registerConstructor(Cosine().className(), &(Cosine::constructor));
registerConstructor(Discrete().className(), &(Discrete::constructor));
registerConstructor(Function().className(), &(Function::constructor));
registerConstructor(Gaussian().className(), &(Gaussian::constructor));
registerConstructor(GaussianProduct().className(), &(GaussianProduct::constructor));
registerConstructor(Linear().className(), &(Linear::constructor));
registerConstructor(PiShape().className(), &(PiShape::constructor));
registerConstructor(Ramp().className(), &(Ramp::constructor));
registerConstructor(Rectangle().className(), &(Rectangle::constructor));
registerConstructor(SShape().className(), &(SShape::constructor));
registerConstructor(Sigmoid().className(), &(Sigmoid::constructor));
registerConstructor(SigmoidDifference().className(), &(SigmoidDifference::constructor));
registerConstructor(SigmoidProduct().className(), &(SigmoidProduct::constructor));
registerConstructor(Spike().className(), &(Spike::constructor));
registerConstructor(Trapezoid().className(), &(Trapezoid::constructor));
registerConstructor(Triangle().className(), &(Triangle::constructor));
registerConstructor(ZShape().className(), &(ZShape::constructor));
}
Term* TermFactory::create(const std::string& className,
const std::vector<scalar>& params) const {
int requiredParams = -1;
if (className == Discrete().className()) {
if ((int)params.size() % 2 == 0) {
Discrete* term = new Discrete();
for (int i = 0; i < (int)params.size() - 1; i += 2) {
term->x.push_back(params.at(i));
term->y.push_back(params.at(i+1));
}
return term;
} else {
std::ostringstream ex;
ex << "[syntax error] a discrete term requires an even list of values, "
"but found <" << params.size() << "> values";
throw fl::Exception(ex.str(), FL_AT);
}
}
if (className == Bell().className()) {
if ((int)params.size() >= (requiredParams = 3)) {
return new Bell("", params.at(0), params.at(1), params.at(2));
}
}
if (className == Gaussian().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new Gaussian("", params.at(0), params.at(1));
}
}
if (className == GaussianProduct().className()) {
if ((int)params.size() >= (requiredParams = 4)) {
return new GaussianProduct("", params.at(0), params.at(1), params.at(2), params.at(3));
}
}
if (className == PiShape().className()) {
if ((int)params.size() >= (requiredParams = 4)) {
return new PiShape("", params.at(0), params.at(1), params.at(2), params.at(3));
}
}
if (className == Ramp().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new Ramp("", params.at(0), params.at(1));
}
}
if (className == Rectangle().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new Rectangle("", params.at(0), params.at(1));
}
}
if (className == SShape().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new SShape("", params.at(0), params.at(1));
}
}
if (className == Sigmoid().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new Sigmoid("", params.at(0), params.at(1));
}
}
if (className == SigmoidDifference().className()) {
if ((int)params.size() >= (requiredParams = 4)) {
return new SigmoidDifference("", params.at(0), params.at(1), params.at(2), params.at(3));
}
}
if (className == SigmoidProduct().className()) {
if ((int)params.size() >= (requiredParams = 4)) {
return new SigmoidProduct("", params.at(0), params.at(1), params.at(2), params.at(3));
}
}
if (className == Trapezoid().className()) {
if ((int)params.size() >= (requiredParams = 4))
return new Trapezoid("", params.at(0), params.at(1), params.at(2), params.at(3));
}
if (className == Triangle().className()) {
if ((int)params.size() >= (requiredParams = 3))
return new Triangle("", params.at(0), params.at(1), params.at(2));
}
if (className == ZShape().className()) {
if ((int)params.size() >= (requiredParams = 2)) {
return new ZShape("", params.at(0), params.at(1));
}
}
if (requiredParams >= 0) {
std::ostringstream ex;
ex << "[factory error] Term of class<" + className + "> "
"requires " << requiredParams << " parameters";
throw fl::Exception(ex.str(), FL_AT);
}
throw fl::Exception("[factory error] Term of class <" + className + "> not recognized", FL_AT);
}
