Skip to content

Commit 01a0af6

Browse files
previprevi
committed
mpu wip
1 parent 64dd009 commit 01a0af6

File tree

14 files changed

+257
-31
lines changed

14 files changed

+257
-31
lines changed

‎cnn_models/models.json

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
{"test_model_1": {"image_width": "160", "output_layer": "final_result", "status": 1, "image_height": "160"}, "base_high_slow": {"image_width": 224, "status": 1.0, "image_height": 224, "output_layer": "final_result"}, "object_detect": {"image_width": 224, "status": 1.0, "image_height": 224, "output_layer": "final_result"}, "base_low_fast": {"image_width": 224, "status": 100, "image_height": 224, "output_layer": "final_result"}}
1+
{"base_low_fast": {"image_width": 224, "output_layer": "final_result", "status": 100, "image_height": 224}, "object_detect": {"image_width": 224, "output_layer": "final_result", "status": 1.0, "image_height": 224}, "test_model_1": {"image_width": "160", "output_layer": "final_result", "status": 0, "image_height": "160"}, "base_high_slow": {"image_width": 224, "output_layer": "final_result", "status": 1.0, "image_height": 224}}

‎cnn_train.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,7 @@ def _get_hparams(train_epochs=1, learning_rate=0.005):
107107
return lib.HParams(
108108
train_epochs=train_epochs,
109109
do_fine_tuning=False,
110-
batch_size=2,
110+
batch_size=1,
111111
learning_rate=learning_rate,
112112
momentum=0.9)
113113

‎coderbot.py

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ def __init__(self, motor_trim_factor=1.0, encoder=True):
9696
sonar.Sonar(self.pi, PIN_SONAR_4_TRIGGER, PIN_SONAR_4_ECHO)]
9797

9898
try:
99-
self._ag = mpu.AccelGyro()
99+
self._ag = mpu.AccelGyroMag()
100100
except IOError:
101101
logging.info("MPU not available")
102102

@@ -158,6 +158,28 @@ def servo4(self, angle):
158158
def get_sonar_distance(self, sonar_id=0):
159159
return self.sonar[sonar_id].get_distance()
160160

161+
def get_mpu_accel(self, axis=None):
162+
acc = self.mpu.get_acc()
163+
if axis is None:
164+
return acc
165+
else:
166+
return acc[axis]
167+
168+
def get_mpu_gyro(self, axis=None):
169+
gyro = self.mpu.get_gyro()
170+
if axis is None:
171+
return gyro
172+
else:
173+
return gyro[axis]
174+
175+
def get_mpu_heading(self):
176+
hdg = self.mpu.get_hdg()
177+
return hdg
178+
179+
def get_mpu_temp(self):
180+
hdg = self.mpu.get_temp()
181+
return temp
182+
161183
def _servo_control(self, pin, angle):
162184
duty = ((angle + 90) * 100 / 180) + 25
163185

‎lsm9ds1.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -427,5 +427,5 @@ def run_interactive_calibration(i2cbus_no):
427427

428428

429429
if __name__ == '__main__':
430-
mc = run_interactive_calibration(0)
430+
mc = run_interactive_calibration(1)
431431
print(mc.to_json())

‎mpu.py

Lines changed: 18 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
import lsm9ds1
22
import time
33

