void TeleopInit() {
forkState = closing;
SetForkMotor(-MOTOR_SPEED); //start in closing state
absGearToothCount = 0;
curGearToothCount = 0;
lastGearToothCount = 0;
running = true; //
//forkDirection initialized when first used
//curForkSetSpeed initialized when first used
dsBox->SetOutputs(ALL_LEDS_OFF);
}
void TeleopPeriodic()
{
char myString [STATUS_STR_LEN];
if (running)
{
switch (forkState)
{
case closing:
UpdateGearCount (); //update whenever the forks are moving
if (GetForkLimitSwitchMin())
{
sprintf(myString, "min gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 8", myString);
SetForkMotor(MOTOR_STOP);
SetForkMotor(MOTOR_SPEED);
dsBox->SetOutputs(ALL_LEDS_ON);
forkState = opening;
}
break;
case opening:
UpdateGearCount (); //update whenever the forks are moving
if (GetForkLimitSwitchMax())
{
sprintf(myString, "max gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 9", myString);
SetForkMotor(MOTOR_STOP);
SetForkMotor(-MOTOR_SPEED);
dsBox->SetOutputs(ALL_LEDS_OFF);
forkState = closing;
}
break;
}
}
//current monitor check for safety
if (forkMotor->GetOutputCurrent() > MAX_CUR_TH)
{
SetForkMotor(MOTOR_STOP);
running = false;
}
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 0", myString);
sprintf(myString, "State: %d\n", forkState);
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "running: %d\n", running);
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "forkDir: %d\n", forkDirection);
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "abs gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 4", myString);
#if 0
if (minAVal > aIn->GetValue())
minAVal = aIn->GetValue();
if (maxAVal < aIn->GetValue())
maxAVal = aIn->GetValue();
sprintf(myString, "min A val: %d\n", minAVal);
SmartDashboard::PutString("DB/String 5", myString);
sprintf(myString, "max A val: %d\n", maxAVal);
SmartDashboard::PutString("DB/String 6", myString);
#endif
}