urobot_enc_pos.c
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <math.h>
00004 #include <unistd.h>         /* for read, write, close */
00005 #include <sys/ioctl.h>      /* for ioctl */
00006 #include <sys/types.h>      /* for open */
00007 #include <sys/stat.h>       /* for open */
00008 #include <fcntl.h>          /* for open */
00009 
00010 #include <signal.h>
00011 
00012 #include "driver/urbtc.h"          /* Linux specific part */
00013 #include "driver/urobotc.h"        /* OS independent part */
00014 
00015 void exit_program();
00016 
00017 int quit_flag = 1;
00018 
00019 #define numOfControllers 1
00020 static char *devfiles[] = {"/dev/urbtc0"};
00021 
00022 int main(int argc, char **argv)
00023 {
00024   struct uin ibuf;
00025   struct uout obuf[numOfControllers];
00026   struct ccmd cmd;
00027   int fd, fds[numOfControllers];
00028   int i, j;
00029   
00030   signal(SIGINT, exit_program);
00031 
00032   for (i=0; i<numOfControllers; i++) {
00033     if ((fd = open(devfiles[i], O_RDWR)) == -1) {
00034       fprintf(stderr, "%s: Open error\n", devfiles[i]);
00035       exit(1);
00036     }
00037     if (ioctl(fd, URBTC_CONTINUOUS_READ) < 0){
00038       fprintf(stderr, "ioctl: URBTC_CONTINUOUS_READ error\n");
00039       exit(1);
00040     }
00041     if (read(fd, &ibuf, sizeof(ibuf)) != sizeof(ibuf)) {
00042       fprintf(stderr, "Read size mismatch.\n");
00043       exit(1);
00044     }
00045 
00046 #if __BYTE_ORDER == __BIG_ENDIAN
00047     ibuf.magicno = (0xff & ibuf.magicno)<<8 | (0xff00 & ibuf.magicno)>>8;
00048 #endif
00049 
00050     if (ibuf.magicno == 0) {
00051       fds[0] = fd;
00052       fprintf(stderr, "Found controller #0.\n");
00053     } else if (ibuf.magicno == 1) {
00054       fds[1] = fd;
00055       fprintf(stderr, "Found controller #1.\n");
00056     } else {
00057       fprintf(stderr, "Wrong magic no: %d.\n", ibuf.magicno);
00058       exit(1);
00059     }
00060 
00061     cmd.retval = 0 /* RETURN_VAL */;
00062     cmd.setoffset  = CH0 | CH1 | CH2 | CH3;
00063     cmd.setcounter = CH0 | CH1 | CH2 | CH3;
00064     cmd.resetint   = CH0 | CH1 | CH2 | CH3;
00065 
00066     cmd.selin = CH0 | CH1 | SET_SELECT; /* AD in:ch0,ch1    ENC in:ch2,ch3*/
00067     cmd.selout = SET_SELECT | CH0 | CH1 | CH2 | CH3; /*  PWM out:ch0,ch1,ch2,ch3*/
00068 
00069 #if __BYTE_ORDER == __LITTLE_ENDIAN
00070     cmd.offset[0] = cmd.offset[1] = cmd.offset[2] = cmd.offset[3] = 0x7fff;
00071     cmd.counter[0] = cmd.counter[1] = cmd.counter[2] = cmd.counter[3] = 0;
00072 #else
00073     cmd.offset[0] = cmd.offset[1] = cmd.offset[2] = cmd.offset[3] = 0xff7f;
00074     cmd.counter[0] = cmd.counter[1] = cmd.counter[2] = cmd.counter[3] = 0;
00075 #endif
00076 
00077     cmd.posneg = SET_POSNEG | CH0| CH1 | CH2 | CH3; /*POS PWM out*/
00078     cmd.breaks = SET_BREAKS | CH0 | CH1 | CH2 | CH3; /*No Brake*/
00079 
00080     if (ioctl(fd, URBTC_COUNTER_SET) < 0){
00081       fprintf(stderr, "ioctl: URBTC_COUNTER_SET error\n");
00082       exit(1);
00083     }
00084     if (write(fd, &cmd, sizeof(cmd)) < 0) {
00085       fprintf(stderr, "write error\n");
00086       exit(1);
00087     }
00088     if (ioctl(fd, URBTC_DESIRE_SET) < 0){
00089       fprintf(stderr, "ioctl: URBTC_DESIRE_SET error\n");
00090       exit(1);
00091     }
00092   }
00093 
00094   for (j=0; j<numOfControllers; j++) {
00095     for (i=0; i<4; i++) {
00096 #if __BYTE_ORDER == __LITTLE_ENDIAN
00097       obuf[j].ch[i].x = 0;
00098       obuf[j].ch[i].d = 0;
00099       obuf[j].ch[i].kp = 0;
00100       obuf[j].ch[i].kpx = 1;
00101       obuf[j].ch[i].kd = 0;
00102       obuf[j].ch[i].kdx = 1;
00103       obuf[j].ch[i].ki = 0;
00104       obuf[j].ch[i].kix = 1;
00105 #else
00106       obuf[j].ch[i].x = 0;
00107       obuf[j].ch[i].d = 0;
00108       obuf[j].ch[i].kp = 0;
00109       obuf[j].ch[i].kpx = 0x0100;
00110       obuf[j].ch[i].kd = 0;
00111       obuf[j].ch[i].kdx = 0x0100;
00112       obuf[j].ch[i].ki = 0;
00113       obuf[j].ch[i].kix = 0x0100;
00114 #endif
00115     }
00116 #if __BYTE_ORDER == __LITTLE_ENDIAN
00117     obuf[j].ch[2].kp = 0x10;
00118     obuf[j].ch[3].kp = 0x10;
00119 #else
00120     obuf[j].ch[2].kp = 0x1000;
00121     obuf[j].ch[3].kp = 0x1000;
00122 #endif
00123   }
00124 
00125   i = 0;
00126   while(quit_flag) {
00127     unsigned short a = 300.0*sin(i*3.14/655.360);// + 512.0;
00128     a <<= 5;
00129     for (j=0; j<numOfControllers; j++) {
00130 
00131 #if __BYTE_ORDER == __LITTLE_ENDIAN
00132       obuf[j].ch[2].x = obuf[j].ch[3].x = a;
00133 #else
00134       obuf[j].ch[2].x = obuf[j].ch[3].x = ((a & 0xff) << 8 | (a & 0xff00) >> 8);
00135 #endif
00136         printf("%x\r\n",obuf[j].ch[3].x);
00137 
00138       if (write(fds[j], &obuf[j], sizeof(obuf[j])) > 0) {
00139         i++;
00140       } else {
00141         printf("write err\n");
00142         break;
00143       }
00144     }
00145   }
00146 
00147   for (i=0; i<numOfControllers; i++)
00148     close(fds[i]);
00149 
00150   return 0;
00151 }
00152 
00153 void exit_program(sig, code, scp, addr)
00154 int sig;
00155 int code;
00156 struct sigcontext *scp;
00157 char *addr;
00158 {
00159   quit_flag = 0;
00160   fprintf(stderr, "kill signal is received\n");
00161 }


cirkit_unit03_driver
Author(s): CIR-KIT
autogenerated on Thu Jun 6 2019 21:08:20