def test_pidcontroller_calculate_rate7(input, setpoint, expected_error,
                                       expected_output):
    pid = PIDController(0.1, 0, 0.075)
    pid.enableContinuousInput(-180.0, 180.0)
    pid.setIntegratorRange(-1, 1)
    pid.setSetpoint(setpoint)

    out = pid.calculate(input)

    assert pid.getPositionError() == pytest.approx(expected_error, 0.01)
    # assert out == pytest.approx(expected_output, 0.01)
    assert out != 0
def test_pidcontroller_calculate_displacement3(d, source1, source2, output1,
                                               output2, output3):
    pid = PIDController(0, 0, d)

    # D is change in error coeff for kDisplacement
    pid.enableContinuousInput(0, 100.0)
    pid.setIntegratorRange(-1, 1)
    pid.setSetpoint(50.0)

    out = pid.calculate(source1)
    # assert out == pytest.approx(output1, 0.01)
    assert out > 0

    out = pid.calculate(source1)
    assert out == pytest.approx(output2, 0.01)

    out = pid.calculate(source2)
    # assert out == pytest.approx(output3, 0.01)
    assert out < 0
def test_pidcontroller_calculate_displacement1(p, source1, source2, output1,
                                               output2):
    pid = PIDController(p, 0, 0)
    # P is proportional error coeff for kDisplacement
    pid.enableContinuousInput(0, 100.0)
    pid.setIntegratorRange(-1, 1)
    pid.setSetpoint(50.0)

    out = pid.calculate(source1)
    # assert out == pytest.approx(output1, 0.01)
    assert out > 0

    out = pid.calculate(source1)
    # assert out == pytest.approx(output1, 0.01)
    assert out > 0

    out = pid.calculate(source2)
    # assert out == pytest.approx(output2, 0.01)
    assert out > 0
Exemplo n.º 4
0
class MyRobot(wpilib.TimedRobot):
	
	def robotInit(self):
   		self.kP = 0.03
		self.kI = 0.00
		self.kD = 0.00
		self.kF = 0.00
		
		self.exampleButton = 7
		
		#Sets up your PID controller with set constants
		self.controller = PIDController(self.kP, self.kI, self.kD)
		
		#set the range of values the Input will give
		#spark max are 42counts per revolution
		self.controller.enableContinuousInput(0, 42)
		
		#you can find information about the motor stuff in the documentation
		self.speed = self.controller.calculate(rev.CANSparkMax(4,rev.MotorType.kBrushless).getEncoder())

		
		
	def autonomousInit(self):
		pass
	def autonomousPeriodic(self):
		pass
	



		
	def teleopInit(self):
		pass
	def teleopPeriodic(self):
		#use this to control the motor: rev.CANSparkMax(4,rev.MotorType.kBrushless).set(0.5)
		if self.auxiliary1.getRawButton(self.exampleButton):
			self.controller.setpoint(84)
		else:
			rev.CANSparkMax(4,rev.MotorType.kBrushless).set(self.speed)
