Skip to content

Commit 9982d50

Browse files
eigen-valueeigen-value
committed
mod: i2c comm over ext_wire(slave)
example: unoQ test over i2c
1 parent 633e59e commit 9982d50

File tree

4 files changed

+623
-118
lines changed

4 files changed

+623
-118
lines changed

‎.gitignore‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,3 +24,8 @@ Icon
2424
Network Trash Folder
2525
Temporary Items
2626
.apdisk
27+
28+
#VSCODE
29+
.vscode
30+
31+
CMakeLists.txt

‎examples/firmware/firmware.ino‎

Lines changed: 164 additions & 116 deletions
Original file line numberDiff line numberDiff line change
@@ -16,15 +16,17 @@
1616
#include "sensor_line.h"
1717
#include "sensor_tof_matrix.h"
1818
#include "ucPack.h"
19+
#include "CircularBuffer.h"
1920

20-
#define I2C_ADDRESS 0x48
21+
#define OUT_BUF_SIZE 512
2122

2223
Arduino_AlvikCarrier alvik;
2324
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
2425
SensorTofMatrix tof(alvik.wire, EXT_GPIO3, EXT_GPIO2);
2526

2627

2728
ucPack packeter(200);
29+
CircularBuffer out_buffer(OUT_BUF_SIZE);
2830

2931
uint8_t code;
3032
uint8_t label;
@@ -34,6 +36,8 @@ uint8_t ack_required = 0;
3436
bool ack_check = false;
3537
uint8_t ack_code = 0;
3638
uint8_t behaviours;
39+
uint8_t cmd;
40+
uint8_t id;
3741

3842
unsigned long tmotor = 0;
3943
unsigned long tsend = 0;
@@ -58,34 +62,176 @@ float x, y, theta;
5862
uint8_t servo_A, servo_B;
5963
float position_left, position_right;
6064

61-
int counter_version = 9;
6265
uint8_t version[3];
6366

6467

