Added a joystick demonstration to the Bluetooth example

diff --git a/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino b/Workshops/TUES_bluetooth_demo/TUES_bluetooth_demo.ino
index bb73d6acd4bc5b4f6aa481d5d4e678f6841b6f21..37a73a688d82d0bdbf2b930d2ca39c7a0f26cc19 100644 (file)
--- 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