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 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']
#global l_motor = 0.5
#global r_motor = 0.5
#import pdb; set_trace();
#print("hal_data", l_motor)
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
#print("update_sim left right forward rotate", l_motor, r_motor, speed, rotation)
x, y, angle = self.physics_controller.get_position()
#import pdb;pdb.set_trace()
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.target = 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
l_talon = hal_data['CAN'][1]
r_talon = hal_data['CAN'][2]
speed, rotation = drivetrains.two_motor_drivetrain(
l_talon['value'] * -1, r_talon['value'])
self.physics_controller.drive(speed, rotation, tm_diff)
# update position (use tm_diff so the rate is constant)
# encoder increments speed mutiplied by the time by some constant
# -> must be an integer
lspeed = int(4096 * 4 * l_talon['value'] * tm_diff)
l_talon['quad_position'] += lspeed
l_talon['quad_velocity'] = lspeed
rspeed = int(4096 * 4 * r_talon['value'] * tm_diff)
r_talon['quad_position'] += rspeed
r_talon['quad_velocity'] = rspeed
def update_sim(self, hal_data, now, tm_diff):
""" Updates the simulation with new robot positions """
left_speed = hal_data['CAN'][0]['value']
right_speed = hal_data['CAN'][2]['value']
left_distance = left_speed * (3 * tm_diff) * self.srxMagTicks
right_distance = right_speed * (3 * tm_diff) * self.srxMagTicks
elevator_winch_distance = (hal_data['CAN'][6]['value'] - 0.3) * (6 * tm_diff) * self.srxMagTicks
hal_data['CAN'][0]['quad_position'] -= int(left_distance)
hal_data['CAN'][2]['quad_position'] -= int(right_distance)
hal_data['CAN'][0]['quad_velocity'] = -int(left_distance / tm_diff)
hal_data['CAN'][2]['quad_velocity'] = -int(right_distance / tm_diff)
hal_data['CAN'][6]['quad_position'] -= int(elevator_winch_distance)
if -hal_data['CAN'][6]['quad_position'] < 15:
hal_data['dio'][0]['value'] = False
else:
hal_data['dio'][0]['value'] = True
speed, rotation = two_motor_drivetrain(left_speed, right_speed, 3, 4)
self.controller.drive(-speed, -rotation * 0.75, 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!
try:
if hal_data['CAN'][1]['inverted']:
l_motor = -hal_data['CAN'][1]['value']
else:
l_motor = hal_data['CAN'][1]['value']
if hal_data['CAN'][0]['inverted']:
r_motor = -hal_data['CAN'][0]['value']
else:
r_motor = hal_data['CAN'][0]['value']
tm_diff = tm_diff * 0.5
speed, rot = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rot, tm_diff)
except:
l_motor = r_motor = 0
self.physics_controller.drive(0, 0, 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
"""
"""
hal_data['pwm'] looks like this:
[{
'zero_latch': False,
'initialized': False,
'raw_value': 0,
'value': 0,
'period_scale': None,
'type': None
}, {
'zero_latch': True,
'initialized': True,
'raw_value': 1011,
'value': 0.0,
'period_scale': 0,
'type': 'talon'
},...]
"""
# Simulate the drivetrain
l_motor = hal_data['pwm'][5]['value']
r_motor = hal_data['pwm'][4]['value']
speed, rotation = drivetrains.two_motor_drivetrain(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
l_motor = wpilib.DigitalModule._pwm[0]
r_motor = wpilib.DigitalModule._pwm[1]
l_encoder = wpilib.DigitalModule._io[0]
r_encoder = wpilib.DigitalModule._io[2]
if l_motor is not None:
l_enc_disp = -l_motor.Get() * 5 * 360 * tm_diff
r_enc_disp = r_motor.Get() * 5 * 360 * tm_diff
if l_encoder is not None:
if l_encoder.value is not None:
l_encoder.value += l_enc_disp
l_encoder.rate = l_enc_disp/tm_diff
r_encoder.value += r_enc_disp
r_encoder.rate = r_enc_disp/tm_diff
speed, rotation = drivetrains.two_motor_drivetrain(-l_motor.Get(), -r_motor.Get())
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, 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
l_motor = wpilib.DigitalModule._pwm[0].Get()
r_motor = wpilib.DigitalModule._pwm[1].Get()
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
if self.jag_value is None:
return
# update position (use tm_diff so the rate is constant)
self.position += self.jag_value * tm_diff * 3
# update limit switches based on position
if self.position <= 0:
switch1 = True
switch2 = False
elif self.position > 10:
switch1 = False
switch2 = True
else:
switch1 = False
switch2 = False
# set values here
try:
wpilib.DigitalModule._io[0].value = switch1
except:
pass
try:
wpilib.DigitalModule._io[1].value = switch2
except:
pass
try:
wpilib.AnalogModule._channels[1].voltage = self.position
except:
pass
# always reset variables in case the input values aren't updated
# by the robot
self.jag_value = None
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
l_motor = wpilib.DigitalModule._pwm[0].Get()
r_motor = wpilib.DigitalModule._pwm[1].Get()
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
if self.jag_value is None:
return
# update position (use tm_diff so the rate is constant)
self.position += self.jag_value * tm_diff * 3
# update limit switches based on position
if self.position <= 0:
switch1 = True
switch2 = False
elif self.position > 10:
switch1 = False
switch2 = True
else:
switch1 = False
switch2 = False
# set values here
try:
wpilib.DigitalModule._io[0].value = switch1
except:
pass
try:
wpilib.DigitalModule._io[1].value = switch2
except:
pass
try:
wpilib.AnalogModule._channels[1].voltage = self.position
except:
pass
# always reset variables in case the input values aren't updated
# by the robot
self.jag_value = None
def calc_nophys():
speed, rotation = drivetrains.two_motor_drivetrain(l_motor,
r_motor,
speed=16,
x_wheelbase=2)
self.physics_controller.drive(speed, rotation, tm_diff)
ENCODER_TICKS_PER_FT = 4096 / ((6 * math.pi) / 12)
self.l_encoder = int(-l_motor * 16 * tm_diff *
ENCODER_TICKS_PER_FT)
self.r_encoder = int(r_motor * 16 * tm_diff * ENCODER_TICKS_PER_FT)
hal_data['CAN'][0]['quad_position'] += self.l_encoder
hal_data['CAN'][2]['quad_position'] += -self.r_encoder
def update_sim(self, hal_data, now, tm_diff):
'''
called when the simulation parameters for the program need to be updated
'''
try:
r_motor = hal_data['pwm'][9]['value']
l_motor = hal_data['pwm'][8]['value']
speed, rotation = drivetrains.two_motor_drivetrain(
l_motor, r_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
except:
pass
def update_sim(self, hal_data, now, tm_diff):
try:
l0_motor = hal_data['CAN'][0]['value'] / 1023
l1_motor = hal_data['CAN'][1]['value'] / 1023
r0_motor = hal_data['CAN'][2]['value'] / 1023
r1_motor = hal_data['CAN'][3]['value'] / 1023
fwd, rcw = two_motor_drivetrain(l0_motor, r0_motor, speed=5)
if abs(fwd) > 0.1:
rcw += -(0.2 * tm_diff)
self.controller.drive(fwd, rcw, tm_diff)
except:
pass
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
l_motor = wpilib.DigitalModule._pwm[0].Get()
r_motor = wpilib.DigitalModule._pwm[1].Get()
speed, rotation = drivetrains.two_motor_drivetrain(r_motor, l_motor, 5) #fix to account for joystick negatives/axes switched in sim
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
l_talon = hal_data['CAN'][1]
r_talon = hal_data['CAN'][3]
speed, rotation = drivetrains.two_motor_drivetrain(
l_talon['value'], r_talon['value'])
self.physics_controller.drive(speed, rotation, tm_diff)
def dynamics_func(x, u, dt=.1):
# === states and controls:
# x = [x y r x' y' r']' = [x y r]
# u = [r l]' = [right_wheel_out left_wheel_out
# controls
l_out = u[0] # w = right wheel out
r_out = u[1] # a = left wheel out
y_spd, rot_spd = drivetrains.two_motor_drivetrain(l_out, r_out)
theta = x[2] + dt * rot_spd
world_vel = array([sin(theta), cos(theta)]) * y_spd
pos = concatenate((x[:2] + world_vel * dt, theta[None]))
return pos
def dynamics_func(x, u, dt=.1):
# === states and controls:
# x = [x y r x' y' r']' = [x y r]
# u = [r l]' = [right_wheel_out left_wheel_out
# controls
l_out = u[0] # w = right wheel out
r_out = u[1] # a = left wheel out
y_spd, rot_spd = drivetrains.two_motor_drivetrain(l_out, r_out)
theta = x[2] + dt*rot_spd
world_vel = array([sin(theta), cos(theta)]) * y_spd
pos = concatenate((x[:2] + world_vel*dt, theta[None]))
return pos
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.two_motor_drivetrain(
l_motor, r_motor, speed=self.ft_per_sec)
self.physics_controller.drive(speed, rotation, tm_diff)
# Inches we traveled
distance_inches = 12.0 * speed * tm_diff
# Use that to give a rough approximation of encoder values.. not
# accurate for turns, but we don't need that
# -> encoder = distance / (wheel_circumference / 360.0)
hal_data['encoder'][0]['count'] += int(
distance_inches / (self.wheel_circumference / 360.0))
if self.vision:
x, y, angle = self.physics_controller.get_position()
#data = None
data = self.vision.compute(now, x, y, angle)
if data is not None:
self.nt.putNumberArray('/camera/target', data[0][:3])
distance = self.vision.get_immediate_distance()
if distance is None:
distance = 5.0 * 0.3
hal_data['analog_in'][0]['avg_voltage'] = max(min(distance * 0.3, 5.0),
0.0)
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'][4]['value']
r_motor = hal_data['pwm'][3]['value']
h_motor = hal_data['pwm'][2]['value']
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.vector_drive(h_motor, speed, rotation, tm_diff)
# update position (use tm_diff so the rate is constant)
self.position += hal_data['pwm'][4]['value'] * tm_diff * 3
# update limit switches based on position
if self.position <= 0:
switch1 = True
switch2 = False
elif self.position > 10:
switch1 = False
switch2 = True
else:
switch1 = False
switch2 = False
# set values here
hal_data['dio'][1]['value'] = switch1
hal_data['dio'][2]['value'] = switch2
hal_data['analog_in'][2]['voltage'] = self.position
hal_data['encoder'][0]['count'] = hal_data['encoder'][0]['count'] - l_motor * 3
print(hal_data['encoder'][0]['count'])
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
try:
l_motor = hal_data['CAN'][0]['value']
r_motor = hal_data['CAN'][1]['value']
except (KeyError, IndexError):
return
speed, rotation = drivetrains.two_motor_drivetrain(l_motor,
r_motor,
speed=16)
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
l_motor = hal_data["pwm"][0]["value"]
r_motor = hal_data["pwm"][1]["value"]
speed, rotation = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(speed, rotation, 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][: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
left = hal_data['pwm'][1]['value']
right = hal_data['pwm'][2]['value']
speed, rotation = drivetrains.two_motor_drivetrain(left, right)
self.physics_controller.drive(speed, rotation, tm_diff)
# update position (use tm_diff so the rate is constant)
self.position += hal_data['pwm'][4]['value'] * tm_diff * 3
# update limit switches based on position
if self.position <= 0:
switch1 = True
switch2 = False
elif self.position > 10:
switch1 = False
switch2 = True
else:
switch1 = False
switch2 = False
# set values here
hal_data['dio'][1]['value'] = switch1
hal_data['dio'][2]['value'] = switch2
hal_data['analog_in'][2]['voltage'] = self.position
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): # Simulate the drivetrain # -> Remember, in the constructor we inverted the left motors, so # invert the motor values here too! if(self.currentTime != self.pastTime): self.loopTime = self.currentTime - self.pastTime self.pastTime = self.currentTime self.currentTime = time.process_time() #if(not self.robot_enabled): # hal_data['pwm'][backLeftPort]['value'] = 0 # hal_data['pwm'][backRightPort]['value'] = 0 # hal_data['pwm'][frontLeftPort]['value'] = 0 # hal_data['pwm'][frontRightPort]['value'] = 0 # hal_data['pwm'][middleLeftPort]['value'] = 0 # hal_data['pwm'][middleRightPort]['value'] = 0 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'] lm_motor = hal_data['pwm'][middleLeftPort]['value'] rm_motor = -hal_data['pwm'][middleRightPort]['value'] self.lcount -= (lf_motor + lr_motor + lm_motor)/4 self.rcount -= (rf_motor + rr_motor + rm_motor)/4 if(self.lcountP != 0 and hal_data['encoder'][0]['count'] == 0): self.lcount = 0 if(self.rcountP != 0 and hal_data['encoder'][1]['count'] == 0): self.rcount = 0 if(self.lcountP == self.lcount): self.encodeLRate = 0 elif(self.loopTime != 0): self.encodeLRate = (self.lcount - self.lcountP)/self.loopTime if(self.rcountP == self.rcount): self.encodeRRate = 0 elif(self.loopTime != 0): self.encodeRRate = (self.rcount - self.rcountP)/self.loopTime self.encodeLRate = 0 self.encodeRRate = 0 hal_data['encoder'][0]['initialized'] = True hal_data['encoder'][1]['initialized'] = True hal_data['encoder'][0]['has_source'] = True hal_data['encoder'][0]['count'] = int(self.lcount) hal_data['encoder'][0]['rate'] = self.encodeLRate hal_data['encoder'][1]['has_source'] = True hal_data['encoder'][1]['count'] = int(self.rcount) hal_data['encoder'][1]['rate'] = self.encodeRRate self.lcountP = hal_data['encoder'][0]['count'] self.rcountP = hal_data['encoder'][1]['count'] rside = (rf_motor + rr_motor + rm_motor)/3 lside = (lf_motor + lr_motor + lm_motor)/3 vx, vy = drivetrains.two_motor_drivetrain(-rside, lside) #hal_data['analog_in'][0]['accumulator_value'] = hal_data['analog_in'][0]['accumulator_value'] *-1 self.physics_controller.drive(vx, vy, tm_diff)
def update_sim(self, hal_data, now, tm_diff):
# Simulate the drivetrain
# -> Remember, in the constructor we inverted the left motors, so
# invert the motor values here too!
if (self.currentTime != self.pastTime):
self.loopTime = self.currentTime - self.pastTime
self.pastTime = self.currentTime
self.currentTime = time.process_time()
#if(not self.robot_enabled):
# hal_data['pwm'][backLeftPort]['value'] = 0
# hal_data['pwm'][backRightPort]['value'] = 0
# hal_data['pwm'][frontLeftPort]['value'] = 0
# hal_data['pwm'][frontRightPort]['value'] = 0
# hal_data['pwm'][middleLeftPort]['value'] = 0
# hal_data['pwm'][middleRightPort]['value'] = 0
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']
lm_motor = hal_data['pwm'][middleLeftPort]['value']
rm_motor = -hal_data['pwm'][middleRightPort]['value']
self.lcount -= (lf_motor + lr_motor + lm_motor) / 4
self.rcount -= (rf_motor + rr_motor + rm_motor) / 4
if (self.lcountP != 0 and hal_data['encoder'][0]['count'] == 0):
self.lcount = 0
if (self.rcountP != 0 and hal_data['encoder'][1]['count'] == 0):
self.rcount = 0
if (self.lcountP == self.lcount):
self.encodeLRate = 0
elif (self.loopTime != 0):
self.encodeLRate = (self.lcount - self.lcountP) / self.loopTime
if (self.rcountP == self.rcount):
self.encodeRRate = 0
elif (self.loopTime != 0):
self.encodeRRate = (self.rcount - self.rcountP) / self.loopTime
self.encodeLRate = 0
self.encodeRRate = 0
hal_data['encoder'][0]['initialized'] = True
hal_data['encoder'][1]['initialized'] = True
hal_data['encoder'][0]['has_source'] = True
hal_data['encoder'][0]['count'] = int(self.lcount)
hal_data['encoder'][0]['rate'] = self.encodeLRate
hal_data['encoder'][1]['has_source'] = True
hal_data['encoder'][1]['count'] = int(self.rcount)
hal_data['encoder'][1]['rate'] = self.encodeRRate
self.lcountP = hal_data['encoder'][0]['count']
self.rcountP = hal_data['encoder'][1]['count']
rside = (rf_motor + rr_motor + rm_motor) / 3
lside = (lf_motor + lr_motor + lm_motor) / 3
vx, vy = drivetrains.two_motor_drivetrain(-rside, lside)
#hal_data['analog_in'][0]['accumulator_value'] = hal_data['analog_in'][0]['accumulator_value'] *-1
self.physics_controller.drive(vx, vy, tm_diff)
#hal_data['analog_in'][0]['accumulator_value'] = hal_data['analog_in'][0]['accumulator_value'] *-1
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['CAN'][5]['value']
r_motor = hal_data['CAN'][6]['value']
speed, rotation = drivetrains.two_motor_drivetrain(
l_motor, r_motor, speed=self.ft_per_sec)
self.physics_controller.drive(speed, rotation, tm_diff)
# Inches we traveled
distance_inches = 12.0 * speed * tm_diff
# Use that to give a rough approximation of encoder values.. not
# accurate for turns, but we don't need that
# -> encoder = distance / (wheel_circumference / 360.0)
#hal_data['encoder'][0]['count'] += int(distance_inches / (self.wheel_circumference/360.0))
# Elevator simulation
e_motor = hal_data['CAN'][7]['value']
e_distance = e_motor * tm_diff * 2
#print(e_distance)
self.elevator_position = (self.elevator_position + e_distance)
self.elevator_position = max(min(6, self.elevator_position), 0)
hal_data['CAN'][7]['quad_position'] = int(self.elevator_position *
(360 * 4))
# When is at the botton - do this
if 0 <= self.elevator_position < 0.1:
hal_data['CAN'][7]['limit_switch_closed_rev'] = True
# if it's not at the bottom - don't do this
else:
hal_data['CAN'][7]['limit_switch_closed_rev'] = False
# When is at the top - do this
if 5.9 <= self.elevator_position < 6.0:
hal_data['CAN'][7]['limit_switch_closed_for'] = True
# if it's not at the top - don't do this
else:
hal_data['CAN'][7]['limit_switch_closed_for'] = False
# Set our limit switches
# if 0 <= self.elevator_position < 0.2:
# hal_data['dio'][3]['value'] = True
# else:
# hal_data['dio'][3]['value'] = False
#
# if 2.4 < self.elevator_position < 2.6:
# hal_data['dio'][2]['value'] = True
# else:
# hal_data['dio'][2]['value'] = False
#
# if 4.9 < self.elevator_position <= 5:
# hal_data['dio'][1]['value'] = True
# else:
# hal_data['dio'][1]['value'] = False
# arm motor limit switches
# -> it's on if either end is hit
arm_motor = hal_data['CAN'][8]['value']
self.fork_position += arm_motor * tm_diff
self.fork_position = max(min(3, self.fork_position), 0)
if (0 <= self.fork_position <= 0.1) or (2.9 <= self.fork_position <=
3):
hal_data['dio'][3]['value'] = True
else:
hal_data['dio'][3]['value'] = False
def update_sim(self, hal_data, now, tm_diff):
l_motor = hal_data["pwm"][0]["value"]
r_motor = hal_data["pwm"][1]["value"]
y_speed, rot_speed = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(y_speed, rot_speed, tm_diff)
def test_two_motor_drivetrain(l_motor, r_motor, output):
result = drivetrains.two_motor_drivetrain(l_motor, r_motor, speed=1)
assert abs(result[0] - output[0]) < 0.001
assert abs(result[1] - output[1]) < 0.001
def test_two_motor_drivetrain(l_motor, r_motor, output):
result = drivetrains.two_motor_drivetrain(l_motor, r_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):
l_motor = hal_data["pwm"][0]["value"]
r_motor = hal_data["pwm"][1]["value"]
y_speed, rot_speed = drivetrains.two_motor_drivetrain(l_motor, r_motor)
self.physics_controller.drive(y_speed, rot_speed, tm_diff)
