From 9c425e1b69799669905dc836940669f363aeeaf4 Mon Sep 17 00:00:00 2001
From: Vladimir Garistov <vl.garistov@gmail.com>
Date: Thu, 30 Nov 2023 20:36:59 +0100
Subject: [PATCH] Added a joystick demonstration to the Bluetooth example

---
 .../TUES_bluetooth_demo.ino                   | 65 ++++++++++++++++++-
 1 file changed, 64 insertions(+), 1 deletion(-)

diff --git a/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino b/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino
index bb73d6a..37a73a6 100644
--- a/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino
+++ b/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino
@@ -22,22 +22,33 @@
 // Dabble configuration options
 #define CUSTOM_SETTINGS
 #define INCLUDE_TERMINAL_MODULE
+#define INCLUDE_GAMEPAD_MODULE
 #ifdef _ESP32_
 	// Does not compile with increased warnings in Arduino IDE because some are treated as errors.
 	#include <DabbleESP32.h>
+	#include <ESP32Servo.h>
 #else
 	#include <Dabble.h>
+	#include <Servo.h>
 #endif
 
 #ifdef _ESP32_
 	#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BT_BLUEDROID_ENABLED)
 		#error Bluetooth is not enabled! Please run `make menuconfig` to enable it.
 	#endif
-
 	#if !defined(CONFIG_BT_SPP_ENABLED)
 		#error Serial Port Profile for Bluetooth is not available or not enabled.
 	#endif
+
+	#define LED_PIN		2
+	#define SERVO_PIN	22
+	#define MOTOR_A_PIN	19
+	#define MOTOR_B_PIN	21
 #else
+	#define LED_PIN		13
+	#define SERVO_PIN	10
+	#define MOTOR_A_PIN	11
+	#define MOTOR_B_PIN	12
 	#define RX_PIN		2
 	#define TX_PIN		3
 	#define BT_BAUDRATE	9600
@@ -47,11 +58,14 @@
 #define UART_TIMEOUT	10		// ms
 
 static void serial_relay(void);
+static void joystick_execute(void);
 
 #ifdef _ESP32_
 	static const char device_name[] = "ESP32-BT-Slave";
 #endif
 
+static Servo servo_1;
+
 void setup()
 {
 	#ifdef _ESP32_
@@ -60,6 +74,15 @@ void setup()
 		Dabble.begin(BT_BAUDRATE, RX_PIN, TX_PIN);
 	#endif
 
+	pinMode(LED_PIN, OUTPUT);
+	pinMode(MOTOR_A_PIN, OUTPUT);
+	pinMode(MOTOR_B_PIN, OUTPUT);
+	digitalWrite(LED_PIN, LOW);
+	digitalWrite(MOTOR_A_PIN, LOW);
+	digitalWrite(MOTOR_B_PIN, LOW);
+
+	servo_1.attach(SERVO_PIN);
+
 	Serial.begin(UART_BAUDRATE);
 	Serial.setTimeout(UART_TIMEOUT);
 	Serial.println("Setup complete.");
@@ -71,6 +94,8 @@ void loop()
 	// Hence calling this function is mandatory in order to get data properly from your mobile.
 	Dabble.processInput();
 	serial_relay();
+	joystick_execute();
+	delay(50);
 }
 
 /*
@@ -109,3 +134,41 @@ static void serial_relay(void)
 		Serial.println();
 	}
 }
+
+static void joystick_execute(void)
+{
+	static uint8_t servo_pos = 0;
+	static bool led_on = false;
+	bool motor_forward = true;
+	uint8_t motor_speed = 0;
+	float angle_y = 0.0;
+
+	if (GamePad.isStartPressed())
+	{
+		led_on = !led_on;
+		digitalWrite(LED_PIN, led_on);
+	}
+	if (GamePad.isSquarePressed() && servo_pos > 0)
+	{
+		servo_pos--;
+		servo_1.write(servo_pos);
+	}
+	if (GamePad.isCirclePressed() && servo_pos < 180)
+	{
+		servo_pos++;
+		servo_1.write(servo_pos);
+	}
+	angle_y = GamePad.getYaxisData();
+	motor_forward = angle_y > 0.0 ? true : false;
+	motor_speed = (uint8_t) (abs(angle_y) * 36.0);
+	if (motor_forward)
+	{
+		digitalWrite(MOTOR_B_PIN, LOW);
+		analogWrite(MOTOR_A_PIN, motor_speed);
+	}
+	else
+	{
+		digitalWrite(MOTOR_A_PIN, LOW);
+		analogWrite(MOTOR_B_PIN, motor_speed);
+	}
+}
\ No newline at end of file
-- 
2.39.2