void CubicSplineFilter::getFilter(vector<Vector>& samples)
{
for(int i = samples.size() - 2; i >=0; i--)
{
float x = samples[i].x();
float y = samples[i].y();
samples[i].e[0] = cubicFilter(x);
samples[i].e[1] = cubicFilter(y);
}
}
void Robot::OperatorControl()
{
LOGGER.Logf("Starting operator control.");
operatorControlEnabled = true;
operatorControlTask->Start();
while ( IsOperatorControl() )
{
float x, y, rot;
me->joystick1->GetAxis(&x, &y);
rot = me->joystick1->GetRawRotation();
if ( !me->joystick1->GetButton(TURRET_BUTTON) )
DRIVETRAIN.DriveArcade(me->speedMultiplier*rot, me->speedMultiplier * cubicFilter(-y));
me->secondaryDisplay.PrintfLine(2, "X/Y/R: %f/%f/%f", x, y, rot);
me->secondaryDisplay.PrintfLine(3, "rps:%f", ((me->joystick1->GetThrottle() + 1.0) / 2.0) * 15.0 + 20.0);
me->secondaryDisplay.PrintfLine(4, "ratio:%f", Singleton<Shooter>::GetInstance().GetTopRatio());
//This thread NEEDS to run at all times for network communication! DO NOT REMOVE THIS!
Wait(0.01);
}
operatorControlEnabled = false;
Wait(0.5);
operatorControlTask->Stop();
LOGGER.Logf("Stopping operator control.");
}