In the code you posted here, you only read the distance once, then constantly send it to the driver station, this could cause some odd behavior (assuming the sonar has a bit of turn on transient).
Also, if you are using iterative robot, I would suggest using it's Disabled/Autonomous/Teleop functions, instead of the ones used by simple robot (which may not even work). This code should do what you want to do, and uses IterativeRobot functions:
Code:
#include "WPILib.h"
#include "AnalogRangeFinder.h"
typedef enum
{
ANALOG_CHANNEL_1_RANGE_FINDER = 1,
ANALOG_CHANNEL_2_UNUSED,
ANALOG_CHANNEL_3_UNUSED,
ANALOG_CHANNEL_4_UNUSED,
ANALOG_CHANNEL_5_UNUSED,
ANALOG_CHANNEL_6_UNUSED,
ANALOG_CHANNEL_7_UNUSED,
ANALOG_CHANNEL_8_UNUSED,
ANALOG_CHANNEL_9_UNUSED
} ANALOG_CHANNEL_TYPE;
class Robot2012 : public IterativeRobot
{
AnalogRangeFinder rangeFinder;
Timer timeInState;
Timer timeSinceBoot;
DriverStation *driverStation;
DriverStationLCD *driverStationLCD;
// Local variables to count the number of periodic loops performed
UINT32 m_autoPeriodicLoops;
UINT32 m_disabledPeriodicLoops;
UINT32 m_telePeriodicLoops;
public:
Robot2012(void):
rangeFinder(ANALOG_CHANNEL_2_RANGE_FINDER),
timeInState(),
timeSinceBoot()
{
printf("Robot2012 Constructor Started\n");
// Acquire the Driver Station object
driverStation = DriverStation::GetInstance();
driverStationLCD = DriverStationLCD::GetInstance();
// Initialize counters to record the number of loops completed in autonomous and teleop modes
m_autoPeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
m_telePeriodicLoops = 0;
printf("Robot2012 Constructor Completed\n");
}
/********************************** Init Routines *************************************/
void RobotInit(void)
{
// Actions which would be performed once (and only once) upon initialization of the
// robot would be put here.
timeSinceBoot.Start();
printf("RobotInit() completed.\n");
}
void DisabledInit(void)
{
m_autoPeriodicLoops = 0;
m_telePeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
timeInState.Reset();
timeInState.Start();
}
void AutonomousInit(void)
{
m_autoPeriodicLoops = 0;
m_telePeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
timeInState.Reset();
timeInState.Start();
}
void TeleopInit(void)
{
m_autoPeriodicLoops = 0;
m_telePeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
timeInState.Reset();
timeInState.Start();
}
/********************************** Periodic Routines *************************************/
//These routines run at a determined rate (several times per second, but not sure how many ms each) as long as periodic and continuous routines don't take longer to execute than expected
void DisabledPeriodic(void)
{
m_disabledPeriodicLoops++;
driverStationLCD->PrintfLine((DriverStationLCD::Line) 4, "Range to target: %f", rangeFinder.GetRangeInches());
}
void AutonomousPeriodic(void)
{
m_autoPeriodicLoops++;
driverStationLCD->PrintfLine((DriverStationLCD::Line) 4, "Range to target: %f", rangeFinder.GetRangeInches());
}
void TeleopPeriodic(void)
{
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
driverStationLCD->PrintfLine((DriverStationLCD::Line) 4, "Range to target: %f", rangeFinder.GetRangeInches());
}
/********************************** Continuous Routines *************************************/
//These run more frequently than Periodic loops, and should only contain work that requires frequent execution (and doesn't take long to complete)
//Uncomment these if needed
// void DisabledContinuous(void)
// {
// }
//
// void AutonomousContinuous(void)
// {
// }
// void TeleopContinuous(void)
// {
// }
};
START_ROBOT_CLASS( Robot2012);