public void InitSerial(float portValue) {
   // initialize the serial port selected in the listBox
   println(
       "initializing serial "
           + PApplet.parseInt(portValue)
           + " in serial.list()"); // for debugging
   // grab the name of the serial port
   String portPos = Serial.list()[PApplet.parseInt(portValue)];
   // initialize the port
   serial = new Serial(this, portPos, 115200);
   // read bytes into a buffer until you get a linefeed (ASCII 10):
   serial.bufferUntil('\n');
   println("done init serial");
   // initialized com port flag
   init_com = 1;
 }
  public void processSerialData() {
    byte[] inBuf = new byte[20];
    switch (serial.read()) {
      case 'A':
        // wait for all data arrived
        while (serial.available() < 16) {}
        // read all data
        serial.readBytes(inBuf);
        Acc_RAW = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
        Gyro_RAW = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
        // int intbit = 0;
        // intbit = (inBuf[7] << 24) | ((inBuf[6] & 0xFF) << 16) | ((inBuf[5] & 0xFF) << 8) |
        // (inBuf[4] & 0xFF);
        // Angle = Float.intBitsToFloat(intbit);
        int AngleInt = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
        Angle = PApplet.parseFloat(AngleInt) / 10;
        Acc_Angle = (inBuf[7] << 8) + (inBuf[6] & 0xFF);
        Gyro_Rate = (inBuf[9] << 8) + (inBuf[8] & 0xff);
        Drive = (inBuf[11] << 8) + (inBuf[10] & 0xFF);
        statusFlag = (inBuf[13] << 8) + (inBuf[12] & 0xFF);
        int BatLevelInt = (inBuf[15] << 8) + (inBuf[14] & 0xFF);
        Steer = (inBuf[17] << 8) + (inBuf[16] & 0xFF);
        BatLevel = PApplet.parseFloat(BatLevelInt) / 10;
        println(
            "Acc="
                + Acc_RAW
                + "  Gyro="
                + Gyro_RAW
                + "  Angle="
                + Angle
                + "  Acc_Angle="
                + Acc_Angle
                + "  Gyro_Rate="
                + Gyro_Rate
                + "  Drive="
                + Drive
                + "  Status="
                + statusFlag);
        break;

      case 'E':
        // wait for all data arrived
        while (serial.available() < 6) {}
        // read all data
        serial.readBytes(inBuf);
        int P = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
        int I = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
        int D = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
        conf_KP.setValue(P);
        conf_KI.setValue(I);
        conf_KD.setValue(D);
        println("P=" + P + " I=" + I + " D=" + D);
        break;
    }
    serial.clear();
  }
 public void InitComDropdown() {
   // Initialize portCommList
   int posX = 20;
   int posY = 70;
   PortsList = controlP5.addDropdownList("portComList", posX, posY, 100, 84);
   // Set the background color of the list (you wont see this though).
   PortsList.setBackgroundColor(color(200));
   // Set the height of each item when the list is opened.
   PortsList.setItemHeight(20);
   // Set the height of the bar itself.
   PortsList.setBarHeight(15);
   // Set the lable of the bar when nothing is selected.
   PortsList.captionLabel().set("Select COM port");
   // Set the top margin of the lable.
   PortsList.captionLabel().style().marginTop = 3;
   // Set the left margin of the lable.
   PortsList.captionLabel().style().marginLeft = 3;
   // Set the top margin of the value selected.
   PortsList.valueLabel().style().marginTop = 3;
   // Store the Serial ports in the string comList (char array).
   comList = serial.list();
   // We need to know how many ports there are, to know how many items to add to the list, so we
   // will convert it to a String object (part of a class).
   String comlist = join(comList, ",");
   // We also need how many characters there is in a single port name, we\u00b4ll store the chars
   // here for counting later.
   String COMlist = comList[0];
   // Here we count the length of each port name.
   int size2 = COMlist.length();
   // Now we can count how many ports there are, well that is count how many chars there are, so we
   // will divide by the amount of chars per port name.
   int size1 = comlist.length() / size2;
   // Now well add the ports to the list, we use a for loop for that. How many items is determined
   // by the value of size1.
   for (int i = 0; i < size1; i++) {
     // This is the line doing the actual adding of items, we use the current loop we are in to
     // determin what place in the char array to access and what item number to add it as.
     PortsList.addItem(comList[i], i);
   }
   // Set the color of the background of the items and the bar.
   PortsList.setColorBackground(color(60));
   // Set the color of the item your mouse is hovering over.
   PortsList.setColorActive(color(255, 128));
 }
  public void draw() {
    background(0);
    // Strings composition
    textFont(createFont("Arial bold", 24));
    fill(c_red);
    stroke(255);
    text("OpenWheels GUI V1.0", 20, 30);
    textSize(16);
    // textAlign(CENTER);
    fill(c_azure);
    text("Acc_RAW:  " + Acc_RAW, 150, 65);
    AccSlider.setValue(Acc_RAW);
    text("Gyro_RAW: " + Gyro_RAW, 150, 90);
    GyroSlider.setValue(Gyro_RAW);
    text("Acc_Angle:  " + Acc_Angle + "\u00b0", 150, 115);
    text("Gyro_Rate: " + Gyro_Rate + "\u00b0/sec", 150, 140);
    text("Drive: " + Drive, 150, 165);
    text("Steer: " + Steer, 150, 190);
    text("BatLevel: " + nf(BatLevel, 1, 1) + "V", 150, 215);
    text("Status: " + statusFlag, 150, 240);
    graphGauge();
    graphGrid();

    // call the function that plot the angular of acc, with few screen settings
    graphRoll(PApplet.parseFloat(Acc_Angle), VideoBuffer2, c_red); // xPos, YPos, YSpan
    // call the function that plot the estimate angular, with few screen settings
    graphRoll(Angle, VideoBuffer1, c_yellow); // xPos, YPos, YSpan

    // call arduino for data every timePolling [msec]
    int timePolling = 50; // 50msec=20Hz
    time1 = millis();
    if (init_com == 1) {
      while (serial.available() > 0) processSerialData();
      if ((time1 - time2) > timePolling) {
        if (requestPID == true) {
          serial.write('E');
          requestPID = false;
        } else if (writePID == true) {
          int P = PApplet.parseInt(conf_KP.value());
          int I = PApplet.parseInt(conf_KI.value());
          int D = PApplet.parseInt(conf_KD.value());
          char data[] = {0, 0, 0, 0, 0};
          data[0] = 'W';
          data[1] = PApplet.parseChar(P);
          data[2] = PApplet.parseChar(I);
          data[3] = PApplet.parseChar(D);
          data[4] = ' ';
          String str = new String(data);
          serial.write(str);
          println(str);
          // println(" P=" + P + " I=" + I + " D=" + D);
          writePID = false;
        }
        // else if (RUN==true ) {serial.write('A');}
        else {
          serial.write('A');
        }
        time2 = time1;
      }
    }
  }
 public void SAVE_PID(int theValue) {
   // save PID parameter on Arduino EEPROM
   if (init_com == 0) return;
   serial.write('S');
   // println("Save PID");
 }
 public void SET_ZERO(int theValue) {
   // set zero acc e gyro
   if (init_com == 0) return;
   serial.write('Z');
   println("Zet Zero");
 }