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)