def _init_components(self):
config = configparser.ConfigParser()
config.read(os.path.join(os.getcwd(), self._subsystem_config))
MOTOR_SECTION = "HookMotor"
ENCODER_SECTION = "HookEncoder"
ENABLED = "ENABLED"
CHANNEL = "CHANNEL"
INVERTED = "INVERTED"
if (config.getboolean(MOTOR_SECTION, ENABLED)):
motor_channel = config.getint(MOTOR_SECTION, CHANNEL)
motor_inverted = config.getboolean(MOTOR_SECTION, INVERTED)
if (motor_channel):
self._motor = VictorSP(motor_channel)
if (self._motor):
self._motor.setInverted(motor_inverted)
if (config.getboolean(ENCODER_SECTION, ENABLED)):
encoder_a_channel = config.getint(ENCODER_SECTION, "A_CHANNEL")
encoder_b_channel = config.getint(ENCODER_SECTION, "B_CHANNEL")
encoder_inverted = config.getboolean(ENCODER_SECTION, INVERTED)
encoder_type = config.getint(ENCODER_SECTION, "TYPE")
if (encoder_a_channel and encoder_b_channel and encoder_type):
self._encoder = Encoder(encoder_a_channel, encoder_b_channel, encoder_inverted, encoder_type)
class Hook(Subsystem):
_robot = None
_subsystem_config = None
_motor = None
_encoder = None
_encoder_value = 0
def __init__(self, robot, name=None, configfile = '/home/lvuser/configs/subsystems.ini'):
self._robot = robot;
self._subsystem_config = configfile
self._init_components()
self._update_smartdashboard(0.0)
super().__init__(name = name)
def initDefaultCommand(self):
# Make it not a magic number
self.setDefaultCommand(MoveHookAnalog(self._robot, 50))
def move_hook(self, speed):
if (self._motor):
self._motor.setSpeed(speed)
self.get_encoder_value()
self._update_smartdashboard(speed)
def get_encoder_value(self):
if (self._encoder):
self._encoder_value = self._encoder.get()
return self._encoder_value
def reset_encoder_value(self):
if (self._encoder):
self._encoder.reset()
self._encoder_value = self._encoder.get()
self._update_smartdashboard(0.0)
return self._encoder_value
def _update_smartdashboard(self, speed):
SmartDashboard.putNumber("Hook Encoder", self._encoder_value)
SmartDashboard.putNumber("Hook Speed", speed)
def _init_components(self):
config = configparser.ConfigParser()
config.read(os.path.join(os.getcwd(), self._subsystem_config))
MOTOR_SECTION = "HookMotor"
ENCODER_SECTION = "HookEncoder"
ENABLED = "ENABLED"
CHANNEL = "CHANNEL"
INVERTED = "INVERTED"
if (config.getboolean(MOTOR_SECTION, ENABLED)):
motor_channel = config.getint(MOTOR_SECTION, CHANNEL)
motor_inverted = config.getboolean(MOTOR_SECTION, INVERTED)
if (motor_channel):
self._motor = VictorSP(motor_channel)
if (self._motor):
self._motor.setInverted(motor_inverted)
if (config.getboolean(ENCODER_SECTION, ENABLED)):
encoder_a_channel = config.getint(ENCODER_SECTION, "A_CHANNEL")
encoder_b_channel = config.getint(ENCODER_SECTION, "B_CHANNEL")
encoder_inverted = config.getboolean(ENCODER_SECTION, INVERTED)
encoder_type = config.getint(ENCODER_SECTION, "TYPE")
if (encoder_a_channel and encoder_b_channel and encoder_type):
self._encoder = Encoder(encoder_a_channel, encoder_b_channel, encoder_inverted, encoder_type)
def _init_components(self):
self._max_speed = self._config.getfloat(self._general_section,
Drivetrain._max_speed_key)
self._modifier_scaling = self._config.getfloat(
self._general_section, Drivetrain._modifier_scaling_key)
self._dpad_scaling = self._config.getfloat(
self._general_section, Drivetrain._dpad_scaling_key)
if self._config.getboolean(Drivetrain._left_encoder_section,
Drivetrain._enabled_key):
self._left_encoder_a_channel = self._config.getint(
self._left_encoder_section, Drivetrain._a_channel_key)
self._left_encoder_b_channel = self._config.getint(
self._left_encoder_section, Drivetrain._b_channel_key)
self._left_encoder_reversed = self._config.getboolean(
self._left_encoder_section, Drivetrain._reversed_key)
self._left_encoder_type = self._config.getint(
self._left_encoder_section, Drivetrain._type_key)
if self._left_encoder_a_channel and self._left_encoder_b_channel and self._left_encoder_type:
self._left_encoder = Encoder(self._left_encoder_a_channel,
self._left_encoder_b_channel,
self._left_encoder_reversed,
self._left_encoder_type)
if self._config.getboolean(Drivetrain._right_encoder_section,
Drivetrain._enabled_key):
self._right_encoder_a_channel = self._config.getint(
self._right_encoder_section, Drivetrain._a_channel_key)
self._right_encoder_b_channel = self._config.getint(
self._right_encoder_section, Drivetrain._b_channel_key)
self._right_encoder_reversed = self._config.getboolean(
self._right_encoder_section, Drivetrain._reversed_key)
self._right_encoder_type = self._config.getint(
self._right_encoder_section, Drivetrain._type_key)
if self._right_encoder_a_channel and self._right_encoder_b_channel and self._right_encoder_type:
self._right_encoder = Encoder(self._right_encoder_a_channel,
self._right_encoder_b_channel,
self._right_encoder_reversed,
self._right_encoder_type)
if self._config.getboolean(Drivetrain._gyro_section,
Drivetrain._enabled_key):
gyro_channel = self._config.getint(self._gyro_section,
Drivetrain._channel_key)
self._gyro = ADXRS450_Gyro(gyro_channel)
if self._config.getboolean(Drivetrain._left_motor_section,
Drivetrain._enabled_key):
self._left_motor = VictorSP(
self._config.getint(self._left_motor_section,
Drivetrain._channel_key))
if self._config.getboolean(Drivetrain._right_motor_section,
Drivetrain._enabled_key):
self._right_motor = VictorSP(
self._config.getint(self._right_motor_section,
Drivetrain._channel_key))
if self._left_motor and self._right_motor:
self._robot_drive = RobotDrive(self._left_motor, self._right_motor)
self._robot_drive.setSafetyEnabled(False)
self._robot_drive.setInvertedMotor(
RobotDrive.MotorType.kRearLeft,
self._config.getboolean(Drivetrain._left_motor_section,
Drivetrain._inverted_key))
self._robot_drive.setInvertedMotor(
RobotDrive.MotorType.kRearRight,
self._config.getboolean(Drivetrain._right_motor_section,
Drivetrain._inverted_key))
