def autonomous():
print ("Robot autonomous")
util.motd()
wpi.GetWatchdog().SetEnabled(False)
wpi.GetWatchdog().Kill()
drive.holonomicDrive(0.5)
wpi.Wait(0.5)
def autonomous():
print("Robot autonomous")
util.motd()
wpi.GetWatchdog().SetEnabled(False)
wpi.GetWatchdog().Kill()
ad.go(drive, lsg)
while wpi.IsAutonomous() and wpi.IsEnabled():
checkRestart()
wpi.Wait(0.01)
drive.holonomicDriveRaw(1, 0, 0)
def teleop():
dog = wpilib.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
shiftTime = wpilib.Timer()
shiftTime.Start()
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
dog.Feed()
checkRestart()
if shiftTime.Get() > 0.3:
shifter1.Set(False)
shifter2.Set(False)
# Shifter control
if rstick.GetTrigger():
shifter1.Set(True)
shifter2.Set(False)
shiftTime.Reset()
highGear = True
elif lstick.GetTrigger():
shifter1.Set(False)
shifter2.Set(True)
shiftTime.Reset()
highGear = False
# Drive control
drive.TankDrive(lstick, rstick)
wpilib.Wait(0.04)
def teleop():
dog = wpilib.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
dog.Feed()
drive.TankDrive(stick1, stick2)
checkRestart()
wpilib.Wait(0.01)
def teleop():
dog = wpi.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
dog.SetEnabled(False)
dog.Kill()
while wpi.IsOperatorControl() and wpi.IsEnabled():
dog.Feed()
drive.tankDrive(stick1, stick2) #a quiet jaunt around the park
checkRestart()
wpi.Wait(0.01)
def teleop():
dog = wpilib.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
dog.Feed()
checkRestart()
# Motor control
motor.Set(lstick.GetY())
wpilib.Wait(0.04)
def OperatorControl(self):
#yself.pid.Enable()
print("MyRobot::OperatorControl()")
wpilib.GetWatchdog().SetEnabled(False)
#dog = wpilib.GetWatchdog()
#dog.setEnabled(True)
#dog.SetExpiration(10)
while self.IsOperatorControl() and self.IsEnabled():
#dog.Feed()
#self.drive.MecanumDrive_Cartesian(self.Joystick.GetY(), self.Joystick.GetX(), self.Joystick2.GetX(), 0)
self.FromOperatorControl()
wpilib.Wait(0.01)
def teleop():
print("Robot Teleoperated")
dog = wpi.GetWatchdog()
global doggy
if doggy:
dog.SetEnabled(True)
dog.SetExpiration(0.25)
else:
dog.SetEnabled(False)
dog.Kill()
while wpi.IsOperatorControl() and wpi.IsEnabled():
dog.Feed()
drive.arcadeDrive(stick1) #a quiet jaunt around the park
checkRestart()
wpi.Wait(0.01)
def teleop():
print ("Robot Teleoperated")
dog = wpi.GetWatchdog()
global doggy
if doggy:
dog.SetEnabled(True)
dog.SetExpiration(0.25)
else:
dog.SetEnabled(False)
dog.Kill()
while wpi.IsOperatorControl() and wpi.IsEnabled():
dog.Feed()
if stick1.get_button(stick1.a):
drive.arcadeDrive(stick1) #a quiet jaunt around the park
else:
drive.tankDrive(stick1)
#base.send(stick1.getStickAxes())
#base.send(stick1.get_button_list())
checkRestart()
wpi.Wait(0.01)
def autonomous():
wpilib.GetWatchdog().SetEnabled(False)
while wpilib.IsAutonomous() and wpilib.IsEnabled():
checkRestart()
wpilib.Wait(0.01)
def Run(self):
wpilib.GetWatchdog().SetExpiration(1.0)
wpilib.GetWatchdog().SetEnabled(True)
time = wpilib.Timer()
haveTime = wpilib.Timer()
waitTime = wpilib.Timer()
ResetDriveEncoders()
kickerEncoder.Reset() #zeroing kicker
time.Start()
haveTime.Start()
waitTime.Start()
prevState = -1
state = 0
i = 0
kickPower = 188
kickPowerFar = [188 + 66, 188 + 66, 152 + 66, 152 + 33, 152 + 33]
#kickPowerFar = [50, 50, 50, 50, 50]
kickPowerFarBounce = [188 - 66, 188 - 99, 152 - 66, 152 - 66, 152 - 66]
kickPowerMid = [50 + 33, 50 + 33, 50 + 33, 50 + 33, 50 + 33]
kickPowerNear = [0]*5
ballsKicked = 0
intakeSpeed = -1.0
# Set up kick power for each mode
autoConfig = [
(kickPowerNear, 0.0), # do nothing
(kickPowerFar, 0.0), # 1
(kickPowerFar, 1.0), # 2
(kickPowerFar, 2.0), # 3
(kickPowerMid, 0.0), # 4
(kickPowerMid, 1.0), # 5
(kickPowerMid, 2.0), # 6
(kickPowerNear, 0.0), # 7
(kickPowerFar, 0.0), # 8
(kickPowerFarBounce, 0.0)] # 9
kickPowerUse, startDelay = autoConfig[self.autoMode]
while wpilib.IsAutonomous() and wpilib.IsEnabled():
CheckRestart()
wpilib.GetWatchdog().Feed()
# check the kicker pneumatics
self.kicker.AutoPeriodic()
# always run the intake motor
intakeMotor.Set(intakeSpeed)
if prevState != state:
print("AUTO mode:%d state:%d" % (self.autoMode, state))
prevState = state
# ball detection
if not HaveBall():
haveTime.Reset()
i += 1
if (self.autoMode >= 1 and self.autoMode <= 6 or self.autoMode == 8
or self.autoMode == 9):
# drive forward and auto-kick
if state == 0:
# Wait to start
if time.Get() > startDelay:
state = 1
elif state == 1:
# Drive forward while setting up for kick
if i%8 == 0:
print("S0: moving to ball; winch:%d" % kickerEncoder.Get())
self.kicker.AutoArm(kickPowerUse[ballsKicked])
drive.ArcadeDrive(-0.5, 0.0, False) # half speed drive
if time.Get() > 0.25 and HaveBall():
print("GOT BALL!!")
# got a ball; kick it!
drive.ArcadeDrive(0.0, 0.0, False)
state = 2
if leftDriveEncoder.Get() > 102*gDriveTicksPerInch:
state = 4
# stop after 2 balls kicked in mid zone
if (self.autoMode >= 4 and self.autoMode <= 6 and
ballsKicked >= 2):
state = 4
# stop after 3 balls kicked in far zone
if ((self.autoMode == 8 or self.autoMode == 9) and
ballsKicked >= 3):
state = 4
elif state == 2:
# wait until we're ready to fire, then fire!
if i%8 == 0:
print("S1: firing ball; winch:%d" % kickerEncoder.Get())
if self.kicker.AutoArm(kickPowerUse[ballsKicked]):
self.kicker.Fire()
ballsKicked += 1
if ballsKicked > 4:
ballsKicked = 4
state = 3
elif state == 3:
# wait until we finished kicking
if i%8 == 0:
print("S2: waiting to finish kick")
if self.kicker.IsKickComplete():
state = 1
elif state == 4:
if self.autoMode >= 4 and self.autoMode <= 6:
# mid zone modes
state = 5
elif self.autoMode == 8 or self.autoMode == 9:
# far zone go over bump
state = 8
else:
state = 20
elif state == 5:
# back up
self.kicker.AutoArm(kickPowerUse[ballsKicked])
drive.ArcadeDrive(1.0, 0.0, False)
if leftDriveEncoder.Get() < 24*gDriveTicksPerInch:
waitTime.Reset()
state = 6
elif state == 6:
# spin 90 deg
self.kicker.AutoArm(kickPowerUse[ballsKicked])
drive.ArcadeDrive(0.0, 0.85, False)
if waitTime.Get() > 0.85:
state = 20
elif state == 8:
# dry kick before driving over bump if we're armed
if self.kicker.armed:
# finish arming and fire
if self.kicker.AutoArm(kickPowerUse[ballsKicked]):
FireKicker()
state = 9
else:
state = 10
elif state == 9:
if self.kicker.IsKickComplete():
state = 10
elif state == 10:
# drive over bump
intakeSpeed = 1.0 # run roller out so we don't get penalties
self.kicker.AutoArm(kickPowerUse[ballsKicked], False)
if leftDriveEncoder.Get() > (96+36)*gDriveTicksPerInch:
waitTime.Reset()
state = 11
drive.ArcadeDrive(-0.5, 0.0, False) # drive forward
elif state == 11:
self.kicker.AutoArm(kickPowerUse[ballsKicked], False)
drive.ArcadeDrive(-0.85, 0.0, False) # drive forward higher power
if waitTime.Get() > 3.0:
state = 20
elif state == 20:
# make sure we winch back so we don't get a penalty
if i%8 == 0:
print("S10: done")
self.kicker.AutoArm(kickPowerUse[ballsKicked], False)
drive.ArcadeDrive(0.0, 0.0, False)
elif self.autoMode == 7:
# near zone: drive forward and auto-kick 1 ball, then back up
if state == 0:
# Wait to start
if time.Get() > startDelay:
state = 1
elif state == 1:
# Drive forward while setting up for kick
if i%8 == 0:
print("S0: moving to ball; winch:%d" % kickerEncoder.Get())
self.kicker.AutoArm(kickPowerNear[0])
drive.ArcadeDrive(-0.5, 0.0, False) # half speed drive
if time.Get() > 0.25 and haveTime.Get() > 0.25:
print("GOT BALL!!")
# got a ball; kick it!
drive.ArcadeDrive(0.0, 0.0, False)
state = 2
if leftDriveEncoder.Get() > 6*12*gDriveTicksPerInch:
state = 4
elif state == 2:
# wait until we're ready to fire, then fire!
if i%8 == 0:
print("S1: firing ball; winch:%d" % kickerEncoder.Get())
if self.kicker.AutoArm(kickPowerNear[0]):
self.kicker.Fire()
state = 3
elif state == 3:
# wait until we finished kicking
if i%8 == 0:
print("S2: waiting to finish kick")
if self.kicker.IsKickComplete():
state = 4
elif state == 4:
if i%8 == 0:
print("S4: driving back")
drive.ArcadeDrive(1.0, 0.0, False) # full speed drive backwards
if leftDriveEncoder.Get() < 24*gDriveTicksPerInch:
waitTime.Reset()
state = 5
if (self.kicker.IsKickComplete() and
self.kicker.AutoArm(kickPowerNear[0]) and
haveTime.Get() > 0.25):
self.kicker.Fire()
elif state == 5:
if i%8 == 0:
print("S5: wait to drive forward again")
if (self.kicker.IsKickComplete() and
self.kicker.AutoArm(kickPowerNear[0]) and
haveTime.Get() > 0.25):
self.kicker.Fire()
if waitTime.Get() > 5.0:
state = 6
elif state == 6:
if i%8 == 0:
print("S5: drive forward slowly kicking balls again")
if (self.kicker.IsKickComplete() and
self.kicker.AutoArm(kickPowerNear[0]) and
haveTime.Get() > 0.25):
self.kicker.Fire()
drive.ArcadeDrive(-0.35, 0.0, False) # quarter speed drive
if leftDriveEncoder.Get() > 10*12*gDriveTicksPerInch:
state = 10
elif state == 10:
# make sure we winch back so we don't get a penalty
if i%8 == 0:
print("S10: done")
if (self.kicker.IsKickComplete() and
self.kicker.AutoArm(kickPowerNear[0]) and
haveTime.Get() > 0.25):
self.kicker.Fire()
drive.ArcadeDrive(0.0, 0.0, False)
wpilib.Wait(0.01)
