Skip to content

Commit 3365ff6

Browse files
aentingeraentinger
authored
Merge pull request #14 from bcmi-labs/simplify-smart-motor-logic
Cleanup/Refactor `SmartServo`
2 parents 2e256cf + 6a000cb commit 3365ff6

File tree

7 files changed

+686
-648
lines changed

7 files changed

+686
-648
lines changed

‎src/Braccio++.h‎

Lines changed: 172 additions & 170 deletions
Original file line numberDiff line numberDiff line change
@@ -23,208 +23,210 @@
2323
extern const lv_img_dsc_t img_bulb_gif;
2424

2525
extern "C" {
26-
void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
27-
void read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
26+
void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
27+
void read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
2828
};
2929

3030
enum speed_grade_t {
31-
FAST = 10,
32-
MEDIUM = 100,
33-
SLOW = 1000,
31+
FAST = 10,
32+
MEDIUM = 100,
33+
SLOW = 1000,
3434
};
3535

3636
#include <chrono>
3737
using namespace std::chrono;
3838

3939
class MotorsWrapper {
4040
public:
41-
MotorsWrapper(SmartServoClass<7>* servos, int idx) : _servos(servos), _idx(idx) {}
42-
MotorsWrapper& to(float angle) {
43-
_servos->setPosition(_idx, angle, _speed);
44-
return *this;
45-
}
46-
MotorsWrapper& in(std::chrono::milliseconds len) {
47-
_servos->setTime(_idx, len.count());
48-
return *this;
49-
}
50-
MotorsWrapper& move() {
51-
return *this;
52-
}
53-
float position() {
54-
return _servos->getPosition(_idx);
55-
}
56-
bool connected() {
57-
return _servos->ping(_idx) == 0;
58-
}
59-
operator bool() {
60-
return connected();
61-
}
62-
void info(Stream& stream) {
63-
_servos->getInfo(stream, _idx);
64-
}
65-
void disengage() {
66-
_servos->disengage(_idx);
67-
}
68-
void engage() {
69-
_servos->engage(_idx);
70-
}
71-
bool engaged() {
72-
return _servos->isEngaged(_idx);
73-
}
41+
MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {}
42+
MotorsWrapper& to(float angle) {
43+
_servos.setPosition(_idx, angle, _speed);
44+
return *this;
45+
}
46+
MotorsWrapper& in(std::chrono::milliseconds len) {
47+
_servos.setTime(_idx, len.count());
48+
return *this;
49+
}
50+
MotorsWrapper& move() {
51+
return *this;
52+
}
53+
float position() {
54+
return _servos.getPosition(_idx);
55+
}
56+
bool connected() {
57+
return _servos.ping(_idx) == 0;
58+
}
59+
operator bool() {
60+
return connected();
61+
}
62+
void info(Stream& stream) {
63+
_servos.getInfo(stream, _idx);
64+
}
65+
void disengage() {
66+
_servos.disengage(_idx);
67+
}
68+
void engage() {
69+
_servos.engage(_idx);
70+
}
71+
bool engaged() {
72+
return _servos.isEngaged(_idx);
73+
}
7474

7575
private:
76-
SmartServoClass<7>* _servos;
77-
int _idx;
78-
int _speed = 100;
76+
SmartServoClass & _servos;
77+
int _idx;
78+
int _speed = 100;
7979
};
8080

