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 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