Outreach Bots (Part 2: Firmware)

This code can likely be optimized but it functions to the performance needed, so there is not a great need to optimize every part.
The code for the robot is written in Arduino C++, and is compiled and uploaded to the ESP8266-12e that acts as the ‘brain’ of the robot. Note that also this is not the entire set of code, but only includes some of the major functions.

If you notice any major flaws that could crash the program or make the robots unsafe, please email me at ealayne2@illinois.edu

Firmware Components

1. Setup
2. Main Loop
3. Server
4. Motor Controller

The setup function runs when the controller is powered on, and sets up the ESP8266-12e to act as a WiFi ‘router’ and a basic server.

void setup() {
  //setup pin modes
  pinMode(PWMB, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(DIRB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  
  #ifdef EnableServo1
    Servo1.attach(Servo1Pin);
  #else
    pinMode(Servo1Pin, OUTPUT);
  #endif  
  
  #ifdef EnableServo2
    Servo2.attach(Servo2Pin);
  #else
    pinMode(Servo2Pin, OUTPUT);
  #endif
  //end pin modes

  //setup softAP
  WiFi.softAPConfig(local_IP, gateway, subnet);
  WiFi.softAP(ssid, password);
  //end softAP

  //setup server
  server.on("/", controllerHandler);
  server.begin();
  //end server
}

To begin, the board sets the pins (lines 3-6)for motor drive as outputs then depending if servos are implemented it either sets those pins as outputs or attaches the servo objects to those pins (lines 8-18).

Next the WiFi ‘router’ is set up using the prespecified config data and network name/password (lines 22-23).

Finally, the server is setup and attaches a function to handle requests to the root of the address(line 27), and then starts up the server (line 28).

This is the code for the main loop of the controller, that runs over and over until the robot is powered off.

void loop() {  
  if (LOCK  == false) {
    //increase the keep alive counter if needed
    updateFailsafeVars();
  
    //check that only one user is connected 
    //check that the keep alive timer stays under the limit
    //check that the bad requests stay under the limit
    if (failsafeChecksPassed) {
      Failsafe_Active = false;
      //hardware control
      updateDriveSpeeds();
      #ifdef ServosUsed
        updateServos();
      #endif
      //end hardware control
    } else {
      Failsafe(); //if any failsafe is not met, go into failsafe and disable everything
    }
    
    //handle controller requests
    server.handleClient();

  } else {
    //if the lock has been set, disable robot
    Failsafe();
    digitalWrite(2, LOW); //turn LED On
    delay(10000000);      //wait 'forever' in locked loop
  }
}

This loop continuously checks if the “LOCK” variable has been set (line 2) by the wireless controller, and if it has not been set to “true,” then it simply goes on to function normally.

In this normal state, it checks if any of the failsafes have been activated (line 4), and if everything is safe, it updates the drive motor speeds (line 10) and the servo outputs (line 12: only if servos are implemented) according to the requested values. If any of the failsafes have been activated, then it runs a function “Failsafe” (line 16) to disable all motors and outputs. After doing either of those things, it runs code to detect if the controller has made a request (line 20).

If the lock is active, the controller sets the failsafe, and turns on the status LED before ‘waiting’ for 0.5 seconds. After this timer is up, it is stuck in this repeating loop of Failsafe and waiting to ensure the robot is safe after being disabled until it is rebooted.

The server directs all requests made to the root of the device to this function where it is authenticated and parsed.

void controllerHandler() {
  String authKey = server.arg("authKey");                              //.toInt() returns zero in error case, so entire packet is ignored, and possibly goes directly to full failsafe
  if (authKey == AUTH_KEY) {
    String STOP = server.arg("STOP");
    if (STOP == "STOP") {   //immediate Stop and shut down
      Failsafe();
      LOCK = true;
      return;
    }
    
    int validation = server.arg("validation").toInt();
  
    String motor1Str = server.arg("motor1");                                  //Range: -1023 to 1023
    String motor2Str = server.arg("motor2");
    #ifdef EnableServo1
      String servo1Str = server.arg("servo1");                                  //Range: 0 to 1023
    #endif
    #ifdef EnableServo2
      String servo2Str = server.arg("servo2");
    #endif

  
    String ValidationString = "***************"    //redacted for security
    if (validation != Validation(ValidationString)) {  //function "Validation" changed for security
      BadRequests++;
      server.send(400, "text/plain", "Bad Request: Invalid Validation");
      return;
    }
    BadRequests = max((BadRequests-1), 0);

    
    if (Failsafe_Active == false) {
      int motor1Val = motor1Str.toInt();
      SetMotorSpeed(0, motor1Val);
      int motor2Val = motor2Str.toInt();
      SetMotorSpeed(1, motor2Val);
      
      #ifdef EnableServo1
      int servo1Val = servo1Str.toInt();
      SetServoPos(0, servo1Val);
      #endif
      
      #ifdef EnableServo2
      int servo2Val = servo2Str.toInt();
      SetServoPos(1, servo2Val);
      #endif 

      server.send(200, "", ""); //20ms
      KeepAliveTimer = 0;                                                       //recieved valid request, reset timer
      
      return;
    } else {
      String FailsafeReason = "";
      FailsafeReason += (WiFi.softAPgetStationNum() == 1) ? "" : " More Than One User Connected.";
      FailsafeReason += (KeepAliveTimer < KEEP_ALIVE_TIMEOUT) ? "" : " Keep Alive Limit Reached.";
      FailsafeReason += (BadRequests < BAD_REQUESTS_LIMIT) ? "" : " Bad Requests Limit Reached.";
      String response = "Failsafe Active - Error(s):" + FailsafeReason; 
      server.send(200, "text/plain", response);
      KeepAliveTimer = 0;                                                       //recieved valid request, reset timer

      return;
    }
  } else {
    server.send(403, "text/plain", "Forbidden: Invalid authentication Key");
    return;
  }
}

This code gradually adjusts motor speed to avoid current spikes by rapidly changing direction and speed.

void updateDriveSpeeds() {
  for (int i = 0; i <= 1; i++) {
    if (motorSpeedsDifferent[i]) {
      if (requestMotorSpeeds[i] < currentMotorSpeeds[i]) {
        currentMotorSpeeds[i] = max((currentMotorSpeeds[i] - accelStep), requestMotorSpeeds[i]);
      } else if (requestMotorSpeeds[i] > currentMotorSpeeds[i]) {
        currentMotorSpeeds[i] = min((currentMotorSpeeds[i] + accelStep), requestMotorSpeeds[i]);
      }

      if (currentMotorSpeeds[i] < 0) {
        digitalWrite(DIR_Pins[i], LOW);
      } else if (currentMotorSpeeds[i] > 0) {
        digitalWrite(DIR_Pins[i], HIGH);
      }

      analogWrite(PWM_Pins[i], abs(currentMotorSpeeds[i]));
      
      if (requestMotorSpeeds[i] == currentMotorSpeeds[i]) {
        motorSpeedsDifferent[i] = false;
      }
    }
  }
}

This code is called every time through the main loop, and for each motor shifting the speed toward the requested speed by the same amount each time. This allows it to be a very basic way to smooth out the power and acceleration of the motors.

For each motor, if the current and requested speed is different (line 3) then it adjusts the current speed towards the requested speed by the variable “accelStep” and stopping at the requested speed once it overshoots that value (lines 4-8).

Next if the speed is being updated, it also sets the direction pin for that motor depending on the new speed’s direction (lines 10-14).

Finally, it sets the voltage of the pin to the correct value depending on the new speed for each motor (line 16). Afterwards it checks if the requested speed has been reached, and if so it sets a variable to indicate this and skip this motor the next time the function is called (lines 18-20).