/*
  Built by KrumpGamingCo@gmail.com 
  This is the Control Pannel code to run the trains of my train board using pulse position modulation

  Use this is the base code. Use it control the track H-Bridge. You do not need a NAND Logic gate to use this code, but it will still work if you have one.

  The pulse Position protocall and code is baised off of the below files
  http://dankohn.info/projects/my_robot_2010/
  http://dankohn.info/projects/my_robot_2007/documentation/VEX_RC.pdf
  
  
*/
unsigned long v1;
unsigned long v2;
unsigned long v3;
long d1;
long d2;
long d3;
int s1;
int s2;
int s3;
// Pin outs
int t1 = 2;  //Train 1 direction switch
int t2 = 3;  //Train 2 dircetion switch
int t3 = 4;  //Train 3 direction Switch
int in1 = 10; //Connect to H-Bridge in1
int in2 = 11;  //Connect to H-Brige in2


void setup() {                
  // initialize the digital 10 pin as an output to Vex Signal Splitter.
  // Pin 13 has an LED connected on most Arduino boards:
  pinMode(t1, INPUT_PULLUP);//Train 1
  pinMode(t2, INPUT_PULLUP);//Train 2
  pinMode(t3, INPUT_PULLUP);//Train 3
  pinMode(in1, OUTPUT); //out to H brigde 1
  pinMode(in2, OUTPUT);// out to H bridge 2
  pinMode(13, OUTPUT); //Signal light
  digitalWrite(13, HIGH);    // set the LED on
}

void loop() {
   v1 = analogRead(0);
   s1 = digitalRead(t1);
   if(s1 == LOW){
    d1 = ((v1*500)/1023)+1000;
   }
   else{
    d1= -(((v1*500)/1023)-1000);
   }
   
   v2 = analogRead(1);
   s2 = digitalRead(t2);
   if(s2 == LOW){
    d2 = ((v2*500)/1023)+1000;
   }
   else{
    d2= -(((v2*500)/1023)-1000);
   }
   
   v3 = analogRead(2);
   s3 = digitalRead(t3);
   if(s3 == LOW){
    d3 = ((v3*500)/1023)+1000;
   }
   else{
    d3= -(((v3*500)/1023)-1000);
   }
   
   // d = ((v*1000)/ 1023.0) + 500; //old modulation math
//This is to the signal Splitter
digitalWrite(in1,LOW);         //Turns off for sync pulse
digitalWrite(in2,HIGH);
delayMicroseconds(9000);          //Keeps off for proper sync duration
digitalWrite(in2,LOW);
digitalWrite(in1,HIGH);
delayMicroseconds(9000);          //Keeps off for proper sync duration
digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(d1);          //sets value of Train 1
digitalWrite(in2, LOW);
digitalWrite(in1 HIGH);
delayMicroseconds(d1);          //Sets Identical value of Train 1

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(d2);          //sets value of Train 2
digitalWrite(in2, LOW);
digitalWrite(in1, HIGH);
delayMicroseconds(d2);          //Sets Identical value of Train 2

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(d3);          //sets value of Train 3
digitalWrite(in2, LOW);
digitalWrite(in1, HIGH);
delayMicroseconds(d3);          //Sets Identical value of Train 3

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(500);          //Place Holder for more trains
digitalWrite(in2, LOW);
digitalWrite(in1, HIGH);
delayMicroseconds(500);          //Place Holder for more trains

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(500);          //Place Holder for more trains
digitalWrite(in2, LOW);
digitalWrite(in1, HIGH);
delayMicroseconds(500);          //Place Holder for more trains

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);
delayMicroseconds(500);          //Place Holder for more trains
digitalWrite(in2, LOW);
digitalWrite(in1, HIGH);
delayMicroseconds(500);          //Place Holder for more trains

digitalWrite(in1,LOW);
digitalWrite(in2, HIGH);         //This sets the signal tail back so that the signal does not get too long incase of delays.

}