void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (initialZeroing) // moving to the inner limit
{
sprintf(myString, "initZero ip\n"); //in progress
SmartDashboard::PutString("DB/String 0", myString);
if (GetForkLimitSwitchMin())
{
SetForkMotor(MOTOR_SPEED_STOP);
gearToothCounter->Reset();
initialZeroing = false;
sprintf(myString, "initZero comp\n"); //complete
SmartDashboard::PutString("DB/String 0", myString);
}
}
else //manual control
{
//motor control
if (joystick->GetRawButton(BUT_JS_OUT) && !GetForkLimitSwitchMax()) // move outwards
SetForkMotor(MOTOR_SPEED_GO);
else if(joystick->GetRawButton(BUT_JS_IN) && !GetForkLimitSwitchMin()) // move inwards
SetForkMotor(-MOTOR_SPEED_GO);
else
SetForkMotor(MOTOR_SPEED_STOP); //stop
//counter control
if (joystick->GetRawButton(BUT_JS_RES_GTC)) // reset the gear tooth counter
gearToothCounter->Reset();
}
//status
sprintf(myString, "jsOut %d\n", joystick->GetRawButton(BUT_JS_OUT));
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "jsIn %d\n", joystick->GetRawButton(BUT_JS_IN));
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "jsResGtc %d\n", joystick->GetRawButton(BUT_JS_RES_GTC));
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "gtc #: %d\n", gearToothCounter->Get()); //gtc count
SmartDashboard::PutString("DB/String 5", myString);
}
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
SmartDashboard::PutNumber("StickZ",stick.GetZ());
SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));
finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
// scalerValue = stick.GetRawAxis(3);
arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
manualShooter();
shootingModes();
toggleIntake();
toggleIntakeMode();
setScalerValue();
// double volts = ourRangefinder->GetVoltage();
// SmartDashboard::PutNumber("Voltage",volts);
//t_motor.Set(stick2.GetZ());
//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
// t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
// finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
// arm_Motor.Set(stick2.GetY());
// Current Control mode Debug
SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
SmartDashboard::PutNumber("Position",t_motor.GetPosition());
// t_motor.Set(stick.GetAxis(Joystick::Slider));
// if (stick.GetRawButton(3) == true){
// t_motor.SetPosition(10000);
// }
// if (stick2.GetRawButton(7) == true and buttonpress == false){
//
// }
// else if (stick2.GetRawButton(3) == false and buttonpress == true){
// buttonpress = false;// drive with arcade style (use right stick)
//
// }
SmartDashboard::PutBoolean("buttonpress state",buttonpress);
if (stick2.GetRawButton(buttonBack) == true){
stateDisarmed = true;
stateArming1 = false;
stateArming2 = false;
stateArmed = false;
stateFiring1 = false;
stateFiring2 = false;
t_motor.SetPosition(0);
}
toggleIntake();
// if (stick2.GetRawButton(5) == true){
// intake_Spin_Motor.Set(1);
// }
// if (stick2.GetRawButton(5) == false){
// intake_Spin_Motor.Set(0);
// }
if (stick.GetRawButton(2) == true and buttonpress2 == false){
myServo->SetAngle(175);
}
if (stick.GetRawButton(2) == false and buttonpress2 == true){
myServo->SetAngle(5);
}
// servoSetPos(servoState);
// myServo->Set(.5);
Wait(0.005);// wait for a motor update time
// motorSetPos(launcherState);
}
}
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
}
