Wi-Fi Remote Controlled Car and Joystick Controller

/*  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();
  }
}