paulsb
Published © GPL3+

Wi-Fi Remote Controlled Car and Joystick Controller

Raspberry Pi PicoW RC Car/Control. V2 has 3 modes: controlled, avoidance & line follow. Also controllable using the UDP Joystick Phone App.

IntermediateFull instructions provided1,005
Wi-Fi Remote Controlled Car and Joystick Controller

Things used in this project

Hardware components

Componenets for Car:
×1
Raspberry Pi Pico W
Raspberry Pi Pico W
With headers and female socket connectors if you wish it to be removable.
×1
EMO Smart Robot Car Chassis Kit with Motors
×1
L298N Motor Drive Controller Board DC Dual H-Bridge Robot Stepper Motor Control and Drives Module
×1
Servo SG90 Servomotor Metal Geared Motor
×1
HC-SR04 Ultrasonic Rangefinder Sensor Module
×1
Line Tracking Sensor Module
×2
Linear Regulator (7805)
Linear Regulator (7805)
×1
Through Hole Resistor, 2.2 kohm
Through Hole Resistor, 2.2 kohm
×1
Ceramic Disc Capacitor, 0.1 µF
Ceramic Disc Capacitor, 0.1 µF
×1
Multilayer Ceramic Capacitor, 0.33 µF
Multilayer Ceramic Capacitor, 0.33 µF
×1
5 mm LED: Red
5 mm LED: Red
×1
9V battery (generic)
9V battery (generic)
×2
Strip Board Printed Circuit PCB Vero Prototyping Track Stripboard
×1
Boat Rocker Switch SPST ON/Off Soldering Terminal Rocker Switch 10x15mm
Or use a double pole switch.
×2
Resistor 1M ohm
Resistor 1M ohm
×1
Through Hole Resistor, 330 kohm
Through Hole Resistor, 330 kohm
×1
Components for controller
×1
Raspberry Pi Pico W
Raspberry Pi Pico W
×1
aAZDelivery Joystick
×1
AZDelivery I2C 0.96-inch OLED Display
×1
ADC PGA Converter Development Board
×1
TP4056 5V 1A TYPE C Micro USB Board Module for 18650 Lithium Battery Charging
×1
Lithium Polymer battery 3.7V 360mAh
×1
On/Off slide switch 13mm (SS-12D10)
×1
OCB Proto Board 9cm x 7cm
×1
Resistor 1M ohm
Resistor 1M ohm
×1
Through Hole Resistor, 330 kohm
Through Hole Resistor, 330 kohm
×1

Software apps and online services

Arduino IDE
Arduino IDE

Story

Read more

Custom parts and enclosures

Controller base STL file

Controller Cover STL

Distance Sensor Mount STL

Servo Holder STL

Battery Holder STL

Line Sensor Bracket STL

Schematics

Fritzing file for the joystick controller

Fritzing file for the Remote Controlled Car

Schematic for the joystick controller

Schematic for the Car

Code

Remote Controlled Car Software

C/C++
/*  V2 Remote Controlled Car based on Raspberry Pi Pico W
    (c) Paul Brace August 2023
*/

#include <WiFi.h>       // WiFi library
#include <WiFiUdp.h>    // library to receive and send Udp packages

// I am using a router to connect as direct connection between 2 PicoWs only
// works if less than 2m apart
// if want to use a direct connection then set car to WIFI_AP mode and do 
// not specify the IP address it will be 192.168.42.1 which will need to be set in the controller
// you will also need to change the static address of JS controller to, say, 192.168.42.115 both
// here and in the controller software.


#ifndef APSSID
#define APSSID "XXXXXXXXX"     // Network name (change to your network ID)
#define APPSW "XXXXXXXXXX"     // Network password (change to your network password)
#endif

// Static local IP
IPAddress local_IP(192, 168, 0, 110);

// IP address to send packets to
IPAddress dest_IP(192, 168, 0, 115);

// Although only 2 channels used still expect a package of 4 to retain compatibility
// with the UDP Joystick app
#define REC_UDP_PKT_MAX_SIZE 16   // package size for packages received
#define SEND_UDP_PKT_MAX_SIZE 4   // package size for packages sent

// if you wish to use the UDP Joystick App rather than make the controller
// uncomment "define UDP_JS" and comment define PSB_JS 
// only control mode will be available in UDP_JS mode
#define PSB_JS true
//#define UDP_JS true

unsigned int localPort = 8888;        // Port to listen for packages
unsigned int destPort = 8888;         // Port to send packages to

char recPacketBuffer[REC_UDP_PKT_MAX_SIZE + 1];     // buffer for receiving packages
char sendPacketBuffer[SEND_UDP_PKT_MAX_SIZE + 1];   // buffer for sending packages

WiFiUDP Udp;      // Object for WiFi UDP class

// Motor Pins
#define LEFT_PWM_SIGNAL 16      // White
#define LEFT_FORWARD 18         // Blue
#define LEFT_BACKWARDS 19       // Yellow
#define RIGHT_PWM_SIGNAL 17     // Grey
#define RIGHT_FORWARD 20        // Purple
#define RIGHT_BACKWARDS 21      // Green

