Skip to content

Commit 3a63895

Browse files
aentingeraentinger
authored
Merge pull request #73 from arduino-libraries/enhance_controlling_manually_braccio
Enhance controlling manually braccio
2 parents 5512554 + fa97865 commit 3a63895

File tree

3 files changed

+601
-0
lines changed

3 files changed

+601
-0
lines changed

‎examples/Controlling_Manually_Braccio/AppState.cpp

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
/**************************************************************************************
2+
* INCLUDE
3+
**************************************************************************************/
4+
5+
#include "AppState.h"
6+
7+
#include <Braccio++.h>
8+
9+
/**************************************************************************************
10+
* TYPEDEF
11+
**************************************************************************************/
12+
13+
enum BUTTONS {
14+
BTN_UP = 1,
15+
BTN_DOWN = 7,
16+
BTN_LEFT = 3,
17+
BTN_RIGHT = 5,
18+
};
19+
20+
/**************************************************************************************
21+
* EXTERN
22+
**************************************************************************************/
23+
24+
extern lv_obj_t * label;
25+
extern lv_obj_t * direction_btnm;
26+
27+
/**************************************************************************************
28+
* GLOBAL VARIABLES
29+
**************************************************************************************/
30+
31+
static auto gripper = Braccio.get(1);
32+
static auto wristRoll = Braccio.get(2);
33+
static auto wristPitch = Braccio.get(3);
34+
static auto elbow = Braccio.get(4);
35+
static auto shoulder = Braccio.get(5);
36+
static auto base = Braccio.get(6);
37+
38+
/**************************************************************************************
39+
* ShoulderState
40+
**************************************************************************************/
41+
42+
ShoulderState::ShoulderState()
43+
{
44+
Braccio.lvgl_lock();
45+
lv_label_set_text(label, "Shoulder");
46+
Braccio.lvgl_unlock();
47+
}
48+
49+
State * ShoulderState::handle_OnEnter()
50+
{
51+
return new ElbowState();
52+
}
53+
54+
State * ShoulderState::handle_OnUp()
55+
{
56+
shoulder.move().to(shoulder.position() + 5.0f);
57+
return this;
58+
}
59+
60+
State * ShoulderState::handle_OnDown()
61+
{
62+
shoulder.move().to(shoulder.position() - 5.0f);
63+
return this;
64+
}
65+
66+
State * ShoulderState::handle_OnLeft()
67+
{
68+
base.move().to(base.position() + 5.0f);
69+
return this;
70+
}
71+
72+
State * ShoulderState::handle_OnRight()
73+
{
74+
base.move().to(base.position() - 5.0f);
75+
return this;
76+
}
77+
78+
/**************************************************************************************
79+
* ElbowState
80+
**************************************************************************************/
81+
82+
ElbowState::ElbowState()
83+
{
84+
Braccio.lvgl_lock();
85+
lv_label_set_text(label, "Elbow");
86+
lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN);
87+
lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN);
88+
Braccio.lvgl_unlock();
89+
}
90+
91+
ElbowState::~ElbowState()
92+
{
93+
Braccio.lvgl_lock();
94+
lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN);
95+
lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN);
96+
Braccio.lvgl_unlock();
97+
}
98+
99+
State * ElbowState::handle_OnEnter()
100+
{
101+
return new WristState();
102+
}
103+
104+
State * ElbowState::handle_OnUp()
105+
{
106+
elbow.move().to(elbow.position() + 5.0f);
107+
return this;
108+
}
109+
110+
State * ElbowState::handle_OnDown()
111+
{
112+
elbow.move().to(elbow.position() - 5.0f);
113+
return this;
114+
}
115+
116+
/**************************************************************************************
117+
* WristState
118+
**************************************************************************************/
119+
120+
WristState::WristState()
121+
{
122+
Braccio.lvgl_lock();
123+
lv_label_set_text(label, "Wrist");
124+
Braccio.lvgl_unlock();
125+
}
126+
127+
State * WristState::handle_OnEnter()
128+
{
129+
return new PinchState();
130+
}
131+
132+
State * WristState::handle_OnUp()
133+
{
134+
wristPitch.move().to(wristPitch.position() + 5.0f);
135+
return this;
136+
}
137+
138+
State * WristState::handle_OnDown()
139+
{
140+
wristPitch.move().to(wristPitch.position() - 5.0f);
141+
return this;
142+
}
143+
144+
State * WristState::handle_OnLeft()
145+
{
146+
wristRoll.move().to(wristRoll.position() + 10.0f);
147+
return this;
148+
}
149+
150+
State * WristState::handle_OnRight()
151+
{
152+
wristRoll.move().to(wristRoll.position() - 10.0f);
153+
return this;
154+
}
155+
156+
/**************************************************************************************
157+
* PinchState
158+
**************************************************************************************/
159+
160+
PinchState::PinchState()
161+
{
162+
Braccio.lvgl_lock();
163+
lv_label_set_text(label, "Pinch");
164+
lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN);
165+
lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN);
166+
Braccio.lvgl_unlock();
167+
}
168+
169+
PinchState::~PinchState()
170+
{
171+
Braccio.lvgl_lock();
172+
lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN);
173+
lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN);
174+
Braccio.lvgl_unlock();
175+
}
176+
177+
State * PinchState::handle_OnEnter()
178+
{
179+
return new ShoulderState();
180+
}
181+
182+
State * PinchState::handle_OnLeft()
183+
{
184+
gripper.move().to(gripper.position() + 5.0f);
185+
return this;
186+
}
187+
188+
State * PinchState::handle_OnRight()
189+
{
190+
gripper.move().to(gripper.position() - 5.0f);
191+
return this;
192+
}

