// Include software serial
#include <SoftwareSerial.h>

// Create instance of software serial
SoftwareSerial pololu(RXPIN, TXPIN);

#define RXPIN 7
#define TXPIN 8

// Setup loop
void setup()
{
  pinMode(RXPIN, INPUT);
  pinMode(TXPIN, OUTPUT);
  Serial.begin(11400); //Serial to USB 
  pololu.begin(19200); //Serial to Pololu Trex
  delay(400);
}

// Main application loop


void loop()
{
  if (Serial.available())
  {
    if (Serial.read()=='H') // Waits for activation from USB
    {
      Serial.println("Received");
      runmotor();
      delay(50);
      runmotor();
      delay(50);
      runmotor();
      delay(50);

            
    }
    
  }
  
}


void runmotor()
{
  pololu.write(0xC2); //send byte to target motor 1 forward
  pololu.write(0x7F); //set motor 1 speed
  
  //pololu.write(0xC9); //send byte to target motor 2 forward
  //pololu.write(0x7F); //set motor 2 speed
  Serial.println("Sent to Trex");
}