/*
  Software serial multple serial test

 Receives from the hardware serial, sends to software serial.
 Receives from software serial, sends to hardware serial.

 The circuit:
 * RX is digital pin 2 (connect to TX of other device)
 * TX is digital pin 3 (connect to RX of other device)

 Note:
 Not all pins on the Mega and Mega 2560 support change interrupts,
 so only the following can be used for RX:
 10, 11, 12, 13, 50, 51, 52, 53, 62, 63, 64, 65, 66, 67, 68, 69

 Not all pins on the Leonardo support change interrupts,
 so only the following can be used for RX:
 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI).

 created back in the mists of time
 modified 25 May 2012
 by Tom Igoe
 based on Mikal Hart's example

 This example code is in the public domain.

 */
#include <SoftwareSerial.h>

SoftwareSerial mySerial(15, 15); // RX, TX
//int trigger = 0;  // keeps track of whether button has been pressed
int count = 0;  // counts the number of bytes received
int junk;   // reads junk data from serial
int analogIn;   // input from potentiometer to determine mode selection
void setup()
{
  // Open serial communications and wait for port to open:
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Native USB only
  }


  Serial.println("Goodnight moon!");

  // set the data rate for the SoftwareSerial port
  mySerial.begin(76800);
  //mySerial.println("Hello, world?");
    pinMode(15,INPUT_PULLUP);
}