// Line followe Pins
#define SENSOR_PIN_LEFT 3
#define SENSOR_PIN_RIGHT 2

// Servo control pin
#define SERVO 15

// PWC values for controlling the servo
// You may need to tweak these
#define FORWARD 148 
#define LEFT 250    
#define RIGHT 60    
#define TURN_LEFT 200
#define TURN_RIGHT 100


int throttle;     // will be 1000 to 2000 - 1500 stationary > 1500 forwards < 1500 backwards
int direction;    // will be 1000 to 2000 - 1500 straight > 1500 right < 1500 left

int leftSpeed;    // the calculated speed of each wheel
int rightSpeed;

// Pins for distance sensor
#define TRIGGER_PIN 0
#define ECHO_PIN 1

// Constants used for calculating distance
#define SOUND_SPEED 340
#define TRIG_PULSE_DURATION 10

// Variable for distance calculation
long echo_duration;

#define BUZZER 9 

#define BATTERY_PIN 26

float voltage;

enum CarMode {
  none,
  controlled,
  avoidance,
  follow   
};

CarMode carMode = none;
CarMode selectedMode;

void setup() {

  analogWriteFreq(50);    // PWM frequency 50 Hz for servo motor
  analogWriteRange(1000); // value corresponding to 100% PWM duty cycle

  // set pins for output
  pinMode(LEFT_PWM_SIGNAL, OUTPUT);
  pinMode(LEFT_FORWARD, OUTPUT);
  pinMode(LEFT_BACKWARDS, OUTPUT);
  pinMode(RIGHT_PWM_SIGNAL, OUTPUT);
  pinMode(RIGHT_FORWARD, OUTPUT);
  pinMode(RIGHT_BACKWARDS, OUTPUT);
  pinMode(SERVO, OUTPUT);
  pinMode(BUZZER, OUTPUT);

  // Set distance sensor pins
  pinMode(TRIGGER_PIN, OUTPUT); 
  pinMode(ECHO_PIN, INPUT); 

  // Set sensor pins for input
  pinMode(SENSOR_PIN_LEFT, INPUT);   
  pinMode(SENSOR_PIN_RIGHT, INPUT);  


  // Turn off all motors
  digitalWrite(LEFT_FORWARD, LOW);
  digitalWrite(LEFT_BACKWARDS, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  digitalWrite(RIGHT_BACKWARDS, LOW);

  digitalWrite(LED_BUILTIN, LOW);   // turn off while waiting

  // WiFi.mode(WIFI_AP);         // set to this if you want to use direct connection
  WiFi.mode(WIFI_STA);           // Station Mode
  WiFi.config(local_IP);         // Comment out if using direct connection
  WiFi.begin(APSSID, APPSW);     // start connection
  while(WiFi.status() != WL_CONNECTED) {  // wait for a connection
    delay(250);    
  }

  Udp.begin(localPort);          // for sending packets

  digitalWrite(LED_BUILTIN, HIGH);   // turn on and beep to indicate connection made
  beep(1);
  setPosition(FORWARD);

  #ifdef PSB_JS
    unsigned long lastSend = 0;
    // Handshake with controller
    // Wait for received message
    while (throttle != 9000) {
      getPacket();
      if (throttle != 9000 && (micros() - lastSend) > 250000) {
      // Send an I am here message
        sendPacket(9000);
        lastSend = micros();
      }
    }
    bool finished = false;
    while (!finished) {
      throttle = 1500;
      getPacket();
      // if left JS pressed then change mode
      if (throttle == 3000)
        switch (carMode) {
          case none:
            carMode = controlled;
            beep(2);
            break;
          case controlled:
            carMode = avoidance;
            beep(3);
            break;
          case avoidance:
            carMode = follow;  
            beep(4);
            break;
          case follow:
            carMode = none;
            beep(1);
            break;
        }
      if (throttle == 4000 && carMode != none) {
        finished = true;
      }
    }
  #endif
  #ifdef UDP_JS
    carMode = controlled;
  #endif
  selectedMode = carMode;

  throttle = 1500;  // set car to stationary
  direction = 1500; // set direction to straight ahead
}

void loop() {
  if (carMode == none){
    // check to see if we have a start instruction for automated mode
    getPacket();
    // check if received a go message
    if (throttle == 4000) {
      switch (selectedMode){
        case avoidance:
          beep(3);
          break;
        case follow:
          beep(4);
          break;
      }
      carMode = selectedMode;
      delay(500);
      throttle = 1500;
    }
  }
  switch (carMode){
    case controlled:
      controlledMode();
      break;
    case avoidance:
      avoidanceMode();
      break;
    case follow: 
      lineFollowMode();
      break;
}
}

void controlledMode() {
  getPacket();
  // set speed based on throttle
  if (throttle != 1500) {
    leftSpeed = throttle - 1500;
    rightSpeed = leftSpeed;
  }
  else {
    leftSpeed = 0;
    rightSpeed = 0;
  }

  if (direction < 1500) {
    adjustLeft();
    // Look left
    setPosition(TURN_LEFT);
  }
  else
    if (direction > 1500) {
      adjustRight();
      // Look right
      setPosition(TURN_RIGHT);
    }
    else {
      // Look forward
      setPosition(FORWARD);
    }
  
  // Set direction and speed of motors  
  setMotors();
  // read distance and send to controller
  getDistance(true);  
}

float distance_cm, left_cm, right_cm;
bool canTurn;

#define MIN_DISTANCE 20.0
#define MIN_TURN_DISTANCE 30.0
#define RIGHT_SPEED 79
#define LEFT_SPEED 80         // adjust if car wanders left or right
#define TURN_DELAY 750

void avoidanceMode() {
  getPacket();
  // check if received a stop message
  if (throttle == 4000) {
    carMode = none;
    stopCar();
    beep(1);
    throttle = 1500;
    return;
  }

  distance_cm = getDistance(true);
  // Check if we are within the minimum distance
  if (distance_cm <= MIN_DISTANCE  && distance_cm > 0.0) {
    stopCar();
    delay(250);
    // reverse a bit
    leftSpeed = -LEFT_SPEED;
    rightSpeed = -RIGHT_SPEED;
    setMotors();
    delay(1000);
    stopCar();
    canTurn = false;
    while (!canTurn) {
      // Check which way to turn
      setPosition(LEFT);
      delay(1000);
      left_cm = getDistance(false);
      if (left_cm > MIN_TURN_DISTANCE)
        canTurn = true;
      setPosition(RIGHT);
      delay(1000);
      right_cm = getDistance(false);
      setPosition(FORWARD);
      if (right_cm > MIN_TURN_DISTANCE)
        canTurn = true;
      if (canTurn) {
        if (left_cm > right_cm){
          // Turn Left
          leftSpeed = 0;
          rightSpeed = RIGHT_SPEED;
          setMotors();
          distance_cm = left_cm;
        }
        else {
          // Turn right
          leftSpeed = LEFT_SPEED;
          rightSpeed = 0;
          setMotors();
          distance_cm = right_cm;
        }
        delay(TURN_DELAY);
      }
      else {
        // Cannot turn so reverse and repeat
        leftSpeed = -LEFT_SPEED;
        rightSpeed = -RIGHT_SPEED;
        setMotors();
        delay(TURN_DELAY);
      }
    }
  }
  else {
    // No obstacle so proceed forward
    setPosition(FORWARD);
    leftSpeed = LEFT_SPEED;
    rightSpeed = RIGHT_SPEED;
    setMotors();
    delay(50);
  }
}

int followSpeed = 25;
int leftSense, rightSense;

void lineFollowMode(){
  getPacket();
  // check if received a stop message
  if (throttle == 4000) {
    carMode = none;
    stopCar();
    beep(1);
    throttle = 1500;
    return;
  }
  // Read sensors
  leftSense = digitalRead(SENSOR_PIN_LEFT);
  rightSense = digitalRead(SENSOR_PIN_RIGHT);
  // Low = white, high = black
  if (leftSense == LOW && rightSense == LOW) {
    // Go stright on
    leftSpeed = followSpeed;
    rightSpeed = followSpeed;
  } 
  else 
  {
    if (leftSense == HIGH && rightSense == LOW) {
      // Go right
      leftSpeed = -followSpeed;
      rightSpeed = followSpeed;
    } 
    else 
    {
      if (leftSense == LOW && rightSense == HIGH) {
        // Go left
        leftSpeed = followSpeed;
        rightSpeed = -followSpeed;
      } 
      else {
        // Both high so stop
        leftSpeed = 0;
        rightSpeed = 0;
      }
    }
  }
  setMotors();
}

void getPacket() {
  int packetSize = Udp.parsePacket();                               // See if there is data waiting
  if (packetSize) {                                                 // there is
    int n = Udp.read(recPacketBuffer, REC_UDP_PKT_MAX_SIZE);        // read the packet
    if (n == REC_UDP_PKT_MAX_SIZE) {                                // only unpack if the correct size package received
      recPacketBuffer[n] = '\0';                                    // add terminator (only really need to do this if we are going to print it)
      char ch1[5], ch2[5];                                          // unpack the x axis of left joystick
      ch1[4] = '\0'; ch2[4] = '\0';                                 // and y axis of right
      for(int i=0; i<4; i++) {
        ch1[i] = recPacketBuffer[i+4];
        ch2[i] = recPacketBuffer[i+8];
      }
      throttle = atoi(ch1);
      direction = atoi(ch2);
    }
  }
}

void stopCar() {
  // Turn off all motors
  digitalWrite(LEFT_FORWARD, LOW);
  digitalWrite(LEFT_BACKWARDS, LOW);
  digitalWrite(RIGHT_FORWARD, LOW);
  digitalWrite(RIGHT_BACKWARDS, LOW);
}

void adjustLeft() {
  // increase right and reduce left speed by amount of direction
  int adj = abs(direction - 1500);
  if (throttle >= 1500) {
    leftSpeed -= adj;
    rightSpeed += adj;
  }
  else {
    leftSpeed += adj;
    rightSpeed -= adj;
  }
}

void adjustRight() {
  // increase left speed and reduce right by amount of direction
  int adj = abs(direction - 1500);
  if (throttle >= 1500) {
    leftSpeed += adj;
    rightSpeed -= adj;
  }
  else {
    leftSpeed -= adj;
    rightSpeed += adj;
  }

}

void setMotors() {
  // Based on speed being > or < 0 set forwards or backwards
  if (leftSpeed == 0) {
    digitalWrite(LEFT_FORWARD, LOW);
    digitalWrite(LEFT_BACKWARDS, LOW);
  }
  else {
    if (leftSpeed > 0) {
      digitalWrite(LEFT_FORWARD, HIGH);
      digitalWrite(LEFT_BACKWARDS, LOW);
    }
    else {
      digitalWrite(LEFT_FORWARD, LOW);
      digitalWrite(LEFT_BACKWARDS, HIGH);
    }
  }
  if (rightSpeed == 0) {
    digitalWrite(RIGHT_FORWARD, LOW);
    digitalWrite(RIGHT_BACKWARDS, LOW);
  }
  else {
    if (rightSpeed > 0) {
      digitalWrite(RIGHT_FORWARD, HIGH);
      digitalWrite(RIGHT_BACKWARDS, LOW);
    }
    else {
      digitalWrite(RIGHT_FORWARD, LOW);
      digitalWrite(RIGHT_BACKWARDS, HIGH);
    }
  }
  int battery = analogRead(BATTERY_PIN);
  // controllerBattery/1023 * 3.3 = voltage measured at pin
  // 2 resistors 1000k and 310K
  voltage = 3.3 * battery/1023 * 1310/310;
  // Map speeds to X to 1000 range
  // where X is minimum to drive motors based on voltage
  // X is varied based on voltage to achive a constant speed
  //    increases by 100 for each 1v drop
  int minValue = int((8.4 - voltage) * 100 + 300);
  leftSpeed = map(abs(leftSpeed), 0, 500, minValue, 1000);
  rightSpeed = map(abs(rightSpeed), 0, 500, minValue, 1000);

  // Set speed
  analogWrite(LEFT_PWM_SIGNAL, leftSpeed);
  analogWrite(RIGHT_PWM_SIGNAL, rightSpeed);
}

int lastPosition;
// Set the postion of to servo to look in the direction indicated by pos 
void setPosition(int pos){
  if (lastPosition != pos) {
    lastPosition = pos;
    analogWrite(SERVO, pos);
  }
}

// Used to smooth the readings as they can be a bit erratic
float distArray[] = {-1.0, -1.0, -1.0, -1.0, -1.0};
float lastDistance = 0;

// If smooth = true then readings are averaged on the last 
// 5 readings
float getDistance(bool smooth){
  if (!smooth) {
    // not smoothing so clear array ready for next smooth
    for (int i = 0; i < 5; i++)
      distArray[i] = -1.0;
  }
  // Prepare to signal to send
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  // Send pulse
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(TRIG_PULSE_DURATION);
  digitalWrite(TRIGGER_PIN, LOW);

  // Wait for the echo and get duration - time out if not received in 1/10 second
  echo_duration = pulseIn(ECHO_PIN, HIGH, 100000);
  float result;
  if (echo_duration != 0) {
    result = echo_duration * SOUND_SPEED/2 * 0.0001;
    lastDistance = result;
  }
  else
    // set to a default distance
    result = lastDistance;
  if (smooth) {
    float sum = result;
    int count = 1;
    for (int i = 0; i < 4; i++){
      distArray[i] = distArray[i + 1];
      if (distArray[i] >= 0.0) {
        sum += distArray[i];
        count++;
      }
      distArray[4] = result;
    }
    result = sum / count;
  }
  // Send reading to controller
  sendPacket(int(result));
  return result;
}

void beep(int no){
  for (int i = 0; i < no; i++) {
    tone(BUZZER, 440); // Middle C
    delay(100);
    noTone(BUZZER);     // Stop sound...
    delay(100);
  }
}


unsigned long lastSend = 0;
// Send a packet of data to the controller every 1/2 second
void sendPacket(int value){
  if ((micros() - lastSend) > 500000) {
    lastSend = micros();
    sprintf(sendPacketBuffer, "%04d", value);
    //Send the package
    Udp.beginPacket(dest_IP, destPort);
    Udp.write(sendPacketBuffer);
    Udp.endPacket();
  }
}

Joystick Controller Software - RC Car only

C/C++
This version of the software should be used if you are only going to control the car using the Controller. Alternative software is provided if you with to control both a drone and car with the same controller.
/*
  (C) Paul Brace August 2023
  V2 Joystick controller for Remote Controlled Car

  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 chanels as the Pico W only has 3
*/

// Serial comms library
#include <SPI.h>
// WiFi libararies
#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,
};

