Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
'''
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'][2]['value']
lf_motor = hal_data['pwm'][3]['value']
rf_motor = hal_data['pwm'][4]['value']
speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
self.physics_controller.drive(speed, rotation, tm_diff)
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