nigs

robot.py

import util

try:
	import wpilib
except ImportError:
	from pyfrc import wpilib


class RobotGuy(wpilib.SimpleRobot):
	def __init__(self):
		super().__init__()

		self.controller = wpilib.Joystick(1)
		self.leftdrive = wpilib.Talon(1)
		self.rightdrive = wpilib.Talon(2)

		self.ds = wpilib.DriverStation.GetInstance()
		self.oldwheel = 0
		self.quickstopbuffer = 0

	def Disabled(self):
		while self.IsDisabled():
			wpilib.Wait(0.01)

	def Autonomous(self):
		self.GetWatchdog().SetEnabled(False)
		while self.IsAutonomous() and self.IsEnabled():
			wpilib.Wait(0.01)

	def OperatorControl(self):
		dog = self.GetWatchdog()
		dog.SetEnabled(True)
		dog.SetExpiration(0.25)

		timer = wpilib.Timer()
		timer.Start()

		while self.IsOperatorControl() and self.IsEnabled():
			dog.Feed()

			# Move a motor with a Joystick
			self.drive()

			if timer.HasPeriodPassed(1.0):
				print("Battery: %s" % self.ds.battery.GetVoltage())

			wpilib.Wait(0.04)

	def drive(self):
		quickturn = self.controller.GetRawButton(6)  # RB on the logitech controller
		wheel = util.deadband(self.controller.GetX(), 0.2)
		throttle = util.deadband(self.controller.GetY(), 0.2)
		negintertia = wheel - self.oldwheel
		self.oldwheel = wheel

		if True:  # TODO if is high gear
			wheel = util.sinscale(wheel, 0.6, count=2)
		else:
			wheel = util.sinscale(wheel, 0.5, count=3)

		negintertiabuffer = 0

		if True:  # TODO if high gear
			sensitivity = 0.65
			negintertiascalar = 5
		else:
			sensitivity = 0.75
			if wheel * negintertia > 0:
				negintertiascalar = 2.5
			else:
				if abs(wheel) > 0.65:
					negintertiascalar = 5
				else:
					negintertiascalar = 3


		negintertiapower = negintertia * negintertiascalar
		negintertiabuffer += negintertiapower

		wheel = wheel + negintertiabuffer
		linearpower = throttle


		if quickturn:
			if abs(linearpower) < 0.2:
				alpha = 0.1
				self.quickstopbuffer = (1 - alpha) * self.quickstopbuffer + alpha * util.limit(wheel, 1.0) * 5
			overpower = 1
			angularpower = wheel
		else:
			overpower = 0
			angularpower = abs(linearpower) * wheel * sensitivity - self.quickstopbuffer

			if self.quickstopbuffer > 1:
				self.quickstopbuffer -= 1
			elif self.quickstopbuffer < -1:
				self.quickstopbuffer += 1
			else:
				self.quickstopbuffer = 0

		lpower = linearpower + angularpower
		rpower = linearpower - angularpower

		if lpower > 1:
			rpower -= overpower * (lpower - 1)
			lpower = 1
		elif rpower > 1:
			lpower -= overpower * (-1 - lpower)
			rpower = 1
		elif lpower < -1:
			rpower += overpower * (-1 * lpower)
			lpower = -1
		elif rpower < -1:
			lpower += overpower * (-1 - rpower)
			rpower = -1

		self.leftdrive.Set(-lpower)
		self.rightdrive.Set(rpower)


def run():
	robot = RobotGuy()
	robot.StartCompetition()

	return robot


if __name__ == '__main__':
	wpilib.require_version('2014.7.2')

	import physics
	wpilib.internal.physics_controller.setup(physics)

	wpilib.run()

util.py

import math


def deadband(rawaxis, amount):
	return rawaxis if abs(rawaxis) > abs(amount) else 0

def sinscale(val, nonlinearity, count=1):
	count -= 1
	s = math.sin(math.pi / 2 * nonlinearity * val) / math.sin(math.pi / 2 * nonlinearity)
	if count > 0:
		return sinscale(s, nonlinearity)
	else:
		return s

def limit(v, limit):
	return v if abs(v) < limit else (-1 if v < 0 else 1)