/*
* Linux_Servo_FT639 is a Linux servo control program for the FT639 chip.
*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termio.h>
#include <termios.h>
#include <signal.h>

/* Debug output */
#define DEBUG 1

/* Microseconds to pause for one byte at 2400 bps */
#define pause 4200 

/* Command codes für FT639 */
#define setup 122
#define shortpulse 85
#define longpulse 90
#define header 96
#define active 117

/* store terminal setting */  		      
struct termios old_options, new_options;

/* Devicehandle */
int SerialDevice;


int SendByte(char byte, int SerialDevice)
  {
  /* send one byte and wait */
  int result;

  result = write(SerialDevice, &byte, 1); 
  usleep(pause);
  return(result);
  }  

int OpenPort(int PortNumber)
  {
  /* Open the serial port */
  switch (PortNumber)
    {
    case 0: SerialDevice = open ("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY); break;
    case 1: SerialDevice = open ("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY); break;
    case 2: SerialDevice = open ("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NDELAY); break;
    case 3: SerialDevice = open ("/dev/ttyS3", O_RDWR | O_NOCTTY | O_NDELAY); break;
    default: SerialDevice = -1;
    }
  if (SerialDevice < 0)
    {
    printf("\nCan't open the serial port: %d\n",SerialDevice);
    return(0);
    }

  /* Get the Original port settings */
  tcgetattr(SerialDevice, &old_options);

  /* Specify new port settings: BAUDRATE=2400, 8 BITS, No Parity, 1 Stopbit */
  new_options.c_cflag = (B2400 | CS8 | CLOCAL);  // raw input = no ICANON

  /* Specify raw data (deselect some settings) */
  new_options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);   

  /* Activate the new port settings */
  tcsetattr(SerialDevice, TCSANOW, &new_options); // TCSAFLUSH

  /* Put the chip in setup mode */
  SendByte(setup,SerialDevice);

  /* if needed ... */
  //  SendByte(shortpulse,SerialDevice); 
  //  SendByte(longpulse,SerialDevice);

  /* Activate the chip */
  SendByte(active,SerialDevice);
#ifdef DEBUG
  printf("\nSerial Device bereit.\n");
#endif
  sleep(1);
  return(1);
  }


void SetServo(int Servo, int Pos, int SerialDevice)
  {
  unsigned char LowByte, HiByte;
  /* Check borders */
  if (Pos < 0) Pos = 0;
  if (Pos > 255) Pos = 255;
  /* Calculate command data */
  LowByte = ((Servo - 1) << 4) + (Pos & 0x0f);
  HiByte  = ((Servo - 1) << 4) + (Pos >> 4) + (1 << 7);
  SendByte(LowByte,SerialDevice);
  SendByte(HiByte,SerialDevice);
#ifdef DEBUG
  printf("Low byte = %02X, High Byte = %02X\r\n", LowByte, HiByte);
#endif
  }


void CenterAll(int SerialDevice)
  {
  int i;
  for (i=1; i <= 5; i++)
    {
    SetServo(i, 128, SerialDevice);
    /* Give them 50 ms to set Position */
    usleep(50000);
    }
  }


int Scanbetrieb(int nservo, int low, int high, int SerialDevice)
  {
  /* Servo im Scanningmode betreiben */
  int i;

  printf("\nZum Beenden CTRL-C druecken\n");
    
  while(1)
    {    /* Endless loop */
    for (i = low; i <= high; i++)
      {
      SetServo(nservo, i, SerialDevice);
      usleep(50000);
      }
    for(i = high; i >= low; i--)
      {
      SetServo(nservo, i, SerialDevice);
      usleep(50000);
      }
    }
  }


void Handbetrieb(int nservo, int SerialDevice)
  {
  /* Servo von Hand steuern */
  int step = 0;

  printf("\nZum Beenden -1 eingeben ...\n");

  while(step >= 0)
    {       
    printf("Zielposition 0 to 255: ");
    scanf("%d", &step);
    if (step < 0) return;
    if (step > 255) step = 255;
    SetServo(nservo, step, SerialDevice);
    usleep(50000);
    }
  }

void ClosePort(void)
  {
  /* FT639 in den Setup mode schalten and Device schliessen */
#ifdef DEBUG
  printf("\nSetup-Mode setzen und Schnittstelle schliessen.\n");
#endif
  SendByte(setup,SerialDevice);
  tcsetattr(SerialDevice, TCSANOW, &old_options); 
  close(SerialDevice);
  exit(0);
  }


int main (int argc, char *argv[])
  {

  char control_mode = '1';
  int low, high, nservo;

  /* Seriellen Port oeffnen */
  if(! OpenPort(0)) exit(1);

  /* Signal-Handler setzen */
  signal(SIGINT, ClosePort);
  signal(SIGTERM, ClosePort);
  /* Modus waehlen */
  printf("Überwachungsmodus [1] oder Handsteuerung [2]:\n");
  scanf("%c", &control_mode);
  switch (control_mode)
    {
    case '1': 
      printf("\nStarte Überwachungsmodus\n");
      /* Center all servos */      
      CenterAll(SerialDevice);

      printf("Untere Auslenkung eingeben [0 to 100]:\n");      
      scanf("%d", &low);
      if (low > 100)
         {
         printf("Wert > 100, verwende 100\n");
         low = 100;
         }
      printf("Obere Auslenkung eingeben [0 to 255]:\n");      
      scanf("%d", &high);
      if(high > 255) high = 255;
      if (high < low)
         {
         printf("Wert kleiner %d, verwende 255\n", low);
         high=255;
         }
      printf("Servo auswählen: 1 - 5: ");
      scanf("%d", &nservo);

      Scanbetrieb(nservo, low, high, SerialDevice);  
      break;
   case '2':
      printf("Servo auswählen: 1 - 5: ");
      scanf("%d", &nservo);
      printf("\nStarte Handsteuerung\n");

      Handbetrieb(nservo, SerialDevice);
      break;
   default: printf("Bitte 1 oder 2 eingeben.\n");
   }
  ClosePort();
  }