//DC_Servo
#include <stdlib.h>
int PS1B;
int gPC_T,gPC_N;

char inputString[50]; 
boolean stringComplete = false;  // whether the string is complete

int gi=0;

void setup() 
{
  Serial.begin(9600);

  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  gPC_T=0;
  gPC_N=0;
  PS1B=digitalRead(4);
}

// the loop function runs over and over again forever
void loop() 
{
  String str;
  int PS1=digitalRead(4);
  int PS2=digitalRead(5);
  long T1,T2;
  long DPC=gPC_T-gPC_N;
  if( DPC > 0)
  {
    T1=DPC*400;
    if(T1 > 1000){T1=1000;}
    T2=1000-T1;
    digitalWrite(2,LOW);
    digitalWrite(3,HIGH);
    delayMicroseconds(T1);
    digitalWrite(2, HIGH);
    digitalWrite(3, HIGH);
    delayMicroseconds(T2);
  }
  else
  {
    T1=(1-DPC)*400;
    if(T1 > 1000){T1=1000;}
    T2=1000-T1;
    digitalWrite(2, HIGH);
    digitalWrite(3, LOW);
    delayMicroseconds(T1);
    digitalWrite(2, HIGH);
    digitalWrite(3, HIGH);
    delayMicroseconds(T2);
    
  }

  str=String(gPC_T)+"  "+String(gPC_N);

  if(PS1B==HIGH)
  {
    if(PS1==LOW)
    {
      PS1B=LOW;
      if(PS2==HIGH)
      {
        gPC_N++;
        Serial.println(str);
      }
      else
      {
        gPC_N--;
        Serial.println(str);
      }
    }
  }
  else
  {
    if(PS1==HIGH)
    {
       PS1B=HIGH;
       if(PS2==LOW)
      {
        gPC_N++;
        Serial.println(str);
      }
      else
      {
        gPC_N--;
        Serial.println(str);
      }
    }
  }
  if (stringComplete)
  {
    //Serial.println(inputString);
    gPC_T = atoi(inputString);
    gi=0;
    stringComplete= false;
  }
}

void serialEvent() 
{
  
  while (Serial.available()) 
  {
    char inChar = (char)Serial.read();
    inputString[gi]=inChar;
    if (inChar == '\n')
    {
      stringComplete = true;
      inputString[gi-1]=0;
    }
    gi++;
  }
}