#ifndef APSSID_CAR
#define APSSID_CAR "XXXXXXXXXX"     // Network name (change to your network ID)
#define APPSW_CAR "XXXXXXXXXX"      // Network password (change to your network password)
#endif

#define UP_UDP_PKT_MAX_SIZE 16 //  number of characters in a UDP send packet
#define DOWN_UDP_PKT_MAX_SIZE 4 //  number of characters in a UDP receive packet

unsigned int destinationPort = 8888;  // drone port for UDP communication will be 8888
unsigned int localPort = 8888;  // local port for UDP communication
// send packet
char upPacketBuffer[UP_UDP_PKT_MAX_SIZE];  // max number of characters sent in one message
// receive packet
char downPacketBuffer[DOWN_UDP_PKT_MAX_SIZE + 1];  // max number of characters received in one message

// Car IP address to send packets to
IPAddress dest_IP(192, 168, 0, 110);

// Set a static local ip address
IPAddress local_IP(192, 168, 0, 115);

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 used store the value read from the joysticks
int jThrottle;  // Left y axis
int jDirection; // Right x axis

int throttle = 1000;  // Current level of the throttle start at minimum
int direction;

float controllerVoltage;

enum CarMode {
  none,
  controlled,
  avoidance,
  follow       
};

CarMode carMode = none;


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.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);

  // Initialise wifi comms
  WiFi.mode(WIFI_STA);        // Station Mode
  WiFi.config(local_IP);  
  WiFi.begin(APSSID_CAR, APPSW_CAR);  // start connection

  connectToWiFi();

  bool finished = false;
  // Allow user to select mode of operation
  while (!finished) {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Select drive mode:");
    display.setCursor(0, 15);
    display.print("Press left to change");
    display.setCursor(10, 30);
    display.print("Mode: ");
    display.setCursor(50, 30);
    switch (carMode) {
      case none:
        display.print("NONE");
        break;
      case controlled:
        display.print("CONTROL");
        break;
      case avoidance:
        display.print("AVOIDANCE");
        break;
      case follow:
        display.print("LINE FOLLOW");
        break;
    }
    display.setCursor(0, 45);
    display.print("Press right to go");
    display.display();
    if (digitalRead(LEFT_BUTTON) == LOW) {
      switch (carMode) {
        case none:
          carMode = controlled;
          break;
        case controlled:
          carMode = avoidance;
          break;
        case avoidance:
          carMode = follow;
          break;
        case follow:
          carMode = none;
          break;
      }
      // send message to car to change mode
      sendPacket(3000, 0, 0, 1500);    
      delay(250);    
    }
    if (digitalRead(RIGHT_BUTTON) == LOW && carMode != none) {
      // send message to indicate setup finished
      sendPacket(4000, 0, 0, 1500);    
      delay(250);
      finished = true;    
    }
  }
  // set throttle level to stop and direction to straight
  throttle = 1500;
  direction = 1500;
  sendPacket(throttle, 0, 0, direction ); 

  // final instruction for automated mode
  if (carMode == avoidance || carMode == follow) {  
    display.clearDisplay();
    display.setCursor(0, 10);
    display.print("Automated mode");
    display.setCursor(0, 25);
    display.print("Press right to stop");
    display.setCursor(10, 40);
    display.print("and start car");
    display.display();
    delay(2000);
  }
}

