Arduino ESP32 Crawler Powered by CAN

1// << ESP32 Crawler >>
2// board is "D-duino-32"
3// CAN Driver  https://github.com/nhatuan84/esp32-can-protocol-demo
4// Adafruit SSD1306 via  Library Manager (needs also Adafruit GFX)
5// PS4 Driver https://github.com/NURobotics/PS4-esp32
6//  PS4 MAC https://dancingpixelstudios.com/sixaxis-controller/sixaxispairtool/
7//  in case the controller is no longer pairing but was working before,
8// run "esptool  --chip esp32 erase_flash" in C:\\Users\\<user>\\AppData\\Local\\Arduino15\\packages\\esp32\	ools\\esptool_py\\2.6.1
9//  see https://github.com/NURobotics/PS4-esp32/issues/2#issuecomment-602852034
10
11#include  <Wire.h>
12#include <Adafruit_GFX.h>
13#include <Adafruit_SSD1306.h> // works  only if board and variant config for D-duino-32 is available
14
15#define SCREEN_WIDTH  128 // OLED display width, in pixels
16#define SCREEN_HEIGHT 64 // OLED display  height, in pixels
17
18#define OLED_RESET 3
19#define OLED_ROTATION 0
20Adafruit_SSD1306  display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
21
22#if (SSD1306_LCDHEIGHT  != 64)
23#error("Height incorrect, please fix Adafruit_SSD1306.h!");
24#endif
25
26#include  <PS4Controller.h>
27#define PS4_HOST_MAC "00:1e:ab:4c:4e:c8"
28#define TILT_RUMBLE  127
29
30#include <ESP32CAN.h>
31#define CAN_SPEED CAN_SPEED_250KBPS
32#define  CAN_TX GPIO_NUM_15
33#define CAN_RX GPIO_NUM_13
34#include <CAN_config.h>
35
36CAN_device_t  CAN_cfg;
37const int rx_queue_size = 10;
38
39#define STATUS_CAN_TIMEOUT 250
40unsigned  long lastStatusReveiced;
41
42bool auto_break = false;
43bool break_active =  false;
44int distance = 0;
45byte tilt_warn = false;
46
47void task_rxCAN(void  * pvParameters) {
48  for(;;) {
49    CAN_frame_t rx_frame;
50
51    if(xQueueReceive(CAN_cfg.rx_queue,&rx_frame,  3*portTICK_PERIOD_MS)==pdTRUE){
52      // start of message evaluation
53      if(rx_frame.FIR.B.FF==CAN_frame_std)  { // standard frame format
54        if (rx_frame.MsgID == 0x20 && rx_frame.FIR.B.DLC  == 8) {
55          auto_break =   bool(rx_frame.data.u8[0]);
56          break_active  = bool(rx_frame.data.u8[1]);
57          tilt_warn =    bool(rx_frame.data.u8[2]);
58          distance  =     int(rx_frame.data.u8[4])<<(3*8);
59          distance  +=     int(rx_frame.data.u8[5])<<(2*8);
60          distance +=     int(rx_frame.data.u8[6])<<(1*8);
61          distance +=     int(rx_frame.data.u8[7]);
62          
63          lastStatusReveiced  = millis();
64        }
65      }
66      // end of message evaluation
67    }  
68  }
69}
70
71void setup() 
72{
73  setCpuFrequencyMhz(160); // reduce heat  emission & power consumption (default would be 240Mhz)
74  
75//  Serial.begin(57600);
76
77  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) { // 0x3c for on-board OLED of  D-duino-32
78//    Serial.println(F("SSD1306 allocation failed"));
79    while  (1) {}
80  }
81  display.clearDisplay();
82  display.setTextSize(1);
83  display.setTextColor(WHITE,  BLACK);
84  display.setCursor(5,5);
85  display.print("<< ESP32 CRAWLER >>");
86  display.display();
87  
88  PS4.begin(PS4_HOST_MAC);
89
90  CAN_cfg.speed=CAN_SPEED;
91  CAN_cfg.tx_pin_id = CAN_TX;
92  CAN_cfg.rx_pin_id = CAN_RX;
93  CAN_cfg.rx_queue  = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
94  
95  ESP32Can.CANInit();
96
97  // Task: read CAN
98  xTaskCreatePinnedToCore(
99    task_rxCAN,     /* Function  to implement the task */
100    "task_rxCAN",   /* Name of the task */
101    4096,      /* Stack size in words */
102    NULL,      /* Task input parameter */
103    1,         /* Priority of the task */
104    NULL,      /* Task handle. */
105    1);        /* Core where the task should run */  
106
107  lastStatusReveiced  = 0;
108}
109
110void loop() 
111{ 
112  if(PS4.isConnected()) {
113
114//    Serial.print("R_X:  "); Serial.print(PS4.data.analog.stick.rx, DEC);
115//    Serial.print(" R_Y:  "); Serial.print(PS4.data.analog.stick.ry, DEC);
116//    Serial.print(" L_X:  "); Serial.print(PS4.data.analog.stick.lx, DEC);
117//    Serial.print(" L_Y:  "); Serial.print(PS4.data.analog.stick.ly, DEC);
118//    Serial.println();
119
120//    Serial.print("DIS: "); Serial.println(distance);
121
122    display.setCursor(83,25);
123    display.print("PS4 OK ");
124
125    display.setCursor(5, 40);
126    if (auto_break)  {
127      if (break_active) {
128        display.print("FRONT ASSIST ACTIVE");
129        PS4.setLed(255, 0, 0);
130      }
131      else {
132        display.print("FRONT  ASSIST     ON ");
133        PS4.setLed(0, 255, 0);
134      }
135    }
136    else  {
137      display.print("FRONT ASSIST     OFF");
138      PS4.setLed(0, 0, 255);
139    }
140
141    #ifdef TILT_RUMBLE
142    if (tilt_warn) {
143      PS4.setRumble(0,  TILT_RUMBLE);
144    }
145    else {
146      PS4.setRumble(0, 0);
147    }
148    #endif
149    
150    // PS4.sendToController(); // moved after sending frames
151    // delay(15); // replaced by delay between frames
152
153    display.setCursor(5,  55);
154    display.print("DISTANCE");
155    char sf[4];
156    sprintf(sf, "%4d",  distance);
157    display.setCursor(95, 55);
158    display.print(sf);
159
160    //  read analog sticks (integers)
161    int rgt_stick_x = PS4.data.analog.stick.rx;  // -128 0 127
162    int rgt_stick_y = PS4.data.analog.stick.ry; // -128 0 127
163    int lft_stick_x = PS4.data.analog.stick.lx; // -128 0 127
164    int lft_stick_y  = PS4.data.analog.stick.ly; // -128 0 127
165
166    // prepare analog sticks (bytes)
167    byte b_rgt_stick_x_val;
168    byte b_rgt_stick_x_dir;
169    if (rgt_stick_x  >= 0) {
170      b_rgt_stick_x_val = byte(rgt_stick_x);
171      b_rgt_stick_x_dir  = 1;
172    }
173    else {
174      b_rgt_stick_x_val = byte(abs(rgt_stick_x)-1);
175      b_rgt_stick_x_dir = 0;
176    }
177
178    byte b_rgt_stick_y_val;
179    byte  b_rgt_stick_y_dir;
180    if (rgt_stick_y >= 0) {
181      b_rgt_stick_y_val = byte(rgt_stick_y);
182      b_rgt_stick_y_dir = 1;
183    }
184    else {
185      b_rgt_stick_y_val =  byte(abs(rgt_stick_y)-1);
186      b_rgt_stick_y_dir = 0;
187    }
188
189    byte  b_lft_stick_x_val;
190    byte b_lft_stick_x_dir;
191    if (lft_stick_x >= 0) {
192      b_lft_stick_x_val = byte(lft_stick_x);
193      b_lft_stick_x_dir = 1;
194    }
195    else {
196      b_lft_stick_x_val = byte(abs(lft_stick_x)-1);
197      b_lft_stick_x_dir  = 0;
198    }
199
200    byte b_lft_stick_y_val;
201    byte b_lft_stick_y_dir;
202    if (lft_stick_y >= 0) {
203      b_lft_stick_y_val = byte(lft_stick_y);
204      b_lft_stick_y_dir = 1;
205    }
206    else {
207      b_lft_stick_y_val =  byte(abs(lft_stick_y)-1);
208      b_lft_stick_y_dir = 0;
209    }
210    
211    CAN_frame_t  tx_control;
212    
213    // Send analog sticks bytes on CAN
214    tx_control.FIR.B.FF==CAN_frame_std;
215    tx_control.MsgID = 0x10;
216    tx_control.FIR.B.DLC = 8;
217    tx_control.data.u8[0]  = b_rgt_stick_x_val;
218    tx_control.data.u8[1] = b_rgt_stick_x_dir;
219    tx_control.data.u8[2]  = b_rgt_stick_y_val;
220    tx_control.data.u8[3] = b_rgt_stick_y_dir;
221    tx_control.data.u8[4]  = b_lft_stick_x_val;
222    tx_control.data.u8[5] = b_lft_stick_x_dir;
223    tx_control.data.u8[6]  = b_lft_stick_y_val;
224    tx_control.data.u8[7] = b_lft_stick_y_dir;
225    
226    ESP32Can.CANWriteFrame(&tx_control);
227
228    delay(5);
229
230    // send  button states as byte on CAN
231    tx_control.FIR.B.FF==CAN_frame_std;
232    tx_control.MsgID  = 0x11;
233    tx_control.FIR.B.DLC = 8;
234    tx_control.data.u8[0] = byte(PS4.data.button.right);
235    tx_control.data.u8[1] = byte(PS4.data.button.left);
236    tx_control.data.u8[2]  = byte(PS4.data.button.up);
237    tx_control.data.u8[3] = byte(PS4.data.button.down);
238    tx_control.data.u8[4] = byte(PS4.data.button.square);
239    tx_control.data.u8[5]  = byte(PS4.data.button.circle);
240    tx_control.data.u8[6] = byte(PS4.data.button.triangle);
241    tx_control.data.u8[7] = byte(PS4.data.button.cross);
242    ESP32Can.CANWriteFrame(&tx_control);
243    
244    delay(5);
245    
246    // send button states as byte on CAN
247    tx_control.FIR.B.FF==CAN_frame_std;
248    tx_control.MsgID = 0x12;
249    tx_control.FIR.B.DLC = 8;
250    tx_control.data.u8[0]  = byte(PS4.data.button.r1);
251    tx_control.data.u8[1] = byte(PS4.data.button.l1);
252    tx_control.data.u8[2] = byte(PS4.data.button.r2);
253    tx_control.data.u8[3]  = byte(PS4.data.analog.button.r2);
254    tx_control.data.u8[4] = byte(PS4.data.button.l2);
255    tx_control.data.u8[5] = byte(PS4.data.analog.button.l2);
256    tx_control.data.u8[6]  = byte(PS4.data.button.r3);
257    tx_control.data.u8[7] = byte(PS4.data.button.l3);
258    ESP32Can.CANWriteFrame(&tx_control);
259
260    delay(5);
261    
262    PS4.sendToController();  // should not be done more than each 15ms, but with delay (5+5+5) from tx CAN should  be ok
263  }
264  else {
265//    Serial.println("No PS4 controller connected.");
266    display.setCursor(83,25);
267    display.print("PS4 NOK");
268    display.setCursor(5,  40);
269    display.print("                    ");
270    display.setCursor(5,  55);
271    display.print("                    ");
272    display.display();
273
274    // delay(5);
275
276    CAN_frame_t tx_control;
277    
278    // Send analog  sticks bytes on CAN
279    tx_control.FIR.B.FF==CAN_frame_std;
280    tx_control.MsgID  = 0x10;
281    tx_control.FIR.B.DLC = 8;
282    tx_control.data.u8[0] = 0;
283    tx_control.data.u8[1]  = 0;
284    tx_control.data.u8[2] = 0;
285    tx_control.data.u8[3] = 0;
286    tx_control.data.u8[4]  = 0;
287    tx_control.data.u8[5] = 0;
288    tx_control.data.u8[6] = 0;
289    tx_control.data.u8[7]  = 0;
290    ESP32Can.CANWriteFrame(&tx_control);
291
292    delay(5);
293
294    //  send button states as byte on CAN
295    tx_control.FIR.B.FF==CAN_frame_std;
296    tx_control.MsgID = 0x11;
297    tx_control.FIR.B.DLC = 8;
298    tx_control.data.u8[0]  = 0;
299    tx_control.data.u8[1] = 0;
300    tx_control.data.u8[2] = 0;
301    tx_control.data.u8[3]  = 0;
302    tx_control.data.u8[4] = 0;
303    tx_control.data.u8[5] = 0;
304    tx_control.data.u8[6]  = 0;
305    tx_control.data.u8[7] = 0;
306    ESP32Can.CANWriteFrame(&tx_control);
307    
308    delay(5);
309    
310    // send button states as byte on CAN
311    tx_control.FIR.B.FF==CAN_frame_std;
312    tx_control.MsgID = 0x12;
313    tx_control.FIR.B.DLC = 8;
314    tx_control.data.u8[0]  = 0;
315    tx_control.data.u8[1] = 0;
316    tx_control.data.u8[2] = 0;
317    tx_control.data.u8[3]  = 0;
318    tx_control.data.u8[4] = 0;
319    tx_control.data.u8[5] = 0;
320    tx_control.data.u8[6]  = 0;
321    tx_control.data.u8[7] = 0;
322    ESP32Can.CANWriteFrame(&tx_control);
323
324    delay(5);
325  }
326  if ((millis() - lastStatusReveiced) < STATUS_CAN_TIMEOUT)  {
327    display.setCursor(5,25);
328    display.print("CAN OK ");
329  }
330  else {
331    auto_break = false;
332    break_active = false;
333    distance  = 0;
334    tilt_warn = false;
335    display.setCursor(5,25);
336    display.print("CAN  NOK");
337    display.setCursor(5, 40);
338    display.print("                    ");
339    display.setCursor(5, 55);
340    display.print("                    ");
341    delay(15); 
342  }
343  display.display();
344}
345

