def __init__(self, robot):
Subsystem.__init__(self, 'Turret')
self.robot = robot
self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X)
motors = {}
self.map = self.robot.botMap
self.tmotors = motors
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.tmotors:
self.tmotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast)
self.kP = 0.05
self.kI = 0.000
self.kD = 0.002
self.turretPID = PID(self.kP, self.kI, self.kD)
self.turretPID.limitVal(0.3)
self.setPow = 0
def __init__(self):
Behaviour.__init__(self, "yaw")
# Set default parameters
self._params['gaussian_variance'] = GAUSSIAN_VARIANCE
self._params['reverseAction'] = False
self.pid = PID(gains=self.pid_coeffs["yaw"])
def __init__(self, robot):
Subsystem.__init__(self, 'Flywheel')
self.CurPos = 0
self.PasPos = 0
self.robot = robot
self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X)
self.map = self.robot.botMap
motor = {}
piston = {}
for name in self.robot.botMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(
self.robot.botMap.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
piston[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.fmotor = motor
self.fpiston = piston
for name in self.fmotor:
self.fmotor[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.fmotor[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Fly']), 40)
self.kP = 0.008
self.kI = 0.00002
self.kD = 0.00018
self.kF = 0.0 # tune me when testing
self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF)
self.flywheelPID.MaxIn(680)
self.flywheelPID.MaxOut(1)
self.flywheelPID.limitVal(0.95) # Limit PID output power to 50%
self.feetToRPS = 51.111
class Turret(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Turret')
self.robot = robot
self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X)
motors = {}
self.map = self.robot.botMap
self.tmotors = motors
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.tmotors:
self.tmotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast)
self.kP = 0.05
self.kI = 0.000
self.kD = 0.002
self.turretPID = PID(self.kP, self.kI, self.kD)
self.turretPID.limitVal(0.3)
self.setPow = 0
def set(self, pw):
self.tmotors['turret'].set(ctre.ControlMode.PercentOutput, pw)
def setPower(self, pow):
self.setPow = pow
def getEnc(self):
return self.tEncoder.get()
def periodic(self):
if self.robot.Limelight.isExisting():
self.set(0) # self.turretPID.outVal(self.robot.Limelight.getX()))
else:
self.set(self.setPow)
class TranslationalBehaviour(Behaviour):
_params = {}
LIMITED_SPEED = np.array([1.0, 0.6, 0.6, 0.0, 2.0,
2.0]) # max speed (m/s and rad/s)
def __init__(self):
Behaviour.__init__(self, "yaw")
# Set default parameters
self._params['gaussian_variance'] = GAUSSIAN_VARIANCE
self._params['reverseAction'] = False
self.pid = PID(gains=self.pid_coeffs["yaw"])
def navCallback(self, msg):
"""
:param msg: navigation msg
method overrides parent class navCallack method
"""
self._nav = (msg.position.north, msg.position.east)
self.nav_yaw = msg.orientation.yaw
def doWork(self):
# rospy.loginfo("Computing behaviours output ...")
# print(self.goal_position)
if self.ip_position is not None and self._nav is not None:
distance_to_goal = np.sqrt(
sum((np.array(self.ip_position[0:2]) -
np.array(self._nav))**2))
# print("Distance to target: {0}".format(self.distance_to_wp))
# print("Angle to target: {0}".format(self.raw_angle_to_wp))
# print("Goal position: {0}".format(self.goal_position))
if self.distance_to_ip < 1.0:
if self.ip_position[-1] >= 0.0:
pos_err = (self.ip_position[-1] -
abs(self.nav_yaw)) * np.sign(self.nav_yaw)
else:
pos_err = (self.ip_position[-1] - self.nav_yaw
) # * np.sign(self.nav_yaw)
else:
pos_err = self.raw_angle_to_ip
pid_output = self.pid.GenOut(pos_err)
# Limit the velocities by the maximum possible for the vehicle if required.
scaled_velocity = np.array([0, 0, 0, 0, 0, pid_output
]) * self.LIMITED_SPEED
# Send the result msg to the coordinator
self.publishResult(Vector6(values=scaled_velocity))
def __init__(self, robot):
Subsystem.__init__(self, 'Drive')
self.robot = robot
motors = {}
pistons = {}
self.map = self.robot.botMap
self.rEnc = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.lEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X)
self.Gyro = ADXRS450_Gyro()
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
if name == 'Shifter':
pistons[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.dMotors = motors
self.dPistons = pistons
for name in self.dMotors:
self.dMotors[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.dMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.dMotors[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Drive']), 40)
self.kP = 0.0
self.kI = 0.0
self.kD = 0.0
self.DrivePID = PID(self.kP, self.kI, self.kD)
class Flywheel(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Flywheel')
self.CurPos = 0
self.PasPos = 0
self.robot = robot
self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X)
self.map = self.robot.botMap
motor = {}
piston = {}
for name in self.robot.botMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(
self.robot.botMap.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
piston[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.fmotor = motor
self.fpiston = piston
for name in self.fmotor:
self.fmotor[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.fmotor[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Fly']), 40)
self.kP = 0.008
self.kI = 0.00002
self.kD = 0.00018
self.kF = 0.0 # tune me when testing
self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF)
self.flywheelPID.MaxIn(680)
self.flywheelPID.MaxOut(1)
self.flywheelPID.limitVal(0.95) # Limit PID output power to 50%
self.feetToRPS = 51.111
def log(self):
wpilib.SmartDashboard.putNumber('Flywheel Enc', self.Fenc.get())
wpilib.SmartDashboard.putNumber('Flywheel Vel',
self.getVelocity(self.Fenc.get()))
def init(self):
self.Fenc.reset()
def set(self, pow):
self.fmotor['RFly'].set(ctre.ControlMode.PercentOutput, pow)
self.fmotor['LFly'].set(ctre.ControlMode.PercentOutput, pow)
def setVelocityPID(self, rps):
self.flywheelPID.setPoint(rps)
if rps == 0:
pow = 0
else:
pow = self.flywheelPID.outVel(self.getVelocity(self.get()))
return pow
def get(self):
return self.Fenc.get()
def getVelocity(self, input):
self.CurPos = -(input / 1025) * (2 * math.pi)
vel = (self.CurPos - self.PasPos) / 0.02
self.PasPos = self.CurPos
return vel