public void CONFIGoff() {

    myPort.write("j"); // exit config command
    buttonWRITE.setColorBackground(gray_);
    buttonREAD.setColorBackground(gray_);
    buttonCONFon.setColorBackground(gray_);
    buttonCONFoff.setColorBackground(green_);
    writeEnable = false;
    readEnable = false;
  }
  public void CONFIGon() {

    myPort.write("i"); // enter config command
    buttonWRITE.setColorBackground(green_);
    buttonREAD.setColorBackground(green_);
    buttonCONFon.setColorBackground(green_);
    buttonCONFoff.setColorBackground(gray_);
    writeEnable = true;
    readEnable = true;
  }
  public void READ() {
    if (readEnable == false) return;

    myPort.write("g"); // sends get values command

    while (i < 200000000) {
      i++;
    } // delay
    i = 0;

    // myPort.write("OK");
    readStatus = "Can't Read";

    if (myPort.read() == 'x') {
      PitchP.setValue(myPort.read() / 100.00f);
      RollP.setValue(myPort.read() / 100.00f);
      YawP.setValue(myPort.read() / 100.00f);

      PitchD.setValue(myPort.read() / 100.00f);
      RollD.setValue(myPort.read() / 100.00f);
      YawD.setValue(myPort.read() / 100.00f);

      PitchPWR.setValue(myPort.read());
      RollPWR.setValue(myPort.read());
      YawPWR.setValue(myPort.read());

      RCcontrol = PApplet.parseChar(myPort.read());
      YawRCon = PApplet.parseChar(myPort.read());

      RollCal.setValue((myPort.read() - 100.00f) / 10.00f);

      if (RCcontrol == '0') {
        buttonRCOff.setColorBackground(green_);
        buttonRCOn.setColorBackground(gray_);
      }
      if (RCcontrol == '1') {
        buttonRCOff.setColorBackground(gray_);
        buttonRCOn.setColorBackground(green_);
      }

      if (YawRCon == '0') {
        buttonYawRC.setColorBackground(gray_);
        buttonYawAut.setColorBackground(green_);
      }
      if (YawRCon == '1') {
        buttonYawRC.setColorBackground(green_);
        buttonYawAut.setColorBackground(gray_);
      }

      readStatus = "Read OK";
    }
  }
  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");
 }
  public void WRITE() {
    if (writeEnable == false) return;

    myPort.write("h");

    myPort.write(PApplet.parseInt(PitchP.value() * 100));
    myPort.write(PApplet.parseInt(RollP.value() * 100));
    myPort.write(PApplet.parseInt(YawP.value() * 100));

    myPort.write(PApplet.parseInt(PitchD.value() * 100));
    myPort.write(PApplet.parseInt(RollD.value() * 100));
    myPort.write(PApplet.parseInt(YawD.value() * 100));

    myPort.write(PApplet.parseInt(PitchPWR.value()));
    myPort.write(PApplet.parseInt(RollPWR.value()));
    myPort.write(PApplet.parseInt(YawPWR.value()));

    myPort.write(RCcontrol);
    myPort.write(YawRCon);

    myPort.write(PApplet.parseInt(RollCal.value() * 10 + 100));
    // println (RollCal.value());
    // println (int (RollCal.value()*10+100));

    readStatus = "Write OK";
  }