void SetAxisFrequency(struct MotorStatus* pStatus, float frequency, Direction direction)
{
pStatus->targetDirection = direction;
pStatus->targetFrequency = frequency;
pStatus->ramping = 1;
if (pStatus->currentDirection == 0)
{
pStatus->currentDirection = pStatus->targetDirection;
}
Ramp(pStatus);
}
////////////////////////////////////////////////////////////////////////////// TASK MAIN!!! \(^0^)/
task main()
{
clearDebugStream();
gyroCal();
getSeeker();
int left = 0;
int delay = 0;
int endDump = 0;
int ramp = 0;
int* switches = getButtons();
//asign meaning to the touch sensors
if(switches[3])
left = 1;
else
left = 0;
if(switches[2])
delay = 1;
else
delay = 0;
if(switches[1])
endDump = 1;
else
endDump = 0;
if(switches[0])
ramp = 1;
else
ramp = 0;
//Round starts here
waitForStart();
nMotorEncoder[BR] = 0;
nMotorEncoder[wrist1] = 0;
nMotorEncoder[arms] = 0;
if(endDump)
EndDump(left, delay);
else if(ramp)
Ramp(left, delay);
else
IRramp(left , delay);
}
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));
}
void RampMotors()
{
Ramp(raMotorStatusP);
}
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);
}
const std::vector <float> & CARCONTROLMAP_LOCAL::ProcessInput(const std::string & joytype, EVENTSYSTEM_SDL & eventsystem, float steerpos, float dt, bool joy_200, float carms, float speedsens, int screenw, int screenh, float button_ramp, bool hgateshifter)
{
assert(inputs.size() == CARINPUT::INVALID); //this looks weird, but it ensures that our inputs vector contains exactly one item per input
assert(lastinputs.size() == CARINPUT::INVALID); //this looks weird, but it ensures that our inputs vector contains exactly one item per input
for (std::map <CARINPUT::CARINPUT, std::vector <CONTROL> >::iterator n = controls.begin(); n != controls.end(); ++n)
{
float newval = 0.0;
for (std::vector <CONTROL>::iterator i = n->second.begin(); i != n->second.end(); ++i)
{
bool handled = false;
float tempval = newval;
if (i->type == CONTROL::JOY)
{
//cout << "type joy" << std::endl;
if (i->joytype == CONTROL::JOYAXIS)
{
float val = eventsystem.GetJoyAxis(i->joynum, i->joyaxis);
if (i->joyaxistype == CONTROL::NEGATIVE)
val = -val;
val = ApplyDeadzone(i->deadzone,val);
val = ApplyGain(i->gain,val);
double absval = val;
bool neg = false;
if (val < 0)
{
absval = -val;
neg = true;
}
val = ApplyExponent(i->exponent,absval);
if (neg)
val = -val;
tempval = val;
handled = true;
}
else if (i->joytype == CONTROL::JOYBUTTON)
{
TOGGLE button = eventsystem.GetJoyButton(i->joynum, i->joybutton);
if (i->onetime)
{
if (i->joypushdown && button.GetImpulseRising())
tempval = 1.0;
else if (!i->joypushdown && button.GetImpulseFalling())
tempval = 1.0;
else
tempval = 0.0;
handled = true;
}
else
{
float downval = 1.0;
float upval = 0.0;
if (!i->joypushdown)
{
downval = 0.0;
upval = 1.0;
}
tempval = Ramp(lastinputs[n->first], button.GetState() ? downval : upval, button_ramp, dt);
handled = true;
}
}
}
else if (i->type == CONTROL::KEY)
{
//cout << "type key" << std::endl;
EVENTSYSTEM_SDL::BUTTON_STATE keystate = eventsystem.GetKeyState(SDLKey(i->keycode));
if (i->onetime)
{
if (i->keypushdown && keystate.just_down)
tempval = 1.0;
else if (!i->keypushdown && keystate.just_up)
tempval = 1.0;
else
tempval = 0.0;
handled = true;
}
else
{
float downval = 1.0;
float upval = 0.0;
if (!i->keypushdown)
{
downval = 0.0;
upval = 1.0;
}
//if (inputs[n->first] != keystate.down ? downval : upval) std::cout << "Key ramp: " << i->keycode << ", " << n->first << std::endl;
tempval = Ramp(lastinputs[n->first], keystate.down ? downval : upval, button_ramp, dt);
handled = true;
}
}
else if (i->type == CONTROL::MOUSE)
{
//cout << "type mouse" << std::endl;
if (i->mousetype == CONTROL::MOUSEBUTTON)
{
//cout << "mousebutton" << std::endl;
EVENTSYSTEM_SDL::BUTTON_STATE buttonstate = eventsystem.GetMouseButtonState(i->mbutton);
if (i->onetime)
{
if (i->mouse_push_down && buttonstate.just_down)
tempval = 1.0;
else if (!i->mouse_push_down && buttonstate.just_up)
tempval = 1.0;
else
tempval = 0.0;
handled = true;
}
else
{
float downval = 1.0;
float upval = 0.0;
if (!i->mouse_push_down)
{
downval = 0.0;
upval = 1.0;
}
tempval = Ramp(lastinputs[n->first], buttonstate.down ? downval : upval, button_ramp, dt);
handled = true;
}
}
else if (i->mousetype == CONTROL::MOUSEMOTION)
{
//cout << "mousemotion" << std::endl;
std::vector <int> pos = eventsystem.GetMousePosition();
//std::cout << pos[0] << "," << pos[1] << std::endl;
float xval = (pos[0]-screenw/2.0)/(screenw/4.0);
if (xval < -1) xval = -1;
if (xval > 1) xval = 1;
float yval = (pos[1]-screenh/2.0)/(screenh/4.0);
if (yval < -1) yval = -1;
if (yval > 1) yval = 1;
float val = 0;
if (i->mdir == CONTROL::UP)
val = -yval;
else if (i->mdir == CONTROL::DOWN)
val = yval;
else if (i->mdir == CONTROL::LEFT)
val = -xval;
else if (i->mdir == CONTROL::RIGHT)
val = xval;
if (val < 0)
val = 0;
else if (val > 1)
val = 1;
val = ApplyDeadzone(i->deadzone,val);
val = ApplyGain(i->gain,val);
if (val < 0)
val = 0;
else if (val > 1)
val = 1;
double absval = val;
bool neg = false;
if (val < 0)
{
absval = -val;
neg = true;
}
val = ApplyExponent(i->exponent,absval);
if (neg)
val = -val;
if (val < 0)
val = 0;
else if (val > 1)
val = 1;
tempval = val;
//cout << val << std::endl;
handled = true;
}
//else cout << "mouse???" << std::endl;
}
//else cout << "type invalid" << std::endl;
if (tempval > newval)
newval = tempval;
assert(handled);
}
if (newval < 0)
newval = 0;
if (newval > 1.0)
newval = 1.0;
inputs[n->first] = newval;
//std::cout << "New input value: " << inputs[n->first] << std::endl;
}
if (hgateshifter)
{
bool havegear = inputs[CARINPUT::FIRST_GEAR] ||
inputs[CARINPUT::SECOND_GEAR] ||
inputs[CARINPUT::THIRD_GEAR] ||
inputs[CARINPUT::FOURTH_GEAR] ||
inputs[CARINPUT::FIFTH_GEAR] ||
inputs[CARINPUT::SIXTH_GEAR] ||
inputs[CARINPUT::REVERSE];
if (!havegear)
inputs[CARINPUT::NEUTRAL] = 1.0;
}
lastinputs = inputs;
//do steering processing
ProcessSteering(joytype, steerpos, dt, joy_200, carms*2.23693629, speedsens);
return inputs;
}