bool AcceptableToPrime()
{
if (shootertime.Get() > 5 && shottime.Get() > primewaittime)
return true;
else
return false;
}
inline void grabberPositionTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t backOutPtr, uint32_t grabPowerPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
Talon *grabTalon = (Talon *) grabTalonPtr;
Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
bool *isGrabbing = (bool *) isGrabbingPtr;
bool *backOut = (bool *) backOutPtr;
double *grabPower = (double *) grabPowerPtr;
Timer timer;
timer.Start();
*isGrabbing = true;//tells robot.cpp that thread is running
while (grabInnerLimit->Get() && timer.Get() < Constants::grabDelay) {//starts to spin motor to pass startup current
grabTalon->Set(1);//move in
}
while (pdp->GetCurrent(Constants::grabPdpChannel) < *grabPower && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
grabTalon->Set(1);
SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
}
if (*backOut) {
grabTalon->Set(0);//stop moving
timer.Reset();
while (timer.Get() < Constants::liftBackoutTime && joystick->GetRawButton(Constants::pickupCancelButton) == false) {
grabTalon->Set(-.75);
}
}
grabTalon->Set(0);//stop moving
timer.Stop();
*isGrabbing = false;//tells that thread is over
}
void AutonomousStateMachine() {
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
enum AutoState {
AutoDrive, AutoSetup, AutoShoot
};
AutoState autoState;
autoState = AutoDrive;
bool feederState = false;
bool hasFired = false;
Timer feeder;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
switch (autoState) {//Start of Case Machine
case AutoDrive://Drives the robot to set Destination
bool atDestination = driveControl.autoPIDDrive2();
if (atDestination) {//If at Destination, Transition to Shooting Setup
autoState = AutoSetup;
}
break;
case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
pneumaticsControl->ballGrabberExtend();
}
if (!feederState) {//Started the feeder timer once
feederState = true;
feeder.Start();
autoState = AutoShoot;
}
break;
case AutoShoot://Shoots the ball
if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
shooterControl->feed(true);
} else if (feeder.Get() >= 2.0) {//Transition to Shooting
shooterControl->feed(false);
feeder.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
hasFired = true;
}
} else if (hasFired) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
break;
}
dsLCD->UpdateLCD();
}
}
TEST(TimeUtils, TimerPositive) {
Timer t;
t.Start();
for (int i = 0; i < 10000; i++) {
t.Get();
}
ASSERT_GT(t.Get(), 0 * s);
}
TEST(TimeUtils, TimerReset) {
Timer t;
t.Start();
for (int i = 0; i < 10000; i++) {
t.Get();
}
t.Reset();
ASSERT_LT(t.Get(), 0.1 * s);
}
void RobotDemo::intelligent_shooter()
{
RPS_control_code(37.5);
switch (smart_autonomous_state)
{
case unstable:
cout << "State Unstable" << endl;
printf("%i %i", fabs(error_back) < 4, fabs(error_front) < 4);
if (fabs(error_back) < 4 || fabs(error_front) < 4)
{
smart_autonomous_state = stabilizing;
stabilizing_timer->Reset();
}
if (override_timer->Get() > 4)
{
smart_autonomous_state = fire;
}
break;
case stabilizing:
cout << "State Stabilizing " << endl;
if (stabilizing_timer->Get() > 1 || override_timer->Get() > 4)
{
smart_autonomous_state = fire;
}
if (fabs(error_back) > 4 || fabs(error_front) > 4)
{
smart_autonomous_state = unstable;
}
break;
case fire:
cout << "State Firing " << endl;
shooter_fire_piston_A ->Set(false);//piston -->
shooter_fire_piston_B ->Set(true);
smart_autonomous_state = retracting;
retraction_timer->Reset();
break;
case retracting:
cout << "State Retracting " << endl;
if (retraction_timer->Get() > 1)
{
smart_autonomous_state = retract;
break;
}
break;
case retract:
cout << "State Has Retracted " << endl;
shooter_fire_piston_A ->Set(true);//piston <--
shooter_fire_piston_B ->Set(false);
smart_autonomous_state = unstable;
override_timer->Reset();
break;
}
printf("%i\n", smart_autonomous_state);
}
void AutonomousGyroTurn() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
//State: Stopped
//Transition: Driving State
currentState = 2;
break;
case 2:
//State: Driving
//Stay in State until 2 seconds have passed--`
//Transition: Gyroturn State
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
currentState = 3;
turnController->SetSetpoint(90);
turnController->Enable();
}
break;
case 3:
//State: Gyroturn
//Stay in state until navx yaw has reached 90 degrees
//Transition: Driving State
drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
// if (ahrs->GetYaw() >= 90) {
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
//State:Driving
//Stay in state until 2 seconds have passed
//Transition: State Stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
currentState = 5;
timer->Stop();
}
break;
case 5:
//State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
void DriveThenShootAuto() {
//initizlizes all parts of robot
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
bool destinationPrevious = false;
bool autoShot = false; //true if autoShoot
//creates a timer for the ball grabber motors
Timer feeding;
bool started = false;
while (IsAutonomous() && IsEnabled()) {
//GetWatchdog().Feed();
//drives forward to shooting point
bool atDestination = destinationPrevious || driveControl.autoPIDDrive2(); //autoDrive returns true when robot reached it's goal
if (atDestination) {
// The robot has reached the destination on the floor and is now ready to open and shoot
if (!started) {
started = true;
destinationPrevious = true;
//starts feeding-timer controls feeder motors so the ball doesn't get stuck
feeding.Start();
}
pneumaticsControl->ballGrabberExtend();
//waits for feeding to be done
if (feeding.Get() < 2.0) {//3.0 was
shooterControl->feed(true);
} else if (feeding.Get() >= 2.0) { // 3.0 was
shooterControl->feed(false);
feeding.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !autoShot) {
shooterControl->autoShoot();
//TODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
//"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//works only after shoot is done firing
autoShot = true;
}
} else if (autoShot) {//runs only after shoot is done
//tODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
//"AutoMode Finished");
}
}
//TODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Feeder Time: %f",
//feeding.Get());
//TODO dsLCD->UpdateLCD();
}
}
void AutonomousPeriodic()
{
Scheduler::GetInstance()->Run(); // Was in default code don't know what it does
if (autoTimer.Get() > 0.0 ) { // If timer is greater then zero
if (autoTimer.Get() < 1.5 ) { // If the timer is less then 2.5
myDrive_all->TankDrive(0.8,0.8); // Make all motors go backwards
} else {// If the timer is greater then 2.5
myDrive_all->TankDrive(0.0,0.0); // Make all motors stop
}
} else { // If the timer is less then zero
autoTimer.Start(); // Starts timer
}
}
int vision::vision_entry() {
AxisCamera* camera=&(AxisCamera::GetInstance("10.6.12.11"));
while(true) {
Timer timer;
timer.Start();
std::printf("==== BEGIN VISION ITERATION ====\n");
ColorImage* image=camera->GetImage();
BinaryImage* binImage=image->ThresholdHSL(100,120,200,255,5,140);
BinaryImage* convexImage=binImage->ConvexHull(false);
ParticleFilterCriteria2 criteria[]={{IMAQ_MT_AREA,500,65535,false,false}};
BinaryImage* filteredImage=binImage->ParticleFilter(criteria,1);
int numParticles=filteredImage->GetNumberParticles();
std::printf("Number of particles: %d\n",numParticles);
for(int i=0;i<numParticles;i++) {
ParticleAnalysisReport report=filteredImage->GetParticleAnalysisReport(i);
std::printf("Area for particle %d: %f\n",i,report.particleArea);
}
delete image;
delete binImage;
delete convexImage;
delete filteredImage;
double timeElapsed=timer.Get();
std::printf("---- TIME TAKEN: %f ----\n",timeElapsed);
}
}
double GetRate() {
int dist = Get();
double rate = (double)(dist - lastDistance) / timer.Get();
lastDistance = dist;
timer.Reset();
return rate;
}
bool timeExpired()
{
if((arm->Get())>waitTill)
return true;
else
return false;
}
void RobotDemo::pneumatic_feeder_code()
{
if (operator_stick ->GetRawButton(shooter_piston_button)) //Button 1
{
if (kicker_button_on == false)
{
shooter_reset ->Reset();
kicker_button_on = true;
//kicker_piston_on = false;
shooter_piston_timer->Start();
shooter_fire_piston_A ->Set(false);//pushes
shooter_fire_piston_B ->Set(true);
cout << "out" << endl;
}
}
else
{
kicker_button_on = false;
}
if (shooter_piston_timer->Get() >= shooter_piston_delay)
{
shooter_fire_piston_A ->Set(true);//retracts
shooter_fire_piston_B ->Set(false);
shooter_piston_timer ->Reset();
shooter_piston_timer ->Stop();
cout << "back" << endl;
}
}
bool AcceptableToFire()
{
if (shottime.Get() > shootwaittime)
return true;
else
return false;
}
/**
* @brief Main thread function for Proxy166.
* Runs forever, until MyTaskInitialized is false.
*
* @todo Update DS switch array
*/
int Proxy::Main( int a2, int a3, int a4, int a5,
int a6, int a7, int a8, int a9, int a10) {
Robot *lHandle = NULL;
WaitForGoAhead();
lHandle = Robot::getInstance();
Timer matchTimer;
while(MyTaskInitialized) {
setNewpress();
if(lHandle->IsOperatorControl() && lHandle->IsEnabled()) {
if(manualDSIO) {
SetEnhancedIO();
}
if(manualJoystick[0]) {
SetJoystick(1, stick1);
}
if(manualJoystick[1]) {
SetJoystick(2, stick2);
}
if(manualJoystick[2]) {
SetJoystick(3, stick3);
}
if(manualJoystick[3]) {
SetJoystick(4, stick4);
}
}
if(!lHandle->IsEnabled()) {
matchTimer.Reset();
// It became disabled
matchTimer.Stop();
set("matchtimer",0);
} else {
// It became enabled
matchTimer.Start();
if(lHandle->IsAutonomous()) {
set("matchtimer",max( 15 - matchTimer.Get(),0));
} else {
set("matchtimer",max(120 - matchTimer.Get(),0));
}
}
// The task ends if it's not initialized
WaitForNextLoop();
}
return 0;
}
void AutonomousLowBar() {
// Strategy 2 - start in a normal position lined up with low bar, go through low bars and score boulder in lower goal.
// -------------------------------------------------------------------------------------------------------------------
// backUp straight for a little bit
// drop arm
// backup more under lowbar
// stop (we might add going to lowgoal later)
switch(currentState)
{
case 1:
timer->Reset();
timer->Start();
currentState = 2;
break;
case 2:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= .4)
{
drive->TankDrive(0.0,0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
intakeLever->Set(autoIntakeSpeed);
if(timer->Get() >= .5)
{
intakeLever->Set(0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= autoLength)
{
drive->TankDrive(0.0,0.0);
currentState = 5;
timer->Reset();
timer->Stop();
}
break;
}
}
//
// Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used
// to maintain control until the end of tele operator mode
//
void OperatorControl()
{
//myRobot.SetSafetyEnabled(true);
timer->Start();
Relay* reddlight = new Relay(4);
//Timer* lighttimer = new Timer();
//lighttimer->Start();
while (IsOperatorControl() && IsEnabled())
{
reddlight->Set(reddlight->kForward);
/*
if (lighttimer->Get()<=0.5) {
reddlight->Set(reddlight->kForward);
}
else if(lighttimer->Get()<1){
reddlight->Set(reddlight->kOff);
}
else {
lighttimer->Reset();
}
*/
//
// Get inputs
//
driverInput->GetInputs();
drive->GetInputs();
catapult->GetInputs();
feeder->GetInputs();
//
// Pass values between components as necessary
//
//catapult->SetSafeToFire(feeder->GetAngle()<95);
//
// Execute one step on each component
//
drive->ExecStep();
catapult->ExecStep();
feeder->ExecStep();
//
// Set Outputs on all components
//
catapult->SetOutputs();
feeder->SetOutputs();
//
// Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the
// timer for the next loop
//
while (timer->Get()<(PERIOD_IN_SECONDS));
timer->Reset();
}
}
void AutonomousAdjustableStraight() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(autoIntakeSpeed);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(autoSpeed, autoSpeed);
intakeLever->Set(-0.1);
if (timer->Get() >= autoLength) {
intakeLever->Set(0.0);
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
shooter->Set(-0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
shooter->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void Autonomous()
{
double forwardTime = SmartDashboard::GetNumber("ForwardTime",2.0);
double forwardSpeed = SmartDashboard::GetNumber("ForwardSpeed",0.5);
double twistTime = SmartDashboard::GetNumber("TwistTime",2.0);
double twistSpeed = SmartDashboard::GetNumber("TwistSpeed",0.5);
Timer *timer = new Timer();
timer->Start();
// int Loop = SmartDashboard::GetNumber("ForwardTime",400);
// autoLoopCounter = 0;
while(timer->Get() < forwardTime && IsEnabled())
{
robotDrive.ArcadeDrive(0.0, forwardSpeed);
// autoLoopCounter++;
// SmartDashboard::PutNumber("Counter",autoLoopCounter);
Wait(.005);
}
timer->Reset();
// int Loop2 = SmartDashboard::GetNumber("TwistTime",400);
// autoLoopCounter2 = 0;
while(timer->Get() < twistTime && IsEnabled())
{
robotDrive.ArcadeDrive(twistSpeed, 0.0);
autoLoopCounter2++;
Wait(.005);
}
timer->Stop();
robotDrive.ArcadeDrive(0.0,0.0);
stateDisarmed = false;
stateArming1 = false;
stateArming2 = false;
stateArmed = true;
stateFiring1 = false;
stateFiring2 = false;
free(timer);
}
void wait(double secToWait)
{
currentWaitTime = timer->Get();
if(bTimerInit)
{
initWaitTime = currentWaitTime;
bTimerInit = false;
isWait = true;
}
if(currentWaitTime < secToWait + initWaitTime)
{
isWait = true;
}
else
{
isWait = false;
bTimerInit = true;
bTimerLatch = false;
}
currentWaitTime = timer->Get();
}
void AutonomousStraightSpy() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(0.25);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 5) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void Print ()
{
if (PrintTime.Get() > PRINT_TIME)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
lcd->UpdateLCD();
PrintTime.Reset();
PrintTime.Start();
}
}
void smartDashboardPrint()
{
oi->dashboard->PutNumber("Drive Linear Speed: ", drive->getLinVelocity());
oi->dashboard->PutNumber("Drive Turn Speed: ", drive->getTurnSpeed());
oi->dashboard->PutNumber("Timer: ", timer->Get());
oi->dashboard->PutNumber("Pot Raw Value: ", manipulator->pot->GetVoltage());
oi->dashboard->PutBoolean(" Wait (Motors Disabled)", isWait);
oi->dashboard->PutBoolean(" Compressor", comp599->Enabled());
oi->dashboard->PutString("Arm Position: ", manipulator->getArmPosition() ? "Intake" : "Stored");
oi->dashboard->PutString("Shift State: ", drive->getShiftState() ? "High" : "Low");
oi->dashboard->PutString("Launch State: ", launcher->launchState > 0 ? (launcher->launchState == 1 ? "HOLD" : (launcher->launchState == 2 ? "RESET" : (launcher->launchState == 3 ? "COCKED" : "FIRE"))) : "OFF");
oi->dashboard->PutString("Camera Position: ", manipulator->getCameraPosition() > 0 ? ((manipulator->getCameraPosition() == 2) ? "Forward" : "Back") : "Inbetween");
oi->dashboard->PutBoolean(" Ready to Fire", launcher->launchState == STATE_COCKED ? true : false);
}
// Use only in autonomous mode
void MainRobot::WatchdogWait(double time) {
Timer* timer = new Timer();
timer->Start();
while (true) {
RobotBase::getInstance().GetWatchdog().Feed();
m_drive->Drive(autonomousDriveMagnitude, autonomousDriveCurve);
if (timer->Get() >= time) {
break;
}
Wait(.05);
}
timer->Stop();
delete timer;
}
void MainRobot::OperatorControl(void)
{
// Enables the watchdog (if comms are dropped, commit suicide)
mWatchdog->SetEnabled(false);
Timer timer;
mTestJag1->Set(1.0f);
mTestJag2->Set(1.0f);
timer.Start();
while (IsOperatorControl())
{
timer.Reset();
mWatchdog->Feed();
mDrive->Update();
if (timer.Get() < 0.0050)
Wait(0.0050 - timer.Get());
}
mTestJag1->Set(0.0f);
mTestJag2->Set(0.0f);
}
void RA14Robot::EndOfCycleMaintenance() {
//CurrentSensorReset->Set(1);
// ResetSetting = ! ResetSetting;
// CurrentSensorReset->Set(ResetSetting);
if (resetCurrentSensorTimer->Get() > Config::GetSetting(
"curent_sensor_reset_time", .1)) {
CurrentSensorReset->Set(1);
resetCurrentSensorTimer->Reset();
CurrentSensorReset->Set(0);
}
logging();
target->Parse("");
//signalOutCycle->Set(0);
}
void logdata()
{
//log data
fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ',';
fout << magicBox->getGyroAngle() << ',';
autoLog();
myDrive->log(fout);
myShooter->log(fout);
//myClimber->Log(fout);
log_gamepad(fout, driverGamepad);
log_gamepad(fout, operatorGamepad);
fout<<LEDTimer<<",";
fout<<LEDPercent(LEDTimer)<<",";
fout<<LEDPercent(LEDTimer + 30)<<",";
fout<<endl;
}
TEST_F(DIOLoopTest, SynchronousInterruptWorks) {
// Given a synchronous interrupt
m_input->RequestInterrupts();
// If we have another thread trigger the interrupt in a few seconds
pthread_t interruptTriggererLoop;
pthread_create(&interruptTriggererLoop, nullptr, InterruptTriggerer,
m_output);
// Then this thread should pause and resume after that number of seconds
Timer timer;
timer.Start();
m_input->WaitForInterrupt(kSynchronousInterruptTime + 1.0);
EXPECT_NEAR(kSynchronousInterruptTime, timer.Get(),
kSynchronousInterruptTimeTolerance);
}
/****************************************
* UpdateDashboard:
* Input = none
* Output = Safety mode
* Watchdog state
* Robot Speed
* System state (Autonomous or Teleoperated?)
* Robot state (Enabled or Disabled?)
* Timer
* Minibot alert
* Dependent on the 'SmartDashboard' class from the WPI library.
* TODO:
* - Test to see if this works.
*/
void UpdateDashboard(void)
{
// Setup here (default values set):
const char *watchdogCheck;
if (GetWatchdog().IsAlive()) {
watchdogCheck = GetWatchdog().GetEnabled() ? "Enabled" : "DISABLED";
} else {
watchdogCheck = "DEAD";
}
const char *systemState;
if (IsOperatorControl()) {
systemState = "Teleoperate";
} else if (IsAutonomous()) {
systemState = "Autonomous";
} else {
systemState = "???";
}
const char *minibotStatus;
float currentTime = timer.Get();
if (currentTime >= (GAMEPLAY_TIME - 15)) {
minibotStatus = "Get Ready";
if (currentTime >= (GAMEPLAY_TIME - 10)) {
minibotStatus = "DEPLOY";
}
} else {
minibotStatus = "...";
}
// Safety info
SmartDashboard::Log(isSafetyModeOn ? "ENABLED" : "Disabled", "Safety mode: ");
SmartDashboard::Log(watchdogCheck, "Watchdog State: ");
// Robot actions
SmartDashboard::Log(isFastSpeedOn ? "Fast" : "Normal", "Speed: ");
SmartDashboard::Log(lift->getCurrentHeight(), "Current Lift Height: ");
// Info about the field state
SmartDashboard::Log(systemState, "System State: ");
SmartDashboard::Log(IsEnabled() ? "Enabled" : "DISABLED", "Robot State: ");
// Info about time
SmartDashboard::Log(minibotStatus, "MINIBOT ALERT: ");
}
void ElevatorModule::checkError(){
return;
std::cout<<"ELEVATOR MODULE: CHECKING ERROR" << std::endl;
Timer encoderTimeOut;
encoderTimeOut.Start();
while(true) {
if(m_Enabled && !m_Manual) {
if(abs(m_Encoder->GetRate()) < .05) { }
else encoderTimeOut.Reset();
if(encoderTimeOut.Get() > 0.1) {
throw MovementError(this, "ElevatorModule::checkError()" , "check if Encoder is plugged in");
}
}
}
}
