void CPMotion::Smooth(short Velocity, short Duration)
{
//OK
RampUp(Velocity);
SWait(Duration);
RampDown(Velocity);
}
void CPMotion::Staccato(short Velocity, short Duration, short Iterations)
{
//do only 3 interations to demo
for (int i = 1; i < Iterations; i++)
{
RampUp(Velocity);
RampDown(Velocity);
SWait(Duration);
}
}
void Robot::Autonomous()
{
Singleton<Logger>::GetInstance().Logf("Starting Autonomous Mode.");
Singleton<Collector>::GetInstance().SetBallCount( 2); // preloaded with 2 balls in autonomous
primaryDisplay.PrintfLine(0, "Shooting 2");
ShootBasket( 2 );
KinectStick leftStick(1);
KinectStick rightStick(2);
Timer displayUpdateFrequency;
displayUpdateFrequency.Start();
primaryDisplay.PrintfLine(0, "Kinect Controls");
while (IsAutonomous() )
{
secondaryDisplay.PrintfLine(1, "Shot-Dir: %.2f", shotDirectionModifier());
secondaryDisplay.PrintfLine(2, "Shot-Dist: %.1f\"", shotDistanceModifier());
DRIVETRAIN.SetLeft(-leftStick.GetY());
DRIVETRAIN.SetRight(rightStick.GetY());
if (leftStick.GetRawButton(1) ) // Left Leg Out
{
primaryDisplay.PrintfLine(0, "Ramp UP");
RampUp();
}
if (leftStick.GetRawButton(2) ) // Right Leg Out
{
primaryDisplay.PrintfLine(0, "Ramp DOWN");
RampDown();
}
if ( !leftStick.GetRawButton(1) && !leftStick.GetRawButton(2) ) // both legs in
RampOff();
// The Joystick Throttle controls the scrolling of the display
// The display is updated at a controlled pace
DisplayWrapper::GetInstance()->SetScrollLocation(joystick1->GetThrottle());
if (displayUpdateFrequency.HasPeriodPassed(1.0 / 5)) {
secondaryDisplay.PrintfLine(1, "Shot-Dir: %.2f", shotDirectionModifier());
secondaryDisplay.PrintfLine(2, "Shot-Dist: %.1f\'", shotDistanceModifier());
displayUpdateFrequency.Reset();
DisplayWrapper::GetInstance()->Output();
}
Wait(0.1);
}
Singleton<Logger>::GetInstance().Logf("Stopping Autonomous Mode.");
}