void loop() // run over and over
{
//  if (mySerial.available()) {
//    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
    //Serial.write(mySerial.read());   // sends the serial data directly to serial monitor which converts it to ascii
    //count++;   // counter was used for timing of servo mimic signal
//  }

 // Serial.println(count);

  //if (Serial.available())
  //  mySerial.write(Serial.read());  // sends the characters typed in serial monitor to myserial output

/*   // SERVO MIMIC SIGNAL was used for testing how the programmer responds to a simulated servo
if (count==6) {    // first part of signal has been received from programmer, now the first servo reply is sent out
    count++; //(instead of resetting the counter it increments so that the send does not repeat)
    pinMode(15,OUTPUT);
    Serial.println("SEND 1");
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(2);
    mySerial.write((byte) 0);
    mySerial.write(252);
    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of next programmer signal
    delay(2); // delay of 2 milliseconds times the second servo reply
    pinMode(15,OUTPUT);
    Serial.println("SEND 2");
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(3);
    mySerial.write((byte)0);
    mySerial.write(1);
    mySerial.write(250);
    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of next programmer signal
}
*/

    analogIn = analogRead(A1);  // Read the potentiometer analog input (the knob to control the servo position)
       //Serial.println(analogIn);

    pinMode(15,OUTPUT);
    digitalWrite(15, LOW);
        delay(10);
    digitalWrite(15, HIGH);
        delayMicroseconds(analogIn+1000);
    digitalWrite(15, LOW);
        delay(10);
    digitalWrite(15, HIGH);
        while (mySerial.available()) {
    junk = mySerial.read();  // reads the data from the buffer without printing it (you get a lot of zeros from the PWM signal)
  }



  if (digitalRead(4)==1) {  // button is newly pressed at pin 4, now the first part of the programmer signal will be sent out
        // You don't need to check for newly pressed unless you are doing something really quick here

    analogIn = analogRead(A2);  // Read the potentiometer analog input (the slider to determine which mode we are going to request)



    pinMode(15,OUTPUT);

/*
     for (int i = 0; i <= 50; i++) {   // holds the servo at the specified PWM value for a period of time
    digitalWrite(15, LOW);
        delay(10);
    digitalWrite(15, HIGH);
        delayMicroseconds(1500);
    digitalWrite(15, LOW);
        delay(1);
     }
    digitalWrite(15, HIGH);
        while (mySerial.available()) {
    junk = mySerial.read();  // reads the data from the buffer without printing it (you get a lot of zeros from the PWM signal)
  } */

    Serial.println();
    Serial.println("SEND 1"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(254);
    mySerial.write(2);
    mySerial.write(1);
    mySerial.write(254);

    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of servo reply with oscilloscope
    delay(2); // delay of 2 milliseconds while the servo reply occurs
  while (mySerial.available()) {
    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
  }
    pinMode(15,OUTPUT);

    Serial.println();
    Serial.println("SEND 2"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(4);
    mySerial.write(2);
    mySerial.write(21);
    mySerial.write(1);
    mySerial.write(226);



    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of servo reply with oscilloscope
    delay(2); // delay of 2 milliseconds while the servo reply occurs
  while (mySerial.available()) {
    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
  }
    pinMode(15,OUTPUT);

    Serial.println();
    Serial.println("SEND 3"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(4);
    mySerial.write(3);
    mySerial.write(52);
    mySerial.write((byte)0);
    mySerial.write(195);

    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of servo reply with oscilloscope
    delay(3); // delay of 3 milliseconds while the servo reply occurs
      while (mySerial.available()) {
    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
  }
    pinMode(15,OUTPUT);

if (analogIn>500) {
    Serial.println();
    Serial.println("SEND 4S"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(47);
    mySerial.write(3);
    mySerial.write(6);
    mySerial.write(2);
    mySerial.write(30);
    mySerial.write((byte)0);
    mySerial.write(5);
    mySerial.write((byte)0);
    mySerial.write(15);
    mySerial.write((byte)0);
    mySerial.write(45);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(15);
    mySerial.write(3);
    mySerial.write(252);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(65);
    mySerial.write(3);
    mySerial.write(197);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(1);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write((byte)0);
    mySerial.write(2);
    mySerial.write(9);
    mySerial.write(196);
    mySerial.write(1);
    mySerial.write(244);
    mySerial.write(3);
    mySerial.write(232);
    mySerial.write((byte)0);
    mySerial.write(1);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(3);
    mySerial.write(232);
    mySerial.write((byte)0);
    mySerial.write(178);
}

 else {
    Serial.println();
    Serial.println("SEND 4C"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(47);
    mySerial.write(3);
    mySerial.write(6);
    mySerial.write(50);
    mySerial.write(20);
    mySerial.write((byte)0);
    mySerial.write(5);
    mySerial.write((byte)0);
    mySerial.write(10);
    mySerial.write((byte)0);
    mySerial.write(10);
    mySerial.write((byte)0);
    mySerial.write(30);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(3);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(65);
    mySerial.write(3);
    mySerial.write(197);
    mySerial.write(1);
    mySerial.write(244);
    mySerial.write(1);
    mySerial.write(255);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(2);
    mySerial.write(6);
    mySerial.write(14);
    mySerial.write(5);
    mySerial.write(170);
    mySerial.write(3);
    mySerial.write(232);
    mySerial.write((byte)0);
    mySerial.write(20);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write((byte)0);
    mySerial.write(3);
    mySerial.write(232);
    mySerial.write((byte)0);
    mySerial.write(153);
 }

    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of servo reply with oscilloscope
    delay(6); // delay of 6 milliseconds while the servo reply occurs
      while (mySerial.available()) {
    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
  }
    pinMode(15,OUTPUT);

    Serial.println();
    Serial.println("SEND 5"); // status report is sent to the serial monitor
    mySerial.write(255);
    mySerial.write(255);
    mySerial.write(1);
    mySerial.write(4);
    mySerial.write(3);
    mySerial.write(52);
    mySerial.write(1);
    mySerial.write(194);

    pinMode(15,INPUT_PULLUP);  // changes the transmit pin to input to enable monitoring of servo reply with oscilloscope
    delay (2);  // allows time for the servo reply
      while (mySerial.available()) {
    Serial.println(mySerial.read(),DEC);  // prints the serial data as decimal numbers (or use BIN for binary with leading zeros removed)
  }
        delay (500);  // allows time for the button to be released before going back to the main loop
  }

}