-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathgb_diag.py
162 lines (118 loc) · 6.19 KB
/
gb_diag.py
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
import microbit
import ustruct
from micropython import const
_GIGGLEBOT_ADDRESS = const(0x04)
_GET_FIRMWARE_VERSION = b'\x01'
_GET_MANUFACTURER = b'\x02'
_GET_BOARD = b'\x03'
_GET_VOLTAGE_BATTERY = b'\x04'
_GET_LINE_SENSORS = b'\x05'
_GET_LIGHT_SENSORS = b'\x06'
_GET_MOTOR_STATUS_LEFT = b'\x07'
_GET_MOTOR_STATUS_RIGHT = b'\x08'
_SET_MOTOR_POWER = b'\x09'
_SET_MOTOR_POWERS = b'\x0a'
_GET_VOLTAGE_RAIL = b'\x0b'
MOTOR_RIGHT = b'\x01' #: Left motor constant.
MOTOR_LEFT = b'\x02' #: Right motor constant.
_MOTOR_FLOAT = const(-128)
class GiggleBot():
'''
Class to get details and check the status of the `GiggleBot`_ hardware.
'''
def __init__(self):
'''
Constructor to initialize a :py:class:`~gb_diag.GiggleBot` object.
'''
self.write = microbit.i2c.write
self.read = microbit.i2c.read
self.buff = bytearray(8)
def get_details(self):
'''
Gets details about the Gigglebot robot.
:returns: The manufacturer name of the board, the name of the board and the firmware version of it in this specific order.
:rtype: tuple(string, string, int)
'''
self.write(_GIGGLEBOT_ADDRESS, _GET_MANUFACTURER)
man = str(self.read(_GIGGLEBOT_ADDRESS, 20), 'utf-8').strip()
self.write(_GIGGLEBOT_ADDRESS, _GET_BOARD)
board = str(self.read(_GIGGLEBOT_ADDRESS, 20), 'utf-8').strip()
self.write(_GIGGLEBOT_ADDRESS, _GET_FIRMWARE_VERSION)
fw = ustruct.unpack('>H', self.read(_GIGGLEBOT_ADDRESS, 2))[0]
return (man, board, fw)
def get_voltages(self):
'''
Gets battery and rail voltages.
If the power switch is off, the value returned for the battery voltage is unequivocally close to **0**.
:returns: The battery and rail voltages in this order.
:rtype: (float, float)
'''
self.write(_GIGGLEBOT_ADDRESS, _GET_VOLTAGE_BATTERY)
bat = ustruct.unpack('>H', self.read(_GIGGLEBOT_ADDRESS, 2))[0] / 1000
self.write(_GIGGLEBOT_ADDRESS, _GET_VOLTAGE_RAIL)
rail = ustruct.unpack('>H', self.read(_GIGGLEBOT_ADDRESS, 2))[0] / 1000
return (bat, rail)
# def get_line_sensors(self):
# '''
# The line sensor values on the GiggleBot.
# Lower values indicate a darker material and on the opposite, higher values signify lighter colors (closing to white).
# :returns: A 2-element tuple with the values of the left and right sensor in this specific order.
# :rtype: tuple(int,int)
# '''
# self.write(_GIGGLEBOT_ADDRESS, _GET_LINE_SENSORS)
# array = self.read(_GIGGLEBOT_ADDRESS, 3)
# for i in b'\x00\x01':
# ustruct.pack_into('f', self.buff, 1 - i, (1023 - (array[i] << 2) | (((array[2] << (i * 2)) & 0xC0) >> 6)) / 1023.0)
# return ustruct.unpack('ff', self.buff)
# def get_light_sensors(self):
# '''
# The light sensor values on the GiggleBot.
# The lower the values, the darker is the environment around the GiggleBot. It's vice-versa for higher values.
# :returns: A 2-element tuple with the values of the left and right light sensor in this specific order.
# :rtype: tuple(int,int)
# '''
# self.write(_GIGGLEBOT_ADDRESS, _GET_LIGHT_SENSORS)
# array = self.read(_GIGGLEBOT_ADDRESS, 3)
# for i in b'\x00\x01':
# ustruct.pack_into('f', self.buff, 1 - i, (1023 - (array[i] << 2) | (((array[2] << (i * 2)) & 0xC0) >> 6)) / 1023.0)
# return ustruct.unpack('ff', self.buff)
def set_motor_power(self, port, power):
'''
Sets the power of a single motor on the GiggleBot.
:param port: Either :py:attr:`~gb_diag.MOTOR_LEFT` or :py:attr:`~gb_diag.MOTOR_RIGHT` depending on which motor has to be controlled.
:param int power: **-100** to **0** for reverse and from **0** to **100** for full speed ahead.
'''
self.write(_GIGGLEBOT_ADDRESS, _SET_MOTOR_POWER + port + ustruct.pack('b', power))
def set_motor_powers(self, powerLeft, powerRight):
'''
Sets the power of both motors of the GiggleBot.
:param int powerLeft: Anywhere between from **-100** to **100** for the left motor.
:param int powerRight: Anywhere between from **-100** to **100** for the right motor.
'''
self.write(_GIGGLEBOT_ADDRESS, _SET_MOTOR_POWERS + ustruct.pack('bb', powerRight, powerLeft))
def get_motor_status(self, port):
'''
Returns a report of the status of a given motor on the GiggleBot.
:param port: Either :py:attr:`~gb_diag.MOTOR_LEFT` or :py:attr:`~gb_diag.MOTOR_RIGHT`.
This method returns a 2-element list where the 1st element is called the ``status_flag`` and the 2nd one is
represents the current speed set for the given motor.
The ``status_flag`` can have the following values:
* For **0** it means the conditions are normal (the motor can run).
* For **1** it means the battery voltage is too low or the power switch is off.
Also, if the battery voltage is **<= 3.3V**, then it's too low and the motors float.
If the battery voltage is **>= 3.4V**, then it's high enought that the motors are set to run.
This difference of **0.1V** is introduced in order to prevent the motors from quickly turning on and off if the battery
voltage is right on the edge.
:returns: A 2-element list containing the status of the motor and the current speed/power of it.
:rtype: list(int,int)
'''
self.write(_GIGGLEBOT_ADDRESS, _GET_MOTOR_STATUS_LEFT if port == MOTOR_LEFT else _GET_MOTOR_STATUS_RIGHT)
array = list(bytearray(self.read(_GIGGLEBOT_ADDRESS, 2)))
if array[1] & 0x80:
array[1] -= 0x100
return array
def reset_all(self):
'''
Brings the GiggleBot to a full stop, while also cutting power to the motors, so that the battery life is preserved.
'''
self.write(_GIGGLEBOT_ADDRESS, _SET_MOTOR_POWERS + ustruct.pack('bb', _MOTOR_FLOAT, _MOTOR_FLOAT))