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
rr_motor = hal_data['CAN'][1]['value']
lr_motor = hal_data['CAN'][2]['value']
rf_motor = hal_data['CAN'][3]['value']
lf_motor = hal_data['CAN'][4]['value']
speed, rotation = drivetrains.four_motor_drivetrain(
lr_motor * -1,
rr_motor * -1,
lf_motor * -1,
rf_motor * -1,
x_wheelbase=2,
speed=6,
deadzone=drivetrains.linear_deadzone(0.1))
self.physics_controller.drive(speed, rotation, 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
# -> CANTalon values are from -1023..1023, scale it to -1..1
lr_motor = hal_data['CAN'][rMap.conf_leftRearBaseTalon]['value'] / 1023
rr_motor = hal_data['CAN'][
rMap.conf_rightRearBaseTalon]['value'] / 1023
lf_motor = hal_data['CAN'][
rMap.conf_leftFrontBaseTalon]['value'] / 1023
rf_motor = hal_data['CAN'][
rMap.conf_rightFrontBaseTalon]['value'] / 1023
# remember, if we use setInverted in the code, then we have to invert
# the motor outputs in simulation too!
lr_motor *= -1
lf_motor *= -1
speed, rotation = drivetrains.four_motor_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, 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['CAN'][1]['value']
rr_motor = hal_data['CAN'][9]['value']
lf_motor = hal_data['CAN'][2]['value']
rf_motor = hal_data['CAN'][10]['value']
speed = 5
# encoders
l = -(lf_motor + lr_motor) * 0.5 * speed
r = (rf_motor + rr_motor) * 0.5 * speed
hal_data['CAN'][1]['quad_position'] += int(l*tm_diff*10240)
hal_data['CAN'][10]['quad_position'] += int(r*tm_diff*10240)
speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=speed)
self.physics_controller.drive(speed, rotation, 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, 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_four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, output):
result = drivetrains.four_motor_drivetrain(lr_motor,
rr_motor,
lf_motor,
rf_motor,
speed=1)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
def update_sim(self, hal_data, now, tm_diff):
lr_motor = hal_data['CAN'][1]['value']
rr_motor = hal_data['CAN'][2]['value']
lf_motor = hal_data['CAN'][3]['value']
rf_motor = hal_data['CAN'][4]['value']
speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
lr_motor = hal_data['CAN'][1]['value']
rr_motor = hal_data['CAN'][2]['value']
lf_motor = hal_data['CAN'][3]['value']
rf_motor = hal_data['CAN'][4]['value']
speed, rotation = drivetrains.four_motor_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
def update_sim(self, hal_data, now, dt):
front_left_motor = hal_data['CAN'][0]['value']
front_right_motor = hal_data['CAN'][4]['value']
rear_left_motor = hal_data['CAN'][2]['value']
rear_right_motor = hal_data['CAN'][3]['value']
speed, rotation = drivetrains.four_motor_drivetrain(
rear_left_motor, rear_right_motor, front_left_motor,
front_right_motor)
self.physics_controller.drive(speed, rotation, dt)
def update_sim(self, hal_data, now, tm_diff):
""" Updates the simulation with new robot positions """
fl = hal_data['CAN'][0]['value']
bl = hal_data['CAN'][1]['value']
fr = -hal_data['CAN'][2]['value']
br = -hal_data['CAN'][3]['value']
rotation, speed = four_motor_drivetrain(bl, br, fl, fr, 3, 0.025)
self.controller.drive(speed, rotation * 0.75, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
# Simulate the drivetrain
lf_motor = -hal_data['pwm'][0]['value']/5
lr_motor = -hal_data['pwm'][1]['value']/5
rf_motor = -hal_data['pwm'][2]['value']/5
rr_motor = -hal_data['pwm'][3]['value']/5
fwd, rcw = four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=10)
self.controller.drive(fwd, rcw, 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
l_motor = hal_data['pwm'][0]['value']
r_motor = hal_data['pwm'][1]['value']
speed, rotation = drivetrains.four_motor_drivetrain(
l_motor, r_motor, l_motor, r_motor)
self.physics_controller.drive(speed, rotation, 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
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()
speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, 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
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()
speed, rotation = drivetrains.four_motor_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, 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']
speed, rotation = drivetrains.four_motor_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
# Simulate the drivetrain motors.
lf_motor = hal_data['CAN'][robotMap.left1]['value'] / -5
lr_motor = hal_data['CAN'][robotMap.left2]['value'] / -5
rf_motor = hal_data['CAN'][robotMap.right1]['value'] / 5
rr_motor = hal_data['CAN'][robotMap.right2]['value'] / 5
# Simulate movement.
speed, rotation = four_motor_drivetrain(lr_motor,
rr_motor,
lf_motor,
rf_motor,
speed=10)
self.controller.drive(speed, rotation, tm_diff)
# Simulate encoders (NOTE: These values have not been calibrated yet.)
hal_data['CAN'][robotMap.left1]['quad_position'] -= int(
lf_motor * robotMap.countsPerRevolution)
hal_data['CAN'][robotMap.right1]['quad_position'] += int(
rf_motor * robotMap.countsPerRevolution)
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
if 1 in hal_data['CAN']:
lr_motor = hal_data['CAN'][1]['value']/1024
rr_motor = hal_data['CAN'][3]['value']/1024
lf_motor = hal_data['CAN'][2]['value']/1024
rf_motor = hal_data['CAN'][4]['value']/1024
else:
lr_motor = 0
rr_motor = 0
lf_motor = 0
rf_motor = 0
speed, rotation = drivetrains.four_motor_drivetrain(-lr_motor, -rr_motor, -lf_motor, -rf_motor)
self.physics_controller.drive(speed, rotation, 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
if 1 in hal_data['CAN']:
lr_motor = hal_data['CAN'][1]['value'] / 1024
rr_motor = hal_data['CAN'][3]['value'] / 1024
lf_motor = hal_data['CAN'][2]['value'] / 1024
rf_motor = hal_data['CAN'][4]['value'] / 1024
else:
lr_motor = 0
rr_motor = 0
lf_motor = 0
rf_motor = 0
speed, rotation = drivetrains.four_motor_drivetrain(
-lr_motor, -rr_motor, -lf_motor, -rf_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
# Simulate the arm
try:
armDict = hal_data['CAN'][25] # armDict is the dictionary of variables assigned to CANTalon 25
lfDict = hal_data['CAN'][5]
rfDict = hal_data['CAN'][15]
armPercentVal = int(armDict['value']*tm_diff*2) # Encoder position increaser when in PercentVbus mode based on time and thrust value
posVal = int(tm_diff*1023) # Encoder Position difference when in Position mode based on time
armDict['limit_switch_closed_for'] = True
armDict['limit_switch_closed_rev'] = True
lWheelPercentVal = int(lfDict['value']*tm_diff*10)
rWheelPercentVal = int(rfDict['value']*tm_diff*10)
except:
pass
else:
if armDict['mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus: # If manual operation
self.armAct += armPercentVal # Add the calculated encoder change value to the 'actual value'
armDict['enc_position'] += armPercentVal # Add the calculated encoder change value to the recorded encoder value
elif armDict['mode_select'] == wpilib.CANTalon.ControlMode.Position: #If in auto mode
if armDict['enc_position'] < armDict['value']: #If the current position is less than the target position
armDict['enc_position'] += posVal # Add calculated encoder value to recorded value
self.armAct += posVal
else: # If the current position is more than the target position
armDict['enc_position'] -= posVal # Subtract calculated encoder position
self.armAct -=posVal
if self.armAct in range(-50, 0): # If the measured encoder value is within this range
armDict['limit_switch_closed_rev'] = False # Fake closing the limit switch
armDict['enc_position'] = 0
if self.armAct in range(2700, 2800):
armDict['limit_switch_closed_for'] = False
armDict['enc_position'] = 2764
lfDict['analog_in_position']+=lWheelPercentVal
rfDict['analog_in_position']+=rWheelPercentVal
#armDict['enc_position'] = max(min(armDict['enc_position'], 1440), 0) # Keep encoder between these values
self.armAct = max(min(self.armAct, 2764), 0)
armDict['enc_velocity'] = ((self.armAct - self.prev_armAct) / tm_diff)/1440
self.prev_armAct = self.armAct
# Simulate the drivetrain
lf_motor = -hal_data['CAN'][5]['value']/1023
lr_motor = -hal_data['CAN'][10]['value']/1023
rf_motor = -hal_data['CAN'][15]['value']/1023
rr_motor = -hal_data['CAN'][20]['value']/1023
fwd, rcw = four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=5)
if abs(fwd) > 0.1:
rcw += -(0.2*tm_diff)
self.controller.drive(fwd, rcw, tm_diff)
# Simulate the camera approaching the tower
# -> this is a very simple approximation, should be good enough
# -> calculation updated at 15hz
if self.camera_enabled and now - self.last_cam_update > self.camera_update_rate:
x, y, angle = self.controller.get_position()
tx, ty = self.target_location
dx = tx - x
dy = ty - y
distance = math.hypot(dx, dy)
target_present = False
if distance > 6 and distance < 17:
# determine the absolute angle
target_angle = math.atan2(dy, dx)
angle = ((angle + math.pi) % (math.pi*2)) - math.pi
# Calculate the offset, if its within 30 degrees then
# the robot can 'see' it
offset = math.degrees(target_angle - angle)
if abs(offset) < 30:
target_present = True
# target 'height' is a number between -18 and 18, where
# the value is related to the distance away. -11 is ideal.
self.target_angle = offset
self.target_height = -(-(distance*3)+30)
self.target_present = target_present
self.last_cam_update = now
def test_four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, output):
result = drivetrains.four_motor_drivetrain(
lr_motor, rr_motor, lf_motor, rf_motor, speed=1
)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 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
lf_motor = hal_data["CAN"][4]["value"] / -1024
lr_motor = hal_data["CAN"][5]["value"] / -1024
rf_motor = hal_data["CAN"][2]["value"] / -1024
rr_motor = hal_data["CAN"][3]["value"] / -1024
speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=8)
self.physics_controller.drive(speed, rotation, tm_diff)
# Simulate the firing mechanism, max is around 8000 in one second
pitcher = hal_data["CAN"][7]
max_v = 8000 * tm_diff
vel = pitcher["enc_velocity"]
if pitcher["mode_select"] == wpilib.CANTalon.ControlMode.Speed:
# when in pid mode, converge to the correct value
err = pitcher["value"] - vel
aerr = abs(err)
max_v = max(min(max_v, aerr), -aerr)
vel += math.copysign(max_v, err)
else:
# Otherwise increment linearly
vel += max_v * pitcher["value"]
pitcher["enc_velocity"] = max(min(vel, 8000), -8000)
x, y, angle = self.physics_controller.get_position()
# encoder simulation
lx, ly = self.last_location
dx = x - lx
dy = y - ly
length = math.hypot(dx, dy)
direction = math.atan2(dy, dx)
error = angle - direction
if abs(error) > math.pi:
if error > 0:
error = error - math.pi * 2
else:
error = error + math.pi * 2
if abs(error) > math.pi / 2.0:
length = length * -1
self.moved += length
hal_data["encoder"][0]["count"] = int(self.moved * self.ticks_per_ft) * 4
self.last_location = x, y
# Simulate the camera approaching the tower
# -> this is a very simple approximation, should be good enough
# -> calculation updated at 15hz
self.target.clear()
# simulate latency by delaying camera output
if self.last_cam_value is not None:
self.target += self.last_cam_value
if self.camera_enabled:
if now - self.last_cam_update > self.camera_update_rate:
tx, ty = self.target_location
dx = tx - x
dy = ty - y
distance = math.hypot(dx, dy)
if distance > 6 and distance < 17:
# determine the absolute angle
target_angle = math.atan2(dy, dx)
angle = ((angle + math.pi) % (math.pi * 2)) - math.pi
# Calculate the offset, if its within 30 degrees then
# the robot can 'see' it
offset = math.degrees(target_angle - angle)
if abs(offset) < 30:
# target 'height' is a number between -18 and 18, where
# the value is related to the distance away. -11 is ideal.
self.last_cam_value = [offset, -(-(distance * 3) + 30), distance, now]
self.last_cam_update = now
else:
self.last_cam_value = None
self.nt.putValue("target", self.target)
