Skip to content

Commit 633e59e

Browse files
eigen-valueeigen-value
committed
feat: i2c comm over ext_wire(master)
1 parent b34f2a4 commit 633e59e

File tree

2 files changed

+55
-20
lines changed

2 files changed

+55
-20
lines changed

‎examples/firmware/firmware.ino‎

Lines changed: 53 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717
#include "sensor_tof_matrix.h"
1818
#include "ucPack.h"
1919

20+
#define I2C_ADDRESS 0x48
21+
2022
Arduino_AlvikCarrier alvik;
2123
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
2224
SensorTofMatrix tof(alvik.wire, EXT_GPIO3, EXT_GPIO2);
@@ -60,6 +62,21 @@ int counter_version = 9;
6062
uint8_t version[3];
6163

6264

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;
70+
}
71+
72+
void requestMessage() {
73+
while (true) {
74+
alvik.ext_wire->requestFrom(I2C_ADDRESS, 1);
75+
if (!alvik.ext_wire->available()) break;
76+
packeter.buffer.push(alvik.ext_wire->read());
77+
}
78+
}
79+
6380
void setup(){
6481
alvik.begin();
6582
alvik.disableIlluminator();
@@ -71,11 +88,13 @@ void setup(){
7188

7289
alvik.getVersion(version[0], version[1], version[2]);
7390
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
74-
alvik.serial->write(packeter.msg,msg_size);
91+
// alvik.serial->write(packeter.msg,msg_size);
92+
sendMessage(packeter.msg,msg_size);
7593

7694
alvik.updateBMS();
7795
msg_size = packeter.packetC1F('p', alvik.getBatteryChargePercentage());
78-
alvik.serial->write(packeter.msg,msg_size);
96+
// alvik.serial->write(packeter.msg,msg_size);
97+
sendMessage(packeter.msg,msg_size);
7998

8099
alvik.setLedBuiltin(LOW);
81100
alvik.setLeds(COLOR_BLACK);
@@ -91,9 +110,10 @@ void setup(){
91110
}
92111

93112
void loop(){
94-
while(alvik.serial->available() > 0) {
95-
packeter.buffer.push(alvik.serial->read());
96-
}
113+
// while(alvik.serial->available() > 0) {
114+
// packeter.buffer.push(alvik.serial->read());
115+
// }
116+
requestMessage();
97117
if (packeter.checkPayload()) {
98118
code = packeter.payloadTop();
99119
if (!alvik.isBatteryAlert()){
@@ -233,29 +253,35 @@ void loop(){
233253
case 0:
234254
line.update();
235255
msg_size = packeter.packetC3I('l', line.getLeft(), line.getCenter(), line.getRight());
236-
alvik.serial->write(packeter.msg,msg_size);
256+
// alvik.serial->write(packeter.msg,msg_size);
257+
sendMessage(packeter.msg,msg_size);
237258
break;
238259
case 1:
239260
alvik.updateTouch();
240261
msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
241-
alvik.serial->write(packeter.msg,msg_size);
262+
// alvik.serial->write(packeter.msg,msg_size);
263+
sendMessage(packeter.msg,msg_size);
242264
msg_size = packeter.packetC1B('m', alvik.getMotion());
243-
alvik.serial->write(packeter.msg,msg_size);
265+
// alvik.serial->write(packeter.msg,msg_size);
266+
sendMessage(packeter.msg,msg_size);
244267
break;
245268
case 2:
246269
alvik.updateAPDS();
247270
msg_size = packeter.packetC3I('c', alvik.getRed(), alvik.getGreen(), alvik.getBlue());
248-
alvik.serial->write(packeter.msg,msg_size);
271+
// alvik.serial->write(packeter.msg,msg_size);
272+
sendMessage(packeter.msg,msg_size);
249273
break;
250274
case 3:
251275
if (tof.update_rois()){
252276
msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
253-
alvik.serial->write(packeter.msg,msg_size);
277+
// alvik.serial->write(packeter.msg,msg_size);
278+
sendMessage(packeter.msg,msg_size);
254279
}
255280
break;
256281
case 4:
257282
msg_size = packeter.packetC3F('q', alvik.getRoll(), alvik.getPitch(), alvik.getYaw());
258-
alvik.serial->write(packeter.msg,msg_size);
283+
// alvik.serial->write(packeter.msg,msg_size);
284+
sendMessage(packeter.msg,msg_size);
259285
break;
260286
}
261287
sensor_id++;
@@ -271,16 +297,20 @@ void loop(){
271297
alvik.updateKinematics();
272298
// joint speed
273299
msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
274-
alvik.serial->write(packeter.msg,msg_size);
300+
// alvik.serial->write(packeter.msg,msg_size);
301+
sendMessage(packeter.msg,msg_size);
275302
// joint position
276303
msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
277-
alvik.serial->write(packeter.msg, msg_size);
304+
// alvik.serial->write(packeter.msg, msg_size);
305+
sendMessage(packeter.msg,msg_size);
278306
// robot speed
279307
msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
280-
alvik.serial->write(packeter.msg, msg_size);
308+
// alvik.serial->write(packeter.msg, msg_size);
309+
sendMessage(packeter.msg,msg_size);
281310
// pose
282311
msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta());
283-
alvik.serial->write(packeter.msg, msg_size);
312+
// alvik.serial->write(packeter.msg, msg_size);
313+
sendMessage(packeter.msg,msg_size);
284314
}
285315

286316
// acknowledge
@@ -290,7 +320,8 @@ void loop(){
290320
counter_version--;
291321
alvik.getVersion(version[0], version[1], version[2]);
292322
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
293-
alvik.serial->write(packeter.msg,msg_size);
323+
// alvik.serial->write(packeter.msg,msg_size);
324+
sendMessage(packeter.msg,msg_size);
294325
}
295326
if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
296327
if (ack_required == MOVEMENT_ROTATE){
@@ -312,7 +343,8 @@ void loop(){
312343
else{
313344
msg_size = packeter.packetC1B('x', 0);
314345
}
315-
alvik.serial->write(packeter.msg, msg_size);
346+
// alvik.serial->write(packeter.msg, msg_size);
347+
sendMessage(packeter.msg,msg_size);
316348
}
317349

318350
if (millis()-tbehaviours > 100){
@@ -325,14 +357,16 @@ void loop(){
325357
timu=millis();
326358
alvik.updateImu();
327359
msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
328-
alvik.serial->write(packeter.msg,msg_size);
360+
// alvik.serial->write(packeter.msg,msg_size);
361+
sendMessage(packeter.msg,msg_size);
329362
}
330363

331364
// battery update
332365
if (millis()-tbattery>1000){
333366
tbattery = millis();
334367
alvik.updateBMS();
335368
msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage());
336-
alvik.serial->write(packeter.msg,msg_size);
369+
// alvik.serial->write(packeter.msg,msg_size);
370+
sendMessage(packeter.msg,msg_size);
337371
}
338372
}

‎src/Arduino_AlvikCarrier.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,8 @@ int Arduino_AlvikCarrier::begin(){
128128
wire->setClock(400000);
129129

130130
connectExternalI2C();
131-
ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
131+
//ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
132+
ext_wire->begin();
132133

133134

134135
if (beginAPDS()!=0){

0 commit comments

Comments
 (0)