4-
class AccelGyro:
4+
class AccelGyroMag:
55
X_IND = 0
66
Y_IND = 1
77
Z_IND = 2
@@ -15,9 +15,9 @@ class AccelGyro:
1515
has new data and then reads all the sensors."""
1616
def __init__(self):
1717
self.driver = lsm9ds1.make_i2c(1)
18-
mc = lsm9ds1.MagCalibration(xmin=-0.3612, xmax=-0.17836000000000002,
19-
ymin=-0.08750000000000001, ymax=0.07826000000000001,
20-
heading_offset=95.3491645593403)
18+
mc = lsm9ds1.MagCalibration(xmin=0.03234, xmax=0.25718,
19+
ymin=0.036120000000000006, ymax=0.19138000000000002,
20+
heading_offset=-130.29698965718163)
2121
self.driver.configure(mc)
2222

2323

@@ -26,32 +26,33 @@ def __init__(self):
2626
def read_ag(self):
2727
temp, acc, gyro = self.driver.read_values()
2828
print("Temp: %.1f °f" % temp)
29-
print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[SimpleExample.ROLL_IND],
30-
gyro[SimpleExample.PITCH_IND],
31-
gyro[SimpleExample.YAW_IND]))
32-
print("X: %.4f, Y: %.4f, Z: %.4f" % (acc[SimpleExample.X_IND],
33-
acc[SimpleExample.Y_IND],
34-
acc[SimpleExample.Z_IND]))
29+
print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[AccelGyroMag.ROLL_IND],
30+
gyro[AccelGyroMag.PITCH_IND],
31+
gyro[AccelGyroMag.YAW_IND]))
32+
print("X: %.4f, Y: %.4f, Z: %.4f" % (acc[AccelGyroMag.X_IND],
33+
acc[AccelGyroMag.Y_IND],
34+
acc[AccelGyroMag.Z_IND]))
3535

3636
def read_magnetometer(self):
3737
hdg = self.driver.mag_heading()
38-
print("Heading: %.2f" % hdg)
38+
print("Heading: " + str(hdg))
3939

4040

4141
def get_gyro(self):
4242
temp, acc, gyro = self.driver.read_values()
43-
return (gyro[SimpleExample.ROLL_IND],
44-
gyro[SimpleExample.PITCH_IND],
45-
gyro[SimpleExample.YAW_IND])
43+
return (gyro[AccelGyroMag.ROLL_IND],
44+
gyro[AccelGyroMag.PITCH_IND],
45+
gyro[AccelGyroMag.YAW_IND])
4646

4747
def get_acc(self):
4848
temp, acc, gyro = self.driver.read_values()
49-
return (gyro[SimpleExample.X_IND],
50-
gyro[SimpleExample.Y_IND],
51-
gyro[SimpleExample.Z_IND])
49+
return (gyro[AccelGyroMag.X_IND],
50+
gyro[AccelGyroMag.Y_IND],
51+
gyro[AccelGyroMag.Z_IND])
5252

5353
def get_hdg(self):
5454
hdg = self.driver.mag_heading()
55+
return hdg
5556

5657
def get_temp(self):
5758
temp, acc, gyro = self.driver.read_values()

‎mpu2.py

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,9 @@ class SimpleExample:
1616
has new data and then reads all the sensors."""
1717
def __init__(self):
1818
self.driver = lsm9ds1.make_i2c(1)
19-
mc = lsm9ds1.MagCalibration(xmin=-0.3612, xmax=-0.17836000000000002,
20-
ymin=-0.08750000000000001, ymax=0.07826000000000001,
21-
heading_offset=95.3491645593403)
19+
mc = lsm9ds1.MagCalibration(xmin=0.03234, xmax=0.25718,
20+
ymin=0.036120000000000006, ymax=0.19138000000000002,
21+
heading_offset=-130.29698965718163)
2222
self.driver.configure(mc)
2323

2424
def main(self):
@@ -36,17 +36,19 @@ def main(self):
3636

3737
def read_ag(self):
3838
temp, acc, gyro = self.driver.read_values()
39-
print("Temp: %.1f °f" % temp)
39+
print("Temp: %.1f °f" % temp, end='')
4040
print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[SimpleExample.ROLL_IND],
4141
gyro[SimpleExample.PITCH_IND],
42-
gyro[SimpleExample.YAW_IND]))
42+
gyro[SimpleExample.YAW_IND]), end='')
4343
print("X: %.4f, Y: %.4f, Z: %.4f" % (acc[SimpleExample.X_IND],
4444
acc[SimpleExample.Y_IND],
45-
acc[SimpleExample.Z_IND]))
45+
acc[SimpleExample.Z_IND]), end='')
4646

4747
def read_magnetometer(self):
4848
hdg = self.driver.mag_heading()
49-
print("Heading: %.2f" % hdg)
49+
print("headind: %f ", hdg, end='')
50+
mag = self.driver.read_magnetometer()
51+
print('Mag {}'.format(mag))
5052

5153

5254
if __name__ == '__main__':

