Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00041 #include <serial.h>
00042 #include <odometry.h>
00043 #include <utility.h>
00044
00045
00046 #include <ypspur.h>
00047
00048 int ad[16];
00049 int ad_num = 0;
00050 int dio_num = 0;
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
00065 ad_num++;
00066 }
00067 return ad_num;
00068 }
00069
00070
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
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
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 }