81-
class BraccioClass {
81+
class BraccioClass
82+
{
8283
public:
83-
BraccioClass() {}
84-
bool begin(voidFuncPtr customMenu = nullptr);
85-
86-
// setters
87-
MotorsWrapper move(int joint_index) {
88-
MotorsWrapper wrapper(servos, joint_index);
89-
return wrapper;
90-
}
91-
MotorsWrapper get(int joint_index) {
92-
return move(joint_index);
93-
}
94-
void moveTo(int joint_index, int position) {
95-
//servos->setPosition(joint_index, position, 100);
96-
}
97-
void moveTo(int joint_index, float angle) {
98-
servos->setPosition(joint_index, angle, 100);
99-
}
100-
void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) {
101-
servos->setPositionMode(pmSYNC);
102-
servos->setPosition(1, a1, runTime);
103-
servos->setPosition(2, a2, runTime);
104-
servos->setPosition(3, a3, runTime);
105-
servos->setPosition(4, a4, runTime);
106-
servos->setPosition(5, a5, runTime);
107-
servos->setPosition(6, a6, runTime);
108-
servos->synchronize();
109-
servos->setPositionMode(pmIMMEDIATE);
110-
}
111-
// getters
112-
void positions(float* buffer) {
113-
for (int i = 1; i < 7; i++) {
114-
*buffer++ = servos->getPosition(i);
115-
}
116-
}
117-
void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) {
118-
// TODO: add check if motors are actually connected
119-
a1 = servos->getPosition(1);
120-
a2 = servos->getPosition(2);
121-
a3 = servos->getPosition(3);
122-
a4 = servos->getPosition(4);
123-
a5 = servos->getPosition(5);
124-
a6 = servos->getPosition(6);
125-
}
126-
float position(int joint_index);
127-
bool connected(int joint_index) {
128-
return _connected[joint_index];
129-
}
130-
131-
void speed(speed_grade_t speed_grade) {
132-
runTime = speed_grade;
133-
}
134-
135-
void disengage(int id = SmartServoClass<7>::BROADCAST) {
136-
servos->disengage(id);
137-
}
138-
139-
void engage(int id = SmartServoClass<7>::BROADCAST) {
140-
servos->engage(id);
141-
}
142-
143-
int getKey();
144-
void connectJoystickTo(lv_obj_t* obj);
145-
146-
TFT_eSPI gfx = TFT_eSPI();
147-
148-
bool ping_allowed = true;
149-
150-
static BraccioClass& get_default_instance() {
151-
static BraccioClass dev;
152-
return dev;
153-
}
84+
85+
BraccioClass();
86+
87+
bool begin(voidFuncPtr customMenu = nullptr);
88+
89+
// setters
90+
MotorsWrapper move(int joint_index) {
91+
MotorsWrapper wrapper(servos, joint_index);
92+
return wrapper;
93+
}
94+
MotorsWrapper get(int joint_index) {
95+
return move(joint_index);
96+
}
97+
void moveTo(int joint_index, int position) {
98+
//servos.setPosition(joint_index, position, 100);
99+
}
100+
void moveTo(int joint_index, float angle) {
101+
servos.setPosition(joint_index, angle, 100);
102+
}
103+
void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) {
104+
servos.setPositionMode(PositionMode::SYNC);
105+
servos.setPosition(1, a1, runTime);
106+
servos.setPosition(2, a2, runTime);
107+
servos.setPosition(3, a3, runTime);
108+
servos.setPosition(4, a4, runTime);
109+
servos.setPosition(5, a5, runTime);
110+
servos.setPosition(6, a6, runTime);
111+
servos.synchronize();
112+
servos.setPositionMode(PositionMode::IMMEDIATE);
113+
}
114+
// getters
115+
void positions(float* buffer) {
116+
for (int i = 1; i < 7; i++) {
117+
*buffer++ = servos.getPosition(i);
118+
}
119+
}
120+
void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) {
121+
// TODO: add check if motors are actually connected
122+
a1 = servos.getPosition(1);
123+
a2 = servos.getPosition(2);
124+
a3 = servos.getPosition(3);
125+
a4 = servos.getPosition(4);
126+
a5 = servos.getPosition(5);
127+
a6 = servos.getPosition(6);
128+
}
129+
float position(int joint_index);
130+
bool connected(int joint_index) {
131+
return _connected[joint_index];
132+
}
133+
134+
void speed(speed_grade_t speed_grade) {
135+
runTime = speed_grade;
136+
}
137+
138+
void disengage(int id = SmartServoClass::BROADCAST) {
139+
servos.disengage(id);
140+
}
141+
142+
void engage(int id = SmartServoClass::BROADCAST) {
143+
servos.engage(id);
144+
}
145+
146+
int getKey();
147+
void connectJoystickTo(lv_obj_t* obj);
148+
149+
TFT_eSPI gfx = TFT_eSPI();
150+
151+
bool ping_allowed = true;
152+
153+
static BraccioClass& get_default_instance() {
154+
static BraccioClass dev;
155+
return dev;
156+
}
154157

155158
protected:
156-
// ioexpander APIs
157-
void digitalWrite(int pin, uint8_t value);
159+
// ioexpander APIs
160+
void digitalWrite(int pin, uint8_t value);
158161

