def wait(self):
'''Waits until the delay period has passed'''
# we must *always* yield here, so other things can run
wpilib.Wait(0.001)
while not self.timer.HasPeriodPassed(self.delay_period):
wpilib.Wait(0.001)
def calcValue(self):
while not self.stopThread:
try:
#This tells us how much it has traveled since it was last looked at by comparing last to current
Rate = 0.0
lastRate = self.rate
potVal = self.potObj.GetVoltage()
potDiff = (self.potObj.GetVoltage() - self.lastPotVal)
#The pots give some jumpy data when not moving so this is a very basic filter that seprates out any garbage in readings
if (abs(potDiff) >= 0.02):
#here we actually calulate out the rate, rate is just distance travled over time, we time how fast the pot moved
#Becuase of the way the shooter works, we don't want negitive values
if (potDiff > 0):
Rate = potDiff / self.timerObj.Get()
Rate = (Rate + lastRate) / 2
else:
pass
except ZeroDivisionError:
Rate = 0.0
print("RPM error zero divison")
except:
print("RPM error unhaneled")
self.stopThread = True
self.rate = Rate
self.lastPotVal = potVal
self.timerObj.Reset()
wpilib.Wait(0.01)
def OperatorControl(self):
# print(self.IsEnabled())
print("MyRobot::OperatorControl()")
# for testing, we don't care about this
dog = self.GetWatchdog()
dog.SetExpiration(0.25)
dog.SetEnabled(True)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
#Driving
self.robot_drive.MecanumDrive_Cartesian(self.joystick1.GetY(),
self.joystick1.GetX(),
-1 * self.joystick2.GetX())
self.Intake()
self.Catapult()
self.Solenoids()
self.SmartDash()
wpilib.Wait(0.05)
def autonomous():
print ("Robot autonomous")
util.motd()
wpi.GetWatchdog().SetEnabled(False)
wpi.GetWatchdog().Kill()
drive.holonomicDrive(0.5)
wpi.Wait(0.5)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
shiftTime = wpilib.Timer()
haveTime = wpilib.Timer()
shiftTime.Start()
haveTime.Start()
intakeVelocity = -0.75
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
if not HaveBall():
haveTime.Reset()
# Reset pneumatics
if shiftTime.Get() > 0.3:
shifter1.Set(False)
shifter2.Set(False)
# Kicker control
self.kicker.OperatorControl()
# Drive control
drive.TankDrive(lstick, rstick)
# Shifter control
if rstick.GetTrigger():
shifter1.Set(True)
shifter2.Set(False)
shiftTime.Reset()
elif lstick.GetTrigger():
shifter1.Set(False)
shifter2.Set(True)
shiftTime.Reset()
# Intake motor
if stick3.GetRawButton(6):
intakeVelocity = 1.0
elif stick3.GetRawButton(7):
intakeVelocity = -0.75
elif stick3.GetRawButton(8):
intakeVelocity = 0.0
#elif rstick.GetRawButton(11):
# intakeVelocity = 0.75
# The motors are stopped when kicking; start them up again after
# timeout
if self.kicker.kickTime.Get() > 0.1:
if (haveTime.Get() > 2.0 and intakeVelocity < 0.0
and lstick.GetY() <= 0.0 and rstick.GetY() <= 0.0):
intakeMotor.Set(-0.35)
else:
intakeMotor.Set(intakeVelocity)
wpilib.Wait(0.01)
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 RobotInit(self):
wpilib.Wait(0.25)
winchControl.SetOutputRange(-1.0, 1.0)
kickerEncoder.Start()
leftDriveEncoder.Start()
rightDriveEncoder.Start()
compressor.Start()
def Autonomous(self):
self.GetWatchdog().SetEnabled(False)
self.autonomous.reset()
while self.IsAutonomous() and self.IsEnabled():
self.autonomous.iterate()
wpilib.Wait(0.01)
def run_old():
"""Main loop, old"""
while 1:
if wpi.IsDisabled():
print("Running disabled()")
disabled()
while wpi.IsDisabled():
wpi.Wait(0.01)
elif wpi.IsAutonomous():
print("Running autonomous()")
autonomous()
while wpi.IsAutonomous() and wpi.IsEnabled():
wpi.Wait(0.01)
else:
print("Running teleop()")
teleop()
while wpi.IsOperatorControl() and wpi.IsEnabled():
wpi.Wait(0.01)
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)
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
dog.Feed()
drive.TankDrive(stick1, stick2)
checkRestart()
wpilib.Wait(0.01)
def Disabled(self):
'''Called when the robot is in disabled mode'''
wpilib.SmartDashboard.PutNumber('RobotMode', MODE_DISABLED)
while self.IsDisabled():
self.communicateWithSmartDashboard(True)
wpilib.Wait(0.01)
def place_dingus(robot):
robot.stop()
self.raise_()
wpilib.Wait(0.25)
self.stop()
robot.tankDrive(0.25, 0.25)
wpilib.Wait(0.125)
robot.stop()
self.put_down()
self.lower()
wpilib.Wait(0.125)
robot.tankdrive(-0.5, -0.5)
wpilib.Wait(0.125)
self.pick_up()
wpi.Wait(
0.25
) #TODO: Find out how long it takes for robot to turn self around
self.stop()
robot.tankDrive(0.5, -0.5)
wpi.Wait(0.3)
def run():
"""Main loop"""
print("Start compressor")
compressor.Start()
while 1:
if wpilib.IsDisabled():
print("Running disabled()")
disabled()
while wpilib.IsDisabled():
wpilib.Wait(0.01)
elif wpilib.IsAutonomous():
print("Running autonomous()")
autonomous()
while wpilib.IsAutonomous() and wpilib.IsEnabled():
wpilib.Wait(0.01)
else:
print("Running teleop()")
teleop()
while wpilib.IsOperatorControl() and wpilib.IsEnabled():
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):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
# Motor control
motor.Set(lstick.GetY())
wpilib.Wait(0.04)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
wpilib.SmartDashboard.PutString("python_robot_says",
"I'm not dead yet.")
print("python_robot says, I'm not dead yet, to the console")
wpilib.Wait(0.04)
def TeleopContinuous(self):
wpilib.Wait(0.01)
#lmotor.Set(lstick.GetY())
#rmotor.Set(-rstick.GetY())
#left=lstick.GetY()
#right=-rstick.GetY()
#roboDR.ArcadeDrive(lstic,true)
x = lstick.GetX()
y = lstick.GetY()
left = x - y
right = x + y
#cruise=(lstick.GetZ()-1)/-2;
lmotor.Set((left * math.fabs(left)))
rmotor.Set(-1 * (right * math.fabs(right)))
def Disabled(self):
rPrevious = False
lPrevious = False
self.auto.DisplayMode()
while self.IsDisabled():
CheckRestart()
self.auto.SelectMode(lstick.GetTrigger() and not lPrevious,
rstick.GetTrigger() and not rPrevious)
rPrevious = rstick.GetTrigger()
lPrevious = lstick.GetTrigger()
wpilib.Wait(0.01)
def Disabled(self):
self.dog.SetEnabled(True)
for type, component in self.components.items():
component.disabled_init()
while wpilib.IsDisabled():
self.dog.Feed()
for type, component in self.components.items():
component.disabled_tick(wpilib.Timer.GetFPGATimestamp())
wpilib.Wait(0.01)
self.dog.SetEnabled(False)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
self.mecanum_drive.iterate()
self.intake.iterate()
self.shooter.iterate()
self.shooter_service.iterate()
wpilib.Wait(0.04)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
pidController.Enable()
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
# Motor control
pidController.SetSetpoint(2.5+lstick.GetY()*2.5)
wpilib.Wait(0.04)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
pidController.Enable()
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
# Motor control
pidController.SetSetpoint(2.5+lstick.GetY()*2.5)
# print("in robot operator_control loop, pidOutput.motor.Get()=%d",pidOutput.motor.Get())
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 OperatorControl(self):
self.dog.SetEnabled(True)
for type, component in self.components.items():
component.op_init()
while self.IsOperatorControl() and self.IsEnabled():
self.dog.Feed()
for type, component in self.components.items():
component.op_tick(wpilib.Timer.GetFPGATimestamp())
## Debug & Tuning
# ??
wpilib.Wait(0.01)
self.dog.SetEnabled(False)
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 OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
pidController.Enable()
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
# Motor control
#pidController.SetSetpoint(2.5+lstick.GetY()*2.5) #joystick output scaled to 0-5volts
pidController.SetSetpoint(0) #aim for a bearing signal of zero
print("bearing = %s" % listener.get_bearing())
# print("in robot operator_control loop, pidOutput.motor.Get()=%d",pidOutput.motor.Get())
wpilib.Wait(0.04)
def Autonomous(self):
self.dog.SetEnabled(True)
for type, component in self.components.items():
component.auto_init()
START = 'start'
SHOOTING = 'shooting'
DRIVE_FORWARD = 'drive_forward'
STOP = 'stop'
current_state = START
start_time = wpilib.Timer.GetFPGATimestamp()
while wpilib.IsAutonomous() and wpilib.IsEnabled():
self.dog.Feed()
wpilib.SmartDashboard.PutString('auto robot state', current_state)
current_time = wpilib.Timer.GetFPGATimestamp()
elapsed_time = current_time - start_time
if current_state == START:
if self.goal_is_hot() or elapsed_time > 5:
current_state = SHOOTING
elif current_state == SHOOTING:
self.components['shooter'].auto_shoot_tick(current_time)
if self.components['shooter'].is_auto_shoot_done():
current_state = DRIVE_FORWARD
elif current_state == DRIVE_FORWARD:
self.components['drive'].auto_drive_forward_tick(current_time)
if self.components['drive'].is_auto_drive_done():
current_state = STOP
# for type, component in self.components.items():
# component.auto_tick(wpilib.Timer.GetFPGATimestamp())
wpilib.Wait(0.01)
self.dog.SetEnabled(False)
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)