def run(self):
while True:
if self.enabled:
err = self.error()
power = self.target_speed / self.max_speed
power += self.kP * err
self.output(power)
robot_time.sleep(millis=10)
def _update_estimator(pose_estimator: PoseEstimator, sleep_sec=(10/1000)):
ct = 0
while True:
pose_estimator.update(dt=sleep_sec)
ct += sleep_sec
if ct > 1:
print(get_current_pose())
ct = 0
robot_time.sleep(seconds=sleep_sec)
def disabled(self):
# Init
while self.isDisabled():
# Loop
self.current_state = MyRobot.State.DISABLED
self.periodic()
robot_time.sleep(millis=self.delay_millis)
for cmd in self.current_commands:
cmd.cancel()
def _update_model(self):
while True:
print("Model!")
now = robot_time.millis()
dt = (now - self._model_last_time) / 1000
self._model_last_time = now
if self._mode == SmartDrivetrain.Mode.PercentVbus:
factor = 1
elif self._mode == SmartDrivetrain.Mode.Voltage:
factor = 1/12
else:
print("Can't update model outside of PercentVBus")
continue
self._model_left_dist += self._left_motor.get() * self.max_speed * dt * factor
self._model_right_dist += self._right_motor.get() * self.max_speed * dt * factor
robot_time.sleep(millis=20)
def operatorControl(self):
# Init
if self.current_state != self.State.TELEOP:
self.cmd_queue.append(OpDriveCommand(self))
self.cmd_queue.append(OpFuelTankCommand(self))
self.cmd_queue.append(OpShooterCommand(self))
self.cmd_queue.append(OpClimberCommand(self))
self.cmd_queue.append(OpGearCommand(self))
while self.isOperatorControl():
# Loop
self.current_state = MyRobot.State.TELEOP
self.periodic()
robot_time.sleep(millis=self.delay_millis)
for cmd in self.current_commands:
cmd.cancel()
def robotInit(self):
if wpilib.hal.isSimulation():
basedir = ""
else:
basedir = "/home/lvuser/py"
dashboard2.run(basedir)
wpilib.CameraServer.launch()
dashboard2.chooser("Autonomous", [
"None", "Drive MotionMagic", "Align Shoot", "Pure Pursuit 1",
"Pure Pursuit 2", "Pure Pursuit 3", "Gear Center", "Gear Left",
"Gear Right", "Hopper/Boiler Blue", "Hopper/Boiler Red",
"Boiler/Mobility Blue", "Boiler/Mobility Red"
],
default="None")
self.talon_left_front = ctre.CANTalon(0)
self.talon_left_rear = ctre.CANTalon(1)
self.talon_right_rear = ctre.CANTalon(2)
self.talon_right_front = ctre.CANTalon(3)
self.talon_shooter = ctre.CANTalon(4)
self.victor_intake = PWMMotor(wpilib.VictorSP, pwm_port=1, pdp_port=5)
self.victor_blender = PWMMotor(wpilib.VictorSP, pwm_port=0, pdp_port=4)
self.talon_climber = PWMMotor(wpilib.Talon, pwm_port=2, pdp_port=14)
self.talon_shooter.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.talon_shooter.reverseSensor(True)
self.talon_left_rear.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.talon_left_rear.set(0)
self.talon_right_rear.setControlMode(
ctre.CANTalon.ControlMode.Follower)
self.talon_right_rear.set(3)
self.talon_left = self.talon_left_front
self.talon_right = self.talon_right_front
self.talon_left.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.talon_right.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.talon_left.reverseSensor(True)
f_l = 0.1808
p_l = 1.28
i_l = 0.005
d_l = 12.8
f_r = 0.1724
p_r = 1.28
i_r = 0.005
d_r = 12.8
self.talon_left.setP(p_l)
self.talon_left.setI(i_l)
self.talon_left.setD(d_l)
self.talon_left.setF(f_l)
self.talon_right.setP(p_r)
self.talon_right.setI(i_r)
self.talon_right.setD(d_r)
self.talon_right.setF(f_r)
volt_ramp_time = 1 / 5
volt_ramp = 12 / volt_ramp_time
self.talon_left.setVoltageRampRate(volt_ramp)
self.talon_right.setVoltageRampRate(volt_ramp)
dashboard2.number_input("Turn Kp", 0.005)
dashboard2.number_input("Lookahead", 1)
dashboard2.number_input("Cruise speed", 0.3)
dashboard2.graph("Blender Current", self.victor_blender.get_current)
dashboard2.graph("Climber Current", self.talon_climber.get_current)
dashboard2.graph("Drive Current", self.talon_left.getOutputCurrent)
sensor_type = ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative
sensor_present = ctre.CANTalon.FeedbackDeviceStatus.Present
dashboard2.indicator(
"Left Encoder", lambda: self.talon_left.isSensorPresent(
sensor_type) == sensor_present)
dashboard2.indicator(
"Right Encoder", lambda: self.talon_right.isSensorPresent(
sensor_type) == sensor_present)
self._drive = SmartDrivetrain(self.talon_left, self.talon_right)
self._fueltank = FuelTank(self, self.victor_intake,
self.victor_blender)
self._shooter = Shooter(self, self.talon_shooter)
self._climber = Climber(self, self.talon_climber)
self._gear_lifter = GearLifter(self)
dashboard2.number_input("Drive Vel", 10)
dashboard2.number_input("Drive Acc", 10)
dashboard2.number_input("Drive Dist", 10)
self.systems = {
"drive": self._drive,
"climber": self._climber,
"intake": self._fueltank,
"shooter": self._shooter,
"gear_lifter": self._gear_lifter
}
self.js_left = wpilib.Joystick(0)
self.js_right = wpilib.Joystick(1)
self.gamepad = wpilib.XboxController(2)
if not wpilib.hal.isSimulation():
t_wait = 30
print("Waiting for vision... ({}s max)".format(t_wait))
_vision_table = NetworkTables.getTable("vision")
t_begin = time.time()
# Wait until either vision is ready or 30s has passed
while not _vision_table.getBoolean(
"ready", False) and not (time.time() - t_begin) > t_wait:
robot_time.sleep(seconds=1)
print("Robot ready!")
def autonomous(self):
if self.current_state != self.State.AUTO:
mode = dashboard2.get_chooser("Autonomous")
self.talon_left.setPosition(0)
self.talon_right.setPosition(0)
vel = float(dashboard2.number_inputs["Drive Vel"])
acc = float(dashboard2.number_inputs["Drive Acc"])
dist = float(dashboard2.number_inputs["Drive Dist"])
drive_vel = 0.02
drive_acc = 0.01
def gear_side(is_right=True):
cmds = []
cmds.append(AutoGearCommand(self, AutoGearCommand.State.up))
cmds.append(
MotionProfileDriveCommand(self,
(108 - 15.5 - 10 + 5 +
(0 if is_right else 5)) / 12,
drive_vel, drive_acc))
cmds.append(
TurnToAngleCommand(self, (1 if is_right else -1) * 12))
cmds.append(
MotionProfileDriveCommand(self, (44 - 4 - 5) / 12,
drive_vel, drive_acc))
cmds.append(AutoGearCommand(self, AutoGearCommand.State.down))
cmds.append(
MotionProfileDriveCommand(self, -30 / 12, drive_vel,
drive_acc))
return cmds
def align_shoot(is_right=True):
cmds = []
cmds.append(
TurnToAngleCommand(self, (1 if is_right else -1) * 90))
cmds.append(TurnToBoilerCommand(self))
cmds.append(AutoShooterCommand(self, 10, 3750))
return cmds
if mode == "Drive MotionMagic":
cmds = []
cmds.append(MotionProfileDriveCommand(self, dist, vel, acc))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Align Shoot":
cmds = []
cmds += align_shoot(True)
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Gear Center":
cmds = []
cmds.append(AutoGearCommand(self, AutoGearCommand.State.up))
cmds.append(
MotionProfileDriveCommand(self, (110 - 15.5 - 4 - 10) / 12,
drive_vel, drive_acc))
cmds.append(AutoGearCommand(self, AutoGearCommand.State.down))
cmds.append(
MotionProfileDriveCommand(self, -40 / 12, drive_vel,
drive_acc))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Gear Left":
cmds = gear_side(False)
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Gear Right":
cmds = gear_side(True)
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Gear Left + Shoot":
cmds = gear_side(False)
cmds += align_shoot(False)
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Gear Right + Shoot":
cmds = gear_side(True)
cmds += align_shoot(True)
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Hopper/Boiler Blue":
cmds = []
cmds.append(DistanceDriveCommand(self, 93.5, 0.6))
cmds.append(TurnToAngleCommand(self, 90))
cmds.append(TimeDriveCommand(self, 2, 0.6))
cmds.append(AutoShooterCommand(self, 10, 3700))
cmds.append(AutoFuelTankCommand(self, 10))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Hopper/Boiler Red":
cmds = []
cmds.append(DistanceDriveCommand(self, 93.5, 0.6))
cmds.append(TurnToAngleCommand(self, 90))
cmds.append(TimeDriveCommand(self, 2, -0.6))
cmds.append(AutoShooterCommand(self, 10, 3700))
cmds.append(AutoFuelTankCommand(self, 10))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Boiler/Mobility Red":
cmds = []
cmds.append(AutoShooterCommand(self, 10, 3650))
#cmds.append(AutoFuelTankCommand(self, 10))
cmds.append(DistanceDriveCommand(self, 100, -0.6))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Boiler/Mobility Blue":
cmds = []
cmds.append(AutoShooterCommand(self, 10, 3650))
#cmds.append(AutoFuelTankCommand(self, 10))
cmds.append(DistanceDriveCommand(self, 100, 0.6))
self.cmd_queue.append(CommandSequence(self, cmds))
lookahead = float(dashboard2.number_inputs["Lookahead"])
cruise = float(dashboard2.number_inputs["Cruise speed"])
if mode == "Pure Pursuit 1":
cmds = []
cmds.append(
PursuitDriveCommand(
self, [Vector2(0, 0), Vector2(7, 0)],
cruise_vel=0.4,
acc=0.1,
lookahead=lookahead))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Pure Pursuit 2":
cmds = []
cmds.append(
PursuitDriveCommand(
self, [Vector2(0, 0),
Vector2(6, 0),
Vector2(9, -4)],
cruise_vel=0.3,
acc=0.15,
lookahead=lookahead))
self.cmd_queue.append(CommandSequence(self, cmds))
if mode == "Pure Pursuit 3":
cmds = []
cmds.append(
PursuitDriveCommand(
self, [Vector2(0, 0),
Vector2(6, -4),
Vector2(9, -4)],
cruise_vel=0.3,
acc=0.15,
lookahead=lookahead))
self.cmd_queue.append(CommandSequence(self, cmds))
# Init
while self.isAutonomous():
# Loop
self.current_state = MyRobot.State.AUTO
self.periodic()
robot_time.sleep(millis=self.delay_millis)
for cmd in self.current_commands:
cmd.cancel()