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)
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 mecanumDrive_Cartesian(self, x, y, rotation, gyroAngle):
"""Drive method for Mecanum wheeled robots.
A method for driving with Mecanum wheeled robots. There are 4 wheels
on the robot, arranged so that the front and back wheels are toed in
45 degrees. When looking at the wheels from the top, the roller
axles should form an X across the robot.
This is designed to be directly driven by joystick axes.
:param x: The speed that the robot should drive in the X direction.
[-1.0..1.0]
:param y: The speed that the robot should drive in the Y direction.
This input is inverted to match the forward == -1.0 that
joysticks produce. [-1.0..1.0]
:param rotation: The rate of rotation for the robot that is
completely independent of the translation. [-1.0..1.0]
:param gyroAngle: The current angle reading from the gyro. Use this
to implement field-oriented controls.
"""
if not wpilib.RobotDrive.kMecanumCartesian_Reported:
hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive,
self.getNumMotors(),
hal.HALUsageReporting.kRobotDrive_MecanumCartesian)
RobotDrive.kMecanumCartesian_Reported = True
xIn = x
yIn = y
# Negate y for the joystick.
yIn = -yIn
# Compenstate for gyro angle.
xIn, yIn = RobotDrive.rotateVector(xIn, yIn, gyroAngle)
wheelSpeeds = [0]*self.kMaxNumberOfMotors
wheelSpeeds[self.MotorType.kFrontLeft] = xIn + yIn + rotation
wheelSpeeds[self.MotorType.kFrontRight] = -xIn + yIn - rotation
wheelSpeeds[self.MotorType.kRearLeft] = -xIn + yIn + ( rotation * self.weight_multiplier )
wheelSpeeds[self.MotorType.kRearRight] = xIn + yIn - ( rotation * self.weight_multiplier )
RobotDrive.normalize(wheelSpeeds)
self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.invertedMotors[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup)
self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.invertedMotors[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup)
self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.invertedMotors[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup)
self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.invertedMotors[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup)
if self.syncGroup != 0:
wpilib.CANJaguar.updateSyncGroup(self.syncGroup)
self.feed()
def __init__(self):
super().__init__()
"""
Motor objects init
Reason for recall is because MagicRobot is looking for the CANTalon
Object instance before init
"""
self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor)
self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor)
self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor)
self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor)
self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.navx = AHRS(SPI.Port.kMXP)
self.left_motor_one.enableBrakeMode(True)
self.left_motor_two.enableBrakeMode(True)
self.right_motor_one.enableBrakeMode(True)
self.right_motor_two.enableBrakeMode(True)
self.base = RobotDrive(self.left_motor_one, self.left_motor_two,
self.right_motor_one, self.right_motor_two)
self.dpp = sensor_map.wheel_diameter * math.pi / 360
self.left_encoder.setDistancePerPulse(self.dpp)
self.right_encoder.setDistancePerPulse(self.dpp)
self.left_encoder.setSamplesToAverage(sensor_map.samples_average)
self.right_encoder.setSamplesToAverage(sensor_map.samples_average)
self.left_encoder.setMinRate(sensor_map.min_enc_rate)
self.right_encoder.setMinRate(sensor_map.min_enc_rate)
self.auto_pid_out = AutoPIDOut()
self.pid_d_controller = PIDController(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F,
self.navx, self.auto_pid_out, 0.005)
self.type_flag = ("DRIVE", "TURN")
self.current_flag = self.type_flag[0]
self.auto_pid_out.drive_base = self
self.auto_pid_out.current_flag = self.current_flag
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
class DriveTrain(Subsystem):
"""DriveTrain: is the subsystem responsible for motors and
devices associated with driving subystem.
As a subsystem, we represent the single point of contact
for all drivetrain-related controls. Specifically, commands
that manipulate the drivetrain should 'require' the singleton
instance (via require(robot.driveTrain)). Unless overridden,
our default command, JoystickDriver, is the means by which
driving occurs.
"""
k_minThrottleScale = 0.5
k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally
k_maxDriveSpeed = 150.0 # ~20 ft/sec
k_maxTurnSpeed = 40.0 # ~3-4 ft/sec
k_fastTurn = -1
k_mediumTurn = -.72
k_slowTurn = -.55
k_quadTicksPerWheelRev = 9830
k_wheelDiameterInInches = 14.0
k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi
k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches
k_turnKp = .1
k_turnKi = 0
k_turnKd = .3
k_turnKf = .001
k_ControlModeSpeed = 0
k_ControlModeVBus = 1
k_ControlModeDisabled = 2
class _turnHelper(PIDOutput):
"""a private helper class for PIDController-based
imu-guided turning.
"""
def __init__(self, driveTrain):
super().__init__()
self.driveTrain = driveTrain
def pidWrite(self, output):
self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed)
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
def initForCommand(self, controlMode):
self.leftMasterMotor.setEncPosition(0) # async call
self.rightMasterMotor.setEncPosition(0)
self.robotDrive.stopMotor()
self.robotDrive.setMaxOutput(self.maxSpeed)
if controlMode == self.k_ControlModeSpeed:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
elif controlMode == self.k_ControlModeVBus:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
elif controlMode == self.k_ControlModeDisabled:
# unverified codepath
self.leftMasterMotor.disableControl()
self.rightMasterMotor.disableControl()
else:
self.robot.error("Unexpected control mode")
self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" %
(self.leftMasterMotor.getControlMode(),
self.rightMasterMotor.getControlMode()))
def initDefaultCommand(self):
# control modes are integers values:
# 0 percentvbux
# 1 position
# 2 velocity
self.setDefaultCommand(JoystickDriver(self.robot))
self.robotDrive.stopMotor();
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
def stop(self):
self.robotDrive.stopMotor()
def joystickDrive(self, jsY, jsX, throttle):
""" joystickDrive - called by JoystickDriver command. Inputs
are always on the range [-1, 1]... These values can be scaled
for a better "feel", but should always be in a subset of this
range.
"""
if self.robot.isAutonomous or \
(math.fabs(jx) < 0.075 and math.fabs(jy) < .075):
# joystick dead zone or auto (where we've been scheduled via
# default command machinery)
self.robotDrive.stopMotor()
else:
st = self.scaleThrottle(throttle)
self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st)
def drive(self, outputmag, curve):
""" drive - used by drivestraight command..
"""
self.robotDrive.drive(outputmag, curve)
def driveStraight(self, speed):
"""driveStraight: invoked from AutoDriveStraight..
"""
# NB: maxOuput isn't applied via set so
# speed is supposedly measured in rpm but this depends on our
# initialization establishing encoding ticks per revolution.
# This is approximate so we rely on the observed values above.
# (DEFAULT_SPEED_MAX_OUTPUT)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = speed / self.maxSpeed
rotateValue = 0
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def startAutoTurn(self, degrees):
self.robot.info("start autoturn from %d to %d" %
(self.getHeading(), degrees))
self.turnPID.reset()
self.turnPID.setSetpoint(degrees)
self.turnPID.enable()
def endAutoTurn(self):
if self.turnPID.isEnabled():
self.turnPID.disable()
def isAutoTurning(self):
return self.turnPID.isEnabled()
def isAutoTurnFinished(self):
return self.turnPID.onTarget()
def turn(self, speed):
"""turn takes a speed, not an angle...
A negative speed is interpretted as turn left.
Note that we bypass application of setMaxOutput Which
only applies to calls to robotDrive.
"""
# In order to turn left, we want the right wheels to go
# forward and left wheels to go backward (cf: tankDrive)
# Oddly, our right master motor is reversed, so we compensate here.
# speed < 0: turn left: rightmotor(negative) (forward),
# leftmotor(negative) (backward)
# speed > 0: turn right: rightmotor(positive) (backward)
# leftmotor(positive) (forward)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = 0
rotateValue = speed / self.maxSpeed
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def trackVision(self):
"""presumed called by either autonomous or teleop commands to
allow the vision subsystem to guide the robot
"""
if self.visionState.DriveLockedOnTarget:
self.stop()
else:
if self.isAutoTurning():
if self.isAutoTurnFinished():
self.endAutoTurn()
self.visionState.DriveLockedOnTarget = True
else:
# setup for an auto-turn
h = self.getCurrentHeading()
tg = self.getTargetHeading(h)
self.startAutoTurn(tg)
def getCurrentHeading(self):
""" getCurrentHeading returns a value between -180 and 180
"""
return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi
def scaleThrottle(self, rawThrottle):
""" scaleThrottle:
returns a scaled value between MIN_THROTTLE_SCALE and 1.0
MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation
Scale the joystick values by throttle before passing to the driveTrain
+1=bottom position; -1=top position
"""
# Throttle in the range of -1 to 1. We would like to change that to a
# range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle
# value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1))
# Then, add 1 to make the range 0-2 rather than -1 to +1
# Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale)
# Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1
#
# Check the results are in the range of k_minThrottleScale to 1, and clip
# it in case the math went horribly wrong.
result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale
if result < self.k_minThrottleScale:
# Somehow our math was wrong. Our value was too low, so force it to the minimum
result = self.k_mintThrottleScale
elif result > 1:
# Somehow our math was wrong. Our value was too high, so force it to the maximum
result = 1.0
return result
def modifyTurnSpeed(self, speedUp):
if speedUp:
self.turnMultiplier = self.k_mediumTurn
else:
self.turnMultiplier = self.k_slowTurn
def inchesToTicks(self, inches):
return int(self.k_quadTicksPerInch * inches)
def destinationReached(self, distance):
return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \
math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
class DriveBase:
left_motor_one = CANTalon
left_motor_two = CANTalon
right_motor_one = CANTalon
right_motor_two = CANTalon
left_encoder = Encoder
right_encoder = Encoder
navx = AHRS
def __init__(self):
super().__init__()
"""
Motor objects init
Reason for recall is because MagicRobot is looking for the CANTalon
Object instance before init
"""
self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor)
self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor)
self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor)
self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor)
self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.navx = AHRS(SPI.Port.kMXP)
self.left_motor_one.enableBrakeMode(True)
self.left_motor_two.enableBrakeMode(True)
self.right_motor_one.enableBrakeMode(True)
self.right_motor_two.enableBrakeMode(True)
self.base = RobotDrive(self.left_motor_one, self.left_motor_two,
self.right_motor_one, self.right_motor_two)
self.dpp = sensor_map.wheel_diameter * math.pi / 360
self.left_encoder.setDistancePerPulse(self.dpp)
self.right_encoder.setDistancePerPulse(self.dpp)
self.left_encoder.setSamplesToAverage(sensor_map.samples_average)
self.right_encoder.setSamplesToAverage(sensor_map.samples_average)
self.left_encoder.setMinRate(sensor_map.min_enc_rate)
self.right_encoder.setMinRate(sensor_map.min_enc_rate)
self.auto_pid_out = AutoPIDOut()
self.pid_d_controller = PIDController(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F,
self.navx, self.auto_pid_out, 0.005)
self.type_flag = ("DRIVE", "TURN")
self.current_flag = self.type_flag[0]
self.auto_pid_out.drive_base = self
self.auto_pid_out.current_flag = self.current_flag
def drive(self, left_power, right_power):
self.base.tankDrive(left_power, right_power)
def execute(self):
if int(self.base.getNumMotors()) < 3:
self.base.drive(0, 0)
def get_drive_distance(self):
return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance())
def rest_base(self):
self.left_encoder.reset()
self.right_encoder.reset()
def pid_drive(self, speed, distance, to_angle=None):
self.navx.isCalibrating()
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F)
self.pid_d_controller.setOutputRange(speed - distance, speed + distance)
if to_angle is None:
set_angle = self.navx.getYaw()
else:
set_angle = to_angle
self.pid_d_controller.setSetpoint(float(set_angle))
self.drive(speed + 0.03, speed)
self.pid_d_controller.enable()
self.current_flag = self.type_flag[0]
self.auto_pid_out.current_flag = self.current_flag
def pid_turn(self, angle):
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.turn_P,
sensor_map.turn_I,
sensor_map.turn_D,
sensor_map.turn_F)
self.pid_d_controller.setOutputRange(sensor_map.output_range_min,
sensor_map.output_range_max)
self.pid_d_controller.setSetpoint(float(angle))
self.pid_d_controller.enable()
self.current_flag = self.type_flag[1]
self.auto_pid_out.current_flag = self.current_flag
def stop_pid(self):
self.pid_d_controller.disable()
self.pid_d_controller.reset()
