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.
221 lines
6.0 KiB
C++
221 lines
6.0 KiB
C++
#include <Arduino.h>
|
|
|
|
#ifndef __LREMOTE_H__
|
|
#include <LRemote.h>
|
|
#define __LREMOTE_H__
|
|
#endif
|
|
|
|
#include "linkit_control/ui_controls.h"
|
|
#include "hexapod/debug.h"
|
|
#include "hexapod/hexapod.h"
|
|
|
|
#define WIDTH 9
|
|
#define HEIGHT 18
|
|
#define L_W WIDTH
|
|
#define L_H 1
|
|
#define B_W 2
|
|
#define B_H 2
|
|
#define T_W 5
|
|
#define T_H 2
|
|
|
|
#define L_X(g) 0
|
|
#define L_Y(g) (L_H+T_H)*g
|
|
#define Bl_X(g) 0
|
|
#define Bl_Y(g) (L_H+T_H)*g+L_H
|
|
#define T_X(g) B_W
|
|
#define T_Y(g) (L_H+T_H)*g+L_H
|
|
#define Br_X(g) B_W+T_W
|
|
#define Br_Y(g) (L_H+T_H)*g+L_H
|
|
|
|
//---------------------------------------------------------------------------
|
|
// UI Group: Adjust setting
|
|
|
|
static Label label1("Select leg", L_X(0), L_Y(0), L_W, L_H);
|
|
static Label text1("", T_X(0), T_Y(0), T_W, T_H, RC_PINK);
|
|
static Button button1l(10, "<", Bl_X(0), Bl_Y(0), B_W, B_H, RC_BLUE);
|
|
static Button button1r(11, ">", Br_X(0), Br_Y(0), B_W, B_H, RC_BLUE);
|
|
|
|
static Label label2("Select joint", L_X(1), L_Y(1), L_W, L_H);
|
|
static Label text2("", T_X(1), T_Y(1), T_W, T_H, RC_PINK);
|
|
static Button button2l(20, "<", Bl_X(1), Bl_Y(1), B_W, B_H, RC_BLUE);
|
|
static Button button2r(21, ">", Br_X(1), Br_Y(1), B_W, B_H, RC_BLUE);
|
|
|
|
static Label label3("Servo offset", L_X(2), L_Y(2), L_W, L_H);
|
|
static Label text3("0", T_X(2), T_Y(2), T_W, T_H, RC_PINK);
|
|
static Button button3l(30, "-", Bl_X(2), Bl_Y(2), B_W, B_H, RC_BLUE);
|
|
static Button button3r(31, "+", Br_X(2), Br_Y(2), B_W, B_H, RC_BLUE);
|
|
|
|
static Label label4("Servo scale", L_X(3), L_Y(3), L_W, L_H);
|
|
static Label text4("0", T_X(3), T_Y(3), T_W, T_H, RC_PINK);
|
|
static Button button4l(40, "-", Bl_X(3), Bl_Y(3), B_W, B_H, RC_BLUE);
|
|
static Button button4r(41, "+", Br_X(3), Br_Y(3), B_W, B_H, RC_BLUE);
|
|
|
|
static Button buttonSave(60, "Save", 0, Bl_Y(5), WIDTH, B_H, RC_GREEN);
|
|
|
|
static ButtonGroup btnGroup;
|
|
|
|
//---------------------------------------------------------------------------
|
|
// UI Group: Test
|
|
|
|
static Label label5("Servo test", L_X(4), L_Y(4), L_W, L_H);
|
|
static Button button5l(-45, "-45°", 0, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
|
static Button button5c(0, "0°", WIDTH/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
|
static Button button5r(45, "+45°", WIDTH*2/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
|
|
|
static ButtonGroup btnGroupTest;
|
|
|
|
//---------------------------------------------------------------------------
|
|
|
|
const static String _leg_table[] = {
|
|
"front right",
|
|
"right",
|
|
"rear right",
|
|
"rear left",
|
|
"left",
|
|
"front left",
|
|
};
|
|
|
|
const static String _joint_table[] = {
|
|
"body",
|
|
"thigh",
|
|
"foot",
|
|
};
|
|
|
|
static int _cur_leg = 0;
|
|
static int _cur_joint = 0;
|
|
static int _cur_offset = 0;
|
|
static int _cur_scale = 0;
|
|
static int _test_angle[3*6] = {0};
|
|
|
|
void update_text(void) {
|
|
text1.updateText(_leg_table[_cur_leg]);
|
|
text2.updateText(_joint_table[_cur_joint]);
|
|
text3.updateText(String(_cur_offset));
|
|
text4.updateText(String(1+0.01*_cur_scale));
|
|
}
|
|
|
|
static uint8_t led_blink_state = 0;
|
|
|
|
void setting_setup(void) {
|
|
|
|
digitalWrite(LED_BUILTIN, HIGH);
|
|
|
|
//
|
|
// initialize config mode UI
|
|
//
|
|
LRemote.setName("Hexapod (S)");
|
|
LRemote.setOrientation(RC_PORTRAIT);
|
|
LRemote.setGrid(WIDTH, HEIGHT);
|
|
|
|
LRemote_addControls({
|
|
&label1, &text1, &button1l, &button1r, // row1
|
|
&label2, &text2, &button2l, &button2r, // row2
|
|
&label3, &text3, &button3l, &button3r, // row3
|
|
&label4, &text4, &button4l, &button4r, // row4
|
|
&label5, &button5l, &button5c, &button5r, // row5
|
|
&buttonSave // row6
|
|
});
|
|
|
|
//
|
|
// Button group
|
|
//
|
|
btnGroup.addControls({
|
|
&button1l, &button1r,
|
|
&button2l, &button2r,
|
|
&button3l, &button3r,
|
|
&button4l, &button4r,
|
|
&buttonSave
|
|
});
|
|
|
|
btnGroup.config([](int btn_id, ButtonGroup::mode mode) {
|
|
if (mode != ButtonGroup::mode::DOWN && mode != ButtonGroup::mode::REPEAT)
|
|
return;
|
|
|
|
Serial.print("button id: ");
|
|
Serial.print(String(btn_id));
|
|
Serial.println(" is triggered");
|
|
|
|
int g = btn_id / 10;
|
|
int lr = btn_id % 10;
|
|
|
|
switch(g) {
|
|
case 1:
|
|
_cur_leg = (_cur_leg + 6 + (lr ? 1 : -1)) % 6;
|
|
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
|
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
|
break;
|
|
case 2:
|
|
_cur_joint = (_cur_joint + 3 + (lr ? 1 : -1)) % 3;
|
|
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
|
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
|
break;
|
|
case 3:
|
|
_cur_offset += (lr ? 1 : -1);
|
|
hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
|
break;
|
|
case 4:
|
|
_cur_scale += (lr ? 1 : -1);
|
|
hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
|
break;
|
|
case 6:
|
|
hexapod::Hexapod.calibrationSave();
|
|
}
|
|
update_text();
|
|
});
|
|
|
|
//
|
|
// Button group: Testing
|
|
//
|
|
btnGroupTest.addControls({
|
|
&button5l, &button5c, &button5r
|
|
});
|
|
|
|
btnGroupTest.config([](int btn_id, ButtonGroup::mode mode) {
|
|
if (mode != ButtonGroup::mode::DOWN)
|
|
return;
|
|
_test_angle[_cur_leg*3+_cur_joint] = btn_id;
|
|
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
|
delay(20);
|
|
});
|
|
|
|
//
|
|
// Ready to go
|
|
//
|
|
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
|
update_text();
|
|
|
|
for(auto i=0;i<6;i++) {
|
|
for(auto j=0;j<3;j++) {
|
|
hexapod::Hexapod.calibrationTest(i, j, _test_angle[i*3+j]);
|
|
}
|
|
}
|
|
|
|
LRemote.begin();
|
|
}
|
|
|
|
void setting_loop(void) {
|
|
|
|
// check if we are connect by some
|
|
// BLE central device, e.g. an mobile app
|
|
if(!LRemote.connected()) {
|
|
delay(500);
|
|
led_blink_state = !led_blink_state;
|
|
} else {
|
|
// The interval between button down/up
|
|
// can be very short - e.g. a quick tap
|
|
// on the screen.
|
|
// We could lose some event if we
|
|
// delay something like 100ms.
|
|
delay(15);
|
|
led_blink_state = 1;
|
|
}
|
|
digitalWrite(LED_BUILTIN, led_blink_state ? HIGH : LOW);
|
|
|
|
// Process the incoming BLE write request
|
|
// and translate them to control events
|
|
LRemote.process();
|
|
|
|
btnGroup.process();
|
|
btnGroupTest.process();
|
|
}
|