def update_sim(self, hal_data, now, tm_diff):
lr_motor = hal_data["CAN"][
f'sparkmax-{robotmap.DRIVE_MOTORS["rl"][0]}']["value"]
lf_motor = hal_data["CAN"][
f'sparkmax-{robotmap.DRIVE_MOTORS["fl"][0]}']["value"]
rr_motor = hal_data["CAN"][
f'sparkmax-{robotmap.DRIVE_MOTORS["rr"][0]}']["value"]
rf_motor = hal_data["CAN"][
f'sparkmax-{robotmap.DRIVE_MOTORS["fr"][0]}']["value"]
elevator_motor = hal_data["CAN"][robotmap.ELEVATOR["motors"][0]]
elevator_last_pos = elevator_motor["quad_position"]
elevator_last_vel = elevator_motor["quad_velocity"]
elevator_curr_set = elevator_motor["value"]
tick_diff = self.elevator_encoder_ticks_per_sec * tm_diff
# Account for natural drift as well
if elevator_last_pos > 200:
hal_data["CAN"][
robotmap.ELEVATOR["motors"][0]]["quad_position"] = (
int(elevator_last_pos + tick_diff * elevator_curr_set) - 5)
else:
hal_data["CAN"][robotmap.ELEVATOR["motors"]
[0]]["quad_position"] = int(elevator_last_pos +
tick_diff *
elevator_curr_set)
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor,
rr_motor,
lf_motor,
rf_motor,
speed=5)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lr_motor = -hal_data["pwm"][1]["value"]
rr_motor = hal_data["pwm"][3]["value"]
lf_motor = -hal_data["pwm"][0]["value"]
rf_motor = hal_data["pwm"][2]["value"]
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, -vy, vw, tm_diff)
x, y, angle = self.physics_controller.get_position()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = [data[0][0], data[0][2], 12 * data[0][3]]
def update_sim(self, hal_data, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lf_motor = -hal_data['pwm'][backLeftPort]['value']
rf_motor = hal_data['pwm'][backRightPort]['value']
lr_motor = -hal_data['pwm'][frontLeftPort]['value']
rr_motor = hal_data['pwm'][frontRightPort]['value']
self.count += hal_data['pwm'][liftMotPort]['value'] / 2
print(int(self.count))
#hal_data['encoder'][0]['has_source'] = True
#print(hal_data['encoder'][0]['initialized'])
#hal_data['encoder'][0]['count'] = int(self.count)
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, data, time, elapsed):
valueName = 'value'
# read CAN data to get motor speeds
maxValue = 1024
fl = (data['CAN'][self.canFL][valueName] * self.canFLInv / maxValue)
fr = (data['CAN'][self.canFR][valueName] * self.canFRInv / maxValue)
if not self.drivetrain == "two":
bl = (data['CAN'][self.canBL][valueName] * self.canBLInv /
maxValue)
br = (data['CAN'][self.canBR][valueName] * self.canBRInv /
maxValue)
if self.drivetrain == "mecanum":
vx, vy, vw = drivetrains.mecanum_drivetrain(
bl, br, fl, fr, self.xLen, self.yLen, self.speed)
self.physicsController.vector_drive(vx, vy, vw, elapsed)
elif self.drivetrain == "four":
speed, rot = drivetrains.four_motor_drivetrain(
bl, br, fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
elif self.drivetrain == "two":
speed, rot = drivetrains.two_motor_drivetrain(
fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
def update_sim(self, data, time, elapsed):
fl = self.flMotor.getSpeed(data, self.maxVel)
fr = self.frMotor.getSpeed(data, self.maxVel)
bl = self.blMotor.getSpeed(data, self.maxVel)
br = self.brMotor.getSpeed(data, self.maxVel)
if self.drivetrain == "mecanum":
vx, vy, vw = drivetrains.mecanum_drivetrain(
bl, br, fl, fr, self.xLen, self.yLen, self.speed)
self.physicsController.vector_drive(vx, vy, vw, elapsed)
elif self.drivetrain == "four":
speed, rot = drivetrains.four_motor_drivetrain(
bl, br, fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
elif self.drivetrain == "two":
speed, rot = drivetrains.two_motor_drivetrain(
fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
# https://github.com/robotpy/robotpy-wpilib/issues/291
data['analog_gyro'][0]['angle'] = math.degrees(
self.physicsController.angle)
x, y, angle = self.physicsController.get_position()
visionData = self.visionSim.compute(time, x, y, angle)
if visionData is not None:
targetData = visionData[0]
self.visionTable.putNumber('tv', targetData[0])
if targetData[0] != 0:
self.visionTable.putNumber('tx', targetData[2])
else:
# DOESN'T mean no vision. vision just doesn't always update
pass
def update_sim(self, hal_data, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lf_motor = -hal_data['pwm'][backLeftPort]['value']
rf_motor = hal_data['pwm'][backRightPort]['value']
lr_motor = -hal_data['pwm'][frontLeftPort]['value']
rr_motor = hal_data['pwm'][frontRightPort]['value']
self.count += hal_data['pwm'][liftMotPort]['value'] / 2
print(int(self.count))
#hal_data['encoder'][0]['has_source'] = True
#print(hal_data['encoder'][0]['initialized'])
#hal_data['encoder'][0]['count'] = int(self.count)
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, data, time, elapsed):
valueName = 'value'
# read CAN data to get motor speeds
maxValue = 1024
try:
fl = (data['CAN'][self.canFL][valueName] * self.canFLInv /
maxValue)
fr = (data['CAN'][self.canFR][valueName] * self.canFRInv /
maxValue)
if not self.drivetrain == "two":
bl = (data['CAN'][self.canBL][valueName] * self.canBLInv /
maxValue)
br = (data['CAN'][self.canBR][valueName] * self.canBRInv /
maxValue)
except KeyError:
return
if self.drivetrain == "mecanum":
vx, vy, vw = drivetrains.mecanum_drivetrain(
bl, br, fl, fr, self.xLen, self.yLen, self.speed)
self.physicsController.vector_drive(vx, vy, vw, elapsed)
elif self.drivetrain == "four":
speed, rot = drivetrains.four_motor_drivetrain(
bl, br, fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
elif self.drivetrain == "two":
speed, rot = drivetrains.two_motor_drivetrain(
fl, fr, self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
# https://github.com/robotpy/robotpy-wpilib/issues/291
data['analog_gyro'][0]['angle'] = math.degrees(
self.physicsController.angle)
def test_mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, output):
result = drivetrains.mecanum_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor, speed=1, x_wheelbase=1, y_wheelbase=1
)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
assert abs(result[2] - output[2]) < 0.001
def update_sim(self, hal_data, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lr_motor = hal_data['pwm'][6]['value']
rr_motor = -hal_data['pwm'][5]['value']
lf_motor = hal_data['pwm'][7]['value']
rf_motor = -hal_data['pwm'][3]['value']
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
# Update encoders
self.distance += vy * tm_diff
hal_data['encoder'][1]['count'] = int(self.distance * self.kEncoder)
def update_sim(self, now, tm_diff):
motor = wpilib.CAN._devices[5]
motor.forward_ok = True
# when the dog is let out, then position will never go down
dog_out = (wpilib.Solenoid._channels[1].value == True)
# let the winch out!
if dog_out:
if self.winch_position > self.winch_min:
self.winch_position += self.winch_range * tm_diff * -3
else:
# calculate winch based on motor value
if self.winch_position <= self.winch_max:
self.winch_position += motor.value * self.winch_range * tm_diff * .7
else:
motor.forward_ok = False
# potentiometer value is position
wpilib.AnalogModule._channels[3].voltage = self.winch_position
# calculate the voltage/current
if motor.value == 0 or motor.forward_ok == False:
self.motor_tm = None
motor.voltage = 0
motor.current = 0
self.n += 1
else:
# if motor is running, voltage is constant (probably not realistic)
motor.voltage = motor.value * 12.5
if self.motor_tm is None:
self.motor_tm = 0
else:
self.motor_tm += tm_diff
# some equation that makes a pretty graph
motor.current = motor.value * math.sin(self.n + 8*self.motor_tm) + 3*self.motor_tm
# Simulate the drivetrain
lf_motor = wpilib.DigitalModule._pwm[0].Get() * -1
lr_motor = wpilib.DigitalModule._pwm[1].Get() * -1
rr_motor = wpilib.DigitalModule._pwm[2].Get()
rf_motor = wpilib.DigitalModule._pwm[3].Get()
# Our robot's wheels are wrong, so switch y/x, and invert everything
vy, vx, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.vector_drive(-vx, vy, -vw, tm_diff)
def update_sim(self, now, tm_diff):
motor = wpilib.CAN._devices[5]
motor.forward_ok = True
# when the dog is let out, then position will never go down
dog_out = (wpilib.Solenoid._channels[1].value == True)
# let the winch out!
if dog_out:
if self.winch_position > self.winch_min:
self.winch_position += self.winch_range * tm_diff * -3
else:
# calculate winch based on motor value
if self.winch_position <= self.winch_max:
self.winch_position += motor.value * self.winch_range * tm_diff * .7
else:
motor.forward_ok = False
# potentiometer value is position
wpilib.AnalogModule._channels[3].voltage = self.winch_position
# calculate the voltage/current
if motor.value == 0 or motor.forward_ok == False:
self.motor_tm = None
motor.voltage = 0
motor.current = 0
self.n += 1
else:
# if motor is running, voltage is constant (probably not realistic)
motor.voltage = motor.value * 12.5
if self.motor_tm is None:
self.motor_tm = 0
else:
self.motor_tm += tm_diff
# some equation that makes a pretty graph
motor.current = motor.value * math.sin(
self.n + 8 * self.motor_tm) + 3 * self.motor_tm
# Simulate the drivetrain
lf_motor = wpilib.DigitalModule._pwm[0].Get() * -1
lr_motor = wpilib.DigitalModule._pwm[1].Get() * -1
rr_motor = wpilib.DigitalModule._pwm[2].Get()
rf_motor = wpilib.DigitalModule._pwm[3].Get()
# Our robot's wheels are wrong, so switch y/x, and invert everything
vy, vx, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(-vx, vy, -vw, tm_diff)
def test_mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, output):
result = drivetrains.mecanum_drivetrain(lr_motor,
rr_motor,
lf_motor,
rf_motor,
speed=1,
x_wheelbase=1,
y_wheelbase=1)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
assert abs(result[2] - output[2]) < 0.001
def update_sim(self, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated. This is mostly when wpilib.Wait is called.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
lr_motor = wpilib.DigitalModule._pwm[0].Get()
rr_motor = wpilib.DigitalModule._pwm[1].Get()
lf_motor = wpilib.DigitalModule._pwm[2].Get()
rf_motor = wpilib.DigitalModule._pwm[3].Get()
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
lf_motor = hal_data['pwm'][2]['value']
lr_motor = hal_data['pwm'][3]['value']
rf_motor = hal_data['pwm'][1]['value']
rr_motor = hal_data['pwm'][0]['value']
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Invert right side because it is inverted in the mecanum drive method
lf_motor = hal_data['CAN'][10]['value']
lr_motor = hal_data['CAN'][15]['value']
rf_motor = -hal_data['CAN'][20]['value']
rr_motor = -hal_data['CAN'][25]['value']
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain
lr_motor = hal_data["pwm"][6]["value"]
rr_motor = hal_data["pwm"][7]["value"]
lf_motor = hal_data["pwm"][8]["value"]
rf_motor = hal_data["pwm"][9]["value"]
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lr_motor = -hal_data["pwm"][1]["value"]
rr_motor = hal_data["pwm"][2]["value"]
lf_motor = -hal_data["pwm"][3]["value"]
rf_motor = hal_data["pwm"][4]["value"]
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, now, tm_diff):
'''
Called when the simulation parameters for the program need to be
updated. This is mostly when wpilib.Wait is called.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
'''
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lr_motor = -wpilib.DigitalModule._pwm[0].Get()
rr_motor = wpilib.DigitalModule._pwm[1].Get()
lf_motor = -wpilib.DigitalModule._pwm[2].Get()
rf_motor = wpilib.DigitalModule._pwm[3].Get()
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lr_motor = hal_data["pwm"][3]["value"]
rr_motor = -hal_data["pwm"][0]["value"]
lf_motor = hal_data["pwm"][2]["value"]
rf_motor = -hal_data["pwm"][1]["value"]
vx, vy, vw = drivetrains.mecanum_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor
)
self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, data, time, elapsed):
valueName = 'value'
# read CAN data to get motor speeds
maxValue = 1024
fl = (data['CAN'][self.canFL][valueName] * self.canFLInv / maxValue)
fr = (data['CAN'][self.canFR][valueName] * self.canFRInv / maxValue)
if not self.drivetrain == "two":
bl = (data['CAN'][self.canBL][valueName] * self.canBLInv / maxValue)
br = (data['CAN'][self.canBR][valueName] * self.canBRInv / maxValue)
if self.drivetrain == "mecanum":
vx, vy, vw = drivetrains.mecanum_drivetrain(bl, br, fl, fr,
self.xLen, self.yLen, self.speed)
self.physicsController.vector_drive(vx, vy, vw, elapsed)
elif self.drivetrain == "four":
speed, rot = drivetrains.four_motor_drivetrain(bl, br, fl, fr,
self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
elif self.drivetrain == "two":
speed, rot = drivetrains.two_motor_drivetrain(fl, fr,
self.xLen, self.speed)
self.physicsController.drive(speed, rot, elapsed)
def update_sim(self, hal_data, now, tm_diff):
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain
#lr_motor = hal_data["pwm"][1]["value"]
#rr_motor = hal_data["pwm"][3]["value"]
# Not needed because front and rear should be in sync
# lf_motor = hal_data['pwm'][3]['value']
# rf_motor = hal_data['pwm'][4]['value']
#x, y, angle = self.drivetrain.get_distance(lr_motor, rr_motor, tm_diff)
#self.physics_controller.distance_drive(x, y, angle)
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
lf_motor = hal_data["pwm"][1]["value"]
lr_motor = hal_data["pwm"][2]["value"]
rf_motor = -hal_data["pwm"][3]["value"]
rr_motor = -hal_data["pwm"][4]["value"]
vx, vy, vw = drivetrains.mecanum_drivetrain(lr_motor, rr_motor,
lf_motor, rf_motor)
#self.physics_controller.vector_drive(vx, vy, vw, tm_diff)
speed_factor = 4.0
strafe_factor = 1.5
self.physics_controller.vector_drive(strafe_factor * vx,
speed_factor * vy,
speed_factor * vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
if not self.canCalibrated:
try:
hal_data['CAN'][15]['limit_switch_closed_rev'] = True
except:
pass
if hal_data['control']['enabled']:
# Simulate the tote forklift
try:
toteDict = hal_data['CAN'][5]
canDict = hal_data['CAN'][15]
totePercentVal = int(toteDict['value'] * tm_diff * 2333 / 1023)
canPercentVal = int(canDict['value'] * tm_diff * 2333 / 1023)
posVal = int(tm_diff * 2333)
except:
pass
else:
if toteDict[
'mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus:
self.toteAct += totePercentVal
toteDict['enc_position'] += totePercentVal
elif toteDict[
'mode_select'] == wpilib.CANTalon.ControlMode.Position:
if toteDict['enc_position'] < toteDict['value']:
self.toteAct += posVal
toteDict['enc_position'] += posVal
else:
self.toteAct = posVal
toteDict['enc_position'] -= posVal
if canDict[
'mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus:
self.canAct += canPercentVal
canDict['enc_position'] += canPercentVal
elif canDict[
'mode_select'] == wpilib.CANTalon.ControlMode.Position:
if canDict['enc_position'] < canDict['value']:
self.toteAct += posVal
canDict['enc_position'] += posVal
else:
self.toteAct -= posVal
canDict['enc_position'] -= posVal
if canDict['enc_position'] < 0 and self.canCalibrated:
canDict['enc_position'] = 0
elif canDict['enc_position'] > 11000:
canDict['enc_position'] = 11000
if self.toteAct in range(-50, 50):
hal_data['dio'][2]['value'] = False
if self.canAct in range(-50, 50):
hal_data['CAN'][15]['limit_switch_closed_rev'] = False
self.canCalibrated = True
# Simulate the can forklift
# Do something about the distance sensors?
# Gyro
# Simulate the drivetrain
lf_motor = hal_data['pwm'][0]['value']
lr_motor = hal_data['pwm'][1]['value']
rf_motor = -hal_data['pwm'][2]['value']
rr_motor = -hal_data['pwm'][3]['value']
vx, vy, vw = mecanum_drivetrain(lr_motor, rr_motor, lf_motor,
rf_motor)
self.controller.vector_drive(vx, vy, vw, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
if not self.canCalibrated:
try:
hal_data['CAN'][15]['limit_switch_closed_rev'] = True
except:
pass
if hal_data['control']['enabled']:
# Simulate the tote forklift
try:
toteDict = hal_data['CAN'][5]
canDict = hal_data['CAN'][15]
totePercentVal = int(toteDict['value']*tm_diff*2333/1023)
canPercentVal = int(canDict['value']*tm_diff*2333/1023)
posVal = int(tm_diff*2333)
except:
pass
else:
if toteDict['mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus:
self.toteAct += totePercentVal
toteDict['enc_position'] += totePercentVal
elif toteDict['mode_select'] == wpilib.CANTalon.ControlMode.Position:
if toteDict['enc_position']<toteDict['value']:
self.toteAct += posVal
toteDict['enc_position'] += posVal
else:
self.toteAct = posVal
toteDict['enc_position'] -= posVal
if canDict['mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus:
self.canAct += canPercentVal
canDict['enc_position'] += canPercentVal
elif canDict['mode_select'] == wpilib.CANTalon.ControlMode.Position:
if canDict['enc_position'] < canDict['value']:
self.toteAct += posVal
canDict['enc_position'] += posVal
else:
self.toteAct -= posVal
canDict['enc_position'] -= posVal
if canDict['enc_position'] < 0 and self.canCalibrated:
canDict['enc_position'] = 0
elif canDict['enc_position'] >11000:
canDict['enc_position'] = 11000
if self.toteAct in range (-50, 50):
hal_data['dio'][2]['value'] = False
if self.canAct in range (-50, 50):
hal_data['CAN'][15]['limit_switch_closed_rev'] = False
self.canCalibrated = True
# Simulate the can forklift
# Do something about the distance sensors?
# Gyro
# Simulate the drivetrain
lf_motor = hal_data['pwm'][0]['value']
lr_motor = hal_data['pwm'][1]['value']
rf_motor = -hal_data['pwm'][2]['value']
rr_motor = -hal_data['pwm'][3]['value']
vx, vy, vw = mecanum_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=3)
self.controller.vector_drive(vx, vy, vw, tm_diff)
