Skip to content

Commit d038faf

Browse files
MaartenS11MaartenS11
authored
Reversible primitives (#250)
* Initial work on io state inspection This allows the debugger to inspect the current io state and load io states from snapshots. Currently only works with chip_digital_write. * Obtain io state through portOutput/InputRegister There currently is no support for analog values. * Remove io map from Module struct * Use NUM_DIGITAL_PINS because SOC_GPIO_PIN_COUNT is esp specific * Serialize/deserialize pin mode in snapshot data * Use digitalPinToBitMask in an attempt to make the code more microcontroller agnostic * Port WARDuino to the Zephyr RTOS The main reason for this is to support boards such as Open Bot Brain which uses an stm32l496zg. By porting to Zephyr we can also add support for some other boards with relative ease. * Try correcting for drift in the servo motor driver * Fixed cmake with branch rebase * Use vfprintf instead of fprintf to handle variadic arguments * Loading snapshots should work again now * Hacky but it's starting to work * Somewhat hacky way of extracting the current sensor value * Put uart heartbeat for sensors in a thread + colour_sensor now returns sensor value * Allow controlling multiple motors * Added anti-stall system to motor driver This makes it so if the motor tries to move but it's not actually moving the voltage applied to the motor is increased until it moves and completes the operation. * Make motor driver reversible * Added some things needed to make uart sensor work * Added invoke_primitive function that makes it easy to call primitives with arguments * New API for reversible primitives Reversible primitives are now defined by using def_prim_reverse and def_prim_serialize. * Rename PinState to IOStateElement and clean up debugger ioState interrupt * Clang format * Only run the reverse operation for the primitive that was just executed and not for all primitives This particularly important for motor primitives that for example drive two motors at the same time vs a primitive that drives one motor at a time, executing the wrong reverse operation will result in the correct angles but the wrong position. * Use ms to configure stall timeout for motors * Adjust speed faster when stalling Also added helper functions to reduce duplicated code between drive_motor_degrees and drive_motor_degrees_relative. * Don't wait for the full 10ms if we have already detected movement * Stripped out open bot brain primitives * Clang format * Make chip_digital_write on arduino reversible using the new macros * Remove io-arduino.cpp and fix usage of PinState * def_prim_serialize should not return anything for chip_digital_write * Add extra clause to if so int32_t and int are bot considered I32 in create_stack template function * Add new macros + restore_external_state + get_io_state to ESP-IDF * Install primitive reverse for chip_digital_write * Just use NUM_PRIMITIVES and set it to 5 * Move last remaining bits of io.h into primitives.h * Add reversible primitive code to emulated.cpp * Fixed zephyr build
1 parent f27ce03 commit d038faf

File tree

9 files changed

+386
-13
lines changed

9 files changed

+386
-13
lines changed

‎platforms/Zephyr/boards/stm32l496g_disco.overlay‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -107,15 +107,20 @@
107107

108108
pwms =
109109
<&pwm8 3 10000 PWM_POLARITY_NORMAL>,
110-
<&pwm8 4 10000 PWM_POLARITY_NORMAL>;
110+
<&pwm8 4 10000 PWM_POLARITY_NORMAL>,
111+
<&pwm8 1 10000 PWM_POLARITY_NORMAL>,
112+
<&pwm8 2 10000 PWM_POLARITY_NORMAL>;
113+
114+
warduino-uarts =
115+
<&usart1>;
111116
};
112117
};
113118

