def test_absolute(robot):
epsilon = 0.01 # Tolerance for angular floating point errors (~0.05 degrees)
chassis = Chassis()
reset_chassis(chassis)
# Absolute argument should point the modules in the absolute direction
# for diagnostic purposes
chassis.drive(-1.0, -1.0, 0.0, absolute=True)
for module in chassis._modules.values():
assert abs(constrain_angle(module.direction + 3.0 / 4.0 * math.pi)) < epsilon
reset_chassis(chassis)
def test_retain_wheel_direction():
# When the joystick is returned to the centre, keep the last direction that the wheels were pointing
chassis = Chassis()
for name, module in chassis._modules.items():
module.steer(math.pi / 4.0)
chassis.drive(0.0, 0.0, 0.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
# Should not matter what the throttle is, even if it is zero
chassis.drive(0.0, 0.0, 0.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
def test_chassis(robot, wpilib):
epsilon = 0.01 # Tolerance for angular floating point errors (~0.05 degrees)
chassis = Chassis()
# vX is out the left side of the robot, vY is out of the front, vZ is upwards, so a +ve rotation is counter-clockwise
# vX vY vZ throttle
reset_chassis(chassis)
# test x axis
chassis.drive(1.0, 0.0, 0.0, 1.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction)) % math.pi < epsilon
reset_chassis(chassis)
# test y axis
chassis.drive(0.0, 1.0, 0.0)
for name, module in chassis._modules.items():
# test weather each module is facing in the right direction
assert abs(constrain_angle(math.pi / 2.0 - module.direction)) < epsilon
reset_chassis(chassis)
vz = {'a': math.atan2(-Chassis.length, Chassis.width), # the angle that module a will go to if we spin on spot
'b': math.atan2(Chassis.length, Chassis.width),
'c': math.atan2(-Chassis.length, Chassis.width),
'd': math.atan2(Chassis.length, Chassis.width)
}
chassis.drive(0.0, 0.0, 1.0)
for name, module in chassis._modules.items():
assert abs(constrain_angle(module.direction - vz[name])) < epsilon
reset_chassis(chassis)
chassis.drive(1.0, 1.0, 0.0)
for module in chassis._modules.values():
assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
reset_chassis(chassis)
