adinput.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
00020 
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <unistd.h>
00026 
00027 #include <fcntl.h>
00028 #include <sys/stat.h>
00029 #include <sys/time.h>
00030 #include <sys/types.h>
00031 #include <time.h>
00032 
00033 #ifdef HAVE_CONFIG_H
00034 #include <config.h>
00035 #endif  // HAVE_CONFIG_H
00036 
00037 /* ボディパラメータ */
00038 #include <shvel-param.h>
00039 
00040 /* yp-spur用 */
00041 #include <serial.h>
00042 #include <odometry.h>
00043 #include <utility.h>
00044 
00045 /* ライブラリ用 */
00046 #include <ypspur.h>
00047 
00048 int ad[16];       // ad値保存用バッファ
00049 int ad_num = 0;   // adでいくつデータを出力するように指令したか
00050 int dio_num = 0;  // dioでいくつデータを出力するように指令したか
00051 
00052 int process_addata(unsigned char *buf, int len)
00053 {
00054   int j;
00055   int ad_num;
00056 
00057   ad_num = 0;
00058   for (j = 0; j < len; j += 2)
00059   {
00060     Short_2Char data;
00061     data.byte[1] = buf[j];
00062     data.byte[0] = buf[j + 1];
00063     ad[((unsigned short)data.integer) >> 12] = (unsigned short)data.integer & 0x0FFF;
00064     // printf("%d=%d\n",((unsigned short)data.integer) >> 12,(unsigned short)data.integer & 0x0FFF);
00065     ad_num++;
00066   }
00067   return ad_num;
00068 }
00069 
00070 /* read ad data func for ssm write */
00071 const int *get_addataptr()
00072 {
00073   return ad;
00074 }
00075 
00076 int get_addata(int num)
00077 {
00078   if (num < 0)
00079     return 0;
00080   if (num > 15)
00081     return 0;
00082   return ad[num];
00083 }
00084 
00085 int get_ad_num(void)
00086 {
00087   return ad_num;
00088 }
00089 
00090 int get_dio_num(void)
00091 {
00092   return dio_num;
00093 }
00094 
00095 int admask_receive(char *buf, int len, double receive_time, void *data)
00096 {
00097   buf[len] = 0;
00098   strcat((char *)data, buf);
00099   if (strstr((char *)data, "00P\n\n"))
00100     return -1;
00101   if (strstr((char *)data, "\n\n"))
00102     return -2;
00103   return 0;
00104 }
00105 
00106 int set_admask(unsigned char mask)
00107 {
00108   /* Send & Recive Buffer */
00109   char buf[2048];
00110   int i;
00111 
00112   ad_num = 0;
00113   strcpy(buf, "\n\n\n\n");
00114   serial_write(buf, strlen(buf));
00115   yp_usleep(50000);
00116   serial_flush_in();
00117   yp_usleep(50000);
00118   strcpy(buf, "ADMASK");
00119   for (i = 0; i < 8; i++)
00120   {
00121     if (mask & 0x80)
00122     {
00123       ad_num++;
00124       strcat(buf, "1");
00125     }
00126     else
00127       strcat(buf, "0");
00128     mask = mask << 1;
00129   }
00130   strcat(buf, "\n");
00131   serial_write(buf, strlen(buf));
00132 
00133   buf[0] = 0;
00134   if (serial_recieve(admask_receive, buf) == -1)
00135     return 1;
00136 
00137   return 0;
00138 }
00139 
00140 int set_diomask(unsigned char enable)
00141 {
00142   /* Send & Recive Buffer */
00143   char buf[2048];
00144   dio_num = 0;
00145 
00146   strcpy(buf, "\n\n\n\n");
00147   serial_write(buf, strlen(buf));
00148   yp_usleep(50000);
00149   serial_flush_in();
00150   yp_usleep(50000);
00151   strcpy(buf, "GETIO");
00152   if (enable)
00153   {
00154     strcat(buf, "1");
00155     dio_num++;
00156   }
00157   else
00158     strcat(buf, "0");
00159   strcat(buf, "\n");
00160   serial_write(buf, strlen(buf));
00161 
00162   buf[0] = 0;
00163   if (serial_recieve(admask_receive, buf) == -1)
00164     return 1;
00165 
00166   return 0;
00167 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19