/*
 This program is used for an Arduino to receive and decode PELCO-D PTZ Commands

 Thanks to Michael Blaylock for his sketch.  Learning how to read and procces serial data

 */

#include <LiquidCrystal.h>

  LiquidCrystal lcd(12, 11, 9, 8, 7, 6);

  byte outArray[7];        // read data Pelco Command
  unsigned stopcheck;      // For checking when a STOP command is received (257 Decimal)
  int checksum;            // For Calculating Checksum. Sum of the payload bytes (bytes 2 through 6) in the message
  int ByteNumber;
  int MotorSpeed;

void setup(){

  lcd.begin(16, 2);        // set up the LCD's number of columns and rows:

  Serial.begin(9600);      // baud rate  9600 can be 1200,2400 or 4800

  pinMode(3,  OUTPUT);
  pinMode(5,  OUTPUT);
  pinMode(13, OUTPUT);

  digitalWrite(3, HIGH);
  digitalWrite(5, HIGH);

}

void displayData()          // Display the array in serial monitor for debugging
{
  for (int j = 0; j<7; j+=1){
  Serial.println(outArray[j],HEX);}
  Serial.println("Data Printed");}

void loop()
{
  if ( Serial.available () > 0) {
       outArray[ByteNumber ++] = Serial.read();}

  if ( ByteNumber > 6){         // process it
       ByteNumber = 0;          // ready for next time
 //    displayData();           // for debugging

       stopcheck = outArray[0] + outArray[1] + outArray[2] + outArray[3] + outArray[4] + outArray[5] + outArray[6] ;   // Calculate if STOP Command is received
  if ( stopcheck == 257){       // When stopcheck is 257 decimal a STOP command is received
       StopActions();}          // Stop all PTZ Actions

 else{ printCommand();}         // Print Pelco command on LCD

  if ( bitRead(outArray[3],0) == 0 ){   // When BIT 0 = 0 command 2 than data is Normal command (PTZ)
       Decoderen();}                    // Try to decode the Pelco Command

  if ( bitRead(outArray[3],0) == 1 ){   // When BIT 0 = 1 command 2 than data is an Extended command
       ExtendedCommands();}             // Try to decode the Extended Pelco Command

}  // end if full
}  // end of loop


void Decoderen()
{

 lcd.setCursor(0,1);

      MotorSpeed = map (outArray[4],  0, 0x3F, 255, 0);
 // PAN TILT:
  if ( bitRead(outArray[3],1) == 1 ){
      analogWrite(3 , MotorSpeed);
      // digitalWrite(3, HIGH);
      lcd.print("RIGHT SPEED: ");
      lcd.print(outArray[4]);}

  if ( bitRead(outArray[3],2) == 1 ){
      analogWrite(5, MotorSpeed);
      // digitalWrite(5, HIGH);
      lcd.print("LEFT  SPEED: ");
      lcd.print(outArray[4]);}

  if ( bitRead(outArray[3],3) == 1 ){
      lcd.print("UP    SPEED: ");
      lcd.print(outArray[5]);}

  if ( bitRead(outArray[3],4) == 1 ){
      lcd.print("DOWN  SPEED: ");
      lcd.print(outArray[5]);}

 // ZOOM IRIS FOCUS:
  if ( bitRead(outArray[2],2) == 1 ){
      lcd.print("Iris Close");}
  if ( bitRead(outArray[2],1) == 1 ){
      lcd.print("Iris Open");}
  if ( bitRead(outArray[2],0) == 1 ){
      lcd.print("Focus Near");}
  if ( bitRead(outArray[3],7) == 1 ){
      lcd.print("Focus Far ");}
  if ( bitRead(outArray[3],6) == 1 ){
      lcd.print("Zoom Wide ");}
  if ( bitRead(outArray[3],5) == 1 ){
      lcd.print("Zoom Tele ");}

}

void ExtendedCommands()
{

     lcd.setCursor(0,1);

  if ( outArray[2] == 0 ){         // Only continu when Word 3 is 0

  if ( outArray[3] == 0x03 ){      // SET PRESET
     lcd.print("Set Preset: ");
     lcd.print(outArray[5]-1);}    // PRINT Preset. -1 to calculate right preset

  if ( outArray[3] == 0x05 ){      // Clear Preset
     lcd.print("Clear Preset:");
     lcd.print(outArray[5]-1);}    // PRINT Preset. -1 to calculate right preset

}}