int distance = 0;  // distance to object sent by car

void loop() {
  if ((carMode == avoidance || carMode == follow) && digitalRead(RIGHT_BUTTON) == LOW){
    // Send start/end sequence to stop the car
    // only accepted if the car is in auto mode
    sendPacket(4000, 0, 0, 1500);
    delay(250);
  }

  // Read the joystick positions
  jThrottle = ADS.readADC(0);  // Forward or back
  jDirection = ADS.readADC(3); // left or right

  // map the values to the range 1000 to 1500
  jThrottle = map(jThrottle, ADC_MIN, ADC_MAX, 1000, 2000);
  jDirection = map(jDirection, ADC_MIN, ADC_MAX, 1000, 2000);

  // To compensate for fluctuations in readings when joystick centered
  jThrottle = smooth(jThrottle);
  jDirection = smooth(jDirection);

  sendPacket(jThrottle, 0, 0, jDirection);

  // Receive any packages here
  distance = receivePacket();

  // Calculate controller voltage
  int controllerBattery = analogRead(BATTERY_PIN);
  // controllerBattery/1023 * 3.3 = voltage measured at pin
  // 1330/330 is voltage drop as larger resistor 1M is on the +ve side and 330K in the -ve
  controllerVoltage = 3.3 * controllerBattery/1023 * 1330/330;
  
  // Update OLED display
  display.clearDisplay();
  if (WiFi.status() == WL_CONNECTED) {
    displayDirection(jThrottle, jDirection);
    displayBattery(controllerVoltage);
    display.setCursor(60, 24);
    display.print("Dist:");
    display.setCursor(100, 24);
    display.print(distance);
    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 car");
    display.setCursor(10, 55);
    display.print("and controller.");
    display.display();
    delay(2000);
  }
}

