Setup

The first step in this exercise is simply to get the example Processing code running. The code will run perfectly well without a connection to an Arduino since all the relevant variables have default values. Copy the code from the box below into a new Processing sketch and make sure that it runs as expected. See if you can keep the vehicle in the red box in the centre of the display.

The code is quite long, but there is no need to go through all of it. Scroll down to lines 284 - 294 and you will find three methods belonging to the Vehicle class. The first gets the current speed of the vehicle and this will be the input into your PID controller. The second gets the value of a variable called gas which represents the current state of the vehicle's controls. A positive value represents acceleration and negative values represent braking. The third method changes the value of the gas variable which is then using to calculate the effect on the vehicle's speed. You can call the setGas() method with the output of your PID controller.

The Controls class at the very bottom of the listing is the one which displays the brake and accelerate controls on the screen. Here you can find an example of using the getGas() method.

    Vehicle vehicle;
    Road road;
    Graph graph;
    Controls controls;
    int i, sectionLength;
    PVector target;
    PVector drag;
    float trackingSpeed;
    float acceleration;
    float gravity;
    float timeStep;
    float dragCoefficient;

    void setup() {
      size(1200,600);
      trackingSpeed = 5;
      acceleration = 0.22;
      drag = new PVector(-0.02, 0);
      gravity = 9.8;
      dragCoefficient = 0.2;
      timeStep = 0.2;
      sectionLength = 250;
      target = new PVector(500, 300);
      fill(0);
      vehicle = new Vehicle(color(255,0,0), new PVector(200,280), new PVector(trackingSpeed, 0));
      road = initialiseRoad();
      graph = new Graph();
      controls = new Controls();
    }

    void draw() {
      background(255);
      road.track();
      road.display();
      vehicle.drive();
      vehicle.display();
      title();
      graph.add(vehicle.getSpeed());
      graph.display();
      controls.display();
    }

    void mousePressed() {
      float factor = 1;
      float newGas = 0;

      newGas = controls.control(mouseX, mouseY) * factor + vehicle.getGas();
      vehicle.setGas(newGas);
    }

    void title() {
      String name = "Cruise Control";
      pushStyle();
      pushMatrix();
      textSize(24);
      translate(width/2 - textWidth(name)/2, 50);
      text(name, 0, 0);
      translate(textWidth(name)/2 - 60, 20);
      noFill();
      stroke(4);
      popMatrix();
      popStyle();
    }

    Road initialiseRoad() {
      PVector roadStart;
      float incline;
      RoadSection[] tempRoad = new RoadSection[10];
      roadStart = new PVector(0, 300);
      incline = 0;
      for (i=0;i<10;i++) {
        tempRoad[i] = new RoadSection(sectionLength, incline, roadStart);
        roadStart = tempRoad[i].end();
        incline = randomIncline(tempRoad[i].incline);
      }
      return new Road(tempRoad);
    }


    float randomIncline(float currentIncline) {
      float incline;
      float limit = 0.3;

      incline = random(currentIncline-0.2, currentIncline+0.2);
      //incline = random(0, currentIncline+0.2);
      if (abs(incline) > limit)
        return 0;
      else
        return incline;
    }

    class Road {
      RoadSection[] sections;
      float yOffset;
      float incline;

      Road(RoadSection[] pSections) {
        sections = pSections;
      }

      float inclineAt(float xpos) {
        for (i=0; i<sections.length; i++) {
          if (xpos >= sections[i].start.x && xpos < sections[i].end().x)
            return sections[i].incline;
        }
        return 0;
      }

      float yAt(float xpos) {
        for (i=0; i<sections.length; i++) {
          if (xpos >= sections[i].start.x && xpos < sections[i].end().x) {
            return sections[i].start.y - (xpos - sections[i].start.x) * tan(sections[i].incline);
          }
        }
        return 0;
      }

      void track() {
        float yOffset=0;

        for (i=0; i<sections.length; i++) {
          sections[i].start.x -= trackingSpeed;
          if (sections[i].onTarget()) {
            yOffset = sections[i].start.y -
                      (target.x - sections[i].start.x) * tan(sections[i].incline) - 300;
          }
        }
        if (sections[0].end().x <=0) {
          for (i=0; i<sections.length-1; i++) {
            sections[i] = sections[i+1];
          }
          sections[i] = new RoadSection(sectionLength,
                                        randomIncline(sections[i-1].incline),
                                        new PVector(sections[i-1].end().x,
                                        sections[i-1].end().y));
        }
        for (i=0; i<sections.length; i++) {
          sections[i].start.y -= yOffset;
        }
      }

      void redBox() {
        pushStyle();
        stroke(255,0,0);
        fill(0,0,0,0);
        rect(560,yAt(600)-40,60, 40);
        popStyle();
      }

      void display() {
        for (i=0; i<sections.length; i++) {
          sections[i].display();
        }
        // redBox();
      }
    }

    class RoadSection {
      float sectionLength;
      float thickness;
      float incline;
      PVector start;

      RoadSection(float pLength, float pIncline, PVector pStart) {
        sectionLength = pLength;
        thickness = 10;
        incline = pIncline;
        start = pStart;
      }

      PVector end() {
        return new PVector(start.x + sectionLength * cos(incline),
                           start.y - sectionLength * sin(incline));
      }

      boolean onTarget() {
        if (target.x >= start.x && target.x < end().x)
          return true;
        else
          return false;
      }

      void display() {
        int i;
        float x, y;

        pushStyle();
        stroke(0);
        strokeWeight(10);
        line(start.x, start.y, end().x, end().y);
        strokeWeight(1);
        stroke(0,255,0);
        for (i=0; i<sectionLength; i+=10) {
          x = start.x + i;
          y = start.y - (x - start.x) * sin(incline);
          line(x, y+5, x, height);
        }
        popStyle();
      }
    }

    class Vehicle {
      color c;
      float mass;
      PVector position;
      PVector velocity;
      PVector acceleration;
      float drag;
      float xspeed;
      float gas;

      Vehicle(color pColor, PVector pPosition, PVector pVelocity) {
        mass = 500;
        c = pColor;
        position = pPosition;
        velocity = pVelocity;
        acceleration = new PVector(0, 0);
        gas = 5;
      }

      void accelerate(PVector force) {
        acceleration = acceleration.add(force.div(mass));
      }

      void display() {
        float incline;

        pushStyle();
        stroke(0);
        fill(c);
        incline = road.inclineAt(position.x);
        position.y = road.yAt(position.x) - 20;
        pushMatrix();
        translate(position.x, position.y);
        rotate(-incline);
        rectMode(CENTER);
        rect(0,0,40,20);
        fill(0);
        ellipse(-10, 10, 16, 16);
        ellipse(10, 10, 16, 16);
        popMatrix();
        popStyle();
      }

      PVector getForce(float incline, float gas, PVector velocity){
        float drag;
        PVector resultant;

        //Gravity
        if (incline > 0)
          resultant = new PVector(-gravity * cos(incline), -gravity * sin(incline));
        else
          resultant = new PVector(gravity * cos(incline), gravity * sin(incline));

        //Drag
        drag = velocity.dot(velocity) * dragCoefficient;
        resultant.add(-drag * cos(incline), -drag * sin(incline) );

        //Driver control
        resultant.add(gas * cos(incline), gas * sin(incline));

        return resultant;
      }


      void drive() {
        float incline = road.inclineAt(position.x);

        accelerate(getForce(incline, gas, velocity));
        velocity.add(acceleration.mult(timeStep));

        if (velocity.x > 10)
          velocity.x = 10;
        if (velocity.x < -10)
          velocity.x = -10;

        position.add(velocity);
        position.add(new PVector(-trackingSpeed, 0));
        if (position.x > width) {
          position.x = 0;
        }
      }

      float getSpeed() {
        return velocity.x;
      }

      float getGas() {
        return gas;
      }

      void setGas(float newValue) {
        gas = newValue;
      }
    }

    class Graph {
      int size = 400;
      float[] values = new float[size];

      void add(float newValue) {
        for (i=0; i<size-1; i++) {
          values[i] = values[i+1];
        }
        values[size-1] = newValue;
      }

      void display() {
        pushMatrix();
        pushStyle();
        fill(255);
        stroke(0);
        rectMode(CORNERS);
        rect(700, 340, 1180, 580);
        line(720, 350, 720, 570);
        line(715, 550, 1160, 550);
        line(715, 450, 1160, 450);
        fill(0);
        textSize(10);
        text("0", 705, 555);
        text("5", 705, 455);
        stroke(255,0,0);
        for (i=0; i<size-1; i++) {
          if (values[i] > -1 && values[i+1] > -1)
            line(720+i, 550-values[i]*20, 720+i+1, 550-values[i+1]*20);
        }
        textSize(16);
        text("Speed:", 900, 360);
        text(nfc(vehicle.velocity.x, 1), 960, 360);
        //text(nfc(vehicle.acceleration.x, 1), 900, 400);
        popStyle();
        popMatrix();
      }
    }

    class Controls {
      int size = 400;
      float speed = vehicle.getSpeed();
      Boolean brake = false;
      Boolean accelerate = false;

      int control(int x, int y) {
        if (y > 500 && x < 560) {
          if (x > 100 && x < 200) {
            brake = true;
            return -1;
          }
          else if (x > 300 && x < 400) {
            accelerate = true;
            return 1;
          }
        }
        return 0;
      }

      void display() {
        pushMatrix();
        pushStyle();
        fill(255);
        stroke(0);

        rectMode(CORNERS);
        if (brake) fill(204, 102, 0);
        else fill(255);
        rect(100, 500, 200, 560);
        if (accelerate) fill(204, 102, 0);
        else fill(255);
        rect(320, 500, 420, 560);
        fill(0);
        textSize(16);
        text("Brake", 130, 530);
        text("Accelerate", 330, 530);
        stroke(255,0,0);
        text("Speed:", 210, 520);
        text(nfc(vehicle.getSpeed(), 1), 270, 520);
        text("Gas:", 210, 540);
        text(nfc(vehicle.getGas(), 1), 270, 540);
        popStyle();
        popMatrix();
        brake = false;
        accelerate = false;
      }
    }