Skip to content

Commit 80c3497

Browse files
eigen-valueeigen-value
committed
mod: restored firmware.ino
feat: firmware_i2c.ino w unoq test
1 parent 13bfd09 commit 80c3497

File tree

5 files changed

+310
-680
lines changed

5 files changed

+310
-680
lines changed

‎examples/firmware/firmware.ino‎

Lines changed: 95 additions & 171 deletions
Original file line numberDiff line numberDiff line change
@@ -16,17 +16,13 @@
1616
#include "sensor_line.h"
1717
#include "sensor_tof_matrix.h"
1818
#include "ucPack.h"
19-
#include "CircularBuffer.h"
20-
21-
#define OUT_BUF_SIZE 512
2219

2320
Arduino_AlvikCarrier alvik;
2421
SensorLine line(EXT_A2,EXT_A1,EXT_A0);
2522
SensorTofMatrix tof(alvik.wire, EXT_GPIO3, EXT_GPIO2);
2623

2724

2825
ucPack packeter(200);
29-
CircularBuffer out_buffer(OUT_BUF_SIZE);
3026

3127
uint8_t code;
3228
uint8_t label;
@@ -36,8 +32,6 @@ uint8_t ack_required = 0;
3632
bool ack_check = false;
3733
uint8_t ack_code = 0;
3834
uint8_t behaviours;
39-
uint8_t cmd;
40-
uint8_t id;
4135

4236
unsigned long tmotor = 0;
4337
unsigned long tsend = 0;
@@ -62,179 +56,26 @@ float x, y, theta;
6256
uint8_t servo_A, servo_B;
6357
float position_left, position_right;
6458

59+
int counter_version = 9;
6560
uint8_t version[3];
6661

6762

68-
uint8_t outBufferFree() {
69-
return OUT_BUF_SIZE - out_buffer.getSize();
70-
}
71-
72-
73-
void receiveEvent(int byte) {
74-
while (alvik.ext_wire->available()) {
75-
packeter.buffer.push(alvik.ext_wire->read());
76-
}
77-
}
78-
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-
22363
void setup(){
22464
alvik.begin();
225-
226-
alvik.ext_wire->onReceive(receiveEvent);
227-
alvik.ext_wire->onRequest(requestEvent);
228-
22965
alvik.disableIlluminator();
23066
alvik.setLeds(COLOR_ORANGE);
23167
alvik.setLedBuiltin(HIGH);
23268
line.begin();
23369
tof.begin();
23470

71+
23572
alvik.getVersion(version[0], version[1], version[2]);
73+
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
74+
alvik.serial->write(packeter.msg,msg_size);
23675

23776
alvik.updateBMS();
77+
msg_size = packeter.packetC1F('p', alvik.getBatteryChargePercentage());
78+
alvik.serial->write(packeter.msg,msg_size);
23879

23980
alvik.setLedBuiltin(LOW);
24081
alvik.setLeds(COLOR_BLACK);
@@ -250,7 +91,9 @@ void setup(){
25091
}
25192

25293
void loop(){
253-
94+
while(alvik.serial->available() > 0) {
95+
packeter.buffer.push(alvik.serial->read());
96+
}
25497
if (packeter.checkPayload()) {
25598
code = packeter.payloadTop();
25699
if (!alvik.isBatteryAlert()){
@@ -380,19 +223,96 @@ void loop(){
380223
alvik.setBehaviour(ALL_BEHAVIOURS, false);
381224
}
382225
break;
383-
case '#':
384-
packeter.unpacketC2B(code, cmd, id);
385-
publicationRequest(cmd, id);
386-
break;
387226
}
388227
}
389228

229+
// sensors publish
230+
if (millis()-tsensor>10){
231+
tsensor=millis();
232+
switch(sensor_id){
233+
case 0:
234+
line.update();
235+
msg_size = packeter.packetC3I('l', line.getLeft(), line.getCenter(), line.getRight());
236+
alvik.serial->write(packeter.msg,msg_size);
237+
break;
238+
case 1:
239+
alvik.updateTouch();
240+
msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
241+
alvik.serial->write(packeter.msg,msg_size);
242+
msg_size = packeter.packetC1B('m', alvik.getMotion());
243+
alvik.serial->write(packeter.msg,msg_size);
244+
break;
245+
case 2:
246+
alvik.updateAPDS();
247+
msg_size = packeter.packetC3I('c', alvik.getRed(), alvik.getGreen(), alvik.getBlue());
248+
alvik.serial->write(packeter.msg,msg_size);
249+
break;
250+
case 3:
251+
if (tof.update_rois()){
252+
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);
254+
}
255+
break;
256+
case 4:
257+
msg_size = packeter.packetC3F('q', alvik.getRoll(), alvik.getPitch(), alvik.getYaw());
258+
alvik.serial->write(packeter.msg,msg_size);
259+
break;
260+
}
261+
sensor_id++;
262+
if (sensor_id>4){
263+
sensor_id=0;
264+
}
265+
}
390266

391-
// motors update
267+
// motors update & publish
392268
if (millis()-tmotor>=20){
393269
tmotor=millis();
394270
alvik.updateMotors();
395271
alvik.updateKinematics();
272+
// joint speed
273+
msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
274+
alvik.serial->write(packeter.msg,msg_size);
275+
// joint position
276+
msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
277+
alvik.serial->write(packeter.msg, msg_size);
278+
// robot speed
279+
msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
280+
alvik.serial->write(packeter.msg, msg_size);
281+
// pose
282+
msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta());
283+
alvik.serial->write(packeter.msg, msg_size);
284+
}
285+
286+
// acknowledge
287+
if (millis()-tack > 100){
288+
tack = millis();
289+
if (counter_version>0){
290+
counter_version--;
291+
alvik.getVersion(version[0], version[1], version[2]);
292+
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
293+
alvik.serial->write(packeter.msg,msg_size);
294+
}
295+
if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
296+
if (ack_required == MOVEMENT_ROTATE){
297+
msg_size = packeter.packetC1B('x', 'R');
298+
}
299+
if (ack_required == MOVEMENT_MOVE){
300+
msg_size = packeter.packetC1B('x', 'M');
301+
}
302+
if (ack_required == MOVEMENT_POSITION){
303+
msg_size = packeter.packetC1B('x', 'P');
304+
}
305+
if (ack_required == MOVEMENT_LEFT){
306+
msg_size = packeter.packetC1B('x', 'P');
307+
}
308+
if (ack_required == MOVEMENT_RIGHT){
309+
msg_size = packeter.packetC1B('x', 'P');
310+
}
311+
}
312+
else{
313+
msg_size = packeter.packetC1B('x', 0);
314+
}
315+
alvik.serial->write(packeter.msg, msg_size);
396316
}
397317

398318
if (millis()-tbehaviours > 100){
@@ -404,11 +324,15 @@ void loop(){
404324
if (millis()-timu>10){
405325
timu=millis();
406326
alvik.updateImu();
327+
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);
407329
}
408330

409331
// battery update
410332
if (millis()-tbattery>1000){
411333
tbattery = millis();
412334
alvik.updateBMS();
335+
msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage());
336+
alvik.serial->write(packeter.msg,msg_size);
413337
}
414338
}

0 commit comments

Comments
 (0)