Skip to content

Add non-PWM examples #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 15 commits into from
Sep 16, 2020
63 changes: 63 additions & 0 deletions examples/can-arcade-drive/robot.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
# ----------------------------------------------------------------------------
# Copyright (c) 2017-2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
# ----------------------------------------------------------------------------

import rev
import wpilib
from wpilib.drive import DifferentialDrive


class Robot(wpilib.TimedRobot):
def robotInit(self):
# SPARK MAX controllers are intialized over CAN by constructing a
# CANSparkMax object
#
# The CAN ID, which can be configured using the SPARK MAX Client, is passed
# as the first parameter
#
# The motor type is passed as the second parameter.
# Motor type can either be:
# rev.MotorType.kBrushless
# rev.MotorType.kBrushed
#
# The example below initializes four brushless motors with CAN IDs
# 1, 2, 3, 4. Change these parameters to match your setup
self.leftLeadMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rightLeadMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.leftFollowMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.rightFollowMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)

# Passing in the lead motors into DifferentialDrive allows any
# commmands sent to the lead motors to be sent to the follower motors.
self.driveTrain = DifferentialDrive(self.leftLeadMotor, self.rightLeadMotor)
self.joystick = wpilib.Joystick(0)

# The RestoreFactoryDefaults method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.leftLeadMotor.restoreFactoryDefaults()
self.rightLeadMotor.restoreFactoryDefaults()
self.leftFollowMotor.restoreFactoryDefaults()
self.rightFollowMotor.restoreFactoryDefaults()

# In CAN mode, one SPARK MAX can be configured to follow another. This
# is done by calling the follow() method on the SPARK MAX you want to
# configure as a follower, and by passing as a parameter the SPARK MAX
# you want to configure as a leader.
#
# This is shown in the example below, where one motor on each side of
# our drive train is configured to follow a lead motor.
self.leftFollowMotor.follow(self.leftLeadMotor)
self.rightFollowMotor.follow(self.rightLeadMotor)

def teleopPeriodic(self):
# Drive with arcade style
self.driveTrain.arcadeDrive(-self.joystick.getY(), self.joystick.getX())


if __name__ == "__main__":
wpilib.run(Robot)
48 changes: 48 additions & 0 deletions examples/can-tank-drive/robot.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
# ----------------------------------------------------------------------------
# Copyright (c) 2017-2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
# ----------------------------------------------------------------------------

import rev
import wpilib
from wpilib.drive import DifferentialDrive


class Robot(wpilib.TimedRobot):
def robotInit(self):
# SPARK MAX controllers are intialized over CAN by constructing a
# CANSparkMax object
#
# The CAN ID, which can be configured using the SPARK MAX Client, is passed
# as the first parameter
#
# The motor type is passed as the second parameter.
# Motor type can either be:
# rev.MotorType.kBrushless
# rev.MotorType.kBrushed
#
# The example below initializes two brushless motors with CAN IDs
# 1 and 2. Change these parameters to match your setup
self.leftMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rightMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)

# The RestoreFactoryDefaults method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.leftMotor.restoreFactoryDefaults()
self.rightMotor.restoreFactoryDefaults()

self.driveTrain = DifferentialDrive(self.leftMotor, self.rightMotor)
self.l_stick = wpilib.Joystick(0)
self.r_stick = wpilib.Joystick(1)

def teleopPeriodic(self):
# Create tank drive
self.driveTrain.tankDrive(self.l_stick.getY(), self.r_stick.getY())


if __name__ == "__main__":
wpilib.run(Robot)
61 changes: 61 additions & 0 deletions examples/get-set-params/robot.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
# ----------------------------------------------------------------------------
# Copyright (c) 2017-2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
# ----------------------------------------------------------------------------

import rev
import wpilib


class Robot(wpilib.TimedRobot):
def robotInit(self):
# Create motor
self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)

self.joystick = wpilib.Joystick(0)

# The restoreFactoryDefaults method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.motor.restoreFactoryDefaults()

# Parameters can be set by calling the appropriate set() method on the
# CANSparkMax object whose properties you want to change
#
# Set methods will return one of three CANError values which will let
# you know if the parameter was successfully set:
# CANError.kOk
# CANError.kError
# CANError.kTimeout
if self.motor.setIdleMode(rev.IdleMode.kCoast) is not rev.CANError.kOK:
wpilib.SmartDashboard.putString("Idle Mode", "Error")

# Similarly, parameters will have a get() method which allows you to
# retrieve their values from the controller
if self.motor.getIdleMode() is rev.IdleMode.kCoast:
wpilib.SmartDashboard.putString("Idle Mode", "Coast")
else:
wpilib.SmartDashboard.putString("Idle Mode", "Brake")

