You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
85 lines
1.3 KiB
C++
85 lines
1.3 KiB
C++
|
|
#include "src/setting_mode.h"
|
|
#include "src/normal_mode.h"
|
|
|
|
#include "src/hexapod/debug.h"
|
|
#include "src/hexapod/hexapod.h"
|
|
|
|
//
|
|
// mode
|
|
// 0: normal mode
|
|
// 1: setting mode (for servo calibration)
|
|
//
|
|
static int _mode = 0;
|
|
|
|
static void log_output(const char* log) {
|
|
Serial.println(log);
|
|
}
|
|
|
|
//
|
|
// wait for N secs, enter setting mode if pressed USR button
|
|
//
|
|
void boot_wait(int wait_for = 2000) {
|
|
digitalWrite(LED_BUILTIN, HIGH);
|
|
|
|
pinMode(6, INPUT);
|
|
while (wait_for > 0) {
|
|
if (HIGH == digitalRead(6)) {
|
|
_mode = 1;
|
|
break;
|
|
}
|
|
delay(100);
|
|
wait_for -= 100;
|
|
}
|
|
|
|
digitalWrite(LED_BUILTIN, LOW);
|
|
}
|
|
|
|
//
|
|
// Do common setup and mode-specific setup
|
|
//
|
|
void setup() {
|
|
//Initialize serial and wait for port to open:
|
|
Serial.begin(115200);
|
|
Serial.println("Starting...");
|
|
|
|
//
|
|
// Common setup()
|
|
//
|
|
|
|
// Initialize GPIO
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
|
|
// Workaround for beta PCB
|
|
pinMode(2, INPUT);
|
|
pinMode(3, INPUT);
|
|
|
|
boot_wait();
|
|
|
|
hexapod::initLogOutput(log_output, millis);
|
|
hexapod::Hexapod.init(_mode == 1);
|
|
|
|
//
|
|
// Mode-specific setup()
|
|
//
|
|
|
|
if (_mode == 0) {
|
|
normal_setup();
|
|
}
|
|
else if (_mode == 1) {
|
|
setting_setup();
|
|
}
|
|
|
|
Serial.print("Started, mode=");
|
|
Serial.println(_mode);
|
|
}
|
|
|
|
void loop() {
|
|
if (_mode == 0) {
|
|
normal_loop();
|
|
}
|
|
else if (_mode == 1) {
|
|
setting_loop();
|
|
}
|
|
}
|