‎mpu3.py

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
import sys, os
2+
import time
3+
4+
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
5+
from lsm9ds1_rjg import Driver, I2CTransport, SPITransport
6+
7+
8+
class SimpleExample:
9+
"""This example shows how to poll the sensor for new data.
10+
It queries the sensor to discover when the accelerometer/gyro
11+
has new data and then reads all the sensors."""
12+
def __init__(self):
13+
#self.driver = self._create_spi_driver()
14+
self.driver = self._create_i2c_driver()
15+
self.driver.configure()
16+
17+
@staticmethod
18+
def _create_i2c_driver() -> Driver:
19+
return Driver(
20+
I2CTransport(1, I2CTransport.I2C_AG_ADDRESS),
21+
I2CTransport(1, I2CTransport.I2C_MAG_ADDRESS))
22+
23+
@staticmethod
24+
def _create_spi_driver() -> Driver:
25+
return Driver(
26+
SPITransport(0, False),
27+
SPITransport(1, True))
28+
29+
def main(self):
30+
try:
31+
count = 0
32+
while True:
33+
ag_data_ready = self.driver.read_ag_status().accelerometer_data_available
34+
if ag_data_ready:
35+
self.read_ag()
36+
self.read_magnetometer()
37+
count += 1
38+
else:
39+
time.sleep(0.1)
40+
finally:
41+
self.driver.close()
42+
43+
def read_ag(self):
44+
temp, acc, gyro = self.driver.read_ag_data()
45+
print('Temp:{} Acc:{} Gryo:{}'.format(temp, acc, gyro), end = '')
46+
47+
def read_magnetometer(self):
48+
mag = self.driver.read_magnetometer()
49+
print('Mag {}'.format(mag))
50+
51+
52+
if __name__ == '__main__':
53+
SimpleExample().main()

‎photos/0.txt

Whitespace-only changes.

‎static/js/blockly/blocks.js

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1101,3 +1101,94 @@ Blockly.Python['coderbot_sonar_get_distance'] = function(block) {
11011101
return [code, Blockly.Python.ORDER_ATOMIC];
11021102
};
11031103

