@@ -30,69 +30,69 @@ BraccioClass::BraccioClass()
30
30
31
31
bool BraccioClass::begin (voidFuncPtr customMenu) {
32
32
33
- Wire.begin ();
34
- Serial.begin (115200 );
33
+ Wire.begin ();
34
+ Serial.begin (115200 );
35
35
36
- pinMode (PIN_FUSB302_INT, INPUT_PULLUP);
36
+ pinMode (PIN_FUSB302_INT, INPUT_PULLUP);
37
37
38
38
#ifdef __MBED__
39
- static rtos::Thread th (osPriorityHigh);
40
- th.start (mbed::callback (this , &BraccioClass::pd_thread));
41
- attachInterrupt (PIN_FUSB302_INT, mbed::callback (this , &BraccioClass::unlock_pd_semaphore_irq), FALLING);
42
- pd_timer.attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 10ms);
39
+ static rtos::Thread th (osPriorityHigh);
40
+ th.start (mbed::callback (this , &BraccioClass::pd_thread));
41
+ attachInterrupt (PIN_FUSB302_INT, mbed::callback (this , &BraccioClass::unlock_pd_semaphore_irq), FALLING);
42
+ pd_timer.attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 10ms);
43
43
#endif
44
44
45
- PD_UFP.init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
45
+ PD_UFP.init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
46
46
47
47
/*
48
- while (millis() < 200) {
49
- PD_UFP.run();
50
- }
48
+ while (millis() < 200) {
49
+ PD_UFP.run();
50
+ }
51
51
*/
52
52
53
- pinMode (1 , INPUT_PULLUP);
53
+ pinMode (1 , INPUT_PULLUP);
54
54
55
- SPI.begin ();
55
+ SPI.begin ();
56
56
57
- i2c_mutex.lock ();
58
- bl.begin ();
59
- if (bl.getChipID () != 0xCE ) {
60
- return false ;
61
- }
62
- bl.setColor (red);
57
+ i2c_mutex.lock ();
58
+ bl.begin ();
59
+ if (bl.getChipID () != 0xCE ) {
60
+ return false ;
61
+ }
62
+ bl.setColor (red);
63
63
64
- int ret = expander.testConnection ();
64
+ int ret = expander.testConnection ();
65
65
66
- if (ret == false ) {
67
- return ret;
68
- }
66
+ if (ret == false ) {
67
+ return ret;
68
+ }
69
69
70
- for (int i = 0 ; i < 14 ; i++) {
71
- expander.setPinDirection (i, 0 );
72
- }
70
+ for (int i = 0 ; i < 14 ; i++) {
71
+ expander.setPinDirection (i, 0 );
72
+ }
73
73
74
- // Set SLEW to low
75
- expander.setPinDirection (21 , 0 ); // P25 = 8 * 2 + 5
76
- expander.writePin (21 , 0 );
74
+ // Set SLEW to low
75
+ expander.setPinDirection (21 , 0 ); // P25 = 8 * 2 + 5
76
+ expander.writePin (21 , 0 );
77
77
78
- // Set TERM to HIGH (default)
79
- expander.setPinDirection (19 , 0 ); // P23 = 8 * 2 + 3
80
- expander.writePin (19 , 1 );
78
+ // Set TERM to HIGH (default)
79
+ expander.setPinDirection (19 , 0 ); // P23 = 8 * 2 + 3
80
+ expander.writePin (19 , 1 );
81
81
82
- expander.setPinDirection (18 , 0 ); // P22 = 8 * 2 + 2
83
- expander.writePin (18 , 0 ); // reset LCD
84
- expander.writePin (18 , 1 ); // LCD out of reset
85
- i2c_mutex.unlock ();
82
+ expander.setPinDirection (18 , 0 ); // P22 = 8 * 2 + 2
83
+ expander.writePin (18 , 0 ); // reset LCD
84
+ expander.writePin (18 , 1 ); // LCD out of reset
85
+ i2c_mutex.unlock ();
86
86
87
- pinMode (BTN_LEFT, INPUT_PULLUP);
88
- pinMode (BTN_RIGHT, INPUT_PULLUP);
89
- pinMode (BTN_UP, INPUT_PULLUP);
90
- pinMode (BTN_DOWN, INPUT_PULLUP);
91
- pinMode (BTN_SEL, INPUT_PULLUP);
92
- pinMode (BTN_ENTER, INPUT_PULLUP);
87
+ pinMode (BTN_LEFT, INPUT_PULLUP);
88
+ pinMode (BTN_RIGHT, INPUT_PULLUP);
89
+ pinMode (BTN_UP, INPUT_PULLUP);
90
+ pinMode (BTN_DOWN, INPUT_PULLUP);
91
+ pinMode (BTN_SEL, INPUT_PULLUP);
92
+ pinMode (BTN_ENTER, INPUT_PULLUP);
93
93
94
94
#if LV_USE_LOG
95
- lv_log_register_print_cb ( my_print );
95
+ lv_log_register_print_cb ( my_print );
96
96
#endif
97
97
98
98
lv_init ();
@@ -112,14 +112,14 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
112
112
indev_drv.read_cb = read_keypad;
113
113
kb_indev = lv_indev_drv_register (&indev_drv);
114
114
115
- gfx.init ();
116
- gfx.setRotation (4 );
117
- gfx.fillScreen (TFT_BLACK);
118
- gfx.setAddrWindow (0 , 0 , 240 , 240 );
119
- gfx.setFreeFont (&FreeSans18pt7b);
115
+ gfx.init ();
116
+ gfx.setRotation (4 );
117
+ gfx.fillScreen (TFT_BLACK);
118
+ gfx.setAddrWindow (0 , 0 , 240 , 240 );
119
+ gfx.setFreeFont (&FreeSans18pt7b);
120
120
121
121
/*
122
- gfx.drawBitmap(44, 60, ArduinoLogo, 152, 72, 0x04B3);
122
+ gfx.drawBitmap(44, 60, ArduinoLogo, 152, 72, 0x04B3);
123
123
gfx.drawBitmap(48, 145, ArduinoText, 144, 23, 0x04B3);
124
124
*/
125
125
@@ -137,34 +137,34 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
137
137
}
138
138
139
139
if (!PD_UFP.is_PPS_ready ()) {
140
- gfx.fillScreen (TFT_BLACK);
141
- gfx.println (" \n\n Please\n connect\n power" );
142
- }
143
-
144
- // PD_UFP.print_status(Serial);
145
- while (!PD_UFP.is_PPS_ready ()) {
146
- i2c_mutex.lock ();
147
- PD_UFP.print_status (Serial);
148
- // PD_UFP.print_status(Serial);
149
- PD_UFP.set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
150
- delay (10 );
151
- i2c_mutex.unlock ();
152
- }
140
+ gfx.fillScreen (TFT_BLACK);
141
+ gfx.println (" \n\n Please\n connect\n power" );
142
+ }
143
+
144
+ // PD_UFP.print_status(Serial);
145
+ while (!PD_UFP.is_PPS_ready ()) {
146
+ i2c_mutex.lock ();
147
+ PD_UFP.print_status (Serial);
148
+ // PD_UFP.print_status(Serial);
149
+ PD_UFP.set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
150
+ delay (10 );
151
+ i2c_mutex.unlock ();
152
+ }
153
153
154
154
#ifdef __MBED__
155
- static rtos::Thread display_th;
156
- display_th.start (mbed::callback (this , &BraccioClass::display_thread));
155
+ static rtos::Thread display_th;
156
+ display_th.start (mbed::callback (this , &BraccioClass::display_thread));
157
157
#endif
158
158
159
- servos.begin ();
160
- servos.setPositionMode (PositionMode::IMMEDIATE);
159
+ servos.begin ();
160
+ servos.setPositionMode (PositionMode::IMMEDIATE);
161
161
162
162
#ifdef __MBED__
163
- static rtos::Thread connected_th;
164
- connected_th.start (mbed::callback (this , &BraccioClass::motors_connected_thread));
163
+ static rtos::Thread connected_th;
164
+ connected_th.start (mbed::callback (this , &BraccioClass::motors_connected_thread));
165
165
#endif
166
166
167
- return true ;
167
+ return true ;
168
168
}
169
169
170
170
void BraccioClass::connectJoystickTo (lv_obj_t * obj) {
@@ -173,8 +173,8 @@ void BraccioClass::connectJoystickTo(lv_obj_t* obj) {
173
173
}
174
174
175
175
void BraccioClass::pd_thread () {
176
- start_pd_burst = millis ();
177
- size_t last_time_ask_pps = 0 ;
176
+ start_pd_burst = millis ();
177
+ size_t last_time_ask_pps = 0 ;
178
178
while (1 ) {
179
179
auto ret = pd_events.wait_any (0xFF );
180
180
if ((ret & 1 ) && (millis () - start_pd_burst > 1000 )) {
@@ -200,7 +200,7 @@ void BraccioClass::pd_thread() {
200
200
201
201
void BraccioClass::display_thread () {
202
202
while (1 ) {
203
- /*
203
+ /*
204
204
if ((braccio::encoder.menu_running) && (braccio::encoder.menu_interrupt)) {
205
205
braccio::encoder.menu_interrupt = false;
206
206
braccio::nav.doInput();
@@ -217,68 +217,68 @@ void BraccioClass::display_thread() {
217
217
#include < extra/libs/gif/lv_gif.h>
218
218
219
219
void BraccioClass::splashScreen (int duration) {
220
- LV_IMG_DECLARE (img_bulb_gif);
221
- lv_obj_t * img = lv_gif_create (lv_scr_act ());
222
- lv_gif_set_src (img, &img_bulb_gif);
223
- lv_obj_align (img, LV_ALIGN_CENTER, 0 , 0 );
220
+ LV_IMG_DECLARE (img_bulb_gif);
221
+ lv_obj_t * img = lv_gif_create (lv_scr_act ());
222
+ lv_gif_set_src (img, &img_bulb_gif);
223
+ lv_obj_align (img, LV_ALIGN_CENTER, 0 , 0 );
224
224
225
- for (long start = millis (); millis () - start < duration;) {
226
- lv_task_handler ();
225
+ for (long start = millis (); millis () - start < duration;) {
226
+ lv_task_handler ();
227
227
lv_tick_inc (LV_DISP_DEF_REFR_PERIOD);
228
228
delay (10 );
229
- }
230
- lv_obj_del (img);
231
- lv_obj_clean (lv_scr_act ());
229
+ }
230
+ lv_obj_del (img);
231
+ lv_obj_clean (lv_scr_act ());
232
232
}
233
233
234
234
void BraccioClass::defaultMenu () {
235
235
236
- // TODO: create a meaningful default menu
236
+ // TODO: create a meaningful default menu
237
237
238
238
}
239
239
240
240
void BraccioClass::motors_connected_thread () {
241
241
while (1 ) {
242
242
if (ping_allowed) {
243
- for (int i = 1 ; i < 7 ; i++) {
244
- _connected[i] = (servos.ping (i) == 0 );
245
- // Serial.print(String(i) + ": ");
246
- // Serial.println(_connected[i]);
247
- }
248
- i2c_mutex.lock ();
249
- for (int i = 1 ; i < 7 ; i++) {
250
- if (_connected[i]) {
251
- setGreen (i);
252
- } else {
253
- setRed (i);
254
- }
255
- }
256
- i2c_mutex.unlock ();
257
- }
243
+ for (int i = 1 ; i < 7 ; i++) {
244
+ _connected[i] = (servos.ping (i) == 0 );
245
+ // Serial.print(String(i) + ": ");
246
+ // Serial.println(_connected[i]);
247
+ }
248
+ i2c_mutex.lock ();
249
+ for (int i = 1 ; i < 7 ; i++) {
250
+ if (_connected[i]) {
251
+ setGreen (i);
252
+ } else {
253
+ setRed (i);
254
+ }
255
+ }
256
+ i2c_mutex.unlock ();
257
+ }
258
258
delay (1000 );
259
259
}
260
260
}
261
261
262
262
int BraccioClass::getKey () {
263
- if (digitalRead (BTN_LEFT) == LOW) {
264
- return 1 ;
265
- }
266
- if (digitalRead (BTN_RIGHT) == LOW) {
267
- return 2 ;
268
- }
269
- if (digitalRead (BTN_SEL) == LOW) {
270
- return 3 ;
271
- }
272
- if (digitalRead (BTN_UP) == LOW) {
273
- return 4 ;
274
- }
275
- if (digitalRead (BTN_DOWN) == LOW) {
276
- return 5 ;
277
- }
278
- if (digitalRead (BTN_ENTER) == LOW) {
279
- return 6 ;
280
- }
281
- return 0 ;
263
+ if (digitalRead (BTN_LEFT) == LOW) {
264
+ return 1 ;
265
+ }
266
+ if (digitalRead (BTN_RIGHT) == LOW) {
267
+ return 2 ;
268
+ }
269
+ if (digitalRead (BTN_SEL) == LOW) {
270
+ return 3 ;
271
+ }
272
+ if (digitalRead (BTN_UP) == LOW) {
273
+ return 4 ;
274
+ }
275
+ if (digitalRead (BTN_DOWN) == LOW) {
276
+ return 5 ;
277
+ }
278
+ if (digitalRead (BTN_ENTER) == LOW) {
279
+ return 6 ;
280
+ }
281
+ return 0 ;
282
282
}
283
283
284
284
/* Display flushing */
0 commit comments