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)