   }
 }
 
+// Write a command (to RPi)
+
+void writeCommand(String cmd) {
+ 
+   String cmdMessage = "CMD " + cmd;
+   
+   // USB debugging
+   Serial.println(cmdMessage);
+   
+   // RPi
+   Serial1.println(cmdMessage);
+  
+}
+
 // Output current status
 
 void writeStatus() {

int distance_slowdown = 150;
int distance_rear_slowdown = 75;
int speed_slow = 60;

int shutdown_pi = 0;

// This is where you declare prototypes for the functions that will be 
// using the TinyGPS library.


void shutdownPi()
{
  writeCommand("SHUTDOWN");
}

void turnLeft(int power)