// Send a packet of data
void sendPacket(int throttle, int yaw, int pitch, int roll){
  sprintf(upPacketBuffer, "%04d%04d%04d%04d", yaw, throttle, roll, pitch);
  Udp.beginPacket(dest_IP, destinationPort);
  Udp.write(upPacketBuffer);
  Udp.endPacket();
}

// Receive a packet if there
int receivePacket() {
  int result = distance;      // Set to last read distance in case no package received
  int packetSize = Udp.parsePacket();
  if(packetSize) {
    int n = Udp.read(downPacketBuffer, DOWN_UDP_PKT_MAX_SIZE);
    if (n == DOWN_UDP_PKT_MAX_SIZE) {
      downPacketBuffer[n] = '\0';
      char batCh[5];
      batCh[4] = '\0';
      for(int i=0; i<4; i++) {
        batCh[i] = downPacketBuffer[i];
      }
      result = atoi(batCh);
    }
  }
  return result;
}


// 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 direction){
  int top, bottom;
  display.setCursor(0, 0);
  display.print("Direction:");
  if (throttle > 1500) {
    bottom = 35;
    top = 35 - (throttle - 1500) / 20;
    display.fillRect(13, top, 19, bottom - top, WHITE);
  }
  if (throttle < 1500) {
    top = 35;
    bottom = 35 - (1500 - throttle) / 20;
    display.fillRect(13, top, 19, top - bottom, WHITE);
  }
  display.drawLine(10, 35, 35, 35, WHITE);

  int left, right;
  if (direction > 1500) {
    left = 90;
    right = 90 + (direction - 1500) / 17;
    display.fillRect(left, 45, right - left, 15, WHITE);
  }
  if (direction < 1500) {
    right = 90;
    left = 90 - (1500 - direction) / 17;
    display.fillRect(left, 45, right - left, 15, WHITE);
  }
  display.drawLine(90, 40, 90, 66, WHITE);
}