# Set ramp rate to 0
if self.motor.setOpenLoopRampRate(0) is not rev.CANError.kOK:
wpilib.SmartDashboard.putString("Ramp Rate", "Error")

# Read back ramp value
wpilib.SmartDashboard.putString("Ramp Rate", self.motor.getOpenLoopRampRate())

def teleopPeriodic(self):
# Pair motor and the joystick's Y Axis
self.motor.set(self.joystick.getY())

# Put Voltage, Temperature, and Motor Output onto SmartDashboard
wpilib.SmartDashboard.putNumber("Voltage", self.motor.getBusVoltage())
wpilib.SmartDashboard.putNumber("Temperature", self.motor.getMotorTemperature())
wpilib.SmartDashboard.putNumber("Output", self.motor.getAppliedOutput())


if __name__ == "__main__":
wpilib.run(Robot)
76 changes: 76 additions & 0 deletions examples/limit-switch/robot.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
# ----------------------------------------------------------------------------
# Copyright (c) 2017-2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
# ----------------------------------------------------------------------------

import rev
import wpilib

# Before Running:
# Open Shuffleboard, select File->Load Layout and select the
# shuffleboard.json that is in the root directory of this example


class Robot(wpilib.TimedRobot):
def robotInit(self):
# Create motor
self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)

self.joystick = wpilib.Joystick(0)

# A CANDigitalInput object is constructed using the
# GetForwardLimitSwitch() or
# GetReverseLimitSwitch() method on an existing CANSparkMax object,
# depending on which direction you would like to limit
#
# Limit switches can be configured to one of two polarities:
# rev.CANDigitalInput.LimitSwitchPolarity.kNormallyOpen
# rev.CANDigitalInput.LimitSwitchPolarity.kNormallyClosed
self.forwardLimit = self.motor.getForwardLimitSwitch(
rev.LimitSwitchPolarity.kNormallyClosed
)
self.reverseLimit = self.motor.getReverseLimitSwitch(
rev.LimitSwitchPolarity.kNormallyClosed
)

self.forwardLimit.enableLimitSwitch(False)
self.reverseLimit.enableLimitSwitch(False)

wpilib.SmartDashboard.putBoolean(
"Forward Limit Enabled", self.forwardLimit.isLimitSwitchEnabled()
)
wpilib.SmartDashboard.putBoolean(
"Reverse Limit Enabled", self.forwardLimit.isLimitSwitchEnabled()
)

def teleopPeriodic(self):
# Pair motor and the joystick's Y Axis
self.motor.set(self.joystick.getY())

# enable/disable limit switches based on value read from SmartDashboard
self.forwardLimit.enableLimitSwitch(
wpilib.SmartDashboard.getBoolean("Forward Limit Enabled", False)
)
self.reverseLimit.enableLimitSwitch(
wpilib.SmartDashboard.getBoolean("Reverse Limit Enabled", False)
)

# The get() method can be used on a CANDigitalInput object to read the
# state of the switch.
#
# In this example, the polarity of the switches are set to normally
# closed. In this case, get() will return true if the switch is
# pressed. It will also return true if you do not have a switch
# connected. get() will return false when the switch is released.
wpilib.SmartDashboard.putBoolean(
"Forward Limit Switch", self.forwardLimit.get()
)
wpilib.SmartDashboard.putBoolean(
"Reverse Limit Switch", self.reverseLimit.get()
)


if __name__ == "__main__":
wpilib.run(Robot)
72 changes: 72 additions & 0 deletions examples/limit-switch/shuffleboard.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
{
"tabPane": [
{
"title": "SmartDashboard",
"autoPopulate": true,
"autoPopulatePrefix": "SmartDashboard/",
"widgetPane": {
"gridSize": 128.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"tiles": {
"0,0": {
"size": [
2,
3
],
"content": {
"_type": "List Layout",
"_title": "Limit Switch",
"Layout/Label position": "BOTTOM",
"_children": [
{
"_type": "Toggle Switch",
"_source0": "network_table:///SmartDashboard/Forward Limit Enabled",
"_title": "Forward Limit Enabled"
},
{
"_type": "Toggle Switch",
"_source0": "network_table:///SmartDashboard/Reverse Limit Enabled",
"_title": "Reverse Limit Enabled"
},
{
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/Forward Limit Switch",
"_title": "Forward Limit Switch",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
},
{
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/Reverse Limit Switch",
"_title": "Reverse Limit Switch",
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
]
}
}
}
}
},
{
"title": "LiveWindow",
"autoPopulate": true,
"autoPopulatePrefix": "LiveWindow/",
"widgetPane": {
"gridSize": 128.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"tiles": {}
}
}
],
"windowGeometry": {
"x": -1640.0,
"y": 54.0,
"width": 1540.0,
"height": 832.0
}
}
Loading