コード例 #1
0
ファイル: robot.py プロジェクト: Team865/FRC-2018-Python
    def teleopPeriodic(self):
        a = 0
        while self.isOperatorControl() and self.isEnabled():
            self.controls.periodic()
            self.limelight.mutiPipeline()
            self.intake.periodic()
            self.lift.periodic()

            b = self.lift.getEncoderVal()
            if a < b:
                a = b
            SmartDashboard.putNumber("pipeline id",
                                     self.limelight.getPipeline())
            SmartDashboard.putBoolean("inake hasCube", self.intake.hasCube())
            #drive.periodic()

            #SmartDashboard.putNumber("liftRTS hz", self.liftRTS.getHz())

            SmartDashboard.putNumber("0", self.a0.getAverageVoltage())
            SmartDashboard.putNumber("1", self.a1.getAverageVoltage())
            SmartDashboard.putNumber("2", self.a2.getAverageVoltage())
            SmartDashboard.putNumber("3", self.a3.getAverageVoltage())

            SmartDashboard.putNumber("Lift", a)
            SmartDashboard.putNumber("Drive Right Dist",
                                     self.drive.getRightDistance())
            SmartDashboard.putNumber("Drive Left Dist",
                                     self.drive.getLeftDistance())
            SmartDashboard.putNumber("pitch", self.navx.getPitch())
コード例 #2
0
ファイル: shooter.py プロジェクト: aesatchien/FRC2429_2021
    def periodic(self) -> None:
        """Perform necessary periodic updates"""
        self.counter += 1
        if self.counter % 5 == 0:
            # pass
            # ten per second updates
            SmartDashboard.putNumber('elevation',
                                     self.hood_encoder.getDistance())
            SmartDashboard.putNumber('rpm',
                                     self.flywheel_encoder.getVelocity())

            watch_axis = False
            if watch_axis:
                self.hood_scale = 0.2
                self.hood_offset = 0.0
                power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) -
                                           0.5) + self.hood_offset
                self.robot.shooter.change_elevation(power)

            maintain_elevation = True
            if maintain_elevation:
                self.error = self.get_angle() - self.hood_setpoint
                pid_out = self.hood_controller.calculate(self.error)
                output = 0.03 + pid_out
                SmartDashboard.putNumber('El PID', pid_out)
                self.change_elevation(output)
        if self.counter % 50 == 0:
            SmartDashboard.putBoolean("hood_low", self.limit_low.get())
            SmartDashboard.putBoolean("hood_high", self.limit_high.get())
コード例 #3
0
 def dashboardPeriodic(self):
     #commented out some values. DON'T DELETE THESE VALUES
     #SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
     SmartDashboard.putNumber("Wrist Angle", self.getAngle())
     #SmartDashboard.putNumber("Output", self.out)
     #self.F = SmartDashboard.getNumber("F Gain", 0)
     #self.wristUpVolts = SmartDashboard.getNumber("Wrist Up Volts", 0)
     #self.wristDownVolts = SmartDashboard.getNumber("Wrist Down Volts", 0)
     SmartDashboard.putBoolean("Limit Switch",
                               self.wrist.isFwdLimitSwitchClosed())
     SmartDashboard.putBoolean("Limit Switch Reverse",
                               self.wrist.isRevLimitSwitchClosed())
コード例 #4
0
 def robotPeriodic(self):
     SmartDashboard.putBoolean('Fwd closed',
                               self.motor.isFwdLimitSwitchClosed())
     SmartDashboard.putBoolean('Rev closed',
                               self.motor.isRevLimitSwitchClosed())
     SmartDashboard.putNumber('Motor position',
                              self.motor.getQuadraturePosition())
     SmartDashboard.putNumber("Motor power",
                              self.motor.getMotorOutputPercent())
     SmartDashboard.putNumber("Motor volts",
                              self.motor.getMotorOutputVoltage())
     SmartDashboard.putNumber("Motor current",
                              self.motor.getOutputCurrent())
