class Car:
def __init__(self):
self.rear_motor = Motor(motor_type="rear_motor")
self.front_motor = Motor(motor_type="front_motor")
self.front_right_blinker = Blinker(7)
self.fron_left_blinker = Blinker(11)
self.range_sensor = RangeSensor()
self.camera = Camera()
def drive_forward(self):
self.rear_motor.forward(0.1)
def drive_backward(self):
self.rear_motor.backward(0.1)
def turn_left(self):
self.front_motor.forward(0.1)
def turn_right(self):
self.front_motor.backward(0.1)
def stop(self):
self.rear_motor.stop()
def __init__(self):
super().__init__("Arm")
self.limiter = DigitalInput(arm_stopper.dio)
self.final_extender_solenoid = SolenoidHandler(
*solenoids.final_armextender)
self.extender_solenoid = SolenoidHandler(*solenoids.armextender)
self.grabber_solenoid = SolenoidHandler(*solenoids.grabber)
self.rotator_motors = {}
self.rotator_motors["L"] = Motor(*arm_motors.L)
#self.rotator_motors["R"] = Motor(*arm_motors.R)
self.wench_motor = Motor(*arm_motors.wench)
self.rotator_encoders = {}
#self.rotator_encoders["L"] = Encoder(*arm_encoders.L)
self.rotator_encoders["R"] = Encoder(*arm_encoders.R)
#self.rotator_encoders["L"].setPIDSourceType(PIDController.PIDSourceType.kRate)
self.rotator_encoders["R"].setPIDSourceType(
PIDController.PIDSourceType.kDisplacement)
self.rotator_encoders["R"].setDistancePerPulse(arm_encoders.R_dpp)
self.last_rot_time = None
def __init__(self):
self.rear_motor = Motor(motor_type="rear_motor")
self.front_motor = Motor(motor_type="front_motor")
self.front_right_blinker = Blinker(7)
self.fron_left_blinker = Blinker(11)
self.range_sensor = RangeSensor()
self.camera = Camera()
def __init__(self):
super().__init__('Arm')
# extender_solenoid is the one that raises/lowers the arm
# grabber_solenoid opens/closes the arms.
self.extender_solenoid = SolenoidHandler(*solenoids.armextender)
self.grabber_solenoid = SolenoidHandler(*solenoids.grabber)
self.rotator_motors = {}
self.rotator_motors["L"] = Motor(*arm_motors.L)
self.rotator_motors["R"] = Motor(*arm_motors.R)
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)
def __init__(self, L0, L1, R0, R1):
super().__init__('Drive')
self.L0 = Motor(L0)
self.L1 = Motor(L1)
self.R0 = Motor(R0)
self.R1 = Motor(R1)
class Drive(Subsystem):
def __init__(self, L0, L1, R0, R1):
super().__init__('Drive')
self.L0 = Motor(L0)
self.L1 = Motor(L1)
self.R0 = Motor(R0)
self.R1 = Motor(R1)
def set(self, Lpow, Rpow):
self.L0.set(Lpow)
self.L1.set(Lpow)
self.R0.set(Rpow)
self.R1.set(Rpow)
def __init__(self):
super().__init__("TankDrive")
self.motors = {
"LF": Motor(*drive_motors.LF),
"LB": Motor(*drive_motors.LB),
"RF": Motor(*drive_motors.RF),
"RB": Motor(*drive_motors.RB)
}
self.encoders = {
"L": Encoder(*drive_encoders.L),
"R": Encoder(*drive_encoders.R)
}
self.encoders["L"].setPIDSourceType(PIDController.PIDSourceType.kRate)
self.encoders["R"].setPIDSourceType(PIDController.PIDSourceType.kRate)
# Now they are the same kind of encoder.
self.encoders["L"].setDistancePerPulse(drive_encoders.L_dpp)
self.encoders["R"].setDistancePerPulse(drive_encoders.R_dpp)
self.gearshift = SolenoidHandler(*solenoids.gearshift)
class Car:
def __init__(self):
self.rear_motor = Motor(motor_type="rear_motor")
self.front_motor = Motor(motor_type="front_motor")
self.range_sensor = RangeSensor()
self.camera = Camera()
def drive_forward(self):
self.rear_motor.forward(0.1)
def drive_backward(self):
self.rear_motor.backward(0.1)
def stop(self):
self.rear_motor.stop()
def __init__(self):
self.rear_motor = Motor(motor_type="rear_motor")
self.front_motor = Motor(motor_type="front_motor")
self.range_sensor = RangeSensor()
self.camera = Camera()
class OldArm(Subsystem):
"""
This should hold motors, and all things related to the arm movement
"""
def __init__(self):
super().__init__("Arm")
self.limiter = DigitalInput(arm_stopper.dio)
self.final_extender_solenoid = SolenoidHandler(
*solenoids.final_armextender)
self.extender_solenoid = SolenoidHandler(*solenoids.armextender)
self.grabber_solenoid = SolenoidHandler(*solenoids.grabber)
self.rotator_motors = {}
self.rotator_motors["L"] = Motor(*arm_motors.L)
#self.rotator_motors["R"] = Motor(*arm_motors.R)
self.wench_motor = Motor(*arm_motors.wench)
self.rotator_encoders = {}
#self.rotator_encoders["L"] = Encoder(*arm_encoders.L)
self.rotator_encoders["R"] = Encoder(*arm_encoders.R)
#self.rotator_encoders["L"].setPIDSourceType(PIDController.PIDSourceType.kRate)
self.rotator_encoders["R"].setPIDSourceType(
PIDController.PIDSourceType.kDisplacement)
self.rotator_encoders["R"].setDistancePerPulse(arm_encoders.R_dpp)
self.last_rot_time = None
def set_wench(self, power):
self.wench_motor.set(power)
def set_extender(self, status):
self.extender_solenoid.set(status)
def get_extender(self):
return self.extender_solenoid.get()
def set_final_extender(self, status):
self.final_extender_solenoid.set(status)
def get_grabber(self):
return self.grabber_solenoid.get()
def set_grabber(self, status):
print("SOLENOID PORTS: " + str(solenoids.grabber))
print("SETTING TO: " + str(status))
self.grabber_solenoid.set(status)
def get_limiter(self):
return self.limiter.get()
def get_arm_proportion(self):
r_ticks = []
for i in self.rotator_encoders.keys():
r_ticks += [self.rotator_encoders[i].getDistance()]
return sum(r_ticks) / len(r_ticks)
def set_rotator(self, amount, raw=True):
if self.get_limiter() != arm_stopper.default:
self.reset_enc()
prop = self.get_arm_proportion()
def envelope(x):
# x is range -1 to +1
p = prop
if x > 1.0:
x = 1.0
elif x < -1.0:
x = -1.0
if p > 1.0:
p = 1.0
elif p < 0:
p = 0
lower_lim = .12
upper_lim = .9
if p < lower_lim:
slope = .15 * p / lower_lim + .85
return slope * x
#elif p > upper_lim:
elif p > upper_lim and x > 0:
slope = .36 * (upper_lim - p) / (1.0 - upper_lim) + .64
return slope * x
else:
return x
prop = self.get_arm_proportion()
'''
if (prop > measures.ROBOT_ARM_RETRACT_RANGE[0] and prop < measures.ROBOT_ARM_RETRACT_RANGE[1] and self.get_extender()) or self.last_rot_time is not None:
let_move = False
closer_to_bottom = abs(prop - measures.ROBOT_ARM_RETRACT_RANGE[0]) < abs(prop - measures.ROBOT_ARM_RETRACT_RANGE[1])
if closer_to_bottom and amount < 0:
let_move = True
if not closer_to_bottom and amount > 0:
let_move = True
if not let_move:
amount = 0
if autoretract:
self.set_extender(False)
if self.last_rot_time is not None:
self.last_rot_time = time.time()
elif time.time() - self.last_rot_time >= measures.ROBOT_ARM_RETRACT_TIME:
self.last_rot_time = None
'''
if not raw:
amount = envelope(amount)
for rot_mot in self.rotator_motors:
self.rotator_motors[rot_mot].set(amount)
def reset_enc(self):
for k in self.rotator_encoders.keys():
self.rotator_encoders[k].reset()
def stop_rotator(self):
self.set_rotator(0.0)