adck/TestRightExample.py at dev · hatfield-c/adck · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
import time
import math
import numpy as np

import ConversionManifold
import UsbConnection
import Cli

def TestRightExample():
	args = Cli.GetSystemArgs()

	port = args.port
	side = args.side
	usb_connection = None

	calibrations = {}
	calibrations["ready"] = np.array([91, 81, 30])
	calibrations["center"] = np.array([92, 89, 19])
	calibrations["forward"] = np.array([65, 112, 22])
	calibrations["left"] = np.array([86, 139, 74])
	calibrations["backward"] = np.array([132, 125, 67])
	calibrations["right"] = np.array([87, 53, 0])
	calibrations["top_right"] = np.array([87, 53, 0])
	calibrations["top_left"] = np.array([73, 147, 76])
	calibrations["bottom_right"] = np.array([124, 54, 0])
	calibrations["bottom_left"] = np.array([112, 147, 76])

	converter = ConversionManifold.ConversionManifold(
		alpha_keys = [
			np.array([0, 0]),
			np.array([1, 0]),
			np.array([0, 1]),
			np.array([-1, 0]),
			np.array([0, -1]),
			np.array([1, 1]),
			np.array([-1, 1]),
			np.array([1, -1]),
			np.array([-1, -1]),
		],
		beta_keys = [
			calibrations["center"],
			calibrations["right"],
			calibrations["forward"],
			calibrations["left"],
			calibrations["backward"],
			calibrations["top_right"],
			calibrations["top_left"],
			calibrations["bottom_right"],
			calibrations["bottom_left"]
		]
	)

	usb_connection = UsbConnection.UsbConnection(port = port, name = side, is_verbose = True)
	time.sleep(10)

	usb_connection.SendAngles(calibrations["ready"])
	input("Press enter to continue...")

	usb_connection.SendAngles(calibrations["center"])
	time.sleep(6)

	angle = converter.AlphaToBeta(np.array([0, 0.9]))
	usb_connection.SendAngles(angle)
	time.sleep(3)

	angle = converter.AlphaToBeta(np.array([0, 0]))
	usb_connection.SendAngles(angle)
	time.sleep(3)

	angle = converter.AlphaToBeta(np.array([1, 0]))
	usb_connection.SendAngles(angle)
	time.sleep(3)

	angle = converter.AlphaToBeta(np.array([0, 0]))
	usb_connection.SendAngles(angle)
	time.sleep(2)

	angle = converter.AlphaToBeta(np.array([0, 0.9]))
	usb_connection.SendAngles(angle)
	time.sleep(2)

	angle = converter.AlphaToBeta(np.array([0, 0]))
	usb_connection.SendAngles(angle)
	time.sleep(12)

	usb_connection.SendAngles(calibrations["ready"])
	time.sleep(3)

	usb_connection.arduino_usb.close()

if __name__ == "__main__":
	TestRightExample()