def setUp(self):
ds = pandas.DataFrame.from_records([
{
"c": "A",
"n": 10,
"b": True,
"s": 1,
"w": 1
},
{
"c": "B",
"n": 11,
"b": False,
"s": 1,
"w": 10
},
{
"c": "C",
"n": 20,
"b": 0.5,
"s": 1,
"w": 5
},
{
"c": "A",
"n": 42,
"b": 0.3,
"s": 1,
"w": 6
},
])
if hasattr(ds, "to_dense"):
ds = ds.to_dense()
ds.loc[:, "c"] = pandas.Series(pandas.Categorical(ds.loc[:, "c"]))
self.schema = {
"c": "categorical",
"n": "numerical",
"b": "binary",
"w": "weight",
"s": "stop", #this won't appear in the dmat
}
self.chs = Chassis(self.schema, ds)
b = ds.loc[:, "b"]
n = ds.loc[:, "n"]
abc = pandas.concat(
[
ds.loc[:, "c"] == "A", ds.loc[:, "c"] == "B", ds.loc[:, "c"]
== "C"
],
axis=1,
).astype(int)
abc.columns = ("c_A", "c_B", "c_C")
self.expected = {
'c': (ds.loc[:, ["b", "n"]], ds["c"]),
'n': (pandas.concat([abc, b], axis=1), n),
'b': (pandas.concat([abc, n], axis=1), b)
}
self.ds = ds
def __init__(self, initialState: State = State.GET_HEALTHY_HIVE):
self.__pos = None
self.__dir = None
self.speedRight = 0
self.speedLeft = 0
self.speedRightOld = 0
self.speedLeftOld = 0
self.target = Point(0, 0)
self.robotDirTargetHist = deque([180.0] * HIST_QUEUE_LENGTH)
self.robotDistTargetHist = deque([math.inf] * HIST_QUEUE_LENGTH)
self.targetDistance = 9999
self.targetAngle = 9999
self.state = initialState
self.stateOld = initialState
self.gameData = None
self.stateChanged = False
self.pidController = PidController()
self.chassis = Chassis()
def testChassisSchemaCorrecttness(self): for dsName, ds in self.datasets.items(): with self.subTest(dsName=dsName): if ds.spec: Chassis(ds.spec, ds.pds) else: print(repr(dsName), ":", Chassis.specFromPandas(ds.pds))
class SimpleTests(unittest.TestCase):
def setUp(self):
ds = pandas.DataFrame.from_records([
{
"c": "A",
"n": 10,
"b": True,
"s": 1,
"w": 1
},
{
"c": "B",
"n": 11,
"b": False,
"s": 1,
"w": 10
},
{
"c": "C",
"n": 20,
"b": 0.5,
"s": 1,
"w": 5
},
{
"c": "A",
"n": 42,
"b": 0.3,
"s": 1,
"w": 6
},
])
if hasattr(ds, "to_dense"):
ds = ds.to_dense()
ds.loc[:, "c"] = pandas.Series(pandas.Categorical(ds.loc[:, "c"]))
self.schema = {
"c": "categorical",
"n": "numerical",
"b": "binary",
"w": "weight",
"s": "stop", #this won't appear in the dmat
}
self.chs = Chassis(self.schema, ds)
b = ds.loc[:, "b"]
n = ds.loc[:, "n"]
abc = pandas.concat(
[
ds.loc[:, "c"] == "A", ds.loc[:, "c"] == "B", ds.loc[:, "c"]
== "C"
],
axis=1,
).astype(int)
abc.columns = ("c_A", "c_B", "c_C")
self.expected = {
'c': (ds.loc[:, ["b", "n"]], ds["c"]),
'n': (pandas.concat([abc, b], axis=1), n),
'b': (pandas.concat([abc, n], axis=1), b)
}
self.ds = ds
def shouldColumnBePresent(self, cn):
return self.schema[cn] not in {"stop", "weight"}
def checkCovariates(self, cn):
if self.shouldColumnBePresent(cn):
assert_frame_equal(self.chs.prepareCovariates(cn),
self.expected[cn][0],
check_like=True,
check_dtype=False)
def checkResult(self, cn):
if self.shouldColumnBePresent(cn):
assert_series_equal(self.chs.prepareResults(cn),
self.expected[cn][1],
check_dtype=False,
check_categorical=False)
else:
with self.assertRaises(Exception):
self.chs.prepareResults(cn)
def testCovariates(self):
for cn in self.schema:
self.checkCovariates(cn)
def testResult(self):
for cn in self.schema:
self.checkResult(cn)
class Controller:
def __init__(self, initialState: State = State.GET_HEALTHY_HIVE):
self.__pos = None
self.__dir = None
self.speedRight = 0
self.speedLeft = 0
self.speedRightOld = 0
self.speedLeftOld = 0
self.target = Point(0, 0)
self.robotDirTargetHist = deque([180.0] * HIST_QUEUE_LENGTH)
self.robotDistTargetHist = deque([math.inf] * HIST_QUEUE_LENGTH)
self.targetDistance = 9999
self.targetAngle = 9999
self.state = initialState
self.stateOld = initialState
self.gameData = None
self.stateChanged = False
self.pidController = PidController()
self.chassis = Chassis()
def update(self, gameData: GameData, target: Point):
if self.state != self.stateOld:
self.stateChanged = True
else:
self.stateChanged = False
self.stateOld = self.state
self.gameData = gameData
self.target = target
if gameData.homeRobot is not None:
self.__pos = gameData.homeRobot.pos
self.__dir = gameData.homeRobot.dir
if self.target is not None:
self.targetDistance = self.distance(self.target)
self.targetAngle = self.angle(self.target)
self.robotDirTargetHist.popleft()
self.robotDirTargetHist.append(self.targetAngle)
self.robotDistTargetHist.popleft()
self.robotDistTargetHist.append(self.targetDistance)
def distance(self, point: Point) -> float:
return self.__pos.distance(point)
def angle(self, point: Point) -> float:
"""
Izračunaj kot, za katerega se mora zavrteti robot, da bo obrnjen proti točki p2.
Robot se nahaja v točki p1 in ima smer (kot) a1.
"""
a = math.degrees(math.atan2(point.y - self.__pos.y, point.x - self.__pos.x))
a_rel = a - self.__dir
if abs(a_rel) > 180:
if a_rel > 0:
a_rel = a_rel - 360
else:
a_rel = a_rel + 360
return a_rel
def getClosestHive(self, hiveType: HiveTypeEnum) -> Hive:
if hiveType == HiveTypeEnum.HIVE_HEALTHY:
return min(self.gameData.healthyHives, key=lambda hive: hive.pos.distance(self.__pos))
elif hiveType == HiveTypeEnum.HIVE_DISEASED:
return min(self.gameData.diseasedHives, key=lambda hive: hive.pos.distance(self.__pos))
def atTargetEPS(self) -> bool:
return self.targetDistance < DIST_EPS
def atTargetNEAR(self) -> bool:
return self.targetDistance < DIST_NEAR
def atTargetHIST(self) -> bool:
err = [d > DIST_EPS for d in self.robotDistTargetHist]
return sum(err) == 0
def isTurned(self) -> bool:
err = [abs(a) > DIR_EPS for a in self.robotDirTargetHist]
if sum(err) == 0:
self.speedRight = 0
self.speedLeft = 0
return True
return False
def hasStateChanged(self) -> bool:
return self.state != self.stateOld
def setStates(self, new: State, old: State) -> None:
self.state = new
self.stateOld = old
def setSpeedToZero(self) -> None:
self.chassis.runMotors(0, 0)
def runMotors(self) -> None:
self.speedRight = round(min(max(self.speedRight, -SPEED_MAX), SPEED_MAX))
self.speedLeft = round(min(max(self.speedLeft, -SPEED_MAX), SPEED_MAX))
self.chassis.runMotors(self.speedRight, self.speedLeft)
self.speedRightOld = self.speedRight
self.speedLeftOld = self.speedLeft
def breakMotors(self) -> None:
self.chassis.breakMotors()
def robotDie(self) -> None:
self.chassis.robotDie()
def isRobotAlive(self) -> bool:
return (self.__pos is not None) and (self.__dir is not None)
def updatePIDTurn(self) -> None:
u = self.pidController.PIDTurn.update(self.targetAngle)
self.speedRight = -u
self.speedLeft = u
def resetPIDTurn(self) -> None:
self.pidController.PIDTurn.reset()
def updatePIDStraight(self) -> None:
base = self.pidController.PIDForwardBase.update(self.targetDistance)
turn = self.pidController.PIDForwardTurn.update(self.targetAngle)
base = min(max(base, -SPEED_BASE_MAX), SPEED_BASE_MAX)
self.speedRight = -base - turn
self.speedLeft = -base + turn
def resetPIDStraight(self) -> None:
self.pidController.PIDForwardBase.reset()
self.pidController.PIDForwardTurn.reset()
def __init__(self):
Chassis.__init__(self, RunningGearAudi(), TransmissionAudi(), ControlSystemAudi)
def __init__(self):
Chassis.__init__(self, RunningGearFord(), TransmissionFord(), ControlSystemFord)
def main():
chassis = Chassis()
chassis.build()
chassis.run()
