void Drive (float speed, int dist)
{
leftDriveEncoder.Reset();
leftDriveEncoder.Start();
int reading = 0;
dist = abs(dist);
// The encoder.Reset() method seems not to set Get() values back to zero,
// so we use a variable to capture the initial value.
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
dsLCD->UpdateLCD();
// Start moving the robot
robotDrive.Drive(speed, 0.0);
while ((IsAutonomous()) && (reading <= dist))
{
reading = abs(leftDriveEncoder.Get());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
dsLCD->UpdateLCD();
}
robotDrive.Drive(0.0, 0.0);
leftDriveEncoder.Stop();
}
void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
void OperatorControl(void)
{
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //This converts from Pos per Second to Rotations per Minute (See second number on back of encoder to replace 250 if you need it)
while (IsOperatorControl())
{
SmartDashboard::PutNumber("Digital Encoder RPM", digEncoder.GetRate()*ppsTOrpm);
Wait(0.1);
}
digEncoder.Stop();
}
/**
* Runs during test mode ```````
*/
void Test() {
TestMode tester(m_ds);
driveDistanceRight.Reset();
driveDistanceRight.Start();
ballGrabber.resetSetPoint();
shooter.motorShutOff();
while (IsTest() && IsEnabled()){
lcd->Clear();
tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
&testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
&ballGrabber.ballDetector, &analogTestSwitch,
&shooter, &ballGrabber
);
lcd->UpdateLCD();
Wait(0.1);
}
driveDistanceRight.Stop();
}
/**
* Stops the counting for the encoder object.
* Stops the counting for the Encoder. It still retains the count, but it doesn't change
* with pulses until it is started again.
*
* @param aSlot The digital module slot for the A Channel on the encoder
* @param aChannel The channel on the digital module for the A Channel of the encoder
* @param bSlot The digital module slot for the B Channel on the encoder
* @param bChannel The channel on the digital module for the B Channel of the encoder
*/
void StopEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
if (encoder != NULL)
encoder->Stop();
}
