Hardware components | ||||||
| × | 1 | ||||
![]() |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 2 | ||||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
![]() |
| × | 2 | |||
| × | 1 | ||||
| × | 2 | ||||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
| × | 1 | ||||
![]() |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
![]() |
| × | 1 | |||
![]() |
| × | 1 | |||
Software apps and online services | ||||||
![]() |
|
The car software has been extended to add a 3rd mode - Line Follow and the controller software updated to provide for this.
I have also added voltage monitoring for the motor drive battery (using GP26 and a 2 resistor divider - see updated schematic) so that the car speed can be adjusted to compensate as the voltage reduces to maintain a constant speed in the automated modes.
Updated:
- Car software
- Controller software
- Schematic for car
- Parts list
- New STL bracket to mount line sensors
This projects contains a remote controlled car and joystick controller based on a Raspberry Pi PicoW. The output of the controller matches the same ranges and package order as the UDP Joystick Phone App plus additional values for the joystick switches to select modes and start and stop in avoidance mode. If you do not wish to make the controller, you can just build the car and select the directive in the code to use the UDP Joystick App to control the car, however, using the App you will not be able to change to avoidance and line follow modes.
For the controller, the project uses an ADS1115 to provide 4 additional ADC channels as the Pico W only has 3.
The software is written in C++ using the Arduino IDE. You will need a library for the ADS1115 and the one I use is shipped with the IDE and can be installed via the library manager:
ADSIX15 by Rob Tillaart
0.310 installed
Should work for ADS1013, ADS1014, ADSI 113 and ADSI 114 Arduino library for
ADS1015 - 12C 12 bit ADC and ADSI 115 12C 16 bit ADC
It can also be downloaded from https://github.com/RobTillaart/ADS1X15.
The controller includes a 0.96inch OLED display which need the Adafruit SSD1306 library which can also be installed via the library manager:
Adafruit SSD1306 by Adafruit
2.5.7 installed
SSD1306 oled driver library for monochrome 128x64 and 128x32 displays
SSD1306 oled driver library for monochrome 128x64 and 128x32 displays
For the car, only the Y access of the left joystick (forward and reverse) and the X axis of the right joystick (left and right) are read and sent to the car.
This project also includes code to allow you to use the controller for both a drone (specifically designed for the PicoW Copter drone by anish-natekar which can be found here: https://github.com/anish-natekar/PicoW_Copter). In this version of the code, you have the option to select either drone or car mode on startup.
I have created a 3d printed enclosure for the controller and holders for the servo, ultrasonic sensor, line sensors and batteries. The STL files for this are also included.
The enclosure has lugs that the prototyping board and joysticks sit on and the top fit onto.
L298N Motor Drive Controller Board DC Dual H-Bridge Robot Stepper Motor Control and Drives Module
This is used to control the motors and includes an H-Bridge to determine spin direction. PWM is used to control the speed.
The board also has a 5v regulator so this is used to power the Pico W and distance sensor.
Separate 5v Regulator
The servo draws too much power to be run from the the same source as the PicoW, hence a second battery has been added to power the servo motor through a separate 5 volt regulator using the LM7805, as follows:
The input voltage must be between 7 and 12 volts.
You can, of course, replace this with a purchased 5v regulator if you wish.
/* 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++/*
(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++/*
(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);
}
}
}
Comments