class Drive(Subsystem):
def __init__(self):
super().__init__('Drive')
# These can be used individually, but you should use RobotDrive to controll these
self.LB = Motor(ids.LB_motor)
self.LF = Motor(ids.LF_motor)
self.RB = Motor(ids.RB_motor)
self.RF = Motor(ids.RF_motor)
self.drive_train = RobotDrive(self.LF, self.LB, self.RF, self.RB)
self.drive_train.setInvertedMotor(0, True)
self.drive_train.setInvertedMotor(1, True)
self.drive_train.setExpiration(0.1)
## Uncomment to fail
#import time
#time.sleep(5)
# doesn't work bc HAL Resource error
#self.pcm0 = Solenoid(ids.PCM_pin0)
#self.pcm1 = Solenoid(ids.PCM_pin1)
def tank(self, L_speed, R_speed):
self.drive_train.tankDrive(L_speed, R_speed, squaredInputs=False)
def mecanum_cartesian(self, X_speed, Y_speed, R_speed=0, gyroHeading=0):
self.drive_train.mecanumDrive_Cartesian(X_speed, Y_speed, R_speed,
gyroHeading)
def mecanum_polar(self, _speed, _direction, R_speed=0):
self.drive_train.mecanumDrive_Polar(_speed, _direction, R_speed)
