1- # ----------------------------------------------------------------------------
2- # Copyright (c) 2017-2018 FIRST. All Rights Reserved.
3- # Open Source Software - may be modified and shared by FRC teams. The code
4- # must be accompanied by the FIRST BSD license file in the root directory of
5- # the project.
6- # ----------------------------------------------------------------------------
1+ #!/usr/bin/env python3
2+ #
3+ # Copyright (c) FIRST and other WPILib contributors.
4+ # Open Source Software; you can modify and/or share it under the terms of
5+ # the WPILib BSD license file in the root directory of this project.
6+ #
77
88import rev
99import wpilib
@@ -25,34 +25,60 @@ def robotInit(self):
2525 #
2626 # The example below initializes four brushless motors with CAN IDs
2727 # 1, 2, 3, 4. Change these parameters to match your setup
28- self .leftLeadMotor = rev .CANSparkMax (1 , rev .CANSparkMax .MotorType .kBrushless )
29- self .rightLeadMotor = rev .CANSparkMax (3 , rev .CANSparkMax .MotorType .kBrushless )
30- self .leftFollowMotor = rev .CANSparkMax (2 , rev .CANSparkMax .MotorType .kBrushless )
31- self .rightFollowMotor = rev .CANSparkMax (4 , rev .CANSparkMax .MotorType .kBrushless )
28+ self .leftLeadMotor = rev .SparkMax (1 , rev .SparkMax .MotorType .kBrushless )
29+ self .rightLeadMotor = rev .SparkMax (3 , rev .SparkMax .MotorType .kBrushless )
30+ self .leftFollowMotor = rev .SparkMax (2 , rev .SparkMax .MotorType .kBrushless )
31+ self .rightFollowMotor = rev .SparkMax (4 , rev .SparkMax .MotorType .kBrushless )
3232
3333 # Passing in the lead motors into DifferentialDrive allows any
3434 # commmands sent to the lead motors to be sent to the follower motors.
3535 self .driveTrain = DifferentialDrive (self .leftLeadMotor , self .rightLeadMotor )
3636 self .joystick = wpilib .Joystick (0 )
3737
38- # The RestoreFactoryDefaults method can be used to reset the
39- # configuration parameters in the SPARK MAX to their factory default
40- # state. If no argument is passed, these parameters will not persist
41- # between power cycles
42- self .leftLeadMotor .restoreFactoryDefaults ()
43- self .rightLeadMotor .restoreFactoryDefaults ()
44- self .leftFollowMotor .restoreFactoryDefaults ()
45- self .rightFollowMotor .restoreFactoryDefaults ()
46-
47- # In CAN mode, one SPARK MAX can be configured to follow another. This
48- # is done by calling the follow() method on the SPARK MAX you want to
49- # configure as a follower, and by passing as a parameter the SPARK MAX
50- # you want to configure as a leader.
38+ # Create new SPARK MAX configuration objects. These will store the
39+ # configuration parameters for the SPARK MAXes that we will set below.
40+ self .globalConfig = rev .SparkMaxConfig ()
41+ self .rightLeaderConfig = rev .SparkMaxConfig ()
42+ self .leftFollowerConfig = rev .SparkMaxConfig ()
43+ self .rightFollowerConfig = rev .SparkMaxConfig ()
44+
45+ # Apply the global config and invert since it is on the opposite side
46+ self .rightLeaderConfig .apply (self .globalConfig ).inverted (True )
47+
48+ # Apply the global config and set the leader SPARK for follower mode
49+ self .leftFollowerConfig .apply (self .globalConfig ).follow (self .leftLeadMotor )
50+
51+ # Apply the global config and set the leader SPARK for follower mode
52+ self .rightFollowerConfig .apply (self .globalConfig ).follow (self .rightLeadMotor )
53+
54+ # Apply the configuration to the SPARKs.
55+ #
56+ # kResetSafeParameters is used to get the SPARK MAX to a known state. This
57+ # is useful in case the SPARK MAX is replaced.
5158 #
52- # This is shown in the example below, where one motor on each side of
53- # our drive train is configured to follow a lead motor.
54- self .leftFollowMotor .follow (self .leftLeadMotor )
55- self .rightFollowMotor .follow (self .rightLeadMotor )
59+ # kPersistParameters is used to ensure the configuration is not lost when
60+ # the SPARK MAX loses power. This is useful for power cycles that may occur
61+ # mid-operation.
62+ self .leftLeadMotor .configure (
63+ self .globalConfig ,
64+ rev ._rev .SparkBase .ResetMode .kResetSafeParameters ,
65+ rev ._rev .SparkBase .PersistMode .kPersistParameters ,
66+ )
67+ self .leftFollowMotor .configure (
68+ self .leftFollowerConfig ,
69+ rev ._rev .SparkBase .ResetMode .kResetSafeParameters ,
70+ rev ._rev .SparkBase .PersistMode .kPersistParameters ,
71+ )
72+ self .rightLeadMotor .configure (
73+ self .rightLeaderConfig ,
74+ rev ._rev .SparkBase .ResetMode .kResetSafeParameters ,
75+ rev ._rev .SparkBase .PersistMode .kPersistParameters ,
76+ )
77+ self .rightFollowMotor .configure (
78+ self .rightFollowerConfig ,
79+ rev ._rev .SparkBase .ResetMode .kResetSafeParameters ,
80+ rev ._rev .SparkBase .PersistMode .kPersistParameters ,
81+ )
5682
5783 def teleopPeriodic (self ):
5884 # Drive with arcade style
0 commit comments