// displays battery level
void displayBattery(float cVoltage){
  display.setCursor(60, 0);
  display.print("Battery");
  display.setCursor(60, 12);
  display.print("Level:");
  drawBatteryLevel(cVoltage, 100, 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 = 10;

void displayDots() {
  display.clearDisplay();
  display.setCursor(10, 10);
  display.write("Linking with");
  display.setCursor(30, 25);
  display.write("Car:");
  display.setCursor(dotPosition, 40);
  display.write(".");
  display.display();
  dotPosition += 10;
  if (dotPosition > 100)
    dotPosition = 10;
}

void connectToWiFi(){
  while(WiFi.status() != WL_CONNECTED) {
    if (dotPosition == 100){
      // try again as drone may not have been switched on
      WiFi.disconnect();
      WiFi.mode(WIFI_STA);        // Station Mode
      WiFi.begin(APSSID_CAR, APPSW_CAR);  // start connection
    }
    displayDots();
    delay(500); // 0.5 sec delay
  }  
  // start listening on port 8888
  Udp.stopAll();
  Udp.begin(localPort); 
  int message = 0;
  // Wait for handshake message
  while (message != 9000) {
    displayDots();
    delay(500);
    message = receivePacket();
    if (message == 9000) 
      // Send respose
      sendPacket(9000, 0, 0, 0);
  }
}

Joystick Controller Dual - Car and Drone

C/C++
Provides an option on start-up to control either the car in this project or the done from project PicoW Copter drone by anish-natekar which can be found here: https://github.com/anish-natekar/PicoW_Copter).
/*
  (C) Paul Brace August 2023
  V2 Dual Joystick controller for a Drone and Remote Controlled Car
  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 chanels as the Pico W only has 3
*/

// Serial comms library
#include <SPI.h>
// WiFi libraries
#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,
};

#ifndef APSSID_DRONE
#define APSSID_DRONE "PicoW"      // Replace with the name of your Drone PicoW Hotspot
#define APPSW_DRONE "password"    // Replace with the password of your Drone PicoW Hotspot
#define APSSID_CAR "XXXXXXXXX"    // Network name (change to your network ID)
#define APPSW_CAR "XXXXXXXXXX"    // Network password (change to your network password)
#endif

#define UP_UDP_PKT_MAX_SIZE 16 //  number of characters in a UDP send packet
#define DOWN_UDP_PKT_MAX_SIZE 4 //  number of characters in a UDP receive packet

unsigned int destinationPort = 8888;  // drone port for UDP communication will be 8888
unsigned int localPort = 8888;  // local port for UDP communication

// send packet
char upPacketBuffer[UP_UDP_PKT_MAX_SIZE];  // max number of characters sent in one message
// receive packet
char downPacketBuffer[DOWN_UDP_PKT_MAX_SIZE + 1];  // max number of characters received in one message

// Car IP address to send packets to
IPAddress dest_IP_car(192, 168, 0, 110);
// Drone IP address to send packets to Pico Hotspot IP default is 192.168.42.1)
// If you wish to connect 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
// Drone IP address to send packets to
IPAddress dest_IP_drone(192, 168, 42, 1);
// Will be set to one of the above
IPAddress dest_IP;

// Set a static local ip address
IPAddress local_IP_car(192, 168, 0, 115);
// Set a static local ip address
IPAddress local_IP_drone(192, 168, 42, 10);   // Not currently used
// Will be set to one of the above
IPAddress local_IP;

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

int jThrottle;  // variable to store the value read from the joysticks
int jYaw;
int jPitch;
int jRoll;
int jDirection; // Used for Car left or right

// used for Car
int direction;

int throttle = 1000;  // Current level of the throttle start at minimum

#define MAX_THROTTLE 1900 // only used for drone

float controllerVoltage;

enum ContMode {
  drone,
  car
};

ContMode controllerMode;

enum CarMode {
  none,
  controlled,
  avoidance,
  follow
};

CarMode carMode = none;


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.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);

  // User selected mode 
  display.setCursor(0, 10);
  display.print("Select mode:");
  display.setCursor(0, 25);
  display.print("Press left for Drone");
  display.setCursor(0, 40);
  display.print("Press right for Car");
  display.display();

  // wait for a button to be pressed
  while (digitalRead(LEFT_BUTTON) == HIGH && digitalRead(RIGHT_BUTTON) == HIGH);
  if (digitalRead(RIGHT_BUTTON) == LOW) {
    controllerMode = car;
    dest_IP = dest_IP_car;
    local_IP = local_IP_car;
  }
  else {
    controllerMode = drone;
    dest_IP = dest_IP_drone;
    local_IP = local_IP_drone;
  }
  // Confirm mode selected
  display.clearDisplay();
  display.setCursor(10, 10);
  display.print("Mode selected");
  display.setCursor(40, 30);
  switch (controllerMode) {
    case drone:
      display.print("DRONE");
      break;
    case car:
      display.print("RC CAR");
      break;
  }
  display.display();
  delay(1000);    // Wait 1 second

  // Initialise wifi comms
  WiFi.mode(WIFI_STA);        // Station Mode
  WiFi.config(local_IP);  
  if (controllerMode == drone)
    WiFi.begin(APSSID_DRONE, APPSW_DRONE);  // start connection
  else
    WiFi.begin(APSSID_CAR, APPSW_CAR);  // start connection

  connectToWiFi();
  
  if (controllerMode == car){
    bool finished = false;
    // Allow user to select mode of operation
    while (!finished) {
      display.clearDisplay();
      display.setCursor(0, 0);
      display.print("Select drive mode:");
      display.setCursor(0, 15);
      display.print("Press left to change");
      display.setCursor(10, 30);
      display.print("Mode: ");
      display.setCursor(50, 30);
      switch (carMode) {
        case none:
          display.print("NONE");
          break;
        case controlled:
          display.print("CONTROL");
          break;
        case avoidance:
          display.print("AVOIDANCE");
          break;
        case follow:
          display.print("LINE FOLLOW");
          break;
      }
      display.setCursor(0, 45);
      display.print("Press right to go");
      display.display();
      if (digitalRead(LEFT_BUTTON) == LOW) {
        switch (carMode) {
          case none:
            carMode = controlled;
            break;
          case controlled:
            carMode = avoidance;
            break;
          case avoidance:
            carMode = follow;
            break;
          case follow:
            carMode = none;
            break;
        }
        // send message to car to change mode
        sendPacket(3000, 0, 0, 1500);    
        delay(250);    
      }
      if (digitalRead(RIGHT_BUTTON) == LOW && carMode != none) {
        // send message to indicate setup finished
        sendPacket(4000, 0, 0, 1500);    
        delay(250);
        finished = true;    
      }
    }
    // set to stop throttle level
    throttle = 1500;
    direction = 1500;
    sendPacket(throttle, 0, 0, direction ); 
    // final instruction for avoidance mode
  if (carMode == avoidance || carMode == follow) {  
      display.clearDisplay();
      display.setCursor(0, 10);
      display.print("Automated mode");
      display.setCursor(0, 25);
      display.print("Press right to stop");
      display.setCursor(10, 40);
      display.print("and start car");
      display.display();
      delay(2000);
    }
  }
  else {
    // Drone mode
    throttle = 0;
  }
}

