def robotInit(self):
#!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.BELT_BROKEN = False
#!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.Lift = Lifter.Lifter()
self.Vision = Vision.Vision()
self.usinggamepad = True
self.FL = wpilib.CANTalon(2)
self.FR = wpilib.CANTalon(1)
self.BL = wpilib.CANTalon(0)
self.BR = wpilib.CANTalon(3)
self.FL.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.FR.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.BL.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.BR.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.FL.setPID(1.0, 0.0, 3.0, 0.0)
self.FR.setPID(1.0, 0.0, 3.0, 0.0)
self.BL.setPID(1.0, 0.0, 3.0, 0.0)
self.BR.setPID(1.0, 0.0, 3.0, 0.0)
self.movegamepad = JoystickLib.Gamepad.Gamepad(port = 0)
self.shootgamepad = JoystickLib.Gamepad.Gamepad(port = 1)
self.MoveJoy = JoystickLib.joystickLib.createJoystick(0)
self.MoveJoy.invertY()
self.TurnJoy = JoystickLib.joystickLib.createJoystick(1)
self.TurnJoy.invertY()
self.Drive = HolonomicDrive(self.FL, self.FR, self.BL, self.BR)
self.Drive.invertDrive(True)
# TODO: move magic number to constant
self.Drive.setWheelOffset(math.radians(27)) #angle of wheels
self.Drive.setDriveMode(HolonomicDrive.DriveMode.JEFF)
self.LeftFly = wpilib.CANTalon(4)
self.RightFly = wpilib.CANTalon(5)
self.LimitSwitch = wpilib.DigitalInput(0)
self.LimitSwitch2 = wpilib.DigitalInput(1)
self.Intake = wpilib.CANTalon(8)
self.Shooter = ShootController.ShootController(\
self.LeftFly, self.RightFly,\
self.Intake, self.LimitSwitch, self.LimitSwitch2)
#self.Shooter.invertFlywheels()
self.armcan = wpilib.CANTalon(7)
self.armcan.changeControlMode(wpilib.CANTalon.ControlMode.Voltage)
self.armcan.set(0)
class MainRobot (wpilib.IterativeRobot):
def robotInit(self):
#!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.BELT_BROKEN = False
#!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.Lift = Lifter.Lifter()
self.Vision = Vision.Vision()
self.usinggamepad = True
self.FL = wpilib.CANTalon(2)
self.FR = wpilib.CANTalon(1)
self.BL = wpilib.CANTalon(0)
self.BR = wpilib.CANTalon(3)
self.FL.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.FR.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.BL.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.BR.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.FL.setPID(1.0, 0.0, 3.0, 0.0)
self.FR.setPID(1.0, 0.0, 3.0, 0.0)
self.BL.setPID(1.0, 0.0, 3.0, 0.0)
self.BR.setPID(1.0, 0.0, 3.0, 0.0)
self.movegamepad = JoystickLib.Gamepad.Gamepad(port = 0)
self.shootgamepad = JoystickLib.Gamepad.Gamepad(port = 1)
self.MoveJoy = JoystickLib.joystickLib.createJoystick(0)
self.MoveJoy.invertY()
self.TurnJoy = JoystickLib.joystickLib.createJoystick(1)
self.TurnJoy.invertY()
self.Drive = HolonomicDrive(self.FL, self.FR, self.BL, self.BR)
self.Drive.invertDrive(True)
# TODO: move magic number to constant
self.Drive.setWheelOffset(math.radians(27)) #angle of wheels
self.Drive.setDriveMode(HolonomicDrive.DriveMode.JEFF)
self.LeftFly = wpilib.CANTalon(4)
self.RightFly = wpilib.CANTalon(5)
self.LimitSwitch = wpilib.DigitalInput(0)
self.LimitSwitch2 = wpilib.DigitalInput(1)
self.Intake = wpilib.CANTalon(8)
self.Shooter = ShootController.ShootController(\
self.LeftFly, self.RightFly,\
self.Intake, self.LimitSwitch, self.LimitSwitch2)
#self.Shooter.invertFlywheels()
self.armcan = wpilib.CANTalon(7)
self.armcan.changeControlMode(wpilib.CANTalon.ControlMode.Voltage)
self.armcan.set(0)
def autonomousInit(self):
self.shoot = False
self.rev = 0
self.time = 0
self.shoot = False
self.turn = 0
self.Drive.zeroEncoderTargets()
self.needed = 0
self.start = True
self.FR.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.FL.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.BL.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.BR.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.turn = 0
self.need = 0
self.armcan.changeControlMode(wpilib.CANTalon.ControlMode.Voltage)
self.armcan.set(0)
def autonomousPeriodic(self):
if not self.BELT_BROKEN:
self.time += 1
if self.time < 250:
self.FL.set(8.5 * 819.2 / 8)
self.BR.set(-9 * 819.2 / 8)
self.FR.set(-9 * 819.2 / 8)
self.BL.set(8.5 * 819.2 / 8)
elif (self.time > 300 and self.time < 310):
self.FL.set(0)
self.BR.set(0)
self.FR.set(0)
self.BL.set(0)
if self.time > 310:
if not self.Vision.centerX().__len__() == 0:
self.need = abs(self.Vision.centerX()[0] - 240)
print (self.need)
if self.need > 50:
self.turn = self.need * .0005
else:
self.turn = self.need * .0002
if abs(self.Vision.centerX()[0] - 240) < 10:
self.shoot = True
elif self.Vision.centerX()[0] - 240 > 0:
self.Drive.drive(0, 0, -self.turn)
self.shoot = False
elif self.Vision.centerX()[0] - 240 < 0:
self.Drive.drive(0, 0, self.turn)
self.shoot = False
else:
self.shoot = False
if self.time > 260 and self.time < 320:
pass #used to have arm
if self.time > 320:
pass
else:
self.shoot = False
# if self.shoot == True:
# self.rev += 1
# if self.rev < 100:
# self.Shooter.update(False, True, False, False)
# else:
# self.Shooter.update(False, True, True, False)
else:
#self.Drive.driveSpeedJeffMode(.5,math.pi/2,0)
pass
# def autonomousPeriodic(self):
#
# self.time += 1
# if self.time < 350:
# self.start = True
# if self.start == True:
# self.Drive.drive(1, math.pi/2, 0)
# if self.time > 350:
# self.start = False
# print ("time is at 350")
# # self.Drive.drive(0, 0, 0)
# if not self.Vision.centerX().__len__() == 0:
# print("passed")
# self.needed = abs(self.Vision.centerX()[0] - 235)
# print (self.needed)
# if self.needed > 50:
# self.turn = self.needed * .009
# else:
# self.turn = self.needed * .0008
#
# if abs(self.Vision.centerX()[0] - 235) < 10:
# self.shoot = True
# print ("Alligned")
# elif self.Vision.centerX()[0] - 235 > 0:
# self.shoot = False
# elif self.Vision.centerX()[0] - 235 < 0:
# self.shoot = False
# else:
# self.shoot = False
# else:
# self.shoot = False
# if self.shoot == True:
# if self.time < 600:
# print ("Rev wheels")
# self.Shooter.update(False, True, False, False)
# elif self.time < 750:
# print ("shooting")
# self.Shooter.update(False, True, True, False)
# else:
# self.Shooter.update(False, False, False, False)
# self.Drive.drive(0, 0, self.turn)
def teleopInit(self):
self.Drive.zeroEncoderTargets()
self.readyToShoot = False
self.Arm = NewArm(self.shootgamepad, self.armcan)
def teleopPeriodic(self):
averageFlySpeed = (abs(self.LeftFly.getEncVelocity()) + abs(self.RightFly.getEncVelocity()))/2
# print(str(self.Vision.centerX()))
print ("FlyWheelSpeed is : " + str(averageFlySpeed))
if self.usinggamepad == False: # using joystick
self.TurnJoy.updateButtons();
self.MoveJoy.updateButtons();
turn = -self.TurnJoy.getX()
magnitude = self.MoveJoy.getMagnitude()
direction = self.MoveJoy.getDirection()
self.Drive.drive(magnitude, direction, turn) # jeff mode
self.Shooter.update(self.MoveJoy.getRawButton(2),\
self.MoveJoy.getRawButton(3),\
self.MoveJoy.getTrigger(),\
self.MoveJoy.getRawButton(5))
else: # using gamepad
if not self.Vision.centerX().__len__() == 0:
self.needed = abs(self.Vision.centerX()[0] - 240)
print (self.needed)
if self.needed > 50:
self.turnSpeed = self.needed * .009
else:
self.turnSpeed = self.needed * .0008
if self.shootgamepad.getButtonByLetter("RB"):
# TODO: move 240 to center pixel constant
if abs(self.Vision.centerX()[0] - 240) < 15:
self.readyToShoot = True
elif self.Vision.centerX()[0] - 240 > 0:
self.Drive.drive(0, 0, -self.turnSpeed)
self.readyToShoot = False
elif self.Vision.centerX()[0] - 240 < 0:
self.Drive.drive(0, 0, self.turnSpeed)
self.readyToShoot = False
else:
self.readyToShoot = False
else:
self.readyToShoot = False
# if self.readyToShoot:
# if averageFlySpeed < 1900:
# self.LeftFly.set(2000)
# self.RightFly.set(-2000)
# elif averageFlySpeed > 1900:
# self.LeftFly.set(2000)
# self.RightFly.set(-2000)
# self.Intake.set(1)
# else:
# self.Shooter.update(False, False, False, False)
if self.movegamepad.getButtonByLetter("LJ"): # faster button
self.slowed = 1
elif self.movegamepad.getButtonByLetter("LB"): # slower button
self.slowed = .2
else: # no button pressed
# TODO: move to constant
self.slowed = .55
# print ("Slowed: " + str(self.slowed))
# switch drive mode with gamepad
if self.movegamepad.getRawButton(Gamepad.A):
self.Drive.setDriveMode(HolonomicDrive.DriveMode.VOLTAGE)
elif self.movegamepad.getRawButton(Gamepad.B):
self.Drive.setDriveMode(HolonomicDrive.DriveMode.SPEED)
elif self.movegamepad.getRawButton(Gamepad.X):
self.Drive.setDriveMode(HolonomicDrive.DriveMode.JEFF)
# print(str(self.Drive.getDriveMode()))
# TODO: refactor duplicate code
if self.movegamepad.getButtonByLetter("RB"):
turn = -self.movegamepad.getRX() * abs(self.movegamepad.getRX()) * (self.slowed / 2)
#magnitude = self.movegamepad.getLMagnitude() * self.slowed
magnitude = self.movegamepad.getLMagnitudePower(2) * self.slowed
direction = self.movegamepad.getLDirection() + math.pi
else:
turn = -self.movegamepad.getRX() * abs(self.movegamepad.getRX()) * (self.slowed / 2)
#magnitude = self.movegamepad.getLMagnitude() * self.slowed
magnitude = self.movegamepad.getLMagnitudePower(2) * self.slowed
direction = self.movegamepad.getLDirection()
self.Drive.drive(magnitude, direction, turn)
self.Shooter.update(self.shootgamepad.getButtonByLetter("B"),\
self.shootgamepad.getButtonByLetter("X"),\
self.shootgamepad.getButtonByLetter("A"),\
self.shootgamepad.getButtonByLetter("Y"))
# if self.shootgamepad.getButtonByLetter("RB"):
# self.Lift.liftUp()
# elif self.shootgamepad.getButtonByLetter("LB"):
# self.Lift.pullUp()
# else:
# self.Lift.stop()
self.Arm.update()
