arduino-sketches – Rev 26
?pathlinks?
///////////////////////////////////////////////////////////////////////////
// Copyright (C) Wizardry and Steamworks 2024 - License: MIT //
// Please see: http://www.gnu.org/licenses/gpl.html for legal details, //
// rights of fair usage, the disclaimer and warranty conditions. //
///////////////////////////////////////////////////////////////////////////
#include <Bluepad32.h>
// ptz camera caddy control
#include "esp32-hal-ledc.h"
// wifi settings
#include <WiFi.h>
#include <ESPmDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
// comment out to remove serial port debugging
//#define DEBUG_CONTROLLER_BUTTONS
///////////////////////////////////////////////////////////////////////////
// wireless and OTA settings //
///////////////////////////////////////////////////////////////////////////
// the wifi network to connect to
#define WIFI_SSID ""
// the wifi password in plaintext
#define WIFI_PASSWORD ""
// the OTA password as an MD5 hash
#define OTA_PASSWORD_HASH ""
///////////////////////////////////////////////////////////////////////////
// controller button definitions //
///////////////////////////////////////////////////////////////////////////
// these can be determined by enabling DEBUG, connecting a gamepad and //
// then watching the console whilst pressing keys on the gamepad //
///////////////////////////////////////////////////////////////////////////
#define CONTROLLER_BUTTON_A 2
#define CONTROLLER_BUTTON_B 1
#define CONTROLLER_BUTTON_Y 4
#define CONTROLLER_BUTTON_X 8
#define CONTROLLER_DPAD_LEFT 8
#define CONTROLLER_DPAD_RIGHT 4
#define CONTROLLER_DPAD_UP 1
#define CONTROLLER_DPAD_DOWN 2
///////////////////////////////////////////////////////////////////////////
// servo motor definitions for the PTZ caddy //
// https://grimore.org/hardware/creating_a_ptz_caddy //
///////////////////////////////////////////////////////////////////////////
#define SERVO_CAMERA_PAN_GPIO 17
#define SERVO_CAMERA_PAN_LOW 1638
#define SERVO_CAMERA_PAN_HIGH 7864
#define SERVO_CAMERA_PAN_TIMER_WIDTH 16
#define SERVO_CAMERA_PAN_CHANNEL 11
#define SERVO_CAMERA_PAN_STEP 100
#define SERVO_CAMERA_PAN_FREQUENCY 50
#define SERVO_CAMERA_TILT_GPIO 5
#define SERVO_CAMERA_TILT_LOW 1638
#define SERVO_CAMERA_TILT_HIGH 7864
#define SERVO_CAMERA_TILT_TIMER_WIDTH 16
#define SERVO_CAMERA_TILT_CHANNEL 12
#define SERVO_CAMERA_TILT_STEP 100
#define SERVO_CAMERA_TILT_FREQUENCY 50
///////////////////////////////////////////////////////////////////////////
// miscellaneous //
///////////////////////////////////////////////////////////////////////////
#define HEADLIGHTS_GPIO 32
///////////////////////////////////////////////////////////////////////////
// declarations //
///////////////////////////////////////////////////////////////////////////
volatile int cameraPtzPan = (int)((SERVO_CAMERA_PAN_LOW + SERVO_CAMERA_PAN_HIGH) / 2.0),
cameraPtzTilt = (int)((SERVO_CAMERA_TILT_LOW + SERVO_CAMERA_TILT_HIGH) / 2.0),
retainPtzPan, retainPtzTilt;
ControllerPtr bleController;
TaskHandle_t task0, task1;
void coreTask_0(void * parameter);
void coreTask_1(void * parameter);
SemaphoreHandle_t mutexPan = xSemaphoreCreateMutex();
SemaphoreHandle_t mutexTilt = xSemaphoreCreateMutex();
volatile bool headlights = false;
///////////////////////////////////////////////////////////////////////////
// BLE connect and disconnect methods //
///////////////////////////////////////////////////////////////////////////
void onConnectedController(ControllerPtr ctl) {
ControllerProperties properties = ctl->getProperties();
Serial.printf("Controller connected model: %s, VID=0x%04x, PID=0x%04x\n",
ctl->getModelName().c_str(),
properties.vendor_id,
properties.product_id
);
bleController = ctl;
// create two tasks scheduled on core 0 that is not used by bluepad32
// that wil handle the pan and tilt of the PTZ caddy for the camera
xTaskCreatePinnedToCore(
coreTask_0,
"pan",
10000,
NULL,
1,
&task0,
0
);
xTaskCreatePinnedToCore(
coreTask_1,
"tilt",
10000,
NULL,
1,
&task1,
0
);
}
void onDisconnectedController(ControllerPtr ctl) {
Serial.printf("Lost controller connection.");
bleController = nullptr;
// delete the camera PTZ control tasks
vTaskDelete(task0);
vTaskDelete(task1);
BP32.forgetBluetoothKeys();
}
///////////////////////////////////////////////////////////////////////////
// camnera controls for the PTZ caddy //
///////////////////////////////////////////////////////////////////////////
void cameraPanLeft() {
Serial.printf("Current pan angle is %d.\n", cameraPtzPan);
if (cameraPtzPan + SERVO_CAMERA_PAN_STEP > SERVO_CAMERA_PAN_HIGH) {
return;
}
cameraPtzPan += SERVO_CAMERA_PAN_STEP;
}
void cameraPanRight() {
Serial.printf("Current pan angle is %d.\n", cameraPtzPan);
if (cameraPtzPan - SERVO_CAMERA_PAN_STEP < SERVO_CAMERA_PAN_LOW) {
return;
}
cameraPtzPan -= SERVO_CAMERA_PAN_STEP;
}
void cameraTiltUp() {
Serial.printf("Current tilt angle is %d.\n", cameraPtzTilt);
if (cameraPtzTilt + SERVO_CAMERA_TILT_STEP > SERVO_CAMERA_TILT_HIGH) {
return;
}
cameraPtzTilt += SERVO_CAMERA_TILT_STEP;
}
void cameraTiltDown() {
Serial.printf("Current tilt angle is %d.\n", cameraPtzTilt);
if (cameraPtzTilt - SERVO_CAMERA_TILT_STEP < SERVO_CAMERA_TILT_LOW) {
return;
}
cameraPtzTilt -= SERVO_CAMERA_TILT_STEP;
}
///////////////////////////////////////////////////////////////////////////
// mecanum engine control //
///////////////////////////////////////////////////////////////////////////
void engineStop() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, LOW);
// rb
digitalWrite(23, LOW);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, LOW);
// lb
digitalWrite(0, LOW);
digitalWrite(2, LOW);
}
void nudgeEngineForward() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, HIGH);
// rb
digitalWrite(23, LOW);
digitalWrite(18, HIGH);
// lf
digitalWrite(16, HIGH);
digitalWrite(4, LOW);
// lb
digitalWrite(0, HIGH);
digitalWrite(2, LOW);
}
void nudgeEngineForwardLeft() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, LOW);
// rb
digitalWrite(23, LOW);
digitalWrite(18, HIGH);
// lf
digitalWrite(16, HIGH);
digitalWrite(4, LOW);
// lb
digitalWrite(0, LOW);
digitalWrite(2, LOW);
}
void nudgeEngineForwardRight() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, HIGH);
// rb
digitalWrite(23, LOW);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, LOW);
// lb
digitalWrite(0, HIGH);
digitalWrite(2, LOW);
}
void nudgeEngineBackward() {
// rf
digitalWrite(22, HIGH);
digitalWrite(19, LOW);
// rb
digitalWrite(23, HIGH);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, HIGH);
// lb
digitalWrite(0, LOW);
digitalWrite(2, HIGH);
}
void nudgeEngineBackwardRight() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, LOW);
// rb
digitalWrite(23, HIGH);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, HIGH);
// lb
digitalWrite(0, LOW);
digitalWrite(2, LOW);
}
void nudgeEngineBackwardLeft() {
// rf
digitalWrite(22, HIGH);
digitalWrite(19, LOW);
// rb
digitalWrite(23, LOW);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, LOW);
// lb
digitalWrite(0, LOW);
digitalWrite(2, HIGH);
}
void nudgeEngineTurnLeft() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, HIGH);
// rb
digitalWrite(23, LOW);
digitalWrite(18, HIGH);
// lf
digitalWrite(16, LOW);
digitalWrite(4, HIGH);
// lb
digitalWrite(0, LOW);
digitalWrite(2, HIGH);
}
void nudgeEngineTurnRight() {
// rf
digitalWrite(22, HIGH);
digitalWrite(19, LOW);
// rb
digitalWrite(23, HIGH);
digitalWrite(18, LOW);
// lf
digitalWrite(16, HIGH);
digitalWrite(4, LOW);
// lb
digitalWrite(0, HIGH);
digitalWrite(2, LOW);
}
void nudgeEngineStrafeLeft() {
// rf
digitalWrite(22, HIGH);
digitalWrite(19, LOW);
// rb
digitalWrite(23, LOW);
digitalWrite(18, HIGH);
// lf
digitalWrite(16, HIGH);
digitalWrite(4, LOW);
// lb
digitalWrite(0, LOW);
digitalWrite(2, HIGH);
}
void nudgeEngineStrafeRight() {
// rf
digitalWrite(22, LOW);
digitalWrite(19, HIGH);
// rb
digitalWrite(23, HIGH);
digitalWrite(18, LOW);
// lf
digitalWrite(16, LOW);
digitalWrite(4, HIGH);
// lb
digitalWrite(0, HIGH);
digitalWrite(2, LOW);
}
///////////////////////////////////////////////////////////////////////////
// process controller actions //
// //
// combos implemented: //
// * up - forward, down - back, left - turn left, right - turn right //
// * hold A + left, up or down, move the camera PTZ caddy //
// * hold B + left or right, strafe left and strafe right //
///////////////////////////////////////////////////////////////////////////
void actControls(ControllerPtr ctrl) {
switch (ctrl->dpad()) {
case CONTROLLER_DPAD_UP:
switch (ctrl->buttons()) {
case CONTROLLER_BUTTON_A:
Serial.println("Nudging camera up...");
cameraTiltUp();
return;
}
// go forward
nudgeEngineForward();
return;
case CONTROLLER_DPAD_UP | CONTROLLER_DPAD_LEFT:
nudgeEngineForwardLeft();
return;
case CONTROLLER_DPAD_DOWN:
switch (ctrl->buttons()) {
case CONTROLLER_BUTTON_A:
Serial.println("Nudging camera down...");
cameraTiltDown();
return;
}
//go back
nudgeEngineBackward();
return;
case CONTROLLER_DPAD_LEFT:
switch (ctrl->buttons()) {
case CONTROLLER_BUTTON_A:
Serial.println("Nudging camera left...");
cameraPanLeft();
return;
case CONTROLLER_BUTTON_X:
nudgeEngineStrafeRight();
return;
}
//turn left (implement strafe)
nudgeEngineTurnLeft();
return;
case CONTROLLER_DPAD_RIGHT:
switch (ctrl->buttons()) {
case CONTROLLER_BUTTON_A:
Serial.println("Nudging camera right...");
cameraPanRight();
return;
case CONTROLLER_BUTTON_X:
nudgeEngineStrafeLeft();
return;
}
// turn right (implement strafe)
nudgeEngineTurnRight();
return;
}
switch(ctrl->buttons()) {
case CONTROLLER_BUTTON_Y:
switch(headlights) {
case false:
Serial.println("Turning headlights on...");
digitalWrite(HEADLIGHTS_GPIO, LOW);
break;
case true:
Serial.println("Turning headlights off...");
digitalWrite(HEADLIGHTS_GPIO, HIGH);
break;
}
headlights = !headlights;
return;
}
engineStop();
}
///////////////////////////////////////////////////////////////////////////
// bluepad32 occupies core 1 such that all processing for the project //
// will be carried out on core 0 for some separation of concerns //
///////////////////////////////////////////////////////////////////////////
void coreTask_0(void * parameter) {
ledcSetup(SERVO_CAMERA_PAN_CHANNEL, SERVO_CAMERA_PAN_FREQUENCY, SERVO_CAMERA_PAN_TIMER_WIDTH);
ledcAttachPin(SERVO_CAMERA_PAN_GPIO, SERVO_CAMERA_PAN_CHANNEL);
do {
if (cameraPtzPan != retainPtzPan) {
ledcWrite(SERVO_CAMERA_PAN_CHANNEL, cameraPtzPan);
retainPtzPan = cameraPtzPan;
delay(100);
ledcWrite(SERVO_CAMERA_PAN_CHANNEL, 0);
}
vTaskDelay(1);
} while (bleController && bleController->isConnected());
}
void coreTask_1(void * parameter) {
ledcSetup(SERVO_CAMERA_TILT_CHANNEL, SERVO_CAMERA_TILT_FREQUENCY, SERVO_CAMERA_TILT_TIMER_WIDTH);
ledcAttachPin(SERVO_CAMERA_TILT_GPIO, SERVO_CAMERA_TILT_CHANNEL);
do {
if (cameraPtzTilt != retainPtzTilt) {
ledcWrite(SERVO_CAMERA_TILT_CHANNEL, cameraPtzTilt);
retainPtzTilt = cameraPtzTilt;
delay(100);
ledcWrite(SERVO_CAMERA_TILT_CHANNEL, 0);
}
vTaskDelay(1);
} while (bleController && bleController->isConnected());
}
///////////////////////////////////////////////////////////////////////////
// ARDUINO FUNCTIONS //
///////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(115200);
// start wifi
WiFi.mode(WIFI_STA);
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
while (WiFi.waitForConnectResult() != WL_CONNECTED) {
Serial.println("Wifi connection failed! Rebooting...");
delay(5000);
ESP.restart();
}
Serial.print("IP address: ");
Serial.println(WiFi.localIP());
// setup OTA
// Port defaults to 3232
// ArduinoOTA.setPort(3232);
// Hostname defaults to esp3232-[MAC]
ArduinoOTA.setHostname("roverone");
// No authentication by default
//ArduinoOTA.setPassword(OTA_PASSWORD);
// Password can be set with it's md5 value as well
// MD5(admin) = 21232f297a57a5a743894a0e4a801fc3
ArduinoOTA.setPasswordHash(OTA_PASSWORD_HASH);
ArduinoOTA
.onStart([]() {
String type;
if (ArduinoOTA.getCommand() == U_FLASH)
type = "sketch";
else // U_SPIFFS
type = "filesystem";
// NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
Serial.println("Start updating " + type);
})
.onEnd([]() {
Serial.println("\nEnd");
delay(5000);
ESP.restart();
})
.onProgress([](unsigned int progress, unsigned int total) {
Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
})
.onError([](ota_error_t error) {
Serial.printf("Error[%u]: ", error);
switch (error) {
case OTA_AUTH_ERROR:
Serial.println("Auth Failed");
break;
case OTA_BEGIN_ERROR:
Serial.println("Begin Failed");
break;
case OTA_CONNECT_ERROR:
Serial.println("Connect Failed");
break;
case OTA_RECEIVE_ERROR:
Serial.println("Receive Failed");
break;
case OTA_END_ERROR:
Serial.println("End Failed");
break;
}
});
ArduinoOTA.begin();
Serial.println("OTA setup complete");
// setup Bluepad32
Serial.printf("Bluepad32 firmware version: %s\n", BP32.firmwareVersion());
const uint8_t* addr = BP32.localBdAddress();
Serial.printf("Bluetooth address: %2X:%2X:%2X:%2X:%2X:%2X\n",
addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
// setup the Bluepad32 callbacks
BP32.setup(&onConnectedController, &onDisconnectedController);
// setup motors
// rf
pinMode(22, OUTPUT);
pinMode(19, OUTPUT);
// rb
pinMode(23, OUTPUT);
pinMode(18, OUTPUT);
// lf
pinMode(16, OUTPUT);
pinMode(4, OUTPUT);
// lb
pinMode(0, OUTPUT);
pinMode(2, OUTPUT);
// set headlights to output
pinMode(HEADLIGHTS_GPIO, OUTPUT);
}
// loop runs on core 1
void loop() {
// handle any OTA updates
ArduinoOTA.handle();
// must be within loop()
BP32.update();
if (bleController && bleController->isConnected()) {
// useful dump of all controls
#ifdef DEBUG_CONTROLLER_BUTTONS
Serial.printf(
"Gamepad control debug: "
"idx=%d, dpad: 0x%02x, buttons: 0x%04x, axis L: %4d, %4d, axis R: %4d, %4d, brake: %4d, throttle: %4d, "
"misc: 0x%02x, gyro x:%6d y:%6d z:%6d, accel x:%6d y:%6d z:%6d\n",
bleController->index(), // Controller Index
bleController->dpad(), // D-pad
bleController->buttons(), // bitmask of pressed buttons
bleController->axisX(), // (-511 - 512) left X Axis
bleController->axisY(), // (-511 - 512) left Y axis
bleController->axisRX(), // (-511 - 512) right X axis
bleController->axisRY(), // (-511 - 512) right Y axis
bleController->brake(), // (0 - 1023): brake button
bleController->throttle(), // (0 - 1023): throttle (AKA gas) button
bleController->miscButtons(), // bitmask of pressed "misc" buttons
bleController->gyroX(), // Gyro X
bleController->gyroY(), // Gyro Y
bleController->gyroZ(), // Gyro Z
bleController->accelX(), // Accelerometer X
bleController->accelY(), // Accelerometer Y
bleController->accelZ() // Accelerometer Z
);
#endif
// run the controller operations
actControls(bleController);
}
/*
delay(5000);
digitalWrite(32, HIGH);
digitalWrite(LED_BUILTIN, HIGH);
delay(5000);
digitalWrite(32, LOW);
digitalWrite(LED_BUILTIN, LOW);
*/
delay(100);
}