void loop() {
  switch (controllerMode) {
    case drone:
      droneLoop();
      break;
    case car:
      carLoop();
      break;
  }
}

bool linkComplete = false;

void droneLoop(){
  // 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);  // read the Joystick positions
  jYaw = ADS.readADC(1);  
  jPitch = ADS.readADC(2);
  jRoll = ADS.readADC(3);

  // map the values to the range 1000 to 1500
  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();
    }
  }
}

int distance = 0;  // distance to object sent by car

void carLoop() {
  if ((carMode == avoidance || carMode == follow) && digitalRead(RIGHT_BUTTON) == LOW){ 
    // Send start/end sequence to stop the car
    // only accepted if the car is in avoidance mode
    sendPacket(4000, 0, 0, 1500);
    delay(250);
  }

  // Read the joystick positions
  jThrottle = ADS.readADC(0);  // Forward or back
  jDirection = ADS.readADC(3); // left or right

  // map the values to the range 1000 to 1500
  jThrottle = map(jThrottle, ADC_MIN, ADC_MAX, 1000, 2000);
  jDirection = map(jDirection, ADC_MIN, ADC_MAX, 1000, 2000);

  // To compensate for fluctuations in readings when joystick centered
  jThrottle = smooth(jThrottle);
  jDirection = smooth(jDirection);

  sendPacket(jThrottle, 0, 0, jDirection);

  distance = receivePacket();

  // Calculate controller voltage
  int controllerBattery = analogRead(BATTERY_PIN);
  // controllerBattery/1023 * 3.3 = voltage measured at pin
  // controllerBattery/1023 * 3.3 = voltage measured at pin
  controllerVoltage = 3.3 * controllerBattery/1023 * 1330/330;

  // Update OLED display
  display.clearDisplay();
  if (WiFi.status() == WL_CONNECTED) {
    displayDirection(jThrottle, jDirection);
    displayBattery(controllerVoltage);
    // display distance reading received
    display.setCursor(60, 24);
    display.print("Dist:");
    display.setCursor(100, 24);
    display.print(distance);
    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 car");
    display.setCursor(10, 55);
    display.print("and controller.");
    display.display();
    delay(2000);
  }
}

