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)
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)