hexapod_ng/legacy/linux/adtest/floortouch.c

#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <fcntl.h>
#include <termios.h>

#include <sys/time.h>

#include "servo.h"
#include "spi.h"

#include "ik.h"

void ik_to_servos(bot *);
int get_time();

int ad_fd;

static bot idle_position = {
   {0,0,-20},  // body position
   {0,0,0},     // body rotation

   { // leg positions
      {{ 166, 130,   0},}, // leg 0
      {{   0, 180,   0},}, // leg 1        
      {{-166, 130,   0},}, // ...
      {{-166,-130,   0},},
      {{   0,-180,   0},},
      {{ 166,-130,   0},}
   }
};


int
main(int argc, char **argv) {
   struct termios t;
   int flags_stdio;

   int quit=0;
   int frame=0;
   int t_frame_start, t_frame_end;

   int dump_fd;

   char c;
   int i,j, int_z;
   int leg;
   unsigned char adbuffer[12];
   unsigned char ad_max[6]={0,0,0,0,0,0};
   unsigned char ad_min[6]={255,255,255,255,255,255};
   unsigned char ad;
   
   load_calibration("calibration.bin");

   spi_open(0,33000000);

   ad_fd=open("/dev/ttyS2",O_RDWR);
   if(ad_fd<0) {
      printf("can't open /dev/ttyS2.");
      exit(2);
   }

   tcgetattr(0,&t);
   t.c_lflag&=~(ICANON|ECHO);
   tcsetattr(0,TCSANOW,&t);

   tcgetattr(ad_fd,&t);
   t.c_lflag&=~(ICANON|ECHO);
   tcsetattr(ad_fd,TCSANOW,&t);

   flags_stdio=fcntl(0, F_GETFL,0);
   flags_stdio|=O_NONBLOCK;
   fcntl(0, F_SETFL,flags_stdio);

   ik(&idle_position);
   ik_to_servos(&idle_position);

   for(j=0;j<200;j++) {
     write(ad_fd,&c,1);
     read(ad_fd,&adbuffer,12);
     for(i=0;i<6;i++) {
        if(i==adbuffer[2*i]) {
           if(adbuffer[2*i+1]>10) {
              if(ad_min[i]>adbuffer[2*i+1]) ad_min[i]=(adbuffer[2*i+1]+ad_min[i])/2;
           }
        }
     }
   }
   leg=0;
                     
   while(!quit) {
      t_frame_start=get_time();
      write(ad_fd,&c,1);
      read(ad_fd,&adbuffer,12);

      for(i=0;i<6;i++) {
         if(i==adbuffer[2*i]) {
            ad=adbuffer[2*i+1];
            if((ad-ad_min[i])<10) {
               if(idle_position.leg[i].position.z<100) idle_position.leg[i].position.z+=1;
            } else if((ad-ad_min[i])>20) {
               if(idle_position.leg[i].position.z>0) idle_position.leg[i].position.z-=1;
            }
         }
      }
      
      if(read(0,&c,1)==1) {
         switch(c) {
            case 27: 
            case 'q':
               quit=1;
               break;
         }
      }
      ik(&idle_position);
      t_frame_end=get_time();
      if(t_frame_end-t_frame_start>20000) {
         printf("frame %d slack %d\n",frame,t_frame_end-t_frame_start);
      }
      while(t_frame_end<t_frame_start+20000) t_frame_end=get_time();
      ik_to_servos(&idle_position);
      frame++;
   }
   close(ad_fd);
   spi_close();

   flags_stdio=fcntl(0, F_GETFL,0); flags_stdio&=~(O_NONBLOCK); fcntl(0,
   F_SETFL,flags_stdio);


   tcgetattr(0,&t);
   t.c_lflag|=(ICANON|ECHO|ECHOE|ISIG);
   tcsetattr(0,TCSANOW,&t);
            
   return 0;
}

void
ik_to_servos(bot *b) {
   int i,j;
   
   for(i=0;i<NUM_LEGS;i++) {
      for(j=0;j<3;j++) {
         if(!isnan(b->leg[i].ik_angle[j])) {
            servo_pwm[i*3+j]=b->leg[i].ik_angle[j]*8.5*1800.0/M_PI+servo_offsets[i*3+j];
         }
      }
   }
   spi_update_servos();
}


int
get_time() {
   struct timeval t;
   gettimeofday(&t,NULL);
   return (t.tv_sec*1000000+t.tv_usec);
   
}