/*
 * File:          move.c
 * Description:   Moves a UMI-RTX robotic arm to joint values specified in
a file, continuously.
 * Warning:       Will not exit unless it fails to read the file (or gets
killed)
 * Author:        Gijs Kruitbosch; Edited by Bruno Jakic for multiple
joint coordinates
 * Last modified: 2006-06-30
 * Course:        Search, Navigate, Actuate (Zoeken, Sturen en Bewegen),
June 2006
 */

#include <stdio.h>
#include <unistd.h>
#include "umi.h"
#include <time.h>

/* Uncomment for debug messages:
 * #define MOVE_DEBUG
 */
#define COUPLED 0

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

int x = 10;
while(x==10)
{
    FILE *f;

    if((f = fopen("/home/bjakic/dump/joints.txt","r")) != NULL)
      {
        char s[1000];
        double zed, shoulder, elbow, yaw, pitch, roll, grip;
        time_t mytime = time(0);

        if (f)
        while (fgets(s,1000,f)!=NULL)
        {
        printf("Taking 7 doubles from the file
/home/bjakic/dump/joints.txt ...\n", asctime(localtime(&mytime)));
        printf("%s",s);
        int i = sscanf(s, "%lf %lf %lf %lf %lf %lf %lf", &zed, &shoulder,
&elbow, &yaw, &pitch, &roll, &grip);

        printf("========NEXTMOVE:\n");
        printf("Zed: %lf\n", zed);
        printf("Shoulder: %lf\n", shoulder);
        printf("Elbow: %lf\n", elbow);
        printf("Yaw: %lf\n", yaw);
        printf("Pitch: %lf\n", pitch);
        printf("Roll: %lf\n", roll);
        printf("Grip: %lf\n", grip);
        umi_move(zed, shoulder, elbow, yaw, pitch, roll, grip);
        sleep(6);
        }
        fclose(f);
        remove("/home/bjakic/dump/joints.txt");
      }
  sleep(10);
}
return 0;

}