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 }