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))