コード例 #5
0
    def robotPeriodic(self):
        self.limelight.readLimelightData()
        SmartDashboard.putBoolean("teleop", self.teleop)

        if (self.dashboard): self.updateDashboardPeriodic()
        '''currAlign = self.joystick0.getRawButton(map.autoAlign)
        if currAlign and currAlign != self.lastAlign:
            AutoAlign(self.limelight.getPath()[0],self.limelight.getPath()[1],self.limelight.getPath()[2],self.limelight.getPath()[3]).start()

        if not currAlign and currAlign != self.lastAlign:
            AutoAlign(self.limelight.getPath()[0],self.limelight.getPath()[1],self.limelight.getPath()[2],self.limelight.getPath()[3]).cancel()
            self.SetSpeedDT.start()

        self.lastAlign = currAlign'''
        '''currStraightAlign = self.joystick0.getRawButton(map.straightAlign)
コード例 #6
0
ファイル: portcullis.py プロジェクト: dbadb/2016-Stronghold
 def updateDashboard(self):
     SmartDashboard.putBoolean("Portcullis Upper Right Limit Switch",
                              self.rightAtTop())
     SmartDashboard.putBoolean("Portcullis Upper Left Limit Switch",
                              self.leftAtTop())
     SmartDashboard.putBoolean("Portcullis Lower Right Limit Switch",
                              self.rightAtBottom())
     SmartDashboard.putBoolean("Portcullis Lower Left Limit Switch",
                              self.leftAtBottom())
コード例 #7
0
    def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick):
        self.robot_mode = robot_mode
        lift_position = self.lift_talon.getAnalogInRaw()

        if lift_position > self.lift_stow_position + 5:
            SmartDashboard.putBoolean("Creep", True)
        else:
            SmartDashboard.putBoolean("Creep", False)

        if robot_mode == RobotMode.TEST:

            if self.test_lift_pneumatic.getSelected() == 1:
                self.lift_pneumatic_extend.set(False)
                self.lift_pneumatic_retract.set(True)
            else:
                self.lift_pneumatic_extend.set(True)
                self.lift_pneumatic_retract.set(False)

            #need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range
            if lift_position > self.min_lift_position or lift_position < self.max_lift_position:
                self.lift_talon.set(
                    ControlMode.PercentOutput,
                    -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS))
            elif lift_position < self.min_lift_position:
                #allow upward motion
                self.lift_talon.set(
                    ControlMode.PercentOutput,
                    max(
                        -1.0 *
                        pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
            elif lift_position > self.max_lift_position:
                #allow downward motion
                self.lift_talon.set(
                    ControlMode.PercentOutput,
                    min(
                        -1.0 *
                        pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
            else:
                self.lift_talon.set(ControlMode.PercentOutput, 0.0)
        else:
            self.iterate_state_machine(pilot_stick, copilot_stick)

        SmartDashboard.putString("Lift State", self.current_state.name)
        SmartDashboard.putString("Lift Preset", self.current_lift_preset.name)
        SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint)
        SmartDashboard.putNumber("Lift Position", lift_position)
コード例 #8
0
ファイル: CargoMech0.py プロジェクト: Cortechs5511/DeepSpace
 def dashboardPeriodic(self):
     #self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
     #self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
     #self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
     #self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
     #self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
     #self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
     SmartDashboard.putNumber("Wrist Position",
                              self.wrist.getQuadraturePosition())
     SmartDashboard.putData("PID Controller", self.cargoController)
     SmartDashboard.putNumber("Wrist Angle", self.getAngle())
     SmartDashboard.putNumber("Wrist Power", self.input)
     SmartDashboard.putNumber("Wrist Power2", self.input2)
     SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand)
     SmartDashboard.putBoolean("Wrist PinState Quad A",
                               self.wrist.getPinStateQuadA())
     SmartDashboard.putBoolean("Wrist PinState Quad B",
                               self.wrist.getPinStateQuadB())
     self.F = SmartDashboard.getNumber("F Gain", 0)
コード例 #9
0
    def execute(self) -> None:
        (ball_detected, rotation_offset,
         distance) = self.vision.getBallValues()
        SmartDashboard.putBoolean('/DriveToBall/ball_detected', ball_detected)

        if ball_detected or abs(rotation_offset) > 5:
            SmartDashboard.putNumber('/DriveToBall/rotation_offset',
                                     rotation_offset)
            SmartDashboard.putNumber('/DriveToBall/distance', distance)

            speed_ratio = abs(rotation_offset) / 30
            added_speed = 0.1
            min_speed = 0.2
            speed = min_speed + added_speed * speed_ratio

            angle_is_negative = rotation_offset < 0
            output = -speed if angle_is_negative else speed

            self.drivetrain.arcade_drive(0, output)
コード例 #10
0
 def dashboardPeriodic(self):
     #commented out some values. DON'T DELETE THESE VALUES
     #SmartDashboard.putNumber("Hatch Current", self.wheels.getOutputCurrent())
     #SmartDashboard.putNumber("Power", self.outPower)
     SmartDashboard.putBoolean("Has Hatch", self.hasHatch)
     SmartDashboard.putBoolean("Slider Out", self.slider.get())
     SmartDashboard.putBoolean("Kicker Out", self.kicker.get())
コード例 #11
0
    def log(self):
        self.counter += 1
        if self.counter % 10 == 0:
            # start keeping track of where the robot is with an x and y position (only good for WCD)
            try:
                distance = 0.5 * (self.sparkneo_encoder_1.getPosition() -
                                  self.sparkneo_encoder_3.getPosition())
            except:
                print(
                    f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}"
                )
                distance = 0
            self.x = self.x + (distance - self.previous_distance) * math.sin(
                math.radians(self.robot.navigation.get_angle()))
            self.y = self.y + (distance - self.previous_distance) * math.cos(
                math.radians(self.robot.navigation.get_angle()))
            self.previous_distance = distance
            # send values to the dash to make sure encoders are working well
            #SmartDashboard.putNumber("Robot X", round(self.x, 2))
            #SmartDashboard.putNumber("Robot Y", round(self.y, 2))
            for ix, encoder in enumerate(self.encoders):
                SmartDashboard.putNumber(
                    f"Position Enc{str(int(1+ix))}",
                    round(encoder.getPosition() - self.encoder_offsets[ix], 2))
                SmartDashboard.putNumber(f"Velocity Enc{str(int(1+ix))}",
                                         round(encoder.getVelocity(), 2))
            SmartDashboard.putNumber(
                "Current M1",
                round(self.spark_neo_left_front.getOutputCurrent(), 2))
            SmartDashboard.putNumber(
                "Current M3",
                round(self.spark_neo_right_front.getOutputCurrent(), 2))
            SmartDashboard.putBoolean('AccLimit', self.is_limited)

        if self.counter % 1000 == 0:
            # self.display_PIDs()
            SmartDashboard.putString(
                "alert",
                f"Position: ({round(self.x, 1)},{round(self.y, 1)})  Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}"
            )
コード例 #12
0
    def robotPeriodic(self):
        wpilib.SmartDashboard.putNumber("YawAngle", self.m_imu.getAngle())
        wpilib.SmartDashboard.putNumber("XCompAngle",
                                        self.m_imu.getXComplementaryAngle())
        wpilib.SmartDashboard.putNumber("YCompAngle",
                                        self.m_imu.getYComplementaryAngle())
        wpilib.SmartDashboard.getBoolean("RunCal", False)
        wpilib.SmartDashboard.getBoolean("RunCal", False)
        wpilib.SmartDashboard.getBoolean("RunCal", False)
        wpilib.SmartDashboard.getBoolean("RunCal", False)
        self.m_yawSelected = self.m_yawChooser.GetSelected()
        self.m_yawSelected = KYAW_DEFAULT

        # Set IMU settings
        if (self.m_configCal):
            self.m_imu.configCalTime(ADIS16470CalibrationTime._8s)
            self.m_configCal = SmartDashboard.putBoolean("ConfigCal", False)

        if (self.m_reset):
            self.m_imu.Reset()
            self.m_reset = SmartDashboard.putBoolean("Reset", False)

        if (self.m_runCal):
            self.m_imu.Calibrate()
            self.m_runCal = SmartDashboard.putBoolean("RunCal", False)

        # Read the desired yaw axis from the dashboard
        if (self.m_yawSelected == "X-Axis"):
            self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kX

        elif (self.m_yawSelected == "Y-Axis"):
            self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kY

        else:
            self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kZ

        # Set the desired yaw axis from the dashboard
        if (self.m_setYawAxis):
            self.m_imu.SetYawAxis(self.m_yawActiveAxis)
            self.m_setYawAxis = SmartDashboard.putBoolean("SetYawAxis", False)
コード例 #13
0
    def on_iteration(self, time_elapsed):
        dash.putBoolean("On Target",
                        self.subsystem_drivetrain.is_done_turning())
        dash.putNumber("Distance from target",
                       self.subsystem_drivetrain.turn_pid.getError())

        turn_kP, turn_kI, turn_kD = dashboard_utils.get_pid_values(
            robotmap.dashboard_pid_drivetrain_turn)
        self.subsystem_drivetrain.update_pid(turn_kP, turn_kI, turn_kD)

        dash.putNumber("total error",
                       self.subsystem_drivetrain.turn_pid.totalError)
        dash.putNumber(
            "integral term", self.subsystem_drivetrain.turn_pid.I *
            self.subsystem_drivetrain.turn_pid.totalError)
        dash.putNumber("current angle",
                       self.subsystem_drivetrain.navx.getAngle())

        dash.putNumber("PID Output", self.subsystem_drivetrain.turn_pid.get())

        self.subsystem_drivetrain.turn_to_angle(
            dash.getNumber("Turn Position", 0))
コード例 #14
0
ファイル: robot.py プロジェクト: 369highvoltage/jessica
 def robotPeriodic(self):
     SmartDashboard.putNumber("driver/current_distance", RobotMap.driver_component.current_distance)
     SmartDashboard.putNumber("driver/left_encoder",
                              RobotMap.driver_component.left_encoder_motor.getSelectedSensorPosition(0))
     SmartDashboard.putNumber("driver/right_encoder",
                              RobotMap.driver_component.right_encoder_motor.getSelectedSensorPosition(0))
     SmartDashboard.putNumber("driver/gyro", RobotMap.driver_component.driver_gyro.getAngle())
     SmartDashboard.putNumber("lifter/current_position", RobotMap.lifter_component.current_position)
     SmartDashboard.putNumber("lifter/current_elevator_position", RobotMap.lifter_component.current_elevator_position)
     SmartDashboard.putNumber("lifter/current_carriage_position", RobotMap.lifter_component.current_carriage_position)
     SmartDashboard.putBoolean("lifter/carriage_top_switch", RobotMap.lifter_component.carriage_top_switch.get())
     SmartDashboard.putBoolean("lifter/carriage_bottom_switch", RobotMap.lifter_component.carriage_bottom_switch.get())
     SmartDashboard.putBoolean("lifter/elevator_bottom_switch", RobotMap.lifter_component.elevator_bottom_switch.get())
     SmartDashboard.putNumber("gripper/gripper_pot", RobotMap.gripper_component.pot.get())
コード例 #15
0
class AutonomousRamsete(Command):
    """Attempting to translate the Ramsete command from commands V2 into a V1 version since robotpy doesn't have this command yet """

    # constants for ramsete follower and velocity PID controllers - don't want a different copy for each command
    beta = drive_constants.ramsete_B
    zeta = drive_constants.ramsete_Zeta
    kp_vel = drive_constants.kp_drive_vel
    kd_vel = 0
    velocity = drive_constants.k_max_speed_meters_per_second
    write_telemetry = False

    dash = True  # ToDo: decide if I ever want to hide this
    if dash is True:
        SmartDashboard.putNumber("ramsete_kpvel", kp_vel)
        SmartDashboard.putNumber("ramsete_B", beta)
        SmartDashboard.putNumber("ramsete_Z", zeta)
        SmartDashboard.putBoolean("ramsete_write", write_telemetry)

    def __init__(self, robot, timeout=50):
        Command.__init__(self, name='auto_ramsete')
        self.robot = robot
        self.requires(robot.drivetrain)
        self.setTimeout(timeout)

        self.previous_time = -1
        self.previous_speeds = None
        self.use_PID = True
        self.counter = 0
        self.telemetry = []
        self.trajectory = None

        self.feed_forward = drive_constants.feed_forward
        self.kinematics = drive_constants.drive_kinematics
        self.course = drive_constants.course

    def initialize(self):
        """Called just before this Command runs the first time."""

        self.previous_time = -1
        self.telemetry = []
        self.init_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)

        self.robot.drivetrain.drive.feed() # this initialization is taking some time now
        # update gains from dash if desired
        if self.dash is True:
            self.kp_vel = SmartDashboard.getNumber("ramsete_kpvel", self.kp_vel)
            self.beta = SmartDashboard.getNumber("ramsete_B", self.beta)
            self.zeta = SmartDashboard.getNumber("ramsete_Z", self.zeta)
            self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry)

        # create controllers
        self.follower = wpilib.controller.RamseteController(self.beta, self.zeta)
        self.left_controller = wpilib.controller.PIDController(self.kp_vel, 0 , self.kd_vel)
        self.right_controller = wpilib.controller.PIDController(self.kp_vel, 0 , self.kd_vel)
        self.left_controller.reset()
        self.right_controller.reset()


        #ToDo - test for real robot -
        trajectory_choice = self.robot.oi.path_chooser.getSelected()  # get path from the GUI
        self.velocity = float(self.robot.oi.velocity_chooser.getSelected())  # get the velocity from the GUI
        if 'z_' not in trajectory_choice:  # let me try a few of the other methods if the path starts with z_
            self.trajectory = drive_constants.generate_trajectory(trajectory_choice, self.velocity, simulation=self.robot.isSimulation(), save=False)
            self.course = trajectory_choice
            if self.robot.isReal():
                self.start_pose = geo.Pose2d(self.trajectory.sample(0).pose.X(), self.trajectory.sample(0).pose.Y(),
                                                 self.robot.drivetrain.get_rotation2d())
            else:
                field_x = SmartDashboard.getNumber('field_x', self.trajectory.sample(0).pose.X())
                field_y = SmartDashboard.getNumber('field_y', self.trajectory.sample(0).pose.Y())
                self.start_pose = geo.Pose2d(field_x, field_y, self.robot.drivetrain.get_rotation2d())
        self.robot.drivetrain.drive.feed()  # this initialization is taking some time now

        # Note - we are setting to pose to have the robot physically in the start position - usually absolute matters

        if 'slalom' in trajectory_choice:
            pass
            #self.start_pose = geo.Pose2d(1.3, 0.66, geo.Rotation2d(0))
        elif 'barrel' in trajectory_choice:
            pass
            # self.start_pose = geo.Pose2d(1.3, 2.40, geo.Rotation2d(0))  # may want to rotate this
        elif 'bounce' in trajectory_choice:
            pass
            # self.start_pose = geo.Pose2d(1.3, 2.62, geo.Rotation2d(0))
        elif 'student' in trajectory_choice:
            pass
            # self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))  # student should put barrel, slalom or bounce in name
        elif 'loop' in trajectory_choice:
            self.course = 'loop'
            self.trajectory = drive_constants.get_loop_trajectory(self.velocity)
            self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
        elif 'poses' in trajectory_choice:
            self.course = 'slalom_poses'
            self.trajectory = drive_constants.get_pose_trajectory(self.velocity)
            self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
        elif 'points' in trajectory_choice:
            self.course = 'slalom_points'
            self.trajectory = drive_constants.get_point_trajectory(self.velocity)
            self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
        elif 'z_test' in trajectory_choice:
            self.course = 'test'
            self.trajectory = drive_constants.get_test_trajectory(self.velocity)
            self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
        else:
            pass
            #self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))

        self.robot.drivetrain.reset_odometry(self.start_pose)
        initial_state = self.trajectory.sample(0)
        # these are all meters in 2021
        self.previous_speeds = self.kinematics.toWheelSpeeds(wpimath.kinematics.ChassisSpeeds(
            initial_state.velocity, 0, initial_state.curvature*initial_state.velocity))

        self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
        print("\n" + f"** Started {self.getName()} on {self.course} with load time {self.start_time-self.init_time:2.2f}s"
                     f" (b={self.beta}, z={self.zeta}, kp_vel={self.kp_vel}) at {self.start_time} s **")
        SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time} s **")

        print('Time\tTr Vel\tTr Rot\tlspd\trspd\tram ang\tram vx\tram vy\tlffw\trffw\tlpid\trpid')

    def execute(self) -> None:

        current_time = self.timeSinceInitialized()
        dt = current_time - self.previous_time

        if self.previous_time < 0:
            self.robot.drivetrain.tank_drive_volts(0, 0)
            self.previous_time = current_time
            return

        # get the robot's current field pose, current trajectory point, and feed to the ramsete controller
        pose = self.robot.drivetrain.get_pose()
        sample = self.trajectory.sample(current_time)
        ramsete = self.follower.calculate(pose, sample)
        target_wheel_speeds = self.kinematics.toWheelSpeeds(ramsete)

        left_speed_setpoint = target_wheel_speeds.left
        right_speed_setpoint = target_wheel_speeds.right

        v_limit = 10 # for some reason the ffwd is putting out monster values of voltage - probably an error on my part in the characterization
        if self.use_PID:
            left_feed_forward = self.feed_forward.calculate(left_speed_setpoint, (left_speed_setpoint - self.previous_speeds.left)/dt)
            left_feed_forward = v_limit if left_feed_forward > v_limit else left_feed_forward
            left_feed_forward = -v_limit if left_feed_forward < -v_limit else left_feed_forward

            right_feed_forward = self.feed_forward.calculate(right_speed_setpoint, (right_speed_setpoint - self.previous_speeds.right)/dt)
            right_feed_forward = v_limit if right_feed_forward > v_limit else right_feed_forward
            right_feed_forward = -v_limit if right_feed_forward < -v_limit else right_feed_forward

            #ws_left, ws_right = self.robot.drivetrain.get_wheel_speeds().left, self.robot.drivetrain.get_wheel_speeds().right
            ws_left, ws_right = self.robot.drivetrain.get_rate(self.robot.drivetrain.l_encoder), -self.robot.drivetrain.get_rate(self.robot.drivetrain.r_encoder)
            left_output_pid = self.left_controller.calculate(ws_left, left_speed_setpoint)
            right_output_pid = self.right_controller.calculate(ws_right, right_speed_setpoint)
            # 100% sure that these signs are right - see plots.   Lots of issues here.  Need to sort out where you correct for encoder signs.
            pid_sign = 1
            left_output = pid_sign*left_output_pid + left_feed_forward
            right_output = pid_sign*right_output_pid + right_feed_forward

        else:  # ToDo - fix this to just be the feed forwards and test it
            left_output = left_speed_setpoint
            right_output = right_speed_setpoint

        self.robot.drivetrain.tank_drive_volts(left_output, -right_output)
        self.previous_speeds = target_wheel_speeds
        self.previous_time = current_time
        self.robot.drivetrain.drive.feed()  # should this be in tank drive?

        if self.counter % 5 == 0:  # ten times per second update the telemetry array
            telemetry_data = {'TIME':current_time, 'RBT_X':pose.X(), 'RBT_Y':pose.Y(), 'RBT_TH':pose.rotation().radians(),
                            'RBT_VEL':self.robot.drivetrain.get_average_encoder_rate(),
                            'RBT_RVEL':ws_right, 'RBT_LVEL':ws_left,
                            'TRAJ_X':sample.pose.X(), 'TRAJ_Y':sample.pose.Y(), 'TRAJ_TH':sample.pose.rotation().radians(), 'TRAJ_VEL':sample.velocity,
                            'RAM_VELX':ramsete.vx, 'RAM_LVEL_SP':left_speed_setpoint, 'RAM_RVEL_SP':right_speed_setpoint,
                            'RAM_OM':ramsete.omega, 'LFF':left_feed_forward, 'RFF':right_feed_forward, 'LPID': left_output_pid, 'RPID':right_output_pid}
            self.telemetry.append(telemetry_data)
        if self.counter % 50 == 0: # once per second send data to the console
            out_string = f'{current_time:4.2f}\t{self.trajectory.sample(current_time).velocity:4.1f}\t{self.trajectory.sample(current_time).pose.rotation().radians():4.2f}\t'
            out_string += f'{left_speed_setpoint:4.2f}\t{right_speed_setpoint:4.2f}\t{ramsete.omega:4.2f}\t{ramsete.vx:4.2f}\t{ramsete.vy:4.2f}\t'
            out_string += f'{left_feed_forward:4.2f}\t{right_feed_forward:4.2f}\t{left_output_pid:4.2f}\t{right_output_pid:4.2f}'
            print(out_string)
        self.counter += 1

    def isFinished(self) -> bool:  # ToDo: investigate the different end methods
        return self.isTimedOut() or self.timeSinceInitialized() > self.trajectory.totalTime()

    def end(self, message='Ended'):
        """Called once after isFinished returns true"""
        end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
        print(f"** {message} {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **")
        SmartDashboard.putString("alert", f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **")
        self.robot.drivetrain.stop()

        self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry)
        if self.write_telemetry:
            location = Path.cwd() if self.robot.isSimulation() else Path('/home/lvuser/py/')  # it's not called robot on the robot
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            file_name = timestamp + '_'+ self.course + f'_kpv_{self.kp_vel:2.1f}'.replace('.','p') + f'_vel_{str(self.velocity).replace(".","p")}'   +'.pkl'
            pickle_file = location / 'sim' / 'data' / file_name
            with open(pickle_file.absolute(), 'wb') as fp:
                out_dict = {'TIMESTAMP':timestamp,'DATA':self.telemetry, 'COURSE':self.course, 'VELOCITY':self.velocity,
                            'KP_VEL':self.kp_vel, 'KD_VEL':self.kd_vel, 'BETA':self.beta, 'ZETA':self.zeta}
                pickle.dump(out_dict, fp)
            print(f'*** Wrote ramsete command data to {file_name} ***')
        else:
            print(f'*** Skipping saving of telemetry to disk ***')

    def interrupted(self):
        self.end(message='Interrupted')
コード例 #16
0
ファイル: Climber.py プロジェクト: Cortechs5511/DeepSpace
 def dashboardPeriodic(self):
     SmartDashboard.putBoolean("Fully Extended Front",
                               self.isFullyExtendedFront())
     SmartDashboard.putBoolean("Fully Extended Back",
                               self.isFullyExtendedBack())
     SmartDashboard.putBoolean("Fully Retracted Front",
                               self.isFullyRetractedFront())
     SmartDashboard.putBoolean("Fully Retracted Back",
                               self.isFullyRetractedBack())
     SmartDashboard.putBoolean("Front Over Ground",
                               self.isFrontOverGround())
     SmartDashboard.putBoolean("Back Over Ground", self.isBackOverGround())
     SmartDashboard.putNumber("self.state", self.state)
     self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed",
                                                self.climbSpeed)
     self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
     self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
     self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
     SmartDashboard.putNumber("Lean", self.getLean())
     SmartDashboard.putNumber("FloorSensor", self.backUp())
     SmartDashboard.putBoolean("Auto Climb Indicator",
                               self.autoClimbIndicator)
     SmartDashboard.putNumber("Retract Start", self.frontRetractStart)
     SmartDashboard.putNumber("Wheels Start", self.wheelsStart2)
コード例 #17
0
    def robotPeriodic(self):
        self.limelight.readLimelightData()
        SmartDashboard.putBoolean("teleop", self.teleop)

        if (self.dashboard): self.updateDashboardPeriodic()
コード例 #18
0
ファイル: robot.py プロジェクト: FRCTeam279/2019DeepSpace
    def disabledPeriodic(self):
        Scheduler.getInstance().run()

        #Tanklift data
        SmartDashboard.putBoolean("Front IR", subsystems.drivelift.frontIR.state)
        SmartDashboard.putBoolean("Back IR", subsystems.drivelift.backIR.state)

        #Elevator Data
        SmartDashboard.putNumber("Elevator Ticks", subsystems.elevator.elevatorEncoder.get())
        SmartDashboard.putNumber("Elevator Height", subsystems.elevator.elevatorEncoder.getDistance())
        SmartDashboard.putNumber("Limit Switch", subsystems.elevator.btmLimitSwitch.get())

        #Game Objective States
        SmartDashboard.putBoolean("Ramp Tog", subsystems.ramp.rampToggle)
        SmartDashboard.putBoolean("Hatch Tog", subsystems.hatchgrab.hatchToggle)
        SmartDashboard.putBoolean("Suction Extend", subsystems.hatchgrab.hatchExtendToggle)
        SmartDashboard.putBoolean("Cargo Tog", subsystems.cargograb.cargoToggle)
        SmartDashboard.putNumber("R Servo Angle", subsystems.cargograb.rightServo.getAngle())
        SmartDashboard.putNumber("L Servo Angle", subsystems.cargograb.leftServo.getAngle())
        SmartDashboard.putBoolean("Relay State", subsystems.hatchgrab.hatchGrabExtendSol.get()) 

        #Driveline data
        SmartDashboard.putNumber("Left Speed", subsystems.driveline.leftSpdCtrl.get())
        SmartDashboard.putNumber("Right Speed", subsystems.driveline.rightSpdCtrl.get())
        SmartDashboard.putNumber("Right Sensors", subsystems.driveline.rSensor.getVoltage())
        SmartDashboard.putNumber("Left Sensors", subsystems.driveline.lSensor.getVoltage())
        SmartDashboard.putNumber("Sensor Dif", subsystems.driveline.lineSensorCompare)
コード例 #19
0
 def robotPeriodic(self):
     #self.limelight.readLimelightData()
     if (self.dashboard): self.updateDashboardPeriodic()
     SmartDashboard.putBoolean("Passed Hatch", self.drive.isCargoPassed())
コード例 #20
0
 def initializeBoolean(label: str, defaultValue: bool) -> bool:
     value: bool = SmartDashboard.getBoolean(label, defaultValue)
     SmartDashboard.putBoolean(label, value)
     return value