114119
&timers8 {
115120
status = "okay";
116121
pwm8: pwm {
117122
status = "okay";
118-
pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9>;
123+
pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9 &tim8_ch1_pc6 &tim8_ch2_pc7>;
119124
pinctrl-names =
120125
"default";
121126
};

‎src/Debug/debugger.cpp‎

Lines changed: 58 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
#endif
1111

1212
#include "../Memory/mem.h"
13+
#include "../Primitives/primitives.h"
1314
#include "../Utils//util.h"
1415
#include "../Utils/macros.h"
1516
#include "../WARDuino/CallbackHandler.h"
@@ -714,11 +715,18 @@ bool Debugger::handlePushedEvent(char *bytes) const {
714715
}
715716

716717
void Debugger::snapshot(Module *m) {
717-
uint16_t numberBytes = 10;
718-
uint8_t state[] = {
719-
pcState, breakpointsState, callstackState, globalsState,
720-
tableState, memoryState, branchingTableState, stackState,
721-
callbacksState, eventsState};
718+
uint16_t numberBytes = 11;
719+
uint8_t state[] = {pcState,
720+
breakpointsState,
721+
callstackState,
722+
globalsState,
723+
tableState,
724+
memoryState,
725+
branchingTableState,
726+
stackState,
727+
callbacksState,
728+
eventsState,
729+
ioState};
722730
inspect(m, numberBytes, state);
723731
}
724732

@@ -843,6 +851,26 @@ void Debugger::inspect(Module *m, uint16_t sizeStateArray, uint8_t *state) {
843851
addComma = true;
844852
break;
845853
}
854+
case ioState: {
855+
this->channel->write("%s", addComma ? "," : "");
856+
this->channel->write("\"io\": [");
857+
bool comma = false;
858+
std::vector<IOStateElement *> external_state = get_io_state(m);
859+
for (auto state_elem : external_state) {
860+
this->channel->write("%s{", comma ? ", " : "");
861+
this->channel->write(
862+
R"("key": "%s", "output": %s, "value": %d)",
863+
state_elem->key.c_str(),
864+
state_elem->output ? "true" : "false",
865+
state_elem->value);
866+
this->channel->write("}");
867+
comma = true;
868+
delete state_elem;
869+
}
870+
this->channel->write("]");
871+
addComma = true;
872+
break;
873+
}
846874
default: {
847875
debug("dumpExecutionState: Received unknown state request\n");
848876
break;
@@ -953,7 +981,8 @@ bool Debugger::saveState(Module *m, uint8_t *interruptData) {
953981
uint8_t *program_state = nullptr;
954982
uint8_t *end_state = nullptr;
955983
program_state = interruptData + 1; // skip interruptLoadSnapshot
956-
end_state = program_state + read_B32(&program_state);
984+
uint32_t len = read_B32(&program_state);
985+
end_state = program_state + len;
957986

958987
debug("saving program_state\n");
959988
while (program_state < end_state) {
@@ -1169,6 +1198,29 @@ bool Debugger::saveState(Module *m, uint8_t *interruptData) {
11691198
}
11701199
break;
11711200
}
1201+
case ioState: {
1202+
debug("receiving ioState\n");
1203+
uint8_t io_state_count = *program_state++;
1204+
std::vector<IOStateElement> external_state;
1205+
external_state.reserve(io_state_count);
1206+
for (int i = 0; i < io_state_count; i++) {
1207+
IOStateElement state_elem;
1208+
state_elem.key = "";
1209+
char c = (char)*program_state++;
1210+
while (c != '\0') {
1211+
state_elem.key += c;
1212+
c = (char)*program_state++;
1213+
}
1214+
state_elem.output = *program_state++;
1215+
state_elem.value = (int)read_B32(&program_state);
1216+
external_state.emplace_back(state_elem);
1217+
debug("pin %s(%s) = %d\n", state_elem.key.c_str(),
1218+
state_elem.output ? "output" : "input",
1219+
state_elem.value);
1220+
}
1221+
restore_external_state(m, external_state);
1222+
break;
1223+
}
11721224
default: {
11731225
FATAL("saveState: Received unknown program state\n");
11741226
}

‎src/Debug/debugger.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,8 @@ enum ExecutionState {
4747
branchingTableState = 0x07,
4848
stackState = 0x08,
4949
callbacksState = 0x09,
50-
eventsState = 0x0A
50+
eventsState = 0x0A,
51+
ioState = 0x0B,
5152
};
5253

5354
enum InterruptTypes {

‎src/Primitives/arduino.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -150,15 +150,32 @@ int prim_index = 0;
150150
PrimitiveEntry *p = &primitives[prim_index++]; \
151151
p->name = #prim_name; \
152152
p->f = &(prim_name); \
153+
p->f_reverse = nullptr; \
154+
p->f_serialize_state = nullptr; \
153155
} else { \
154156
FATAL("pim_index out of bounds"); \
155157
} \
156158
}
157159

160+
#define install_primitive_reverse(prim_name) \
161+
{ \
162+
PrimitiveEntry *p = &primitives[prim_index - 1]; \
163+
p->f_reverse = &(prim_name##_reverse); \
164+
p->f_serialize_state = &(prim_name##_serialize); \
165+
}
166+
158167
#define def_prim(function_name, type) \
159168
Type function_name##_type = type; \
160169
bool function_name(Module *m)
161170

171+
#define def_prim_reverse(function_name) \
172+
void function_name##_reverse(Module *m, \
173+
std::vector<IOStateElement> external_state)
174+
175+
#define def_prim_serialize(function_name) \
176+
void function_name##_serialize( \
177+
std::vector<IOStateElement *> &external_state)
178+
162179
// TODO: use fp
163180
#define pop_args(n) m->sp -= n
164181
#define get_arg(m, arg) m->stack[(m)->sp - (arg)].value
@@ -491,6 +508,36 @@ def_prim(chip_digital_write, twoToNoneU32) {
491508
return true;
492509
}
493510

511+
def_prim_reverse(chip_digital_write) {
512+
for (IOStateElement state : external_state) {
513+
if (!state.output) {
514+
continue;
515+
}
516+
517+
if (state.key[0] == 'p') {
518+
invoke_primitive(m, "chip_digital_write", stoi(state.key.substr(1)),
519+
(uint32_t)state.value);
520+
}
521+
}
522+
}
523+
524+
def_prim_serialize(chip_digital_write) {
525+
for (int i = 0; i < NUM_DIGITAL_PINS; i++) {
526+
uint32_t bit_mask = digitalPinToBitMask(i);
527+
auto *state = new IOStateElement();
528+
state->key = "p" + std::to_string(i);
529+
uint8_t port = digitalPinToPort(i);
530+
if (*portModeRegister(port) & bit_mask) { // DDR
531+
state->output = true;
532+
state->value = (*portOutputRegister(port) & bit_mask) > 0;
533+
} else {
534+
state->output = false;
535+
state->value = (*portInputRegister(port) & bit_mask) > 0;
536+
}
537+
external_state.push_back(state);
538+
}
539+
}
540+
494541
def_prim(chip_delay, oneToNoneU32) {
495542
delay(arg0.uint32);
496543
pop_args(1);
@@ -954,6 +1001,7 @@ void install_primitives() {
9541001

9551002
install_primitive(chip_pin_mode);
9561003
install_primitive(chip_digital_write);
1004+
install_primitive_reverse(chip_digital_write);
9571005
install_primitive(chip_delay);
9581006
install_primitive(chip_digital_read);
9591007
install_primitive(chip_analog_read);
@@ -1026,3 +1074,44 @@ bool resolve_external_memory(char *symbol, Memory **val) {
10261074
FATAL("Could not find memory %s \n", symbol);
10271075
return false;
10281076
}
1077+
1078+
//------------------------------------------------------
1079+
// Restore external state when restoring a snapshot
1080+
//------------------------------------------------------
1081+
void restore_external_state(Module *m,
1082+
std::vector<IOStateElement> external_state) {
1083+
uint8_t opcode = *m->pc_ptr;
1084+
// TODO: Maybe primitives can also be called using the other call
1085+
// instructions such as call_indirect
1086+
// maybe there should just be a function that checks if a certain function
1087+
// is being called that handles all these cases?
1088+
if (opcode == 0x10) { // call opcode
1089+
uint8_t *pc_copy = m->pc_ptr + 1;
1090+
uint32_t fidx = read_LEB_32(&pc_copy);
1091+
if (fidx < m->import_count) {
1092+
for (auto &primitive : primitives) {
1093+
if (!strcmp(primitive.name, m->functions[fidx].import_field)) {
1094+
if (primitive.f_reverse) {
1095+
debug("Reversing action for primitive %s\n",
1096+
primitive.name);
1097+
primitive.f_reverse(m, external_state);
1098+
}
1099+
return;
1100+
}
1101+
}
1102+
}
1103+
}
1104+
}
1105+
1106+
//------------------------------------------------------
1107+
// Serialize external state into a snapshot
1108+
//------------------------------------------------------
1109+
std::vector<IOStateElement *> get_io_state(Module *m) {
1110+
std::vector<IOStateElement *> ioState;
1111+
for (auto &primitive : primitives) {
1112+
if (primitive.f_serialize_state) {
1113+
primitive.f_serialize_state(ioState);
1114+
}
1115+
}
1116+
return ioState;
1117+
}

‎src/Primitives/emulated.cpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,15 +47,32 @@ double sensor_emu = 0;
4747
PrimitiveEntry *p = &primitives[prim_index++]; \
4848
p->name = #prim_name; \
4949
p->f = &(prim_name); \
50+
p->f_reverse = nullptr; \
51+
p->f_serialize_state = nullptr; \
5052
} else { \
5153
FATAL("pim_index out of bounds"); \
5254
} \
5355
}
5456

57+
#define install_primitive_reverse(prim_name) \
58+
{ \
59+
PrimitiveEntry *p = &primitives[prim_index - 1]; \
60+
p->f_reverse = &(prim_name##_reverse); \
61+
p->f_serialize_state = &(prim_name##_serialize); \
62+
}
63+
5564
#define def_prim(function_name, type) \
5665
Type function_name##_type = type; \
5766
bool function_name(Module *m)
5867

68+
#define def_prim_reverse(function_name) \
69+
void function_name##_reverse(Module *m, \
70+
std::vector<IOStateElement> external_state)
71+
72+
#define def_prim_serialize(function_name) \
73+
void function_name##_serialize( \
74+
std::vector<IOStateElement *> &external_state)
75+
5976
// TODO: use fp
6077
#define pop_args(n) m->sp -= n
6178
#define get_arg(m, arg) m->stack[(m)->sp - (arg)].value
@@ -572,4 +589,42 @@ bool resolve_external_memory(char *symbol, Memory **val) {
572589
return false;
573590
}
574591

592+
//------------------------------------------------------
593+
// Restore external state when restoring a snapshot
594+
//------------------------------------------------------
595+
void restore_external_state(Module *m,
596+
std::vector<IOStateElement> external_state) {
597+
uint8_t opcode = *m->pc_ptr;
598+
// TODO: Maybe primitives can also be called using the other call
599+
// instructions such as call_indirect
600+
// maybe there should just be a function that checks if a certain function
601+
// is being called that handles all these cases?
602+
if (opcode == 0x10) { // call opcode
603+
uint8_t *pc_copy = m->pc_ptr + 1;
604+
uint32_t fidx = read_LEB_32(&pc_copy);
605+
if (fidx < m->import_count) {
606+
for (auto &primitive : primitives) {
607+
if (!strcmp(primitive.name, m->functions[fidx].import_field)) {
608+
if (primitive.f_reverse) {
609+
debug("Reversing action for primitive %s\n",
610+
primitive.name);
611+
primitive.f_reverse(m, external_state);
612+
}
613+
return;
614+
}
615+
}
616+
}
617+
}
618+
}
619+
620+
std::vector<IOStateElement *> get_io_state(Module *m) {
621+
std::vector<IOStateElement *> ioState;
622+
for (auto &primitive : primitives) {
623+
if (primitive.f_serialize_state) {
624+
primitive.f_serialize_state(ioState);
625+
}
626+
}
627+
return ioState;
628+
}
629+
575630
#endif // ARDUINO

0 commit comments

Comments
 (0)