Skip to content

Commit dcaa5c6

Browse files
facchinmfacchinm
committed
add menu, fix interrupts and namespaces
1 parent ba9feee commit dcaa5c6

File tree

3 files changed

+213
-17
lines changed

3 files changed

+213
-17
lines changed

‎src/Braccio++.h

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ class MotorsWrapper {
3838
class BraccioClass {
3939
public:
4040
BraccioClass() {}
41-
bool begin(positionMode _positionMode);
41+
bool begin(positionMode _positionMode = pmSYNC);
4242
// setters
4343
MotorsWrapper move(int joint_index) {
4444
MotorsWrapper wrapper(servos, joint_index);
@@ -53,6 +53,9 @@ class BraccioClass {
5353
void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6, float& a7);
5454
int position(int joint_index);
5555
float angle(int joint_index);
56+
bool connected(int joint_index) {
57+
return _connected[joint_index];
58+
}
5659

5760
protected:
5861
// ioexpander APIs
@@ -70,14 +73,8 @@ class BraccioClass {
7073
class PD_UFP_c PD_UFP;
7174
TCA6424A expander = TCA6424A(TCA6424A_ADDRESS_ADDR_HIGH);
7275
Backlight bl;
73-
Adafruit_ST7789 gfx = Adafruit_ST7789(&SPI, 10, 9, -1);
7476

75-
const int BTN_LEFT = 5;
76-
const int BTN_RIGHT = 4;
77-
const int BTN_UP = 2;
78-
const int BTN_DOWN = 3;
79-
const int BTN_SEL = A0;
80-
const int BTN_ENTER = A1;
77+
bool _connected[8];
8178

8279
#ifdef __MBED__
8380
rtos::EventFlags pd_events;
@@ -107,14 +104,18 @@ class BraccioClass {
107104

108105
extern BraccioClass Braccio;
109106

107+
struct __callback__container__ {
108+
mbed::Callback<void()> fn;
109+
};
110110

111+
inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func, PinStatus mode) {
112+
struct __callback__container__* a = new __callback__container__();
113+
a->fn = func;
114+
auto callback = [](void* a) -> void {
115+
((__callback__container__*)a)->fn();
116+
};
111117

112-
113-
114-
115-
116-
117-
118-
118+
attachInterruptParam(interruptNum, callback, mode, (void*)a);
119+
}
119120

120121
#endif //__BRACCIO_PLUSPLUS_H__

‎src/Braccio.cpp

Lines changed: 126 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,129 @@
11
#include "Braccio++.h"
2+
#include "menu.impl"
23

34
bool BraccioClass::begin(positionMode _positionMode) {
4-
5-
}
5+
6+
Serial.begin(115200);
7+
8+
Wire.begin();
9+
SPI.begin();
10+
11+
PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
12+
13+
#ifdef __MBED__
14+
static rtos::Thread th;
15+
th.start(mbed::callback(this, &BraccioClass::pd_thread));
16+
attachInterrupt(PIN_FUSB302_INT, mbed::callback(this, &BraccioClass::unlock_pd_semaphore), FALLING);
17+
pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 0.06f);
18+
#endif
19+
20+
pinMode(1, INPUT_PULLUP);
21+
22+
bl.begin();
23+
if (bl.getChipID() != 0xCE) {
24+
return false;
25+
}
26+
bl.setColor(red);
27+
28+
int ret = expander.testConnection();
29+
30+
if (ret == false) {
31+
return ret;
32+
}
33+
34+
for (int i = 0; i < 14; i++) {
35+
expander.setPinDirection(i, 0);
36+
}
37+
38+
// Set SLEW to low
39+
expander.setPinDirection(21, 0); // P25 = 8 * 2 + 5
40+
expander.writePin(21, 0);
41+
42+
// Set TERM to HIGH (default)
43+
expander.setPinDirection(19, 0); // P23 = 8 * 2 + 3
44+
expander.writePin(19, 1);
45+
46+
expander.setPinDirection(18, 0); // P22 = 8 * 2 + 2
47+
expander.writePin(18, 0); // reset LCD
48+
expander.writePin(18, 1); // LCD out of reset
49+
50+
braccio::encoder.begin();
51+
52+
braccio::gfx.init(240, 240);
53+
braccio::gfx.setRotation(2);
54+
braccio::gfx.setTextSize(textScale);//test scalling
55+
braccio::gfx.setTextWrap(false);
56+
braccio::gfx.fillScreen(ST7735_BLACK);
57+
//gfx.setFont(&FreeMono12pt7b);
58+
braccio::gfx.setTextColor(ST7735_RED, ST7735_BLACK);
59+
braccio::gfx.println("Arduino\nBraccio++");
60+
61+
#ifdef __MBED__
62+
static rtos::Thread display_th;
63+
display_th.start(mbed::callback(this, &BraccioClass::display_thread));
64+
#endif
65+
66+
braccio::gfx.println("Please\nconnect\npower");
67+
68+
while (!PD_UFP.is_PPS_ready() /* && !encoder.menu_interrupt */) {
69+
pd_mutex.lock();
70+
//PD_UFP.print_status(Serial);
71+
PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
72+
pd_mutex.unlock();
73+
}
74+
75+
servos->begin();
76+
servos->setPositionMode(pmSYNC);
77+
78+
braccio::gfx.println("Get Ready!");
79+
80+
#ifdef __MBED__
81+
static rtos::Thread connected_th;
82+
connected_th.start(mbed::callback(this, &BraccioClass::motors_connected_thread));
83+
#endif
84+
}
85+
86+
87+
void BraccioClass::pd_thread() {
88+
while (1) {
89+
pd_events.wait_any(0xFF);
90+
pd_mutex.lock();
91+
PD_UFP.run();
92+
pd_mutex.unlock();
93+
if (PD_UFP.is_power_ready() && PD_UFP.is_PPS_ready()) {
94+
Serial.println("error");
95+
while (1) {}
96+
}
97+
}
98+
}
99+
100+
void BraccioClass::display_thread() {
101+
while (1) {
102+
if ((braccio::encoder.menu_running) && (braccio::encoder.menu_interrupt)) {
103+
braccio::encoder.menu_interrupt = false;
104+
braccio::nav.doInput();
105+
braccio::nav.doOutput();
106+
}
107+
yield();
108+
}
109+
}
110+
111+
void BraccioClass::motors_connected_thread() {
112+
while (1) {
113+
for (int i = 1; i < 7; i++) {
114+
_connected[i] = (servos->ping(i) == 0);
115+
Serial.print(String(i) + ": ");
116+
Serial.println(_connected[i]);
117+
pd_mutex.lock();
118+
if (_connected[i]) {
119+
setGreen(i);
120+
} else {
121+
setRed(i);
122+
}
123+
pd_mutex.unlock();
124+
}
125+
delay(1000);
126+
}
127+
}
128+
129+
BraccioClass Braccio;

‎src/menu.impl

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
/*
2+
UGLY MENU STUFF
3+
*/
4+
5+
namespace braccio {
6+
7+
const int BTN_LEFT = 5;
8+
const int BTN_RIGHT = 4;
9+
const int BTN_UP = 2;
10+
const int BTN_DOWN = 3;
11+
const int BTN_SEL = A0;
12+
const int BTN_ENTER = A1;
13+
14+
Adafruit_ST7789 gfx = Adafruit_ST7789(&SPI, 10, 9, -1);
15+
16+
static int selTest = 0;
17+
static SELECT(selTest, selMenu, "Select", doNothing, noEvent, noStyle
18+
, VALUE("Zero", 0, doNothing, noEvent)
19+
, VALUE("One", 1, doNothing, noEvent)
20+
, VALUE("Two", 2, doNothing, noEvent)
21+
);
22+
23+
static int chooseTest = -1;
24+
static CHOOSE(chooseTest, chooseMenu, "Choose", doNothing, noEvent, noStyle
25+
, VALUE("First", 1, doNothing, noEvent)
26+
, VALUE("Second", 2, doNothing, noEvent)
27+
, VALUE("Third", 3, doNothing, noEvent)
28+
, VALUE("Last", -1, doNothing, noEvent)
29+
);
30+
31+
static MENU(subMenu, "Sub-Menu", doNothing, noEvent, noStyle
32+
, OP("Op", doNothing, noEvent)
33+
, EXIT("<Back")
34+
);
35+
36+
static MENU(mainMenu, "Braccio++", doNothing, noEvent, wrapStyle
37+
, OP("Op1", doNothing, noEvent)
38+
, OP("Op2", doNothing, noEvent)
39+
// ,FIELD(test,"Test","%",0,100,10,1,doNothing,noEvent,wrapStyle)
40+
, SUBMENU(subMenu)
41+
, SUBMENU(selMenu)
42+
, SUBMENU(chooseMenu)
43+
//,OP("Alert test",doAlert,enterEvent)
44+
, EXIT("<Back")
45+
);
46+
47+
#define ST7735_GRAY RGB565(128,128,128)
48+
const colorDef<uint16_t> colors[6] MEMMODE = {
49+
{{(uint16_t)ST7735_BLACK, (uint16_t)ST7735_BLACK}, {(uint16_t)ST7735_BLACK, (uint16_t)ST7735_BLUE, (uint16_t)ST7735_BLUE}}, //bgColor
50+
{{(uint16_t)ST7735_GRAY, (uint16_t)ST7735_GRAY}, {(uint16_t)ST7735_WHITE, (uint16_t)ST7735_WHITE, (uint16_t)ST7735_WHITE}},//fgColor
51+
{{(uint16_t)ST7735_WHITE, (uint16_t)ST7735_BLACK}, {(uint16_t)ST7735_YELLOW, (uint16_t)ST7735_YELLOW, (uint16_t)ST7735_RED}}, //valColor
52+
{{(uint16_t)ST7735_WHITE, (uint16_t)ST7735_BLACK}, {(uint16_t)ST7735_WHITE, (uint16_t)ST7735_YELLOW, (uint16_t)ST7735_YELLOW}}, //unitColor
53+
{{(uint16_t)ST7735_WHITE, (uint16_t)ST7735_GRAY}, {(uint16_t)ST7735_BLACK, (uint16_t)ST7735_BLUE, (uint16_t)ST7735_WHITE}}, //cursorColor
54+
{{(uint16_t)ST7735_WHITE, (uint16_t)ST7735_YELLOW}, {(uint16_t)ST7735_BLUE, (uint16_t)ST7735_RED, (uint16_t)ST7735_RED}}, //titleColor
55+
};
56+
57+
static serialIn serial(Serial);
58+
59+
static encoderIn<BTN_UP, BTN_DOWN, BTN_SEL, BTN_LEFT, BTN_RIGHT> encoder;
60+
static encoderInStream<BTN_UP, BTN_DOWN, BTN_SEL, BTN_LEFT, BTN_RIGHT> encStream(encoder);
61+
static MENU_INPUTS(in, &encStream, &serial);
62+
63+
#define MAX_DEPTH 4
64+
#define textScale 3
65+
static MENU_OUTPUTS(out, MAX_DEPTH
66+
, ADAGFX_OUT(gfx, colors, 6 * textScale, 9 * textScale, {0, 0, 14, 8}, {14, 0, 14, 8})
67+
, SERIAL_OUT(Serial)
68+
);
69+
70+
static NAVROOT(nav, mainMenu, MAX_DEPTH, in, out);
71+
}

0 commit comments

Comments
 (0)