def test_profiler1():
"""
forward velocity, trapezoid, no overshoot
"""
DT = 0.02
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=50,
tolerance=.5,
current_target_v=0)
t = 0
pos = 0
if log_trajectory:
logger = DataLogger("test_profiler1.csv")
logger.log_while_disabled = True
logger.do_print = True
logger.add('t', lambda: t)
logger.add('pos', lambda: pos)
logger.add('v', lambda: profiler.current_target_v)
logger.add('a', lambda: profiler.current_a)
while not profiler.isFinished(pos):
if log_trajectory:
logger.log()
profiler.calculate_new_velocity(pos, DT)
pos += profiler.current_target_v * DT
t += DT
if t > 10:
if log_trajectory:
logger.close()
assert False, "sim loop timed out"
if t < 0.501:
assert profiler.current_a == pytest.approx(20,
0.01), "t = %f" % (t, )
if 0.501 < t < 5:
assert profiler.current_target_v == pytest.approx(
10., 0.01), "t = %f" % (t, )
assert profiler.current_a == 0, "t = %f" % (t, )
if 5 < t < 5.5 - 0.001:
assert profiler.current_a == pytest.approx(-20.,
0.01), "t = %f" % (t, )
if t == pytest.approx(5.50, 0.001):
assert profiler.current_a == pytest.approx(0.,
0.01), "t = %f" % (t, )
assert profiler.current_target_v == pytest.approx(
0., 0.01), "t = %f" % (t, )
if log_trajectory:
logger.log()
logger.close()
assert t == pytest.approx(5.52, 0.01)
assert profiler.current_a == 0
def test_profiler4():
"""
reverse velocity, triangle, no overshoot
"""
DT = 0.02
profiler = TrapezoidalProfile(cruise_v=10,
a=20,
target_pos=-4,
tolerance=.5,
current_target_v=0)
t = 0
pos = 0
if log_trajectory:
logger = DataLogger("test_profiler4.csv")
logger.log_while_disabled = True
logger.add('t', lambda: t)
logger.add('pos', lambda: pos)
logger.add('v', lambda: profiler.current_target_v)
logger.add('a', lambda: profiler.current_a)
while not profiler.isFinished(pos):
if log_trajectory:
logger.log()
profiler.calculate_new_velocity(pos, DT)
pos += profiler.current_target_v * DT
t += DT
if t > 10:
if log_trajectory:
logger.close()
assert False, "sim loop timed out"
if t < 0.4599:
assert profiler.current_a == pytest.approx(-20,
0.01), "t = %f" % (t, )
if t == pytest.approx(0.46, 0.01):
assert profiler.current_target_v == pytest.approx(-9.2, 0.01)
assert profiler.current_a == pytest.approx(-20,
0.01), "t = %f" % (t, )
if 0.4601 < t < 0.92:
assert profiler.current_a == pytest.approx(20,
0.01), "t = %f" % (t, )
if log_trajectory:
logger.log()
logger.close()
assert t == pytest.approx(0.94, 0.01)
assert profiler.current_a == 0
class ElevatorTest2(Command):
def __init__(self):
super().__init__("laksdjfkl")
self.elevator = self.getRobot().elevator
self.requires(self.elevator)
self.timer = wpilib.Timer()
self.done = False
self.state = 1
self.voltage = 0.75
def initialize(self):
self.logger = DataLogger('elevator-majig.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add(
"enc_pos1",
lambda: self.elevator.motor.getSelectedSensorVelocity(0))
self.logger.add("current_motor1",
lambda: self.elevator.motor.getOutputCurrent())
self.logger.add("current_follow1",
lambda: self.elevator.other_motor.getOutputCurrent())
self.logger.add("voltage_motor1",
lambda: self.elevator.motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow1",
lambda: self.elevator.other_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor1",
lambda: self.elevator.motor.getBusVoltage())
self.logger.add("bvoltage_follow1",
lambda: self.elevator.other_motor.getBusVoltage())
self.logger.add(
"enc_pos2", lambda: self.elevator.other_right_motor.
getSelectedSensorVelocity(0))
self.logger.add("current_motor2",
lambda: self.elevator.right_motor.getOutputCurrent())
self.logger.add(
"current_follow2",
lambda: self.elevator.other_right_motor.getOutputCurrent())
self.logger.add(
"voltage_motor2",
lambda: self.elevator.right_motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow2",
lambda: self.elevator.other_right_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor2",
lambda: self.elevator.right_motor.getBusVoltage())
self.logger.add(
"bvoltage_follow2",
lambda: self.elevator.other_right_motor.getBusVoltage())
self.timer.start()
def execute(self):
e = self.elevator.getEncoderPosition()
if self.state == 1:
self.elevator.ascend(self.voltage)
if e > 28000:
self.state = 2
self.elevator.descend(self.voltage)
if self.state == 2:
self.elevator.descend(self.voltage)
if e < 1000 and self.state == 2:
self.state = 1
self.voltage += 0.25
elif self.voltage > 1:
self.voltage = 0
self.done = True
self.logger.log()
def isFinished(self):
return self.done
def end(self):
self.elevator.hover()
self.logger.close()
class Drivetrain(Subsystem):
''''''
#: encoder/ft ratio
ratio = 886.27
def __init__(self):
super().__init__('Drivetrain')
#The set motor controllers for this years robot and how motors are coded
self.motor_rb = ctre.WPI_TalonSRX(1)
self.motor_rf = ctre.WPI_VictorSPX(17)
self.motor_lb = ctre.WPI_TalonSRX(13)
self.motor_lf = ctre.WPI_VictorSPX(12)
self.motor_rf.follow(self.motor_rb)
self.motor_lf.follow(self.motor_lb)
self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf]
self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb)
self.navx = navx.AHRS.create_spi()
self.pdp = wpilib.PowerDistributionPanel(16)
self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.navx_table = networktables.NetworkTables.getTable('/Sensor/Navx')
self.leftEncoder_table = networktables.NetworkTables.getTable("/Encoder/Left")
self.rightEncoder_table = networktables.NetworkTables.getTable("/Encoder/Right")
self.leftError = networktables.NetworkTables.getTable("/TalonL/Error")
self.rightError = networktables.NetworkTables.getTable("/TalonR/Error")
self.motor_lb.setSensorPhase(True)
self.motor_rb.setSensorPhase(True)
self.timer = wpilib.Timer()
self.timer.start()
self.mode = ""
self.computed_velocity = 0
self.logger = None
def dumb_turn(self):
self.drive.arcadeDrive(0, 0.4, False)
def execute_turn(self, angle):
position = angle / 60.
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.drive.feed()
def initDefaultCommand(self):
self.setDefaultCommand(Drive())
def init_logger(self):
self.logger = DataLogger('drivetrain.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("heading", lambda: self.navx.getAngle())
self.logger.add("enc_pos_l", lambda: self.motor_lb.getSelectedSensorPosition(0))
self.logger.add("enc_pos_r", lambda: self.motor_rb.getSelectedSensorPosition(0))
self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0))
self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0))
self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0))
self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0))
self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0))
self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0))
self.logger.add("computed_velocity", lambda: self.computed_velocity)
self.logger.add("mode", lambda: self.mode)
self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage())
self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent())
self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent())
self.logger.add("current_rf", lambda: self.pdp.getCurrent(0))
self.logger.add("current_rb", lambda: self.pdp.getCurrent(1))
self.logger.add("current_lf", lambda: self.pdp.getCurrent(15))
self.logger.add("current_lb", lambda: self.pdp.getCurrent(13))
def zeroEncoders(self):
self.motor_rb.setSelectedSensorPosition(0, 0, 0)
self.motor_lb.setSelectedSensorPosition(0, 0, 0)
def zeroNavx(self):
self.navx.reset()
def initialize_driveTurnlike(self):
#The PID values with the motors
self.zeroEncoders()
self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0)
self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0)
self.motor_rb.configNominalOutputForward(.1, 0)
self.motor_lb.configNominalOutputForward(.1, 0)
self.motor_rb.configNominalOutputReverse(-0.1, 0)
self.motor_lb.configNominalOutputReverse(-0.1, 0)
self.motor_rb.configPeakOutputForward(0.4, 0)
self.motor_lb.configPeakOutputForward(0.4, 0)
self.motor_rb.configPeakOutputReverse(-0.4, 0)
self.motor_lb.configPeakOutputReverse(-0.4, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# self.motor_rb.config_kF(0, 0, 0)
# self.motor_lb.config_kF(0, 0, 0)
self.motor_rb.config_kP(0, 0.18, 0)
self.motor_lb.config_kP(0, 0.18, 0)
self.motor_rb.config_kI(0, 0, 0)
self.motor_lb.config_kI(0, 0, 0)
self.motor_rb.config_kD(0, 8, 0)
self.motor_lb.config_kD(0, 8, 0)
def uninitialize_driveTurnlike(self):
#The PID values with the motors
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
def initilize_driveForward(self):
self.mode = "Forward"
#The PID values with the motors for drive forward
self.zeroEncoders()
self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0)
self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0)
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
self.motor_lb.setIntegralAccumulator(0, 0, 0)
self.motor_rb.setIntegralAccumulator(0, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.motor_rb.config_kF(0, 0, 0)
self.motor_lb.config_kF(0, 0, 0)
self.motor_rb.config_kP(0, 0.18, 0)
self.motor_lb.config_kP(0, 0.18, 0)
self.motor_rb.config_kI(0, 0.001, 0)
self.motor_lb.config_kI(0, 0.001, 0)
self.motor_rb.config_kD(0, 2, 0)
self.motor_lb.config_kD(0, 2, 0)
def initialize_velocity_closedloop(self):
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# tested for counterclockwise turn
self.motor_rb.config_kF(0, 1.88, 0)
self.motor_rb.config_kP(0, 4.18, 0)
self.motor_rb.config_kI(0, 0.01, 0)
self.motor_rb.config_kD(0, 450, 0)
self.motor_lb.config_kF(0, 0.88, 0)
self.motor_lb.config_kP(0, 3.18, 0)
self.motor_lb.config_kI(0, 0.01, 0)
self.motor_lb.config_kD(0, 450, 0)
def getAngle(self):
return self.navx.getAngle()
def set_turn_velocity(self, v_degps):
velocity_ratio = 1.6
self.computed_velocity = v_encp100ms = velocity_ratio * v_degps
#self.computed_velocity = v_encp100ms = 32
self.motor_rb.set(ctre.ControlMode.Velocity, v_encp100ms)
self.motor_lb.set(ctre.ControlMode.Velocity, v_encp100ms)
def execute_driveforward(self, positionL, positionR):
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionR)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionL)
self.drive.feed()
def isFinished_driveforward(self, target):
sensorPL = self.motor_lb.getSelectedSensorPosition(0)
a1 = (target-0.2)*self.ratio
a2 = (target+0.2)*self.ratio
if a1 < sensorPL < a2:
return True
def end_driveforward(self):
self.motor_rb.set(0)
self.motor_lb.set(0)
self.mode = ""
off = end_driveforward
def getEncoderVelocity(self, fps):
return fps*self.ratio/10
def getEncoderAccel(self, fps2):
return fps2*self.ratio/10
def periodic(self):
#Variables for the Navx
angle = self.navx.getAngle()
self.navx_table.putNumber('Angle', angle)
#Variables used for the dashboard
sensorPL = self.motor_lb.getSelectedSensorPosition(0)
self.leftEncoder_table.putNumber("Position", sensorPL)
sensorPR = self.motor_rb.getSelectedSensorPosition(0)
self.rightEncoder_table.putNumber("Position", sensorPR)
sensorVL = self.motor_lb.getSelectedSensorVelocity(0)
self.leftEncoder_table.putNumber("Velocity", sensorVL)
sensorVR = self.motor_rb.getSelectedSensorVelocity(0)
self.rightEncoder_table.putNumber("Velocity", sensorVR)
voltageL = self.motor_lb.getMotorOutputPercent()
voltageR = self.motor_rb.getMotorOutputPercent()
self.leftError.putNumber("Voltage", voltageL)
self.rightError.putNumber("Voltage", voltageR)
if self.logger is not None:
self.logger.log()
class Intake(Subsystem):
def __init__(self):
super().__init__('Intake')
self.intake_motor_closeOpen = wpilib.VictorSP(8)
self.intake_motor_rightWheel = wpilib.VictorSP(9)
self.intake_motor_leftWheel = wpilib.VictorSP(7)
self.limit_switch = wpilib.DigitalOutput(0)
self.pdp = wpilib.PowerDistributionPanel(16)
self.intake_table = networktables.NetworkTables.getTable('/Intake')
self.timer = wpilib.Timer()
self.timer.start()
self.logger = None
#self.init_logger()
def initDefaultCommand(self):
self.setDefaultCommand(grabber.Grabber())
def closeGrabber(self, x):
self.motor_closeOpen_set(x)
def init_logger(self):
self.logger = DataLogger('intake.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("voltagep_r",
lambda: self.intake_motor_rightWheel.get())
self.logger.add("voltagep_m",
lambda: self.intake_motor_closeOpen.get())
self.logger.add("voltagep_l",
lambda: self.intake_motor_leftWheel.get())
self.logger.add("voltage", lambda: self.pdp.getVoltage())
self.logger.add("current_r", lambda: self.pdp.getCurrent(0))
self.logger.add("current_m", lambda: self.pdp.getCurrent(1))
self.logger.add("current_l", lambda: self.pdp.getCurrent(15))
def openGrabber(self):
'''
if self.limit_switch == True:
self.grabberOff()
else:'''
self.motor_closeOpen_set(-0.5)
def open2Grabber(self):
self.intake_motor_closeOpen.set(-0.4)
self.cubeOut()
def grabberOff(self):
self.motor_closeOpen_set(0)
def cubeOut(self):
self.intake_motor_rightWheel.set(0.5)
self.intake_motor_leftWheel.set(-0.5)
def cubeIn(self):
self.intake_motor_rightWheel.set(-1)
self.intake_motor_leftWheel.set(1)
def intakeWheelsOff(self):
self.intake_motor_rightWheel.set(0)
self.intake_motor_leftWheel.set(0)
def motor_closeOpen_set(self, voltage_percent):
self.intake_motor_closeOpen.set(voltage_percent)
def motor_rightWheel_set(self, voltage_percent):
self.intake_motor_rightWheel.set(voltage_percent)
def motor_leftWheel_set(self, voltage_percent):
self.intake_motor_leftWheel.set(voltage_percent)
def periodic(self):
self.intake_table.putBoolean("LimitSwitch", self.limit_switch.get())
if self.logger is not None:
self.logger.log()
class CsvTrajectoryCommand(_CsvTrajectoryCommand):
def __init__(self, fnom):
super().__init__(fnom)
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.ctrl_heading.setContinuous(True)
self.max_velocity_fps = 11
self.max_velocity_encps = self.drivetrain.fps_to_encp100ms(self.max_velocity_fps)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("csv_trajectory1.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("i", lambda: self.i)
self.timer.start()
self.i = 0
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder() / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder() / self.drivetrain.ratio
(_, _, _, self.target_v_l, self.target_v_r, self.target_a_l,
self.target_a_r, self.target_heading) = self.get_trajectory_point_enc(self.i)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
self.ctrl_heading.setSetpoint(self.target_heading)
self.drivetrain.feed()
self.logger.log()
self.i += 1
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
pass
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("profiled_forward.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos)
self.logger.add("adist_l", lambda: self.profiler_l.adist)
self.logger.add("err_l", lambda: self.profiler_l.err)
self.logger.add("choice_l", lambda: self.profiler_l.choice)
self.logger.add("adist_r", lambda: self.profiler_l.adist)
self.logger.add("err_r", lambda: self.profiler_l.err)
self.logger.add("choice_r", lambda: self.profiler_l.choice)
self.timer.start()
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder(
) / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder(
) / self.drivetrain.ratio
#print ('pdf exec ', self.timer.get())
self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period)
self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period)
self.target_v_l = self.drivetrain.fps_to_encp100ms(
self.profiler_l.current_target_v)
self.target_v_r = self.drivetrain.fps_to_encp100ms(
self.profiler_r.current_target_v)
self.target_a_l = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_l.current_a)
self.target_a_r = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_r.current_a)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
#self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward())
#self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward())
self.logger.log()
self.drivetrain.feed()
def isFinished(self):
return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3
and self.profiler_l.current_target_v == 0
and self.profiler_l.current_a == 0
and self.profiler_r.current_target_v == 0
and self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
self.profiler_l.setCruiseVelocityScale(1 + correction)
self.profiler_r.setCruiseVelocityScale(1 - correction)
class Elevator(Subsystem):
#: encoder ticks per revolution (need ticks per ft)
ratio = 4096
max_pos = 3
min_pos = 0
def __init__(self):
super().__init__('Elevator')
self.sensor = wpilib.DigitalInput(9) # temp num, true is on
self.motor = ctre.WPI_TalonSRX(3)
self.other_motor = ctre.WPI_TalonSRX(2)
self.other_motor.follow(self.motor)
self.zeroed = False
self.motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.elevator_table = networktables.NetworkTables.getTable('/Elevator')
self.motor.setSensorPhase(True)
self.initialize_motionMagic()
self.timer = wpilib.Timer()
self.timer.start()
self.logger = None
self.init_logger()
def init_logger(self):
self.logger = DataLogger('elevator.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("enc_pos",
lambda: self.motor.getSelectedSensorPosition(0))
self.logger.add("voltagep_motor",
lambda: self.motor.getMotorOutputPercent())
self.logger.add("voltagep_othermotor",
lambda: self.other_motor.getMotorOutputPercent())
self.logger.add("voltage", lambda: self.motor.getBusVoltage())
self.logger.add("current_motor", lambda: self.motor.getOutputCurrent())
self.logger.add("current_othermotor",
lambda: self.other_motor.getOutputCurrent())
self.logger.add("zeroed", lambda: 1 if self.zeroed else 0)
def initialize_motionMagic(self):
self.motor.configMotionAcceleration(int(self.ftToEncoder_accel(1)), 0)
self.motor.configMotionCruiseVelocity(int(self.ftToEncoder_vel(1)), 0)
self.motor.configNominalOutputForward(0, 0)
self.motor.configNominalOutputReverse(0, 0)
self.motor.configPeakOutputForward(0.8, 0)
self.motor.configPeakOutputReverse(-0.8, 0)
self.motor.selectProfileSlot(0, 0)
self.motor.config_kF(0, 0, 0)
self.motor.config_kP(0, 0.018, 0)
self.motor.config_kI(0, 0, 0)
self.motor.config_kD(0, 0, 0)
def set_position(self, position):
if (position > self.max_pos):
position = self.max_pos
if (position < self.min_pos):
position = self.min_pos
self.motor.set(ctre.ControlMode.MotionMagic, position * self.ratio)
def initDefaultCommand(self):
self.setDefaultCommand(ElevatorTest())
def ftToEncoder_accel(self, ftPerSec_sq):
return ftPerSec_sq * self.ratio / 10
def ftToEncoder_vel(self, ftPerSec):
return ftPerSec * self.ratio / 10
def hover(self):
self.motor.set(0.1)
pass
def descend(self, voltage):
pass
def ascend(self, voltage):
self.motor.set(voltage)
def test_drive_x(self, x):
self.motor.set(x)
def test_drive_positive(self):
self.motor.set(0.5)
def test_drive_negative(self):
self.motor.set(-0.1)
def off(self):
self.motor.stopMotor()
def zeroEncoder(self):
self.zeroed = True
self.motor.setSelectedSensorPosition(0, 0, 0)
def getSensor(self):
return self.sensor.get()
def periodic(self):
position = self.motor.getSelectedSensorPosition(0)
self.elevator_table.putNumber("Position", position)
self.elevator_table.putNumber("Motor Current",
self.motor.getOutputCurrent())
self.elevator_table.putNumber("Other Motor Current",
self.other_motor.getOutputCurrent())
if self.logger is not None:
self.logger.log()
def test_ProfiledForward2(Notifier, sim_hooks):
global log_trajectory
robot = Rockslide()
robot.robotInit()
DT = robot.getPeriod()
robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock()
robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock()
getLeftEncoder.return_value = 0
getRightEncoder.return_value = 0
command = ProfiledForward(10)
command.initialize()
t = 0
pos_ft = 0
if log_trajectory:
logger = DataLogger("test_profiled_forward2.csv")
logger.log_while_disabled = True
logger.do_print = True
logger.add('t', lambda: t)
logger.add('pos', lambda: pos_ft)
logger.add('target_pos', lambda: command.dist_ft)
logger.add('v', lambda: command.profiler_l.current_target_v)
logger.add('max_v', lambda: command.max_v_encps)
logger.add('a', lambda: command.profiler_l.current_a)
logger.add('max_a', lambda: command.max_acceleration)
logger.add('voltage', lambda: command.drivetrain.getVoltage())
logger.add('vpl', lambda: command.drivetrain.motor_lb.get())
logger.add('adist', lambda: command.profiler_l.adist)
logger.add('err', lambda: command.profiler_l.err)
while t < 10:
if log_trajectory:
logger.log()
getLeftEncoder.return_value = pos_ft * command.drivetrain.ratio
getRightEncoder.return_value = -pos_ft * command.drivetrain.ratio
command.execute()
v = command.profiler_l.current_target_v
pos_ft += v * DT
t += DT
sim_hooks.time = t
if command.isFinished():
break
command.end()
if log_trajectory:
logger.log()
logger.close()
class Drivetrain(Subsystem):
''''''
#: encoder/ft ratio
ratio = 630
def __init__(self):
super().__init__('Drivetrain')
#The set motor controllers for this years robot and how motors are coded
self.motor_rb = ctre.WPI_TalonSRX(1)
self.motor_rf = ctre.WPI_VictorSPX(17)
self.motor_lb = ctre.WPI_TalonSRX(13)
self.motor_lf = ctre.WPI_VictorSPX(12)
self.motor_rf.follow(self.motor_rb)
self.motor_lf.follow(self.motor_lb)
self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf]
self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb)
self.navx = navx.AHRS.create_spi()
self.pdp = wpilib.PowerDistributionPanel(16)
self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.table = networktables.NetworkTables.getTable("/Drivetrain")
self.sd_table = networktables.NetworkTables.getTable("/SmartDashboard")
self.motor_lb.setSensorPhase(False)
self.motor_rb.setSensorPhase(False)
self.left_offset = 0
self.right_offset = 0
self.timer = wpilib.Timer()
self.timer.start()
self.computed_velocity = 0
self.logger = None
#self.init_logger()
def drive_forward(self, motorF):
#self.drive.arcadeDrive(-motorF, 0, squaredInputs= False)
self.motor_lb.set(motorF)
self.motor_rb.set(-motorF * 1.0)
self.drive.feed()
def custom_move(self, rvolt, lvolt):
self.motor_lb.set(lvolt)
self.morot_rb.set(-rvolt)
self.drive.feed()
def turn_right(self, voltage):
self.motor_lb.set(voltage)
self.motor_rb.set(voltage)
self.drive.feed()
def turn_left(self, voltage):
self.motor_lb.set(-voltage)
self.motor_rb.set(-voltage)
self.drive.feed()
def wheels_stopped(self):
return abs(self.motor_lb.getSelectedSensorVelocity(0)) <= 30 and abs(
self.motor_rb.getSelectedSensorVelocity(0)) <= 30
def voltage_ramp_on(self):
self.motor_lb.configOpenLoopRamp(0.2, 0)
self.motor_rb.configOpenLoopRamp(0.2, 0)
def voltage_ramp_off(self):
self.motor_lb.configOpenLoopRamp(0, 0)
self.motor_rb.configOpenLoopRamp(0, 0)
def execute_turn(self, angle):
position = angle / 60.
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.drive.feed()
def initDefaultCommand(self):
self.setDefaultCommand(Drive())
def init_logger(self):
self.logger = DataLogger('drivetrain.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("heading", lambda: self.navx.getAngle())
self.logger.add("enc_pos_l", lambda: self.getLeftEncoder())
self.logger.add("enc_pos_r", lambda: self.getRightEncoder())
self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0))
self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0))
#self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0))
#self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0))
#self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0))
#self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0))
self.logger.add("computed_velocity", lambda: self.computed_velocity)
self.logger.add("mode", lambda: self.running_command_name())
self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage())
#self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent())
#self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent())
self.logger.add("current_rf", lambda: self.pdp.getCurrent(0))
self.logger.add("current_rb", lambda: self.pdp.getCurrent(1))
self.logger.add("current_lf", lambda: self.pdp.getCurrent(15))
self.logger.add("current_lb", lambda: self.pdp.getCurrent(13))
self.logger.add("enc_offset_l", lambda: self.left_offset)
self.logger.add("enc_offset_r", lambda: self.right_offset)
def running_command_name(self):
command = self.getCurrentCommand()
if command is not None:
return command.getName()
def zeroEncoders(self):
self.right_offset = self.motor_rb.getSelectedSensorPosition(0)
self.left_offset = self.motor_lb.getSelectedSensorPosition(0)
def getLeftEncoder(self):
return self.motor_lb.getSelectedSensorPosition(0) - self.left_offset
def getRightEncoder(self):
return self.motor_rb.getSelectedSensorPosition(0) - self.right_offset
def zeroNavx(self):
self.navx.reset()
def initialize_driveTurnlike(self):
#The PID values with the motors
self.zeroEncoders()
self.motor_rb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(1.25)), 0)
self.motor_lb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(1.25)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(2.5)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(2.5)), 0)
self.motor_rb.configNominalOutputForward(.1, 0)
self.motor_lb.configNominalOutputForward(.1, 0)
self.motor_rb.configNominalOutputReverse(-0.1, 0)
self.motor_lb.configNominalOutputReverse(-0.1, 0)
self.motor_rb.configPeakOutputForward(0.4, 0)
self.motor_lb.configPeakOutputForward(0.4, 0)
self.motor_rb.configPeakOutputReverse(-0.4, 0)
self.motor_lb.configPeakOutputReverse(-0.4, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# self.motor_rb.config_kF(0, 0, 0)
# self.motor_lb.config_kF(0, 0, 0)
self.motor_rb.config_kP(0, 0.18, 0)
self.motor_lb.config_kP(0, 0.18, 0)
self.motor_rb.config_kI(0, 0, 0)
self.motor_lb.config_kI(0, 0, 0)
self.motor_rb.config_kD(0, 8, 0)
self.motor_lb.config_kD(0, 8, 0)
def uninitialize_driveTurnlike(self):
#The PID values with the motors
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
def initialize_driveForward(self):
self.zeroEncoders()
self.config_parameters(p=0.18, i=0.001, d=2, f=0)
self.config_motionmagic(v_fps=3.5, a_fps2=5.0)
def initialize_velocity_closedloop(self):
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# tested for counterclockwise turn
self.motor_rb.config_kF(0, 1.88, 0)
self.motor_rb.config_kP(0, 2.18, 0)
self.motor_rb.config_kI(0, 0.00, 0)
self.motor_rb.config_kD(0, 450, 0)
self.motor_lb.config_kF(0, 1.88, 0)
self.motor_lb.config_kP(0, 2.18, 0)
self.motor_lb.config_kI(0, 0.00, 0)
self.motor_lb.config_kD(0, 450, 0)
def config_parameters(self, p, i, f, d, nominal_forward = 0, nominal_reverse = 0, peak_forward = 1, peak_reverse = -1):
self.motor_rb.configNominalOutputForward(nominal_forward, 0)
self.motor_lb.configNominalOutputForward(nominal_forward, 0)
self.motor_rb.configNominalOutputReverse(nominal_reverse, 0)
self.motor_lb.configNominalOutputReverse(nominal_reverse, 0)
self.motor_rb.configPeakOutputForward(peak_forward, 0)
self.motor_lb.configPeakOutputForward(peak_forward, 0)
self.motor_rb.configPeakOutputReverse(peak_reverse, 0)
self.motor_lb.configPeakOutputReverse(peak_reverse, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# tested for counterclockwise turn
self.motor_rb.config_kF(0, f, 0)
self.motor_rb.config_kP(0, p, 0)
self.motor_rb.config_kI(0, i, 0)
self.motor_rb.config_kD(0, d, 0)
self.motor_lb.config_kF(0, f, 0)
self.motor_lb.config_kP(0, p, 0)
self.motor_lb.config_kI(0, i, 0)
self.motor_lb.config_kD(0, d, 0)
def config_motionmagic(self, v_fps, a_fps2):
self.motor_rb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(v_fps)), 0)
self.motor_lb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(v_fps)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(a_fps2)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(a_fps2)), 0)
def getAngle(self):
return self.navx.getAngle()
def set_turn_velocity(self, v_degps):
print(v_degps)
velocity_ratio = 1.6
self.computed_velocity = v_encp100ms = velocity_ratio * v_degps
#self.computed_velocity = v_encp100ms = 32
self.motor_rb.set(ctre.ControlMode.Velocity, v_encp100ms)
self.motor_lb.set(ctre.ControlMode.Velocity, v_encp100ms)
self.drive.feed()
def execute_driveforward(self, positionL, positionR):
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionR + self.right_offset)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionL + self.left_offset)
self.drive.feed()
def isFinished_driveforward(self, target):
sensorPL = self.motor_lb.getSelectedSensorPosition(0)
a1 = (target-0.2)*self.ratio
a2 = (target+0.2)*self.ratio
if a1 < sensorPL < a2:
return True
def end_driveforward(self):
self.motor_rb.set(0)
self.motor_lb.set(0)
off = end_driveforward
def fps_to_encp100ms(self, fps):
return fps*self.ratio/10
def fps2_to_encpsp100ms(self, fps2):
return fps2*self.ratio/10
def periodic(self):
#Variables for the Navx
autoMode = self.sd_table.getString("autonomousMode", None)
self.sd_table.putString("robotAutoMode", autoMode)
switch_attempt = self.sd_table.getBoolean("switchAttempt", None)
self.sd_table.putBoolean("robotSwitchAttempt", switch_attempt)
scale_attempt = self.sd_table.getBoolean("scaleAttempt", None)
self.sd_table.putBoolean("robotScaleAttempt", scale_attempt)
angle = self.navx.getAngle()
self.table.putNumber('Angle', angle)
#Variables used for the dashboard
sensorPL = self.getLeftEncoder()
self.table.putNumber("Left/Position", sensorPL)
sensorPR = self.getRightEncoder()
self.table.putNumber("Right/Position", sensorPR)
sensorVL = self.motor_lb.getSelectedSensorVelocity(0)
self.table.putNumber("Left/Velocity", sensorVL)
sensorVR = self.motor_rb.getSelectedSensorVelocity(0)
self.table.putNumber("Right/Velocity", sensorVR)
#voltageL = self.motor_lb.getMotorOutputPercent()
#voltageR = self.motor_rb.getMotorOutputPercent()
#self.table.putNumber("Left/Voltage", voltageL)
#self.table.putNumber("Right/Voltage", voltageR)
if self.logger is not None:
self.logger.log()
def test_StateSpaceDriveCommand(Notifier):
global log_trajectory
left_drive = DriveSide(
Ks=1.293985,
Kv=0.014172 * 63. / 0.3048,
Ka=0.005938 * 63. / 0.3048)
right_drive = DriveSide(
Ks=1.320812,
Kv=0.013736 * 63. / 0.3048,
Ka=0.005938 * 63. / 0.3048)
robot = Rockslide()
robot.robotInit()
robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock()
robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock()
robot.drivetrain.getVoltage = MagicMock(return_value=10)
command = StateSpaceDriveCommand("straight3m.tra")
command.initialize()
dt = robot.getPeriod()
t = 0
if log_trajectory:
logger = DataLogger("test_StateSpaceDriveCommand.csv")
logger.log_while_disabled = True
logger.do_print = False
logger.add('t', lambda: t)
logger.add('pos_l_m', lambda: left_drive.pos_m)
logger.add('pos_r_m', lambda: right_drive.pos_m)
logger.add('m_pos_l_m', lambda: command.y[0,0])
logger.add('m_pos_r_m', lambda: command.y[1,0])
logger.add('vel_l_mps', lambda: left_drive.v_mps)
logger.add('vel_r_mps', lambda: right_drive.v_mps)
logger.add('target_pos_l_m', lambda: command.r[0,0])
logger.add('target_pos_r_m', lambda: command.r[2,0])
logger.add('target_vel_l_mps', lambda: command.r[1,0])
logger.add('target_vel_r_mps', lambda: command.r[3,0])
logger.add('voltage', lambda: command.drivetrain.getVoltage())
logger.add('vpl', lambda: command.drivetrain.motor_lb.get())
logger.add('vpr', lambda: command.drivetrain.motor_rb.get())
while not command.isFinished():
logger.log()
getLeftEncoder.return_value = left_drive.pos_m * 630 / 0.3048
getRightEncoder.return_value = right_drive.pos_m * 630 / 0.3048
command.execute()
V = command.drivetrain.getVoltage()
vpl = command.drivetrain.motor_lb.get()
vpr = command.drivetrain.motor_rb.get()
left_drive.update(V * vpl, dt)
right_drive.update(V * vpr, dt)
t += dt
assert t < 10
command.end()
