/**
* Get the range in inches from the ultrasonic sensor.
* @return double Range in inches of the target returned from the ultrasonic sensor. If there is
* no valid value yet, i.e. at least one measurement hasn't completed, then return 0.
*
* @param pingSlot The slot for the digital module for the ping connection
* @param pingChannel The channel on the digital module for the ping connection
* @param echoSlot The slot for the digital module for the echo connection
* @param echoChannel The channel on the digital module for the echo connection
*/
double GetUltrasonicInches(UINT32 pingSlot, UINT32 pingChannel, UINT32 echoSlot, UINT32 echoChannel)
{
Ultrasonic *us = USptr(pingSlot, pingChannel);
if (us != NULL)
return us->GetRangeInches();
else
return 0.0;
}
void AutonomousPeriodic()
{
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
while(EncDist.GetDistance()<EncVal1)
{
myRobot.Drive(-0.5, 0.0);
}
ang = -ang;
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
launcher.Set(1.0);
sonic.SetAutomaticMode(true);
while(sonic.GetRangeInches()>DefToShot)
{
myRobot.Drive(-0.5,0.0);
}
myRobot.Drive(0.0,0.0);
intake.Set(1.0);
}
