MetzNet
/
v5_field_control
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed timer

main
noah metz 2023-12-14 19:15:36 -07:00
parent 2123b9aa14
commit 6000159a44
1 changed files with 42 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
v5_field_control.ino
Unescape Escape View File

@ -7,11 +7,14 @@
https://www.arduino.cc/en/Tutorial/BuiltInExamples/DigitalReadSerial
*/
#include <SPI.h>
#include <Wire.h>
#include "CRC16.h"
volatile uint8_t control_flags = 0xC9;
volatile uint32_t timer_end = 0x00000000;
volatile uint32_t length = 0x00000000;
volatile uint32_t last_timer_sent = 0x00000000;
CRC16 crc = CRC16(0x1021, 0xFFFF, 0x0000, false, false);
int read_line = 22;
int rx2 = 16;
@ -65,7 +68,17 @@ typedef struct __attribute__((packed)) {
uint16_t crc;
} pkt_set_poll_rate_t;
CRC16 crc = CRC16(0x1021, 0xFFFF, 0x0000, false, false);
typedef struct __attribute__((packed)) {
uint8_t type;
uint8_t unknown_1;
uint16_t system_status;
uint8_t unknown_2[2];
uint8_t field_status;
uint8_t unknown_3[11];
uint8_t name[10];
uint8_t version[2];
uint16_t crc;
} pkt_poll_t;
void parse_discovery(uint8_t* pkt, size_t len) {
pkt_discovery_t parsed;
@ -142,9 +155,9 @@ void parse_get_poll_rate(uint8_t* pkt, size_t len, uint8_t poll_ms) {
uint16_t calc_crc = crc.calc();
if (parsed_crc != calc_crc) {
Serial.printf("Got init_3 packet with bad crc(%04x != %04x\n", calc_crc, parsed_crc);
Serial.printf("Got get_poll_rate packet with bad crc(%04x != %04x\n", calc_crc, parsed_crc);
} else {
Serial.printf("Got init_3 packet with matching crc(%04x)\n", calc_crc);
Serial.printf("Got get_poll_rate packet with matching crc(%04x)\n", calc_crc);
}
pkt_set_poll_rate_t response;
@ -190,10 +203,10 @@ void send_state(uint8_t control_flags, uint32_t timer) {
response[1] = control_flags;
response[2] = 0x00;
response[3] = 0x00;
response[4] = (timer >> 24 & 0xFF);
response[5] = (timer >> 16 & 0xFF);
response[6] = (timer >> 8 & 0xFF);
response[7] = (timer >> 0 & 0xFF);
response[4] = ((timer >> 0) & 0xFF);
response[5] = ((timer >> 8) & 0xFF);
response[6] = ((timer >> 16) & 0xFF);
response[7] = ((timer >> 24) & 0xFF);
response[8] = 0x03;
crc.restart();
@ -209,22 +222,6 @@ void send_state(uint8_t control_flags, uint32_t timer) {
digitalWrite(read_line, LOW);
}
typedef struct __attribute__((packed)) {
uint8_t type;
uint8_t unknown_1;
uint16_t system_status;
uint8_t unknown_2[2];
uint8_t field_status;
uint8_t unknown_3[11];
uint8_t name[10];
uint8_t version[2];
uint16_t crc;
} pkt_poll_t;
volatile uint8_t control_flags = 0xC9;
volatile uint32_t timer = 0x00002000;
#define POLL_COUNT_MAX 5
void parse_poll(uint8_t* pkt, size_t len) {
pkt_poll_t* parsed = (pkt_poll_t*)pkt;
crc.restart();
@ -237,10 +234,22 @@ void parse_poll(uint8_t* pkt, size_t len) {
//Serial.printf("parsed poll with valid CRC 0x%04x, system status 0x%04x, field status 0x%02x, team name %s\n", htons(parsed->crc), htons(parsed->system_status), parsed->field_status, parsed->name);
}
if (parsed->field_status == control_flags) {
uint32_t now = millis();
uint32_t current_timer = 0;
if (now < timer_end) {
current_timer = (timer_end - millis()) & 0xFFFFFF00;
} else {
current_timer = length;
control_flags = 0xC9;
}
if ((parsed->field_status == control_flags) && (current_timer == last_timer_sent)) {
send_ack(0xA7, 0x01);
} {
send_state(control_flags, timer);
} else if (control_flags == 0xC9) {
send_state(0xC9, length);
} else {
last_timer_sent = current_timer;
send_state(control_flags, current_timer);
}
}
@ -251,8 +260,9 @@ void on_rx(void) {
return;
}
int read = Serial.readBytes(pkt, n);
sscanf((const char*)pkt, "0x%2hhx", &control_flags);
Serial.printf("Parsed new control flags: 0x%02x\n", control_flags);
sscanf((const char*)pkt, "0x%2hhx%8x", &control_flags, &length);
timer_end = millis() + length;
Serial.printf("Parsed new control flags: 0x%02x, length 0x%08x\n", control_flags, length);
}
void on_rx_2(void) {
@ -273,7 +283,7 @@ void on_rx_2(void) {
parse_init(pkt, read);
break;
case 0x11:
parse_get_poll_rate(pkt, read, 50);
parse_get_poll_rate(pkt, read, 20);
break;
case 0x02:
switch (pkt[1]){