Exemplo n.º 5
0
class MyRobot(wpilib.TimedRobot):
	def robotInit(self):
		#I got fed up with how long 'frontLeft, frontRight, etc.' looked
		#Drive motors
		
		#turret caster thing
		self.vP = 0
		self.vI = 0
		self.vD = 0
		self.turretTurnController = PIDController(self.vP, self.vI, self.vD)
		turretTurnController.enableContinuousInput(0, 270)
		
		#controlling the angle of the hood
		self.hoodServo = #find something to address the servo
		
		
		#spinner velocity control
		self.sP = 0
		self.sI = 0
		self.sD = 0
		self.turretVelocityController = PIDController(self.sp,self.sI,self.sD)
		self.turretVelocityController.enableContinuousInput(200,6000)
		
		
		self.turretMotor = rev.CANSparkMax(0, rev.MotorType.kBrushless)
		self.idealAngle= 30
		#metric
		#Confirmation on velocity bounds: False
		self.maxVelocity = 5000*0.0762
		self.minVelocity = 2000*0.0762
		
		self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
		self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
		self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless)
		self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless)
		self.driveMotors = (self.flDriveMotor, self.frDriveMotor, self.rlDriveMotor, self.rrDriveMotor)

		#Turn motors
		self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
		self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
		self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
		self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless)
		self.turnMotors = (self.flTurnMotor, self.frTurnMotor, self.rlTurnMotor, self.rrTurnMotor)

		#Turn encoders
		self.flTurnEncoder = self.flTurnMotor.getEncoder()
		self.frTurnEncoder = self.frTurnMotor.getEncoder()
		self.rlTurnEncoder = self.rlTurnMotor.getEncoder()
		self.rrTurnEncoder = self.rrTurnMotor.getEncoder()
		self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder, self.rlTurnEncoder, self.rrTurnEncoder)
		for encoder in self.turnEncoders:
			encoder.setPositionConversionFactor(20) #makes the encoder output in degrees

		'''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If 
		we use the build-in, the following will be an issue and a potential solution.
		
		Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value.
		I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what
		the absolute encoder says it is. Something like
		for index, encoder in enumerate(self.turnEncoders):
			encoder.setPosition(self.absoluteEncoders[index]%360)
		because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although
		the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently
		out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like
		for index, encoder in enumerate(self.turnEncoders):
			encoder.setPosition(self.absoluteEncoders[index]%360)
			self.turnControllers[index].setReference(0, rev.controlType.kPosition)
		that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0.
		Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions
		to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only
		able to reset the wheel positions by hand'''

		# PID coefficients
		self.kP = .0039
		self.kI = 0
		self.kD = 2.0e-6
		self.PIDTolerance = 1.0

		#PID controllers for the turn motors
		self.flTurnController = PIDController(self.kP, self.kI, self.kD)
		self.frTurnController = PIDController(self.kP, self.kI, self.kD)
		self.rlTurnController = PIDController(self.kP, self.kI, self.kD)
		self.rrTurnController = PIDController(self.kP, self.kI, self.kD)
		self.turnControllers = (self.flTurnController, self.frTurnController, self.rlTurnController, self.rrTurnController)
		for controller in self.turnControllers:
			controller.setTolerance(self.PIDTolerance)
			controller.enableContinuousInput(0, 360)
		
		self.joystick = wpilib.Joystick(0)
		self.joystickDeadband = .05
		self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well
		
		self.robotLength = 10.0
		self.robotWidth = 10.0
	def encoderBoundedPosition(self, encoder):
		#I don't know if there's a set continuous for encoders, but it's easy enough to write
		position = encoder.getPosition()
		position %= 360
		if position < 0:
			position += 360
		return position
	def turnSpeedCalculator(self, i):
		speed = self.turnControllers[i].calculate(self.encoderBoundedPosition(self.turnEncoders[i]))
		if abs(speed) > 1:
			speed /= abs(speed)
		return speed
	def stopDriveMotors(self):
		for motor in self.driveMotors:
			motor.set(0)
		for motor in self.turnMotors:
			motor.set(0)
	def swerveMath(self, x, y, z):
		r = math.hypot(self.robotLength, self.robotWidth)
		
		a = x - z*(self.robotLength/r)
		b = x + z*(self.robotLength/r)
		c = y - z*(self.robotWidth/r)
		d = y + z*(self.robotWidth/r)
		
		flSpeed = math.hypot(b, c)
		frSpeed = math.hypot(b, d)
		rlSpeed = math.hypot(a, c)
		rrSpeed = math.hypot(a, d)
		
		maxSpeed = max(flSpeed, frSpeed, rlSpeed, rrSpeed)
		if maxSpeed > 1: #this way speed proportions are kept the same
			flSpeed /= maxSpeed
			frSpeed /= maxSpeed
			rlSpeed /= maxSpeed
			rrSpeed /= maxSpeed
		flAngle = math.degrees(math.atan2(b, c)) #works for all 4 quadrants
		frAngle = math.degrees(math.atan2(b, d))
		rlAngle = math.degrees(math.atan2(a, c))
		rrAngle = math.degrees(math.atan2(a, d))
		
		return (flSpeed, flAngle, frSpeed, frAngle, 
			rlSpeed, rlAngle, rrSpeed, rrAngle)
	def swerveDrive(self, x, y, z):
		speeds = self.swerveMath(x, y, z)
		if max(abs(x), abs(y), abs(z)) == 0:
			self.stopDriveMotors()
		else:
			for i in range(4):
			#checking whether to go to angle and drive forward or go to other side and drive backward
				position = self.encoderBoundedPosition(self.turnEncoders[i])
				goal = speeds[2*i+1]
				difference = abs(position - goal)
				if difference < 90 or difference > 270:
					self.turnControllers[i].setSetpoint(goal)
					self.driveMotors[i].set(speeds[2*i])
				else:
					if goal < 180:
						self.turnControllers[i].setSetpoint(goal + 180)
						self.driveMotors[i].set(-speeds[2*i])
					else:
						self.turnControllers[i].setSetpoint(goal - 180)
						self.driveMotors[i].set(-speeds[2*i])
				self.turnMotors[i].set(self.turnSpeedCalculator(i))
		
		'''I've been debating adding some checks to try and make this more efficient, but efficiency isn't
		super important right now and, more importantly, I don't really know python so anything I can think
		of would save minimal time (or possibly even make it take longer).'''
	def brakeMode(self):
		for motor in self.driveMotors:
			motor.setIdleMode(rev.IdleMode.kBrake)
		for motor in self.turnMotors:
			motor.setIdleMode(rev.IdleMode.kBrake)
	def coastMode(self):
		for motor in self.driveMotors:
			motor.setIdleMode(rev.IdleMode.kCoast)
		for motor in self.turnMotors:
			motor.setIdleMode(rev.IdleMode.kCoast)
	def checkDeadband(self, axis):
		if abs(axis) < self.joystickDeadband:
			axis = 0
		return axis
		
	def turretShoot(self): 
		#the turret must first align with the plane of the center of the target
		
		turretTurnController.setSetpoint(0)
		
		turretOutput = self.turretTurnController.calculate(Yaw)
		self.turretMotor.set(turretOutput)
		height= self.dx*tan(self.yaw)
		
		
		vDesired= #(insert Liam math here
		if (vDesired > self.maxVelocity) or (vDesired < self.minVelocity):
			desiredAngle = #insert more Liam math here
			self.hoodServo.set(self.idealAngle)
			
		else:
			#velocity control
			self.hoodServo.set(self.idealAngle)
			self.turretVelocityController.setSetpoint(vDesired)
			outputV = self.turretVelocityController.calculate(self.turretMotor.getVelocity)
			self.turretMotor.set(outputV)
		
	def autonomousInit(self):
		self.brakeMode()
		'''We want the motors in brake mode while we are actually using them, which would be anytime
		during auto or teleop. However, we want them in coast mode while disabled so that people can
		easily spin/adjust the wheels as needed. Not super important, but also not hard to add'''
	def autonomousPeriodic(self):
		pass
	def teleopInit(self):
		self.brakeMode()
	def teleopPeriodic(self):
		x = self.checkDeadband(self.joystick.getX())
		y = self.checkDeadband(self.joystick.getY())
		z = self.checkDeadband(self.joystick.getZ())
		self.swerveDrive(x, y, z)
	def disabledInit(self):
		self.coastMode()