1104+
Blockly.Blocks['coderbot_mpu_get_accel'] = {
1105+
/**
1106+
* Block for get_distance function.
1107+
* @this Blockly.Block
1108+
*/
1109+
init: function() {
1110+
this.setHelpUrl(Blockly.Msg.LOGIC_BOOLEAN_HELPURL);
1111+
this.setColour(240);
1112+
this.appendDummyInput()
1113+
.appendField(Blockly.Msg.CODERBOT_MPU_GET_ACCEL)
1114+
.appendField(new Blockly.FieldDropdown([[Blockly.Msg.CODERBOT_MPU_AXIS_X, "0"],
1115+
[Blockly.Msg.CODERBOT_MPU_AXIS_Y, "1"],
1116+
[Blockly.Msg.CODERBOT_MPU_AXIS_Z, "2"], 'AXIS');
1117+
this.setOutput(true, 'Number');
1118+
this.setTooltip(Blockly.Msg.LOGIC_BOOLEAN_TOOLTIP);
1119+
}
1120+
};
1121+
1122+
Blockly.Python['coderbot_mpu_get_accel'] = function(block) {
1123+
// Boolean values true and false.
1124+
var axis = block.getFieldValue('AXIS');
1125+
var code = 'get_bot().get_mpu_accel(' + axis + ')';
1126+
return [code, Blockly.Python.ORDER_ATOMIC];
1127+
};
1128+
1129+
Blockly.Blocks['coderbot_mpu_get_gyro'] = {
1130+
/**
1131+
* Block for get_distance function.
1132+
* @this Blockly.Block
1133+
*/
1134+
init: function() {
1135+
this.setHelpUrl(Blockly.Msg.LOGIC_BOOLEAN_HELPURL);
1136+
this.setColour(240);
1137+
this.appendDummyInput()
1138+
.appendField(Blockly.Msg.CODERBOT_MPU_GET_GYRO)
1139+
.appendField(new Blockly.FieldDropdown([[Blockly.Msg.CODERBOT_MPU_AXIS_X, "0"],
1140+
[Blockly.Msg.CODERBOT_MPU_AXIS_Y, "1"],
1141+
[Blockly.Msg.CODERBOT_MPU_AXIS_Z, "2"], 'AXIS');
1142+
this.setOutput(true, 'Number');
1143+
this.setTooltip(Blockly.Msg.LOGIC_BOOLEAN_TOOLTIP);
1144+
}
1145+
};
1146+
1147+
Blockly.Python['coderbot_mpu_get_gyro'] = function(block) {
1148+
// Boolean values true and false.
1149+
var axis = block.getFieldValue('AXIS');
1150+
var code = 'get_bot().get_mpu_gyro(' + axis + ')';
1151+
return [code, Blockly.Python.ORDER_ATOMIC];
1152+
};
1153+
1154+
Blockly.Blocks['coderbot_mpu_get_heading'] = {
1155+
/**
1156+
* Block for get_distance function.
1157+
* @this Blockly.Block
1158+
*/
1159+
init: function() {
1160+
this.setHelpUrl(Blockly.Msg.LOGIC_BOOLEAN_HELPURL);
1161+
this.setColour(240);
1162+
this.appendDummyInput()
1163+
.appendField(Blockly.Msg.CODERBOT_MPU_GET_HEADING);
1164+
this.setOutput(true, 'Number');
1165+
this.setTooltip(Blockly.Msg.LOGIC_BOOLEAN_TOOLTIP);
1166+
}
1167+
};
1168+
1169+
Blockly.Python['coderbot_mpu_get_heading'] = function(block) {
1170+
// Boolean values true and false.
1171+
var code = 'get_bot().get_mpu_heading()';
1172+
return [code, Blockly.Python.ORDER_ATOMIC];
1173+
};
1174+
1175+
Blockly.Blocks['coderbot_mpu_get_temp'] = {
1176+
/**
1177+
* Block for get_distance function.
1178+
* @this Blockly.Block
1179+
*/
1180+
init: function() {
1181+
this.setHelpUrl(Blockly.Msg.LOGIC_BOOLEAN_HELPURL);
1182+
this.setColour(240);
1183+
this.appendDummyInput()
1184+
.appendField(Blockly.Msg.CODERBOT_MPU_GET_TEMP);
1185+
this.setOutput(true, 'Number');
1186+
this.setTooltip(Blockly.Msg.LOGIC_BOOLEAN_TOOLTIP);
1187+
}
1188+
};
1189+
1190+
Blockly.Python['coderbot_mpu_get_temp'] = function(block) {
1191+
// Boolean values true and false.
1192+
var code = 'get_bot().get_mpu_temp()';
1193+
return [code, Blockly.Python.ORDER_ATOMIC];
1194+
};

‎static/js/blockly/bot_en.js

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,14 @@ Blockly.Msg.CODERBOT_SONAR_SENSOR_1 = "1";
8383
Blockly.Msg.CODERBOT_SONAR_SENSOR_2 = "2";
8484
Blockly.Msg.CODERBOT_SONAR_SENSOR_3 = "3";
8585
Blockly.Msg.CODERBOT_SONAR_SENSOR_4 = "4";
86+
Blockly.Msg.CODERBOT_MPU_GYRO = "gyroscope axis";
87+
Blockly.Msg.CODERBOT_MPU_ACCEL = "gyroscope axis";
88+
Blockly.Msg.CODERBOT_MPU_HDG = "heading";
89+
Blockly.Msg.CODERBOT_MPU_TEMP = "temperature";
90+
Blockly.Msg.CODERBOT_MPU_AXIS = "axis";
91+
Blockly.Msg.CODERBOT_MPU_AXIS_X = "X";
92+
Blockly.Msg.CODERBOT_MPU_AXIS_Y = "Y";
93+
Blockly.Msg.CODERBOT_MPU_AXIS_Z = "Z";
8694
Blockly.Msg.CODERBOT_EVENT_WHEN = "when";
8795
Blockly.Msg.CODERBOT_EVENT_WITH = "with";
8896
Blockly.Msg.CODERBOT_EVENT_PUBLISH = "publish";

0 commit comments

Comments
 (0)