159-
// default display APIs
160-
void drawMenu();
161-
void splashScreen(int duration = 1000);
162-
void hideMenu();
163-
void drawImage(char* image);
164-
void defaultMenu();
162+
// default display APIs
163+
void drawMenu();
164+
void splashScreen(int duration = 1000);
165+
void hideMenu();
166+
void drawImage(char* image);
167+
void defaultMenu();
165168

166-
void setID(int id) {
167-
servos->setID(id);
168-
}
169+
void setID(int id) {
170+
servos.setID(id);
171+
}
169172

170173
private:
171174

172-
RS485Class serial485 = RS485Class(Serial1, 0, 7, 8); // TX, DE, RE
173-
SmartServoClass<7>* servos = new SmartServoClass<7>(serial485);
174-
175-
PD_UFP_log_c PD_UFP = PD_UFP_log_c(PD_LOG_LEVEL_VERBOSE);
176-
TCA6424A expander = TCA6424A(TCA6424A_ADDRESS_ADDR_HIGH);
177-
Backlight bl;
175+
RS485Class serial485;
176+
SmartServoClass servos;
177+
PD_UFP_log_c PD_UFP;
178+
TCA6424A expander;
179+
Backlight bl;
178180

179-
speed_grade_t runTime = SLOW; //ms
181+
speed_grade_t runTime; //ms
180182

181-
voidFuncPtr _customMenu = nullptr;
183+
voidFuncPtr _customMenu;
182184

183-
const int BTN_LEFT = 3;
184-
const int BTN_RIGHT = 4;
185-
const int BTN_UP = 5;
186-
const int BTN_DOWN = 2;
187-
const int BTN_SEL = A0;
188-
const int BTN_ENTER = A1;
185+
const int BTN_LEFT = 3;
186+
const int BTN_RIGHT = 4;
187+
const int BTN_UP = 5;
188+
const int BTN_DOWN = 2;
189+
const int BTN_SEL = A0;
190+
const int BTN_ENTER = A1;
189191

190192
lv_disp_drv_t disp_drv;
191193
lv_indev_drv_t indev_drv;
192-
lv_disp_draw_buf_t disp_buf;
193-
lv_color_t buf[240 * 240 / 10];
194-
lv_group_t* p_objGroup;
195-
lv_indev_t *kb_indev;
194+
lv_disp_draw_buf_t disp_buf;
195+
lv_color_t buf[240 * 240 / 10];
196+
lv_group_t* p_objGroup;
197+
lv_indev_t *kb_indev;
196198

197199
bool _connected[8];
198200

199201
#ifdef __MBED__
200-
rtos::EventFlags pd_events;
201-
rtos::Mutex i2c_mutex;
202-
mbed::Ticker pd_timer;
203-
204-
unsigned int start_pd_burst = 0xFFFFFFFF;
205-
206-
void unlock_pd_semaphore_irq() {
207-
start_pd_burst = millis();
208-
pd_events.set(2);
209-
}
210-
211-
void unlock_pd_semaphore() {
212-
pd_events.set(1);
213-
}
214-
215-
void setGreen(int i) {
216-
expander.writePin(i * 2 - 1, 0);
217-
expander.writePin(i * 2 - 2, 1);
218-
}
219-
220-
void setRed(int i) {
221-
expander.writePin(i * 2 - 1, 1);
222-
expander.writePin(i * 2 - 2, 0);
223-
}
224-
225-
void pd_thread();
226-
void display_thread();
227-
void motors_connected_thread();
202+
rtos::EventFlags pd_events;
203+
rtos::Mutex i2c_mutex;
204+
mbed::Ticker pd_timer;
205+
206+
unsigned int start_pd_burst = 0xFFFFFFFF;
207+
208+
void unlock_pd_semaphore_irq() {
209+
start_pd_burst = millis();
210+
pd_events.set(2);
211+
}
212+
213+
void unlock_pd_semaphore() {
214+
pd_events.set(1);
215+
}
216+
217+
void setGreen(int i) {
218+
expander.writePin(i * 2 - 1, 0);
219+
expander.writePin(i * 2 - 2, 1);
220+
}
221+
222+
void setRed(int i) {
223+
expander.writePin(i * 2 - 1, 1);
224+
expander.writePin(i * 2 - 2, 0);
225+
}
226+
227+
void pd_thread();
228+
void display_thread();
229+
void motors_connected_thread();
228230
#endif
229231

230232
};

0 commit comments

Comments
 (0)