Exemplo n.º 6
0
class MyRobot(wpilib.TimedRobot):
	def robotInit(self):
		self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
		self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
		self.turnEncoder = self.turnMotor.getEncoder()
		self.turnEncoder.setPositionConversionFactor(20)

		# PID coefficients
		self.kP = .0039
		self.kI = 2e-6
		self.kD = 0

		#PID controllers for the turn motors
		self.turnController = PIDController(self.kP, self.kI, self.kD)
		self.PIDTolerance = 1.0
		self.turnController.setTolerance(self.PIDTolerance)
		self.turnController.enableContinuousInput(0, 360)
		
		self.joystick = wpilib.Joystick(0)
		self.joystickDeadband = .1
		self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well
		
		self.robotLength = 10.0
		self.robotWidth = 10.0
		wpilib.CameraServer.launch()
	def encoderBoundedPosition(self, encoder):
		#I don't know if there's a set continuous for encoders, but it's easy enough to write
		position = encoder.getPosition()
		position %= 360
		if position < 0:
			position += 360
		return position
	def turnSpeedCalculator(self):
		speed = self.turnController.calculate(self.encoderBoundedPosition(self.turnEncoder))
		if abs(speed) > 1:
			speed /= abs(speed)
		return speed
	def swerveMath(self, x, y):
		driveSpeed = math.hypot(x, y)
		driveSpeed = min(driveSpeed, 1)
		angle = math.degrees(math.atan2(x, y)) #works for all 4 quadrants
		return (driveSpeed, angle)
	def swerveDrive(self, x, y):
		speeds = self.swerveMath(x, y)
		if max(abs(x), abs(y)) == 0:
			self.driveMotor.set(0)
			self.turnMotor.set(0)
		else:
			#checking whether to go to angle and drive forward or go to other side and drive backward
			position = self.encoderBoundedPosition(self.turnEncoder)
			goal = speeds[1]
			difference = abs(position - goal)
			if difference < 90 or difference > 270:
				self.turnController.setSetpoint(goal)
				self.driveMotor.set(speeds[0])
			else:
				if goal < 180:
					self.turnController.setSetpoint(goal + 180)
					self.driveMotor.set(-speeds[0])
				else:
					self.turnController.setSetpoint(goal - 180)
					self.driveMotor.set(-speeds[0])
		self.turnMotor.set(self.turnSpeedCalculator())
		
		'''I've been debating adding some checks to try and make this more efficient, but efficiency isn't
		super important right now and, more importantly, I don't really know python so anything I can think
		of would save minimal time (or possibly even make it take longer).'''
	def brakeMode(self):
		self.driveMotor.setIdleMode(rev.IdleMode.kBrake)
		self.turnMotor.setIdleMode(rev.IdleMode.kBrake)
	def coastMode(self):
		self.driveMotor.setIdleMode(rev.IdleMode.kCoast)
		self.turnMotor.setIdleMode(rev.IdleMode.kCoast)
	def checkDeadband(self, axis):
		if axis < self.joystickDeadband and axis > -self.joystickDeadband:
			axis = 0
		return axis
	def autonomousInit(self):
		self.brakeMode()
		'''We want the motors in brake mode while we are actually using them, which would be anytime
		during auto or teleop. However, we want them in coast mode while disabled so that people can
		easily spin/adjust the wheels as needed. Not super important, but also not hard to add'''
	def autonomousPeriodic(self):
		pass
	def teleopInit(self):
		self.brakeMode()
	def teleopPeriodic(self):
		if self.joystick.getRawButton(1):
			x = -.4*self.checkDeadband(self.joystick.getX())
			y = -.4*self.checkDeadband(self.joystick.getY())
		else:
			x = -1*self.checkDeadband(self.joystick.getX())
			y = -1*self.checkDeadband(self.joystick.getY())
		self.swerveDrive(x, y)
	def disabledInit(self):
		self.coastMode()