1// << ESP32 Crawler >>
2// board is "D-duino-32"
3// CAN Driver
4  https://github.com/nhatuan84/esp32-can-protocol-demo
5// Adafruit SSD1306 via
6  Library Manager (needs also Adafruit GFX)
7// PS4 Driver https://github.com/NURobotics/PS4-esp32
8//
9  PS4 MAC https://dancingpixelstudios.com/sixaxis-controller/sixaxispairtool/
10//
11  in case the controller is no longer pairing but was working before,
12// run "esptool
13  --chip esp32 erase_flash" in C:\\Users\\<user>\\AppData\\Local\\Arduino15\\packages\\esp32\	ools\\esptool_py\\2.6.1
14//
15  see https://github.com/NURobotics/PS4-esp32/issues/2#issuecomment-602852034
16
17#include
18  <Wire.h>
19#include <Adafruit_GFX.h>
20#include <Adafruit_SSD1306.h> // works
21  only if board and variant config for D-duino-32 is available
22
23#define SCREEN_WIDTH
24  128 // OLED display width, in pixels
25#define SCREEN_HEIGHT 64 // OLED display
26  height, in pixels
27
28#define OLED_RESET 3
29#define OLED_ROTATION 0
30Adafruit_SSD1306
31  display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
32
33#if (SSD1306_LCDHEIGHT
34  != 64)
35#error("Height incorrect, please fix Adafruit_SSD1306.h!");
36#endif
37
38#include
39  <PS4Controller.h>
40#define PS4_HOST_MAC "00:1e:ab:4c:4e:c8"
41#define TILT_RUMBLE
42  127
43
44#include <ESP32CAN.h>
45#define CAN_SPEED CAN_SPEED_250KBPS
46#define
47  CAN_TX GPIO_NUM_15
48#define CAN_RX GPIO_NUM_13
49#include <CAN_config.h>
50
51CAN_device_t
52  CAN_cfg;
53const int rx_queue_size = 10;
54
55#define STATUS_CAN_TIMEOUT 250
56unsigned
57  long lastStatusReveiced;
58
59bool auto_break = false;
60bool break_active =
61  false;
62int distance = 0;
63byte tilt_warn = false;
64
65void task_rxCAN(void
66  * pvParameters) {
67  for(;;) {
68    CAN_frame_t rx_frame;
69
70    if(xQueueReceive(CAN_cfg.rx_queue,&rx_frame,
71  3*portTICK_PERIOD_MS)==pdTRUE){
72      // start of message evaluation
73      if(rx_frame.FIR.B.FF==CAN_frame_std)
74  { // standard frame format
75        if (rx_frame.MsgID == 0x20 && rx_frame.FIR.B.DLC
76  == 8) {
77          auto_break =   bool(rx_frame.data.u8[0]);
78          break_active
79  = bool(rx_frame.data.u8[1]);
80          tilt_warn =    bool(rx_frame.data.u8[2]);
81
82          distance  =     int(rx_frame.data.u8[4])<<(3*8);
83          distance
84  +=     int(rx_frame.data.u8[5])<<(2*8);
85          distance +=     int(rx_frame.data.u8[6])<<(1*8);
86
87          distance +=     int(rx_frame.data.u8[7]);
88          
89          lastStatusReveiced
90  = millis();
91        }
92      }
93      // end of message evaluation
94    }
95  
96  }
97}
98
99void setup() 
100{
101  setCpuFrequencyMhz(160); // reduce heat
102  emission & power consumption (default would be 240Mhz)
103  
104//  Serial.begin(57600);
105
106
107  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) { // 0x3c for on-board OLED of
108  D-duino-32
109//    Serial.println(F("SSD1306 allocation failed"));
110    while
111  (1) {}
112  }
113  display.clearDisplay();
114  display.setTextSize(1);
115  display.setTextColor(WHITE,
116  BLACK);
117  display.setCursor(5,5);
118  display.print("<< ESP32 CRAWLER >>");
119
120  display.display();
121  
122  PS4.begin(PS4_HOST_MAC);
123
124  CAN_cfg.speed=CAN_SPEED;
125
126  CAN_cfg.tx_pin_id = CAN_TX;
127  CAN_cfg.rx_pin_id = CAN_RX;
128  CAN_cfg.rx_queue
129  = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
130  
131  ESP32Can.CANInit();
132
133
134  // Task: read CAN
135  xTaskCreatePinnedToCore(
136    task_rxCAN,     /* Function
137  to implement the task */
138    "task_rxCAN",   /* Name of the task */
139    4096,
140      /* Stack size in words */
141    NULL,      /* Task input parameter */
142
143    1,         /* Priority of the task */
144    NULL,      /* Task handle. */
145
146    1);        /* Core where the task should run */  
147
148  lastStatusReveiced
149  = 0;
150}
151
152void loop() 
153{ 
154  if(PS4.isConnected()) {
155
156//    Serial.print("R_X:
157  "); Serial.print(PS4.data.analog.stick.rx, DEC);
158//    Serial.print(" R_Y:
159  "); Serial.print(PS4.data.analog.stick.ry, DEC);
160//    Serial.print(" L_X:
161  "); Serial.print(PS4.data.analog.stick.lx, DEC);
162//    Serial.print(" L_Y:
163  "); Serial.print(PS4.data.analog.stick.ly, DEC);
164//    Serial.println();
165
166//
167    Serial.print("DIS: "); Serial.println(distance);
168
169    display.setCursor(83,25);
170
171    display.print("PS4 OK ");
172
173    display.setCursor(5, 40);
174    if (auto_break)
175  {
176      if (break_active) {
177        display.print("FRONT ASSIST ACTIVE");
178
179        PS4.setLed(255, 0, 0);
180      }
181      else {
182        display.print("FRONT
183  ASSIST     ON ");
184        PS4.setLed(0, 255, 0);
185      }
186    }
187    else
188  {
189      display.print("FRONT ASSIST     OFF");
190      PS4.setLed(0, 0, 255);
191
192    }
193
194    #ifdef TILT_RUMBLE
195    if (tilt_warn) {
196      PS4.setRumble(0,
197  TILT_RUMBLE);
198    }
199    else {
200      PS4.setRumble(0, 0);
201    }
202
203    #endif
204    
205    // PS4.sendToController(); // moved after sending frames
206
207    // delay(15); // replaced by delay between frames
208
209    display.setCursor(5,
210  55);
211    display.print("DISTANCE");
212    char sf[4];
213    sprintf(sf, "%4d",
214  distance);
215    display.setCursor(95, 55);
216    display.print(sf);
217
218    //
219  read analog sticks (integers)
220    int rgt_stick_x = PS4.data.analog.stick.rx;
221  // -128 0 127
222    int rgt_stick_y = PS4.data.analog.stick.ry; // -128 0 127
223
224    int lft_stick_x = PS4.data.analog.stick.lx; // -128 0 127
225    int lft_stick_y
226  = PS4.data.analog.stick.ly; // -128 0 127
227
228    // prepare analog sticks (bytes)
229
230    byte b_rgt_stick_x_val;
231    byte b_rgt_stick_x_dir;
232    if (rgt_stick_x
233  >= 0) {
234      b_rgt_stick_x_val = byte(rgt_stick_x);
235      b_rgt_stick_x_dir
236  = 1;
237    }
238    else {
239      b_rgt_stick_x_val = byte(abs(rgt_stick_x)-1);
240
241      b_rgt_stick_x_dir = 0;
242    }
243
244    byte b_rgt_stick_y_val;
245    byte
246  b_rgt_stick_y_dir;
247    if (rgt_stick_y >= 0) {
248      b_rgt_stick_y_val = byte(rgt_stick_y);
249
250      b_rgt_stick_y_dir = 1;
251    }
252    else {
253      b_rgt_stick_y_val =
254  byte(abs(rgt_stick_y)-1);
255      b_rgt_stick_y_dir = 0;
256    }
257
258    byte
259  b_lft_stick_x_val;
260    byte b_lft_stick_x_dir;
261    if (lft_stick_x >= 0) {
262
263      b_lft_stick_x_val = byte(lft_stick_x);
264      b_lft_stick_x_dir = 1;
265
266    }
267    else {
268      b_lft_stick_x_val = byte(abs(lft_stick_x)-1);
269      b_lft_stick_x_dir
270  = 0;
271    }
272
273    byte b_lft_stick_y_val;
274    byte b_lft_stick_y_dir;
275
276    if (lft_stick_y >= 0) {
277      b_lft_stick_y_val = byte(lft_stick_y);
278
279      b_lft_stick_y_dir = 1;
280    }
281    else {
282      b_lft_stick_y_val =
283  byte(abs(lft_stick_y)-1);
284      b_lft_stick_y_dir = 0;
285    }
286    
287    CAN_frame_t
288  tx_control;
289    
290    // Send analog sticks bytes on CAN
291    tx_control.FIR.B.FF==CAN_frame_std;
292
293    tx_control.MsgID = 0x10;
294    tx_control.FIR.B.DLC = 8;
295    tx_control.data.u8[0]
296  = b_rgt_stick_x_val;
297    tx_control.data.u8[1] = b_rgt_stick_x_dir;
298    tx_control.data.u8[2]
299  = b_rgt_stick_y_val;
300    tx_control.data.u8[3] = b_rgt_stick_y_dir;
301    tx_control.data.u8[4]
302  = b_lft_stick_x_val;
303    tx_control.data.u8[5] = b_lft_stick_x_dir;
304    tx_control.data.u8[6]
305  = b_lft_stick_y_val;
306    tx_control.data.u8[7] = b_lft_stick_y_dir;
307    
308
309    ESP32Can.CANWriteFrame(&tx_control);
310
311    delay(5);
312
313    // send
314  button states as byte on CAN
315    tx_control.FIR.B.FF==CAN_frame_std;
316    tx_control.MsgID
317  = 0x11;
318    tx_control.FIR.B.DLC = 8;
319    tx_control.data.u8[0] = byte(PS4.data.button.right);
320
321    tx_control.data.u8[1] = byte(PS4.data.button.left);
322    tx_control.data.u8[2]
323  = byte(PS4.data.button.up);
324    tx_control.data.u8[3] = byte(PS4.data.button.down);
325
326    tx_control.data.u8[4] = byte(PS4.data.button.square);
327    tx_control.data.u8[5]
328  = byte(PS4.data.button.circle);
329    tx_control.data.u8[6] = byte(PS4.data.button.triangle);
330
331    tx_control.data.u8[7] = byte(PS4.data.button.cross);
332    ESP32Can.CANWriteFrame(&tx_control);
333
334    
335    delay(5);
336    
337    // send button states as byte on CAN
338    tx_control.FIR.B.FF==CAN_frame_std;
339
340    tx_control.MsgID = 0x12;
341    tx_control.FIR.B.DLC = 8;
342    tx_control.data.u8[0]
343  = byte(PS4.data.button.r1);
344    tx_control.data.u8[1] = byte(PS4.data.button.l1);
345
346    tx_control.data.u8[2] = byte(PS4.data.button.r2);
347    tx_control.data.u8[3]
348  = byte(PS4.data.analog.button.r2);
349    tx_control.data.u8[4] = byte(PS4.data.button.l2);
350
351    tx_control.data.u8[5] = byte(PS4.data.analog.button.l2);
352    tx_control.data.u8[6]
353  = byte(PS4.data.button.r3);
354    tx_control.data.u8[7] = byte(PS4.data.button.l3);
355
356    ESP32Can.CANWriteFrame(&tx_control);
357
358    delay(5);
359    
360    PS4.sendToController();
361  // should not be done more than each 15ms, but with delay (5+5+5) from tx CAN should
362  be ok
363  }
364  else {
365//    Serial.println("No PS4 controller connected.");
366
367    display.setCursor(83,25);
368    display.print("PS4 NOK");
369    display.setCursor(5,
370  40);
371    display.print("                    ");
372    display.setCursor(5,
373  55);
374    display.print("                    ");
375    display.display();
376
377
378    // delay(5);
379
380    CAN_frame_t tx_control;
381    
382    // Send analog
383  sticks bytes on CAN
384    tx_control.FIR.B.FF==CAN_frame_std;
385    tx_control.MsgID
386  = 0x10;
387    tx_control.FIR.B.DLC = 8;
388    tx_control.data.u8[0] = 0;
389    tx_control.data.u8[1]
390  = 0;
391    tx_control.data.u8[2] = 0;
392    tx_control.data.u8[3] = 0;
393    tx_control.data.u8[4]
394  = 0;
395    tx_control.data.u8[5] = 0;
396    tx_control.data.u8[6] = 0;
397    tx_control.data.u8[7]
398  = 0;
399    ESP32Can.CANWriteFrame(&tx_control);
400
401    delay(5);
402
403    //
404  send button states as byte on CAN
405    tx_control.FIR.B.FF==CAN_frame_std;
406
407    tx_control.MsgID = 0x11;
408    tx_control.FIR.B.DLC = 8;
409    tx_control.data.u8[0]
410  = 0;
411    tx_control.data.u8[1] = 0;
412    tx_control.data.u8[2] = 0;
413    tx_control.data.u8[3]
414  = 0;
415    tx_control.data.u8[4] = 0;
416    tx_control.data.u8[5] = 0;
417    tx_control.data.u8[6]
418  = 0;
419    tx_control.data.u8[7] = 0;
420    ESP32Can.CANWriteFrame(&tx_control);
421
422    
423    delay(5);
424    
425    // send button states as byte on CAN
426    tx_control.FIR.B.FF==CAN_frame_std;
427
428    tx_control.MsgID = 0x12;
429    tx_control.FIR.B.DLC = 8;
430    tx_control.data.u8[0]
431  = 0;
432    tx_control.data.u8[1] = 0;
433    tx_control.data.u8[2] = 0;
434    tx_control.data.u8[3]
435  = 0;
436    tx_control.data.u8[4] = 0;
437    tx_control.data.u8[5] = 0;
438    tx_control.data.u8[6]
439  = 0;
440    tx_control.data.u8[7] = 0;
441    ESP32Can.CANWriteFrame(&tx_control);
442
443
444    delay(5);
445  }
446  if ((millis() - lastStatusReveiced) < STATUS_CAN_TIMEOUT)
447  {
448    display.setCursor(5,25);
449    display.print("CAN OK ");
450  }
451
452  else {
453    auto_break = false;
454    break_active = false;
455    distance
456  = 0;
457    tilt_warn = false;
458    display.setCursor(5,25);
459    display.print("CAN
460  NOK");
461    display.setCursor(5, 40);
462    display.print("                    ");
463
464    display.setCursor(5, 55);
465    display.print("                    ");
466
467    delay(15); 
468  }
469  display.display();
470}
471

1// << ESP32 Crawler >>
2// board is "ESP32 Dev Module"
3// CAN Driver  https://github.com/nhatuan84/esp32-can-protocol-demo
4// VL53L0X Driver from Pololu  via Library Manager
5// MPU9250 Driver from Bolder Flight System via Library Manager
6//  Adafruit Motor Shield V2 via Library Manager
7
8#define LED_PIN 2
9
10#include  <Wire.h>
11
12#include <Adafruit_MotorShield.h>
13Adafruit_MotorShield AFMS =  Adafruit_MotorShield();
14Adafruit_DCMotor *right_motor = AFMS.getMotor(3);
15Adafruit_DCMotor  *left_motor = AFMS.getMotor(4);
16
17#define SPEED_MIN 70 // motor min
18#define  SPEED_MAX 255 // max speed
19#define SPEED_NONE 0
20int constSpeeds[] = {100,  150, 200, 250};
21int speedIndex = 1;
22bool incState = false;
23bool incLast  = false;
24bool decState = false;
25bool decLast = false;
26
27#include <VL53L0X.h>
28VL53L0X  TOF;
29#define TOF_INTERVALL 50 // ms
30unsigned long lastTOFmeasurement = 0;
31
32#define  DISTANCE_MIN 170
33int distance = 0;
34bool auto_break  = true;
35bool break_active  = false;
36
37#include "MPU9250.h"
38MPU9250 IMU(Wire, 0x68);
39#define TILT_MAX  45
40bool tilt_warn = false;
41
42#include <ESP32CAN.h>
43#define CAN_SPEED  CAN_SPEED_250KBPS
44#define CAN_TX GPIO_NUM_16
45#define CAN_RX GPIO_NUM_17
46#include  <CAN_config.h>
47
48CAN_device_t CAN_cfg;
49const int rx_queue_size = 10;
50
51#define  STICK_CAN_TIMEOUT 250
52unsigned long lastStickReveiced;
53
54int stick_left_x  = 0;
55int stick_left_y = 0;
56int stick_right_x = 0;
57int stick_right_y = 0;
58
59bool  button_left = false;
60bool button_right = false;
61bool button_up = false;
62bool  button_down = false;
63
64bool button_square = false;
65bool button_circle =  false;
66bool button_triangle = false;
67bool button_cross = false;
68
69bool  button_l1 = false;
70bool button_r1 = false;
71
72void task_rxCAN(void * pvParameters)  {
73  for(;;) {
74    CAN_frame_t rx_frame;
75
76    if(xQueueReceive(CAN_cfg.rx_queue,&rx_frame,  3*portTICK_PERIOD_MS)==pdTRUE){
77      if(rx_frame.FIR.B.FF==CAN_frame_std) {  // standard frame format
78        digitalWrite(LED_PIN, HIGH);
79        // start  of message evaluation
80        if (rx_frame.MsgID == 0x10 && rx_frame.FIR.B.DLC  == 8) {
81          if (rx_frame.data.u8[1]) {
82            stick_right_x = int(rx_frame.data.u8[0]);
83          }
84          else {
85            stick_right_x = int(-(rx_frame.data.u8[0]  + 1));
86          }
87          if (rx_frame.data.u8[3]) {
88            stick_right_y  = int(rx_frame.data.u8[2]);
89          }
90          else {
91            stick_right_y  = int(-(rx_frame.data.u8[2] + 1));
92          }
93          if (rx_frame.data.u8[5])  {
94            stick_left_x = int(rx_frame.data.u8[4]);
95          }
96          else  {
97            stick_left_x = int(-(rx_frame.data.u8[4] + 1));
98          }
99          if (rx_frame.data.u8[7]) {
100            stick_left_y = int(rx_frame.data.u8[6]);
101          }
102          else {
103            stick_left_y = int(-(rx_frame.data.u8[6]  + 1));
104          }
105          lastStickReveiced = millis();
106        }
107        else if (rx_frame.MsgID == 0x11 && rx_frame.FIR.B.DLC == 8) {
108          button_right  = (bool)rx_frame.data.u8[0];
109          button_left = (bool)rx_frame.data.u8[1];
110          button_up = (bool)rx_frame.data.u8[2];
111          button_down = (bool)rx_frame.data.u8[3];
112          button_square = (bool)rx_frame.data.u8[4];
113          button_circle  = (bool)rx_frame.data.u8[5];
114          button_triangle = (bool)rx_frame.data.u8[6];
115          button_cross = (bool)rx_frame.data.u8[7];
116        }
117        else  if (rx_frame.MsgID == 0x12 && rx_frame.FIR.B.DLC == 8) {
118          button_r1  = (bool)rx_frame.data.u8[0];
119          button_l1 = (bool)rx_frame.data.u8[1];
120        }
121        // end of message evaluation
122      }
123    }
124    else  {
125      digitalWrite(LED_PIN, LOW);
126    }
127  }
128}
129
130void task_txCAN(void  * pvParameters) {
131  for(;;) {
132    CAN_frame_t tx_frame;
133    int d = distance;
134    
135    tx_frame.FIR.B.FF==CAN_frame_std;
136    tx_frame.MsgID = 0x20;
137    tx_frame.FIR.B.DLC = 8;
138    tx_frame.data.u8[0] = byte(auto_break);
139    tx_frame.data.u8[1]  = byte(break_active);
140    tx_frame.data.u8[2] = byte(tilt_warn);
141    tx_frame.data.u8[3]  = 0;
142    tx_frame.data.u8[4] = byte(d>>(3*8));
143    tx_frame.data.u8[5] = byte(d<<(1*8)>>(3*8));
144    tx_frame.data.u8[6] = byte(d<<(2*8)>>(3*8));
145    tx_frame.data.u8[7] = byte(d<<(3*8)>>(3*8));
146    
147    ESP32Can.CANWriteFrame(&tx_frame);
148    
149    delay(50);
150  }
151}
152
153void  setup() 
154{
155//  Serial.begin(57600);
156
157  pinMode(LED_PIN, OUTPUT);
158  digitalWrite(LED_PIN, HIGH);
159
160  Wire.begin();
161
162  AFMS.begin();
163
164  right_motor->setSpeed(SPEED_NONE);
165  right_motor->run(RELEASE);
166
167  left_motor->setSpeed(SPEED_NONE);
168  left_motor->run(RELEASE);
169  
170  TOF.setTimeout(500);
171  if (!TOF.init())  {
172//    Serial.println("TOF init failed!");
173    while (1) {
174      digitalWrite(LED_PIN,  LOW);
175      delay(500);
176      digitalWrite(LED_PIN, HIGH);
177      delay(500);
178    }
179  }
180  TOF.startContinuous();
181
182  if (IMU.begin() < 0) {
183//    Serial.println("IMU  init failed!");
184    while (1) {
185      digitalWrite(LED_PIN, LOW);
186      delay(1000);
187      digitalWrite(LED_PIN, HIGH);
188      delay(1000);
189    }
190  }
191  
192  CAN_cfg.speed=CAN_SPEED;
193  CAN_cfg.tx_pin_id = CAN_TX;
194  CAN_cfg.rx_pin_id  = CAN_RX;
195  CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
196  
197  ESP32Can.CANInit();
198
199  digitalWrite(LED_PIN, LOW);
200
201  // Task:  write CAN
202  xTaskCreatePinnedToCore(
203    task_txCAN,     /* Function to implement  the task */
204    "task_txCAN",   /* Name of the task */
205    4096,      /*  Stack size in words */
206    NULL,      /* Task input parameter */
207    1,         /*  Priority of the task */
208    NULL,      /* Task handle. */
209    1);        /*  Core where the task should run */
210
211  // Task: read CAN
212  xTaskCreatePinnedToCore(
213    task_rxCAN,     /* Function to implement the task */
214    "task_rxCAN",   /* Name of the task */
215    4096,      /* Stack size in words */
216    NULL,      /* Task input parameter */
217    1,         /* Priority of the task */
218    NULL,      /* Task handle. */
219    1);        /* Core where the task should  run */
220
221  lastStickReveiced = millis();
222}
223
224void loop() 
225{ 
226//  Serial.print("R_X: "); Serial.print(stick_right_x, DEC);
227//  Serial.print("  R_Y: "); Serial.print(stick_right_y, DEC);
228//  Serial.print(" L_X: "); Serial.print(stick_left_x,  DEC);
229//  Serial.print(" L_Y: "); Serial.print(stick_left_y, DEC);
230//  Serial.println();
231
232  float acX, acY, acZ;
233  float p, r;
234  int pitch = 0, roll = 0;
235  
236  IMU.readSensor();
237  acX = IMU.getAccelX_mss();
238  acY = IMU.getAccelY_mss();
239  acZ = IMU.getAccelZ_mss();
240  p = atan(acX / sqrt((acY * acY) + (acZ * acZ)));
241  r = atan(acY / sqrt((acX * acX) + (acZ * acZ)));
242  //convert radians into degrees
243  pitch = int(p * 57.2958); // 180.0 / pi
244  roll = int(r * 57.2958); // 180.0  / pi
245  tilt_warn = max(abs(pitch), abs(roll)) > TILT_MAX;
246//  Serial.print("IMUP:  "); Serial.print(pitch);
247//  Serial.print(" IMUR: "); Serial.print(roll);
248//  Serial.println();
249
250  delay(10);
251
252  // read TOF only once per intervall
253  if (millis() - lastTOFmeasurement >= TOF_INTERVALL) {
254    distance = TOF.readRangeContinuousMillimeters();
255    if (TOF.timeoutOccurred() || distance > 8191) distance = 8190;
256    lastTOFmeasurement  = millis();
257    delay(10);
258  }
259//  Serial.print("DIS: "); Serial.print(distance);  Serial.print(" ");
260//  Serial.println();
261
262  if ((millis() - lastStickReveiced)  < STICK_CAN_TIMEOUT) {
263
264    int throttle = stick_left_y; // -128 0 127
265    int steering = stick_right_x; // -128 0 127
266    int left_speed = 0, right_speed  = 0;
267
268    if (throttle > 0) throttle +=1; // max out to 128
269    if (steering  > 0) steering +=1; // max out to 128
270
271    if (button_triangle) {
272      auto_break  = true;
273    }
274    else if (button_cross) {
275      auto_break = false;
276    }
277
278    // mix throttle and steering inputs to obtain left & right motor  speeds
279    left_speed = ((throttle * SPEED_MAX / 128) + (steering * SPEED_MAX  / 128));
280    right_speed = ((throttle * SPEED_MAX / 128) - (steering * SPEED_MAX  / 128));
281    
282    // cap speeds to max
283    left_speed = min(max(left_speed,  -SPEED_MAX), SPEED_MAX);
284    right_speed = min(max(right_speed, -SPEED_MAX),  SPEED_MAX);
285
286    // decrease/increase fixed speed for digital pad control)
287    decState = button_l1;
288    if(decState && decState != decLast) {
289      if  (speedIndex > 0) speedIndex--;
290    }
291    decLast = decState;
292    incState  = button_r1;
293    if(incState && incState != incLast) {
294      if (speedIndex  < 3) speedIndex++;
295    }
296    incLast = incState;
297
298    // overwrite analog  input by digital in case active
299    if (button_right){
300      left_speed =  constSpeeds[speedIndex];
301      right_speed = -constSpeeds[speedIndex];
302    }
303    else if (button_left){
304      left_speed = -constSpeeds[speedIndex];
305      right_speed  = constSpeeds[speedIndex];    
306    }
307    else if (button_up) {
308      left_speed  = constSpeeds[speedIndex];
309      right_speed = constSpeeds[speedIndex];
310    }
311    else if (button_down){
312      left_speed = -constSpeeds[speedIndex];
313      right_speed  = -constSpeeds[speedIndex];    
314    }
315  
316    if (abs(left_speed) < SPEED_MIN)  left_speed = 0;
317    if (abs(right_speed) < SPEED_MIN) right_speed = 0;
318
319    if (auto_break) {
320      break_active = false;
321      if (distance < (DISTANCE_MIN  * 1.5) && left_speed > (SPEED_MAX / 2) && right_speed > (SPEED_MAX / 2)) {
322        left_speed  = left_speed / 2;
323        right_speed = right_speed / 2;        
324      }
325      if (distance < DISTANCE_MIN && left_speed > 0 && right_speed > 0) {
326        left_speed  = SPEED_NONE;
327        right_speed = SPEED_NONE;
328        break_active = true;
329      }
330    }
331
332//    Serial.print("RSDP: "); Serial.print(right_speed);
333//    Serial.print(" LSPD: "); Serial.print(left_speed);
334//    Serial.println();
335
336    right_motor->setSpeed(abs(right_speed));
337    if (right_speed >= 0) right_motor->run(FORWARD);
338    else right_motor->run(BACKWARD);
339    
340    left_motor->setSpeed(abs(left_speed));
341    if (left_speed >= 0) left_motor->run(FORWARD);
342    else left_motor->run(BACKWARD);
343
344    delay(10);
345  }
346  else {
347//    Serial.println("TIMEOUT: Stick Frame  not received");
348    
349    stick_left_x = 0;
350    stick_left_y = 0;
351    stick_right_x  = 0;
352    stick_right_y = 0;
353
354    button_left = false;
355    button_right  = false;
356    button_up = false;
357    button_down = false;
358
359    button_square  = false;
360    button_circle = false;
361    button_triangle = false;
362    button_cross  = false;
363
364    button_l1 = false;
365    button_r1 = false;
366    
367    right_motor->setSpeed(SPEED_NONE);
368    right_motor->run(RELEASE);
369
370    left_motor->setSpeed(SPEED_NONE);
371    left_motor->run(RELEASE);
372
373    delay(STICK_CAN_TIMEOUT/2);
374  }
375}
376