// Send a packet of data
void sendPacket(int throttle, int yaw, int pitch, int roll){
  sprintf(upPacketBuffer, "%04d%04d%04d%04d", yaw, throttle, roll, pitch);
  Udp.beginPacket(dest_IP, destinationPort);
  Udp.write(upPacketBuffer);
  Udp.endPacket();
}


// Receive a packet of there
int receivePacket() {
  int result = distance;
  int packetSize = Udp.parsePacket();
  if(packetSize) {
    int n = Udp.read(downPacketBuffer, DOWN_UDP_PKT_MAX_SIZE);
    if (n == DOWN_UDP_PKT_MAX_SIZE) {
      downPacketBuffer[n] = '\0';
      char batCh[5];
      batCh[4] = '\0';
      for(int i=0; i<4; i++) {
        batCh[i] = downPacketBuffer[i];
      }
      result = atoi(batCh);
    }
  }
  return result;
}

// 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
// For Drone
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 = abs(pitch - 2000) / 27 + 25;
  int y = (pitch - 1000) / 28 + 25;
  display.fillRect(x, y, 3, 3, WHITE);
}

// For Car
void displayDirection(int throttle, int direction){
  int top, bottom;
  display.setCursor(0, 0);
  display.print("Direction:");
  if (throttle > 1500) {
    bottom = 35;
    top = 35 - (throttle - 1500) / 20;
    display.fillRect(13, top, 19, bottom - top, WHITE);
  }
  if (throttle < 1500) {
    top = 35;
    bottom = 35 - (1500 - throttle) / 20;
    display.fillRect(13, top, 19, top - bottom, WHITE);
  }
  display.drawLine(10, 35, 35, 35, WHITE);

  int left, right;
  if (direction > 1500) {
    left = 90;
    right = 90 + (direction - 1500) / 17;
    display.fillRect(left, 45, right - left, 15, WHITE);
  }
  if (direction < 1500) {
    right = 90;
    left = 90 - (1500 - direction) / 17;
    display.fillRect(left, 45, right - left, 15, WHITE);
  }
  display.drawLine(90, 40, 90, 66, WHITE);
}

// displays battery level
void displayBattery(float cVoltage){
  switch (controllerMode){
    case drone:
      display.setCursor(92, 0);
      display.print("Battery:");
      drawBatteryLevel(cVoltage, 106, 12);
      break;
    case car:
      display.setCursor(60, 0);
      display.print("Battery");
      display.setCursor(60, 12);
      display.print("Level:");
      drawBatteryLevel(cVoltage, 100, 12);
      break;
  }
}

// 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);
  switch (controllerMode) {
    case drone:
      display.write("Drone:");
      break;
    case car:
      display.write("Car:");
      break;
  }
  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
      if (controllerMode == drone)
        WiFi.begin(APSSID_DRONE, APPSW_DRONE);  // start connection
      else
        WiFi.begin(APSSID_CAR, APPSW_CAR);  // start connection
    }
    displayDots();
    delay(500); // 0.5 sec delay
  }  
  // start listening on port 8888
  Udp.stopAll();
  Udp.begin(localPort); 
  if (controllerMode == car) {
    int message = 0;
    // Wait for handshake message
    while (message != 9000) {
      displayDots();
      delay(500);
      message = receivePacket();
      if (message == 9000)
      // Send respose      
        sendPacket(9000, 0, 0, 0);
    }
  } 
}

Credits

paulsb

paulsb

4 projects • 28 followers

Comments