Exemplo n.º 7
0
class MyRobot(wpilib.TimedRobot):
	def robotInit(self):
		self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
		self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
		self.turnEncoder = self.turnMotor.getEncoder()
		self.turnEncoder.setPositionConversionFactor(20)

		# PID coefficients
		self.kP = 5e-5
		self.kI = 1e-6
		self.kD = 0
		self.kIz = 0
		self.PIDTolerance = 0

		#PID controllers for the turn motors
		self.turnController = PIDController(self.kP, self.kI, self.kD)
		self.turnController.setTolerance(self.PIDTolerance)
		self.turnController.enableContinuousInput(0, 360)
		
		self.joystick = wpilib.Joystick(0)
		self.joystickDeadband = .05
		self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well
		
		# Push PID Coefficients to SmartDashboard
		wpilib.SmartDashboard.putNumber("P Gain", self.kP)
		wpilib.SmartDashboard.putNumber("I Gain", self.kI)
		wpilib.SmartDashboard.putNumber("D Gain", self.kD)
		wpilib.SmartDashboard.putNumber("I Zone", self.kIz)
		wpilib.SmartDashboard.putNumber("Set Rotations", 0)
		wpilib.SmartDashboard.putNumber("Tolerance", self.PIDTolerance)
		wpilib.SmartDashboard.putBoolean("Manual Control", False)
		wpilib.SmartDashboard.putBoolean("Manual Speed", 0)
	def encoderBoundedPosition(self, encoder):
		#I don't know if there's a set continuous for encoders, but it's easy enough to write
		position = encoder.getPosition()
		position %= 360
		if position < 0:
			position += 360
		return position
	def brakeMode(self):
		self.driveMotor.setIdleMode(rev.IdleMode.kBrake)
		self.turnMotor.setIdleMode(rev.IdleMode.kBrake)
	def coastMode(self):
		self.driveMotor.setIdleMode(rev.IdleMode.kCoast)
		self.turnMotor.setIdleMode(rev.IdleMode.kCoast)
	def turnSpeedCalculator(self):
		speed = self.turnController.calculate(self.encoderBoundedPosition(self.turnEncoder))
		if abs(speed) > 1:
			speed /= abs(speed)
		return speed
	def autonomousInit(self):
		self.brakeMode()
	def autonomousPeriodic(self):
		pass
	def teleopInit(self):
		self.brakeMode()
	def teleopPeriodic(self):
		# Read data from SmartDashboard
		p = wpilib.SmartDashboard.getNumber("P Gain", self.kP)
		i = wpilib.SmartDashboard.getNumber("I Gain", self.kI)
		d = wpilib.SmartDashboard.getNumber("D Gain", self.kD)
		iz = wpilib.SmartDashboard.getNumber("I Zone", self.kIz)
		tolerance = wpilib.SmartDashboard.getNumber("Tolerance", self.PIDTolerance)
		test = wpilib.SmartDashboard.getBoolean("Manual Control", False)
		controlSpeed = wpilib.SmartDashboard.getNumber("Manual Speed", 0)

		# Update PIDController datapoints with the latest from SmartDashboard
		if p != self.kP:
			self.turnController.setP(p)
			self.kP = p
		if i != self.kI:
			self.turnController.setI(i)
			self.kI = i
		if d != self.kD:
			self.turnController.setD(d)
			self.kD = d
		if tolerance != self.PIDTolerance:
			self.turnController.setTolerance(tolerance)
			self.PIDTolerance = tolerance

		speed = self.turnSpeedCalculator()

		wpilib.SmartDashboard.putNumber("Process Variable", self.encoderBoundedPosition(self.turnEncoder))
		setpoint = wpilib.SmartDashboard.getNumber("Set Rotations", 0)
		self.turnController.setSetpoint(setpoint)
		wpilib.SmartDashboard.putNumber("Motor Input", speed)
		if test:
			self.turnMotor.set(controlSpeed)
		else:
			self.turnMotor.set(speed)
	def disabledInit(self):
		self.coastMode()
Exemplo n.º 8
0
def pid_controller():
    controller = PIDController(0.05, 0.0, 0.0)
    controller.enableContinuousInput(-input_range / 2, input_range / 2)
    yield controller
    controller.close()