‎examples/Controlling_Manually_Braccio/AppState.h

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
#ifndef APP_STATE_H_
2+
#define APP_STATE_H_
3+
4+
/**************************************************************************************
5+
* INCLUDE
6+
**************************************************************************************/
7+
8+
#include <mbed.h>
9+
10+
/**************************************************************************************
11+
* TYPEDEF
12+
**************************************************************************************/
13+
14+
enum class Button
15+
{
16+
None, Enter, Home, Up, Down, Left, Right
17+
};
18+
19+
/**************************************************************************************
20+
* CLASS DECLARATION
21+
**************************************************************************************/
22+
23+
class State
24+
{
25+
public:
26+
virtual ~State() { }
27+
State * update(Button const button)
28+
{
29+
State * next_state = this;
30+
switch (button)
31+
{
32+
case Button::Home:
33+
case Button::Enter: next_state = handle_OnEnter(); break;
34+
case Button::Up: next_state = handle_OnUp(); break;
35+
case Button::Down: next_state = handle_OnDown(); break;
36+
case Button::Left: next_state = handle_OnLeft(); break;
37+
case Button::Right: next_state = handle_OnRight(); break;
38+
default: break;
39+
}
40+
return next_state;
41+
}
42+
43+
protected:
44+
virtual State * handle_OnEnter() = 0;
45+
virtual State * handle_OnUp () { return this; }
46+
virtual State * handle_OnDown () { return this; }
47+
virtual State * handle_OnLeft () { return this; }
48+
virtual State * handle_OnRight() { return this; }
49+
};
50+
51+
class ShoulderState : public State
52+
{
53+
public:
54+
ShoulderState();
55+
virtual ~ShoulderState() { }
56+
protected:
57+
virtual State * handle_OnEnter() override;
58+
virtual State * handle_OnUp () override;
59+
virtual State * handle_OnDown () override;
60+
virtual State * handle_OnLeft () override;
61+
virtual State * handle_OnRight() override;
62+
};
63+
64+
65+
class ElbowState : public State
66+
{
67+
public:
68+
ElbowState();
69+
virtual ~ElbowState();
70+
protected:
71+
virtual State * handle_OnEnter() override;
72+
virtual State * handle_OnUp () override;
73+
virtual State * handle_OnDown () override;
74+
};
75+
76+
class WristState : public State
77+
{
78+
public:
79+
WristState();
80+
virtual ~WristState() { }
81+
protected:
82+
virtual State * handle_OnEnter() override;
83+
virtual State * handle_OnUp () override;
84+
virtual State * handle_OnDown () override;
85+
virtual State * handle_OnLeft () override;
86+
virtual State * handle_OnRight() override;
87+
};
88+
89+
90+
class PinchState : public State
91+
{
92+
public:
93+
PinchState();
94+
virtual ~PinchState();
95+
protected:
96+
virtual State * handle_OnEnter() override;
97+
virtual State * handle_OnLeft () override;
98+
virtual State * handle_OnRight() override;
99+
};
100+
101+
class ManualControlApp
102+
{
103+
public:
104+
ManualControlApp()
105+
: _state{nullptr}
106+
, _mtx{}
107+
{ }
108+
109+
void update(Button const button)
110+
{
111+
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
112+
113+
if (!_state)
114+
{
115+
_state = new ShoulderState();
116+
return;
117+
}
118+
119+
State * next_state = _state->update(button);
120+
121+
if (next_state != _state) {
122+
delete _state;
123+
_state = next_state;
124+
}
125+
}
126+
127+
private:
128+
State * _state;
129+
rtos::Mutex _mtx;
130+
};
131+
132+
#endif /* APP_STATE_H_ */

0 commit comments

Comments
 (0)