/*
(C) Paul Brace August 2023
Joystick controller for drone:
Designed to fly the PicoW Copter drone by anish-natekar
Drone project can be found here: https://github.com/anish-natekar/PicoW_Copter
Output matches the same ranges and package order as the UDP Joystick app plus additional values for the joystick switches
Project uses an ADS1115 to provide 4 additional ADC channels as the Pico W only has 3
*/
#include <SPI.h>
#include <WiFi.h>
#include <WiFiUdp.h>
// ADS1115 library by Rob Tillaart
#include "ADS1X15.h"
// libraries for OLED display
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C // See datasheet for Address;
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// Create ADS object
ADS1115 ADS(0x48);
// Battery indicator bitmaps
static const unsigned char PROGMEM full[] {
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111111,0b11111000,
0b11111111,0b11111111,0b11111000,
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111111,0b11100000,
};
static const unsigned char PROGMEM three4[] {
0b11111111,0b11111111,0b11100000,
0b11111111,0b11111100,0b00100000,
0b11111111,0b11111100,0b00100000,
0b11111111,0b11111100,0b00111000,
0b11111111,0b11111100,0b00111000,
0b11111111,0b11111100,0b00100000,
0b11111111,0b11111100,0b00100000,
0b11111111,0b11111111,0b11100000,
};
static const unsigned char PROGMEM half[] {
0b11111111,0b11111111,0b11100000,
0b11111111,0b11000000,0b00100000,
0b11111111,0b11000000,0b00100000,
0b11111111,0b11000000,0b00111000,
0b11111111,0b11000000,0b00111000,
0b11111111,0b11000000,0b00100000,
0b11111111,0b11000000,0b00100000,
0b11111111,0b11111111,0b11100000,
};
static const unsigned char PROGMEM one4[] {
0b11111111,0b11111111,0b11100000,
0b11111100,0b00000000,0b00100000,
0b11111100,0b00000000,0b00100000,
0b11111100,0b00000000,0b00111000,
0b11111100,0b00000000,0b00111000,
0b11111100,0b00000000,0b00100000,
0b11111100,0b00000000,0b00100000,
0b11111111,0b11111111,0b11100000,
};
static const unsigned char PROGMEM empty[] {
0b11111111,0b11111111,0b11100000,
0b10000000,0b00000000,0b00100000,
0b10000000,0b00000000,0b00100000,
0b10000000,0b00000000,0b00111000,
0b10000000,0b00000000,0b00111000,
0b10000000,0b00000000,0b00100000,
0b10000000,0b00000000,0b00100000,
0b11111111,0b11111111,0b11100000,
};
// Linking 2 Pico Ws directly only works over a short range of approx 2m
// If using indoors then link via both the controller and the drone via the router and put both in station mode
#ifndef APSSID_DRONE
#define APSSID_DRONE "PicoW" // Replace with the name of your Drone PicoW Hotspot or your router
#define APPSW_DRONE "password" // Replace with the password of your Drone PicoW Hotspot or your router
#endif
#define UDP_PKT_MAX_SIZE 16 // number of characters in the UDP send packet
char packetBuffer[UDP_PKT_MAX_SIZE]; // max number of characters sent in one message
unsigned int destinationPort = 8888; // drone port for UDP communication will be 8888
// Drone IP address to send packets to Pico Hotspot IP default is 192.168.42.1)
// If connecting via router then set a static IP in the drone software e.g. 192.168.0.100 using the following:
// IPAddress local_IP(192, 168, 0, 100);
// WiFi.mode(WIFI_STA); // Station Mode
// WiFi.config(local_IP); // Configure static IP
// and change the following line to use the address
IPAddress dest_IP(192, 168, 42, 1);
WiFiUDP Udp; // Object for WIFI UDP class
#define LEFT_BUTTON 17
#define RIGHT_BUTTON 18
#define BATTERY_PIN 26
// Set the range produced by the joysticks
#define ADC_MIN 0
#define ADC_MAX 26400
// variables to store the value read from the joysticks
int jThrottle;
int jYaw;
int jPitch;
int jRoll;
int throttle = 1000; // Current level of the throttle start at minimum
#define MAX_THROTTLE 1800
float controllerVoltage; // Used for the calculated voltage to display battery level
void setup() {
pinMode(LEFT_BUTTON, INPUT_PULLUP);
pinMode(RIGHT_BUTTON, INPUT_PULLUP);
// initialise ADS1115
ADS.begin();
ADS.setGain(1); // Sets max voltage to be measured 1 = 4.096 0 = 6.144 (default)
ADS.setMode(1); // single shot mode 0 = continuous
// Initialise display
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
display.setTextColor(WHITE);
display.setTextSize(1);
// Initialise wifi comms
WiFi.mode(WIFI_STA); // Station Mode
WiFi.begin(APSSID_DRONE, APPSW_DRONE); // start connection
connectToWiFi();
}
bool linkComplete = false;
void loop(){
// loop takes approx 67ms to complete so package sent every 67ms
if (!linkComplete) {
display.clearDisplay();
display.setCursor(0, 10);
display.print("Press left to connect.");
display.setCursor(0, 25);
display.print("Then motor control:");
display.setCursor(0, 40);
display.print(" Press left to start");
display.setCursor(0, 55);
display.print(" Press right to stop");
display.display();
}
// Check if need to send start up or shut down sequence
// if the button has been pressed pin will go LOW
if (digitalRead(LEFT_BUTTON) == LOW){
// Send sequence to start the drone motors
throttle = 1000;
sendPacket(1000, 1000, 1500, 1500);
delay(250);
sendPacket(1000, 1500, 1500, 1500);
delay(250);
linkComplete = true;
}
if (digitalRead(RIGHT_BUTTON) == LOW){
// Send end sequence to stop the drone motors
throttle = 1000;
sendPacket(1000, 2000, 1500, 1500);
delay(250);
sendPacket(1000, 1500, 1500, 1500);
delay(250);
}
// Read the joystick positions
jThrottle = ADS.readADC(0);
jYaw = ADS.readADC(1);
jPitch = ADS.readADC(2);
jRoll = ADS.readADC(3);
// map the values to the range 1000 to 2000
jThrottle = map(jThrottle, ADC_MIN, ADC_MAX, 1000, 2000);
jYaw = map(jYaw, ADC_MIN, ADC_MAX, 1000, 2000);
// Pitch is reversed so down = 2000 and up = 1000
int revJPitch = abs(jPitch - ADC_MAX);
jPitch = map(revJPitch, ADC_MIN, ADC_MAX, 1000, 2000);
jRoll = map(jRoll, ADC_MIN, ADC_MAX, 1000, 2000);
// Increase or decrease throttle based on joystick postion
if (jThrottle >= 1550 && throttle < MAX_THROTTLE)
throttle += 15;
if (jThrottle <= 1450 && throttle > 1000)
throttle -= 15;
// To compensate for fluctuations in readings when joystick centered
jYaw = smooth(jYaw);
jPitch = smooth(jPitch);
jRoll = smooth(jRoll);
sendPacket(throttle, jYaw, jPitch, jRoll);
// Calculate controller voltage
int controllerBattery = analogRead(BATTERY_PIN);
// controllerBattery/1023 * 3.3 = voltage measured at pin
// 1330/330 is voltage drop as larger resistor is on the + side
controllerVoltage = 3.3 * controllerBattery/1023 * 1330/330;
// Update OLED display if link completed
if (linkComplete) {
display.clearDisplay();
if (WiFi.status() == WL_CONNECTED) {
displayDirection(throttle, jYaw, jPitch, jRoll);
displayBattery(controllerVoltage);
display.display();
}
else {
display.setCursor(10, 10);
display.print("WiFi connection");
display.setCursor(10, 25);
display.print("has been lost");
display.setCursor(10, 40);
display.print("restart both drone");
display.setCursor(10, 55);
display.print("and controller.");
display.display();
delay(2000);
// try to reconnect
connectToWiFi();
}
}
}
// Send a packet of data
void sendPacket(int throttle, int yaw, int pitch, int roll){
sprintf(packetBuffer, "%04d%04d%04d%04d", yaw, throttle, roll, pitch);
Udp.beginPacket(dest_IP, destinationPort);
Udp.write(packetBuffer);
Udp.endPacket();
}
// To compensate for inaccuracy and fluctuations in readings around mid value
int smooth(int value){
if (value < 1525 && value > 1475)
value = 1500;
return value;
}
// OLED display routines
void displayDirection(int throttle, int yaw, int pitch, int roll){
display.setCursor(0, 0);
display.print("Throttle:");
// height of rect at full throttle = 50 pixels so
// 1 pixel = 20 steps
int top = 58 - (throttle - 1000) / 20;
if (top > 57)
top = 57;
int height = 58 - top;
display.fillRect(15, top, 20, height, WHITE);
// draw Yaw
display.drawRect(0, 59, 50, 4, WHITE);
int yawX = (yaw - 1000) / 21;
display.fillRect(yawX, 60, 3, 3, WHITE);
// Draw pitch and roll
display.drawRect(65, 25, 63, 38, WHITE);
display.drawLine(65, 44, 127, 44, WHITE);
display.drawLine(96, 25, 96, 63, WHITE);
// Draw dot based on pitch and roll
int x = (roll - 1000) / 16.5 + 65;
int y = (pitch - 1000) / 28 + 25;
display.fillRect(x, y, 3, 3, WHITE);
}
// displays battery level
void displayBattery(float cVoltage){
display.setCursor(92, 0);
display.print("Battery:");
drawBatteryLevel(cVoltage, 106, 12);
}
// draws the icons for the battery level
void drawBatteryLevel(float voltage, int x, int y){
if (voltage > 3.85)
display.drawBitmap(x, y, full, 24, 8, WHITE);
else
if (voltage > 3.69)
display.drawBitmap(x, y, three4, 24, 8, WHITE);
else
if (voltage > 3.59)
display.drawBitmap(x, y, half, 24, 8, WHITE);
else
if (voltage > 3.5)
display.drawBitmap(x, y, one4, 24, 8, WHITE);
else
display.drawBitmap(x, y, empty, 24, 8, WHITE);
}
int dotPosition;
void displayDots() {
display.clearDisplay();
display.setCursor(10, 10);
display.write("Linking with");
display.setCursor(30, 25);
display.write("Drone:");
display.setCursor(dotPosition, 40);
display.write(".");
display.display();
dotPosition += 10;
if (dotPosition > 100)
dotPosition = 10;
}
void connectToWiFi(){
dotPosition = 10;
while(WiFi.status() != WL_CONNECTED) {
if (dotPosition == 110){
// try again as drone may not have been switched on
WiFi.disconnect();
WiFi.mode(WIFI_STA); // Station Mode
WiFi.begin(APSSID_DRONE, APPSW_DRONE); // start connection
}
displayDots();
delay(500); // 0.5 sec delay
}
}
Comments