65-
int sendMessage(const uint8_t *buffer, size_t size) {
66-
alvik.ext_wire->beginTransmission(I2C_ADDRESS);
67-
size_t out = alvik.ext_wire->write(buffer, size);
68-
alvik.ext_wire->endTransmission(I2C_ADDRESS);
69-
return out;
68+
uint8_t outBufferFree() {
69+
return OUT_BUF_SIZE - out_buffer.getSize();
7070
}
7171

72-
void requestMessage() {
73-
while (true) {
74-
alvik.ext_wire->requestFrom(I2C_ADDRESS, 1);
75-
if (!alvik.ext_wire->available()) break;
72+
73+
void receiveEvent(int byte) {
74+
while (alvik.ext_wire->available()) {
7675
packeter.buffer.push(alvik.ext_wire->read());
7776
}
7877
}
7978

79+
void requestEvent() {
80+
if (out_buffer.isEmpty()) {
81+
return;
82+
}
83+
alvik.ext_wire->write(out_buffer.pop());
84+
}
85+
86+
void sendMessage(const uint8_t * buf, const size_t length) {
87+
if (length > outBufferFree()) {
88+
return;
89+
}
90+
for (uint8_t i = 0; i < length; i++) {
91+
out_buffer.push(buf[i]);
92+
}
93+
94+
}
95+
96+
void publishVersion() {
97+
alvik.getVersion(version[0], version[1], version[2]);
98+
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
99+
sendMessage(packeter.msg, msg_size);
100+
}
101+
102+
void publishSensor(uint8_t sensor_id) {
103+
switch(sensor_id){
104+
case 0:
105+
line.update();
106+
msg_size = packeter.packetC3I('l', line.getLeft(), line.getCenter(), line.getRight());
107+
sendMessage(packeter.msg, msg_size);
108+
break;
109+
case 1:
110+
alvik.updateTouch();
111+
msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
112+
sendMessage(packeter.msg, msg_size);
113+
msg_size = packeter.packetC1B('m', alvik.getMotion());
114+
sendMessage(packeter.msg, msg_size);
115+
break;
116+
case 2:
117+
alvik.updateAPDS();
118+
msg_size = packeter.packetC3I('c', alvik.getRed(), alvik.getGreen(), alvik.getBlue());
119+
sendMessage(packeter.msg, msg_size);
120+
break;
121+
case 3:
122+
if (tof.update_rois()){
123+
msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
124+
sendMessage(packeter.msg, msg_size);
125+
}
126+
break;
127+
case 4:
128+
msg_size = packeter.packetC3F('q', alvik.getRoll(), alvik.getPitch(), alvik.getYaw());
129+
sendMessage(packeter.msg, msg_size);
130+
break;
131+
}
132+
}
133+
134+
void publishMotors(uint8_t c) {
135+
switch(c) {
136+
case 'j':
137+
// joint speed
138+
msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
139+
sendMessage(packeter.msg, msg_size);
140+
break;
141+
case 'w':
142+
// joint position
143+
msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
144+
sendMessage(packeter.msg, msg_size);
145+
break;
146+
case 'v':
147+
// robot speed
148+
msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
149+
sendMessage(packeter.msg, msg_size);
150+
break;
151+
case 'z':
152+
// pose
153+
msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta());
154+
sendMessage(packeter.msg, msg_size);
155+
break;
156+
default:
157+
break;
158+
}
159+
}
160+
161+
void publishImu() {
162+
msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
163+
sendMessage(packeter.msg, msg_size);
164+
}
165+
166+
void publishBattery() {
167+
msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage());
168+
sendMessage(packeter.msg, msg_size);
169+
}
170+
171+
void publishAck() {
172+
if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
173+
if (ack_required == MOVEMENT_ROTATE){
174+
msg_size = packeter.packetC1B('x', 'R');
175+
}
176+
if (ack_required == MOVEMENT_MOVE){
177+
msg_size = packeter.packetC1B('x', 'M');
178+
}
179+
if (ack_required == MOVEMENT_POSITION){
180+
msg_size = packeter.packetC1B('x', 'P');
181+
}
182+
if (ack_required == MOVEMENT_LEFT){
183+
msg_size = packeter.packetC1B('x', 'P');
184+
}
185+
if (ack_required == MOVEMENT_RIGHT){
186+
msg_size = packeter.packetC1B('x', 'P');
187+
}
188+
}
189+
else{
190+
msg_size = packeter.packetC1B('x', 0);
191+
}
192+
// alvik.serial->write(packeter.msg, msg_size);
193+
sendMessage(packeter.msg,msg_size);
194+
}
195+
196+
void publicationRequest(const uint8_t cmd, const uint8_t id) {
197+
198+
switch(cmd) {
199+
case 'v':
200+
publishVersion();
201+
break;
202+
case 'b':
203+
publishBattery();
204+
break;
205+
case 'i':
206+
publishImu();
207+
break;
208+
case 'm':
209+
publishMotors(id);
210+
break;
211+
case 'k':
212+
publishAck();
213+
break;
214+
case 's':
215+
publishSensor(id);
216+
break;
217+
default:
218+
break;
219+
}
220+
221+
}
222+
80223
void setup(){
81224
alvik.begin();
225+
226+
alvik.ext_wire->onReceive(receiveEvent);
227+
alvik.ext_wire->onRequest(requestEvent);
228+
82229
alvik.disableIlluminator();
83230
alvik.setLeds(COLOR_ORANGE);
84231
alvik.setLedBuiltin(HIGH);
85232
line.begin();
86233
tof.begin();
87234

88-
89235
alvik.getVersion(version[0], version[1], version[2]);
90236
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
91237
// alvik.serial->write(packeter.msg,msg_size);
@@ -110,10 +256,7 @@ void setup(){
110256
}
111257

112258
void loop(){
113-
// while(alvik.serial->available() > 0) {
114-
// packeter.buffer.push(alvik.serial->read());
115-
// }
116-
requestMessage();
259+
117260
if (packeter.checkPayload()) {
118261
code = packeter.payloadTop();
119262
if (!alvik.isBatteryAlert()){
@@ -243,108 +386,19 @@ void loop(){
243386
alvik.setBehaviour(ALL_BEHAVIOURS, false);
244387
}
245388
break;
389+
case '#':
390+
packeter.unpacketC2B(code, cmd, id);
391+
publicationRequest(cmd, id);
392+
break;
246393
}
247394
}
248395

249-
// sensors publish
250-
if (millis()-tsensor>10){
251-
tsensor=millis();
252-
switch(sensor_id){
253-
case 0:
254-
line.update();
255-
msg_size = packeter.packetC3I('l', line.getLeft(), line.getCenter(), line.getRight());
256-
// alvik.serial->write(packeter.msg,msg_size);
257-
sendMessage(packeter.msg,msg_size);
258-
break;
259-
case 1:
260-
alvik.updateTouch();
261-
msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
262-
// alvik.serial->write(packeter.msg,msg_size);
263-
sendMessage(packeter.msg,msg_size);
264-
msg_size = packeter.packetC1B('m', alvik.getMotion());
265-
// alvik.serial->write(packeter.msg,msg_size);
266-
sendMessage(packeter.msg,msg_size);
267-
break;
268-
case 2:
269-
alvik.updateAPDS();
270-
msg_size = packeter.packetC3I('c', alvik.getRed(), alvik.getGreen(), alvik.getBlue());
271-
// alvik.serial->write(packeter.msg,msg_size);
272-
sendMessage(packeter.msg,msg_size);
273-
break;
274-
case 3:
275-
if (tof.update_rois()){
276-
msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
277-
// alvik.serial->write(packeter.msg,msg_size);
278-
sendMessage(packeter.msg,msg_size);
279-
}
280-
break;
281-
case 4:
282-
msg_size = packeter.packetC3F('q', alvik.getRoll(), alvik.getPitch(), alvik.getYaw());
283-
// alvik.serial->write(packeter.msg,msg_size);
284-
sendMessage(packeter.msg,msg_size);
285-
break;
286-
}
287-
sensor_id++;
288-
if (sensor_id>4){
289-
sensor_id=0;
290-
}
291-
}
292396

293-
// motors update & publish
397+
// motors update
294398
if (millis()-tmotor>=20){
295399
tmotor=millis();
296400
alvik.updateMotors();
297401
alvik.updateKinematics();
298-
// joint speed
299-
msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
300-
// alvik.serial->write(packeter.msg,msg_size);
301-
sendMessage(packeter.msg,msg_size);
302-
// joint position
303-
msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
304-
// alvik.serial->write(packeter.msg, msg_size);
305-
sendMessage(packeter.msg,msg_size);
306-
// robot speed
307-
msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
308-
// alvik.serial->write(packeter.msg, msg_size);
309-
sendMessage(packeter.msg,msg_size);
310-
// pose
311-
msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta());
312-
// alvik.serial->write(packeter.msg, msg_size);
313-
sendMessage(packeter.msg,msg_size);
314-
}
315-
316-
// acknowledge
317-
if (millis()-tack > 100){
318-
tack = millis();
319-
if (counter_version>0){
320-
counter_version--;
321-
alvik.getVersion(version[0], version[1], version[2]);
322-
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
323-
// alvik.serial->write(packeter.msg,msg_size);
324-
sendMessage(packeter.msg,msg_size);
325-
}
326-
if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
327-
if (ack_required == MOVEMENT_ROTATE){
328-
msg_size = packeter.packetC1B('x', 'R');
329-
}
330-
if (ack_required == MOVEMENT_MOVE){
331-
msg_size = packeter.packetC1B('x', 'M');
332-
}
333-
if (ack_required == MOVEMENT_POSITION){
334-
msg_size = packeter.packetC1B('x', 'P');
335-
}
336-
if (ack_required == MOVEMENT_LEFT){
337-
msg_size = packeter.packetC1B('x', 'P');
338-
}
339-
if (ack_required == MOVEMENT_RIGHT){
340-
msg_size = packeter.packetC1B('x', 'P');
341-
}
342-
}
343-
else{
344-
msg_size = packeter.packetC1B('x', 0);
345-
}
346-
// alvik.serial->write(packeter.msg, msg_size);
347-
sendMessage(packeter.msg,msg_size);
348402
}
349403

350404
if (millis()-tbehaviours > 100){
@@ -356,17 +410,11 @@ void loop(){
356410
if (millis()-timu>10){
357411
timu=millis();
358412
alvik.updateImu();
359-
msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
360-
// alvik.serial->write(packeter.msg,msg_size);
361-
sendMessage(packeter.msg,msg_size);
362413
}
363414

364415
// battery update
365416
if (millis()-tbattery>1000){
366417
tbattery = millis();
367418
alvik.updateBMS();
368-
msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage());
369-
// alvik.serial->write(packeter.msg,msg_size);
370-
sendMessage(packeter.msg,msg_size);
371419
}
372420
}

0 commit comments

Comments
 (0)