analogReference(EXTERNAL);

    if(0){// Para calibrar sensor de Distancia
      Serial.print(dist);
      Serial.print("mm     ADC: ");    
      Serial.println(measure); 
    }

int dcal [] = { // Calibracion de ADC a Distancia
  -193, -160, -110, -60, 0, 40, 60, 90, 120};
int ADCcal [] = {
  177, 189, 231, 273, 372, 483, 558, 742, 970};

   for(int i =0; i<8; i++){ // Aplicamos curva de Calibracion de ADC a mm 
      if (measure >= ADCcal[i] && measure< ADCcal[i+1]){
        dist = map(measure,ADCcal[i],ADCcal[i+1],dcal[i],dcal[i+1]);
      }

173,173, -5, -5 $ // dist, dist, vel, vel $

  for (int i = 0; i <level-1; i ++) {// 我们全部向左移动 
      v [i] = v [i + 1];
    }
    v [nvel-1] = (dist - lastDist); // 我们放入一个新数据
    vel = 0;
    for (int i = 0; i <nvel; i ++) {// 我们计算平均值
      vel = vel + v [i];
    }
    vel = vel / nvel;

pos = Kp * dist

pos = Kd * vel

float Kp =0; 
float Kd = 100;
float Ki =0;

    pos = Kp * dist + Kd * vel；

int Rint = 8;
int Rext = 40;

    if(abs(dist)>Rint && abs(dist)<Rext){
      I=I+dist*Ki;
    } 
    else {
      I=0;
    }
 pos=Kp*dist+Kd*vel+I;

#include <Servo.h>
//#include <Wire.h>
//#include "IICLiquidCrystal.h"

// Connect via i2c, default address #0 (A0-A2 not jumpered)
//LiquidCrystal lcd(0);

float Kp = 3;   //2
float Kd = 1;  //35
float Ki = 0.5; //0.1
int Rint = 8;   //
int Rext = 40;  //
int aim = 0;
unsigned long time = 0; //execution time of the last cycle
unsigned long timeSerial = 0;
int period = 50;        //Sampling period in ms
int sensorPin = 0;        //Analog Pin where the Distance Sensor signal is connected
int measure;            //What the sensor measures. They are ADCs.
int dcal [] = {         //Remote ADC calibration
  -193, -160, -110, -60, 0, 40, 60, 90, 120
};
int ADCcal [] = {
  177, 189, 231, 273, 372, 483, 558, 742, 970
};
int lastDist;     //Previous value of Distance to calculate Speed
int dist;         //distance in mm with 0 in the center of the bar
int nvel = 5;       //number of velocity values over which we calculate the average
int v[5];
int vel;          //mean value of the last speed levels
float I;          //Integral Value


Servo myservo;    //create servo object to control a servo
float pos;
float reposo = 1350; //value held by horizontal bar

int ledPin = 13; //Green led pin.

void setup()
{
  analogReference(EXTERNAL);  //AREF connected to 3.3V
  myservo.attach(3);         //attaches the servo on pin X to the servo object
  Serial.begin(115200);
  pinMode(ledPin, OUTPUT);

  myservo.writeMicroseconds(reposo);
  delay(5000);

  //lcd.begin(16, 2);

}

void loop()
{
  if (millis() > timeSerial + 200)
  {
    timeSerial = millis();
//    Kp = map(analogRead(A1), 0, 1023, 0, 5000) / 100.0;
//    Kd = map(analogRead(A2), 0, 1023, 0, 400) / 100.0;
//    Ki = map(analogRead(A3), 0, 1023, 0, 300) / 100.0;

    //aim = map(analogRead(A5), 0, 1023, -20, 20);

    Serial.println();
    Serial.print("Kp:");
    Serial.println(Kp);

    Serial.print("Kd:");
    Serial.println(Kd);

    Serial.print("Ki:");
    Serial.println(Ki);

    Serial.print("Aim:");
    Serial.println(aim);

    Serial.print("Pos:");
    Serial.println(dist);
  }

  
  if (millis() > time + period) { //
    time = millis();

    //    lcd.setCursor(0, 0);
    //    lcd.print("Kp:");
    //    lcd.print(Kp);
    //
    //    lcd.setCursor(8, 0);
    //    lcd.print("Kd:");
    //    lcd.print(Kd);
    //
    //    lcd.setCursor(0, 1);
    //    lcd.print("Ki:");
    //    lcd.print(Ki);
    //
    //    lcd.setCursor(8, 1);
    //    lcd.print("Pos:");
    //    lcd.print(dist);






    //We measure DISTANCE
    measure = analogRead(sensorPin);
    measure = constrain(measure, ADCcal[0], ADCcal[8]);
    lastDist = dist; //We save the previous value of dist to calculate the speed
    for (int i = 0; i < 8; i++) { //We apply Calibration curve from ADC to mm
      if (measure >= ADCcal[i] && measure < ADCcal[i + 1]) {
        dist = map(measure, ADCcal[i], ADCcal[i + 1], dcal[i], dcal[i + 1]);
      }
    }
    //Average SPEED calculation
    for (int i = 0; i < nvel - 1; i++) { //We all move to the left to free the last one.
      v[i] = v[i + 1];
    }
    v[nvel - 1] = (dist - lastDist); //We put a new data
    vel = 0;
    for (int i = 0; i < nvel; i++) { //We calculate the mean
      vel = vel + v[i];
    }
    vel = vel / nvel;
    // Integral
    if (abs(dist - aim) > Rint && abs(dist - aim) < Rext) { //Only if it is inside (-Rext, Rext) and outside (-Rint, Rint)
      I = I + dist * Ki;
    }
    else {
      I = 0;
    }
    //We calculate servo position
    pos = Kp * (dist - aim) + Kd * vel + I;
    myservo.writeMicroseconds(reposo + pos);

    if (abs(dist) < Rint) { //If we are inside Rint turn on Led
      digitalWrite(ledPin, HIGH);
    }
    else {
      digitalWrite(ledPin, LOW);
    }

    if (1) { //Shipping for PROCESSING
      Serial.print(dist + 200);
      Serial.print(",");
      Serial.print(dist + 200);
      Serial.print(",");
      Serial.print(vel);
      Serial.print(",");
      Serial.print(vel);
      Serial.print("$");
    }
    if (0) { //Debug
      Serial.print(millis());
      Serial.print(" ms|dist: ");
      Serial.print(dist);
      Serial.print("|vel: ");
      Serial.print(vel);
      Serial.print("|Kp*dist: ");
      Serial.print(Kp * dist);
      Serial.print("|Kd*vel: ");
      Serial.print(Kd * vel);
      Serial.print("|Int: ");
      Serial.print(I);
      Serial.print("|pos: ");
      Serial.println(pos);
    }
    if (0) { //To calibrate Distance sensor
      Serial.print(dist);
      Serial.print("mm     ADC: ");
      Serial.println(measure);
    }
    if (0) { //DeBug Speeds
      for (int i = 0; i < (nvel); i++) {
        Serial.print(v[i]);
        Serial.print(",");
      }
      Serial.print("       vel:");
      Serial.println(vel);
    }
  }
}

// Arduino: _14_HV_logger_2Ch
// VOLTAGE IN PIN Analog0
// CURRENT IN PIN Analog1
import processing.serial.*;
Serial USB;
String message = null;
int jmax = 100000; // Stored readings
int[] VminADC; // From 0 to jmax
int[] VmaxADC; // Raw values 
int[] IminADC;
int[] ImaxADC;
float[] Vmin; // From 0 to jmax
float[] Vmax; // in kV and uA
float[] Imin;
float[] Imax;
int j = 0; // Whole Register
int x = 0; // On Screen
int xpant = 1000; // Dimensions of the screen
int ypant = 800;
int mSup = 60; // Margin Sup and Inf of the trace
int mInf = 50;
int mLat = 50;
int xgraf = xpant - 2 * mLat;
int xDisplay = 30; // Beginning of the text
int yDisplay = 30;
float Cal0 = 1;
float Cal1 = 1;
float CalVEL =  300; // 
float CalPOS = 400; // 
PFont fontVerdana;
String [] com = new String [3];
void setup() {
  size(1000, 800, P3D);
  println(Serial.list());
  String portName = Serial.list()[0]; //    Puerto COM 14 
  USB = new Serial(this, portName, 115200);
  VminADC = new int [jmax];
  VmaxADC = new int [jmax];
  IminADC = new int [jmax];
  ImaxADC = new int [jmax];
  Vmin = new float [jmax];
  Vmax = new float [jmax];
  Imin = new float [jmax];
  Imax = new float [jmax];
  com = new String [3];
  fontVerdana = loadFont("Verdana-20.vlw");
}
void draw() {
  background(190);
  // New Data ***************************************************************************
  while (USB.available () > 0) {
    message = USB.readStringUntil(36); // 644,659,725,733$
    if (message != null) {    
      if (j < jmax - 2) {
        j++;
      } 
      else {
        j = 0;
      }
      message = message.substring(0, message.length()-1); // 644,659,725,733
      String[] com = splitTokens(message, ",");
      VminADC [j] = int(com[0])-200;   // Stored in VminADC[j] as ADC 
      VmaxADC [j] = int(com[1])-200;
      IminADC [j] = int(com[2]);
      ImaxADC [j] = int(com[3]);
      print(VminADC [j]);
      print("\t");
      print(VmaxADC [j]);
      print("\t");
      print(IminADC [j]);
      print("\t");
      println(ImaxADC [j]);
      Vmin [j] = VminADC [j] * Cal0;  // Stored in Vmin[j] as kV
      Vmax [j] = VmaxADC [j] * Cal0;
      Imin [j] = IminADC [j] * Cal1;
      Imax [j] = ImaxADC [j] * Cal1;
    }
  }

  //  Axis ********************************************************************************
  stroke(255);
  strokeWeight(2);
  line(mLat-10, ypant-mInf, xpant-mLat, ypant-mInf);
  line(mLat, ypant-mInf+10, mLat, mSup);
  // Reference Axis *************************************************************************************
  /*  fill(#FF1C20); // Red
   //  text("500uA", 10, (ypant-mInf)-500*(ypant-mSup-mInf)/660+4);
   stroke(#FF1C20, 20); // Red
   line(mLat, (ypant-mInf)-500*(ypant-mSup-mInf)/660, xpant-mLat, (ypant-mInf)-500*(ypant-mSup-mInf)/660);
   */
  fill(#321CFF); // Blue
  text("0", 30, mSup+(ypant-mInf-mSup)/2+4);
  stroke(#321CFF, 20); // Blue
  line(mLat, mSup+(ypant-mInf-mSup)/2, xpant-mLat, mSup+(ypant-mInf-mSup)/2);

  // Draw before 1st scroll ********************************************************************************************
  if (j <= xgraf) {
    for ( int i = 0; i < j; i ++) {
      strokeWeight(1);
      stroke(#FF1C20); // Red VELOCIDAD
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imin[i]*(ypant-mInf-mSup)/CalVEL-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imax[i]*(ypant-mInf-mSup)/CalVEL+2);

      stroke(#321CFF); // Blue POSICIÓN
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmin[i]*(ypant-mInf-mSup)/CalPOS-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmax[i]*(ypant-mInf-mSup)/CalPOS+2);
    }
  }
  // Draw with scroll **************************************************************************************************************
  if (j > xgraf) {
    for ( int i = 0; i <= xgraf; i ++) {
      strokeWeight(1);
      stroke(#FF1C20); // Red
      //      line(i+mLat, int((ypant-mInf)-Imin [j-xgraf+i]*(ypant-mSup-mInf)/660), i+mLat, int((ypant-mInf)-Imax [j-xgraf+i]*(ypant-mSup-mInf)/660));      
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imin[j-xgraf+i]*(ypant-mInf-mSup)/CalVEL-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Imax[j-xgraf+i]*(ypant-mInf-mSup)/CalVEL+2);
      stroke(#321CFF); // Blue
      // line(i+mLat, int((ypant-mInf)-Vmin [j-xgraf+i]*(ypant-mSup-mInf)/15), i+mLat, int((ypant-mInf)-Vmax [j-xgraf+i]*(ypant-mSup-mInf)/15));
      line(i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmin[j-xgraf+i]*(ypant-mInf-mSup)/CalPOS-2, i+mLat, int(mSup+(ypant-mInf-mSup)/2)-Vmax[j-xgraf+i]*(ypant-mInf-mSup)/CalPOS+2);
    }
  }
  // Text Channels Readings ***************************************************************
  stroke(190);
  fill(190); // Blue
  rect(30, 12, 330, 20);
  textFont(fontVerdana, 20);
  fill(#321CFF); // Blue
  text(int(Vmin[j]) +" mm", xDisplay, yDisplay);
  textFont(fontVerdana, 10);
  //  text("+/-" + nf((Vmax[j] - Vmin[j]), 0, 1) +" kV", xDisplay + 80, yDisplay);
  textFont(fontVerdana, 20);
  fill(#FF1C20); // Red
  text(int(Imax[j]) +" mm/s", xDisplay + 200, yDisplay);
  textFont(fontVerdana, 10);
  //  text("+/-" + int((Imax[j] - Imin[j])) +" uA", xDisplay + 200 + 80, yDisplay);
}
/*void keyPressed() {
 if (key == 's' || key =='S') {
 grabar();
 }
 if (key == 'v' || key =='V') {
 Cal0 = 9.0/((VminADC [j]+VmaxADC [j])/2); // 9 kV equals 489 ADC
 println("Calibración de Tensión: "+ Cal0);
 }
 if (key == 'i' || key =='I') {
 Cal1 = 660.0/((IminADC [j]+ImaxADC [j])/2);
 println("Calibración de Intensidad: "+ Cal1);
 }
 }
 */
void grabar() {
  String[] lines = new String[j];
  for (int i = 0; i < j; i++) {
    lines[i] = str(Vmin [i+1]) + "\t" + str(Vmax [i+1]) + "\t" + str(Imin [i+1]) + "\t" + str(Imax [i+1]); // Vmin  Vmax  Imin  Imax
  }
  saveStrings("Registro.txt", lines);
  exit();
}