Kuon  1.1.3
RoadNarrows Robotics Large Outdoor Mobile Robot Project
RS160DControl.c
Go to the documentation of this file.
1 ////////////////////////////////////////////////////////////////////////////////
2 //
3 // Package: Kuon
4 //
5 // library: libRS160D
6 //
7 // File: RS160DControl.c
8 //
9 /*! \file
10  *
11  * $LastChangedDate: 2010-08-20 11:36:38 -0600 (Fri, 20 Aug 2010) $
12  * $Rev: 568 $
13  *
14  * \brief Funtions for serial control of RS160D motor controller
15  *
16  * \author Robin Knight (robin.knight@roadnarrows.com)
17  * \author Rob Shiely (rob@roadnarrows.com)
18  * \author Daniel Packard (daniel@roadnarrows.com)
19  *
20  * \copyright
21  * \h_copy 2009-2017. RoadNarrows LLC.\n
22  * http://www.roadnarrows.com\n
23  * All Rights Reserved
24  */
25 // Permission is hereby granted, without written agreement and without
26 // license or royalty fees, to use, copy, modify, and distribute this
27 // software and its documentation for any purpose, provided that
28 // (1) The above copyright notice and the following two paragraphs
29 // appear in all copies of the source code and (2) redistributions
30 // including binaries reproduces these notices in the supporting
31 // documentation. Substantial modifications to this software may be
32 // copyrighted by their authors and need not follow the licensing terms
33 // described here, provided that the new terms are clearly indicated in
34 // all files where they apply.
35 //
36 // IN NO EVENT SHALL THE AUTHOR, ROADNARROWS LLC, OR ANY MEMBERS/EMPLOYEES
37 // OF ROADNARROW LLC OR DISTRIBUTORS OF THIS SOFTWARE BE LIABLE TO ANY
38 // PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
39 // DAMAGES ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
40 // EVEN IF THE AUTHORS OR ANY OF THE ABOVE PARTIES HAVE BEEN ADVISED OF
41 // THE POSSIBILITY OF SUCH DAMAGE.
42 //
43 // THE AUTHOR AND ROADNARROWS LLC SPECIFICALLY DISCLAIM ANY WARRANTIES,
44 // INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
45 // FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
46 // "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO
47 // PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
48 //
49 ////////////////////////////////////////////////////////////////////////////////
50 
51 //
52 // TODO: get speed,
53 // TODO: encoder value
54 // TODO: ...
55 //
56 
57 #include <stdlib.h>
58 #include <string.h>
59 #include <dirent.h>
60 
61 #include "Kuon/RS160DControl.h"
62 #include "rnr/log.h"
63 
64 static int KMotDevCnt = 0;
65 static char *KMotDev[2] = {NULL, NULL};
66 
67 static int RS160GlobFilter(const struct dirent *name)
68 {
69  //printf("%s\n", name->d_name);
70 
71  if( !strncmp(name->d_name, "kmot", strlen("kmot")) )
72  {
73  return 1;
74  }
75  else
76  {
77  return 0;
78  }
79 }
80 
81 static void RS160Glob()
82 {
83  struct dirent **namelist;
84  int n;
85  int i;
86 
87  n = scandir("/dev", &namelist, RS160GlobFilter, alphasort);
88 
89  for(i=0; i<n && i<2; ++i)
90  {
91  KMotDev[i] = (char *)malloc(5+strlen(namelist[i]->d_name)+1);
92  sprintf(KMotDev[i], "/dev/%s", namelist[i]->d_name);
93  //fprintf(stderr, "DBG: [%d] %s\n", i, KMotDev[i]);
94  }
95 
96  free(namelist);
97 
98  KMotDevCnt = i;
99 }
100 
101 
102 int RS160DOpenConnection( const char *Dev,
103  int *Descriptor) {
104  if( KMotDevCnt == 0 )
105  {
106  RS160Glob();
107  }
108 
109  if( !strcmp(Dev, "0") && (KMotDevCnt >= 1) )
110  {
111  Dev = KMotDev[0];
112  }
113  else if( !strcmp(Dev, "1") && (KMotDevCnt >= 2) )
114  {
115  Dev = KMotDev[1];
116  }
117  *Descriptor = SerDevOpen(Dev, BAUDRATE, 8, 'N', 1, false, false);
118  if(*Descriptor < 0) {
119  return -1;
120  }
121  return 0;
122 }
123 
124 int WriteToSerial(const char *ControlString, int Descriptor) {
125  size_t Length;
126  ssize_t err;
127  Length = strlen(ControlString);
128  err = SerDevWrite(Descriptor, (byte_t *)ControlString, Length, 0);
129  if(err < 0) {
130  return -1;
131  }
132  if(err == 0) {
133  return -2;
134  }
135  if(err != Length) {
136  return -3;
137  }
138  return 0;
139 }
140 
141 void SerWriteErrCheck( int err){
142  if(err == -1) {
143  fprintf(stderr, "Complete serial write failure.\n");
144  }
145  if(err == -2) {
146  fprintf(stderr, "Nothing Written.\n");
147  }
148  if(err == -3) {
149  fprintf(stderr, "Incorrect number of bytes written.\n");
150  }
151 }
152 
153 int RS160DSetToSerial(int Descriptor) {
154  int err;
155  err = WriteToSerial( SETLEFTSERIAL, Descriptor);
156  if(err < 0) {
157  fprintf(stderr, "Failed to set left motor to serial control.\n");
158  SerWriteErrCheck(err);
159  return -1;
160  }
161  err = WriteToSerial( SETRIGHTSERIAL, Descriptor);
162  if(err < 0) {
163  fprintf(stderr, "Failed to set right motor to serial control.\n");
164  SerWriteErrCheck(err);
165  return -1;
166  }
167  err = WriteToSerial( SETLEFTPWM, Descriptor);
168  if(err < 0) {
169  fprintf(stderr, "Failed to set left motor to PWM mode.\n");
170  SerWriteErrCheck(err);
171  return -1;
172  }
173  err = WriteToSerial( SETRIGHTPWM, Descriptor);
174  if(err < 0) {
175  fprintf(stderr, "Failed to set right motor to PWM mode.\n");
176  SerWriteErrCheck(err);
177  return -1;
178  }
179  return 0;
180 }
181 
182 int RS160DUpdateMotorSpeeds(int Speed, int Descriptor, int Side) {
183  char BuffToMotor[20];
184  int err;
185  if(Speed > 250) {
186  fprintf(stderr, "Speed (%i) too high.\n",
187  Speed);
188  return -1;
189  }
190  if(Speed < -250) {
191  fprintf(stderr, "Speed (%i) too low.\n",
192  Speed);
193  return -1;
194  }
195  sprintf(BuffToMotor, "@%dst%d\r",
196  Side, Speed);
197  err = WriteToSerial(BuffToMotor, Descriptor);
198  if(err < 0) {
199  fprintf(stderr, "\nFailed to update motor speed.\n");
200  SerWriteErrCheck(err);
201  return -2;
202  }
203  return 0;
204 }
205 
206 int RS160DAlterBraking(int Braking, int Descriptor, int Side) {
207  char BuffToMotor[20];
208  int err;
209  if(Braking > 31) {
210  fprintf(stderr, "Brake rate (%i) too high.\n",
211  Braking);
212  return -1;
213  }
214  if(Braking < 0) {
215  fprintf(stderr, "Brake rate (%i) too low.\n",
216  Braking);
217  return -1;
218  }
219  sprintf(BuffToMotor, "@%dsB%d\r",
220  Side, Braking);
221  err = WriteToSerial(BuffToMotor, Descriptor);
222  if(err < 0) {
223  fprintf(stderr, "\nFailed to update brake rate.\n");
224  SerWriteErrCheck(err);
225  return -2;
226  }
227  return 0;
228 }
229 
230 int RS160DAlterSlew(int Slew, int Descriptor, int Side) {
231  char BuffToMotor[20];
232  int err;
233  if(Slew > 90) {
234  fprintf(stderr, "Slew rate (%i) too high.\n",
235  Slew);
236  return -1;
237  }
238  if(Slew < 0) {
239  fprintf(stderr, "Slew rate (%i) too low.\n",
240  Slew);
241  return -1;
242  }
243  sprintf(BuffToMotor, "@%dsS%d\r",
244  Side, Slew);
245  err = WriteToSerial(BuffToMotor, Descriptor);
246  if(err < 0) {
247  fprintf(stderr, "\nFailed to update slew rate.\n");
248  SerWriteErrCheck(err);
249  return -2;
250  }
251  return 0;
252 }
253 
254 void RS160DEStop(int DescriptorFront, int DescriptorRear){
255  WriteToSerial( "@0sS0\r", DescriptorFront );
256  WriteToSerial( "@1sS0\r", DescriptorFront );
257  WriteToSerial( "@0sS0\r", DescriptorRear );
258  WriteToSerial( "@1sS0\r", DescriptorRear );
259  WriteToSerial( "@0sB31\r", DescriptorFront );
260  WriteToSerial( "@1sB31\r", DescriptorFront );
261  WriteToSerial( "@0sB31\r", DescriptorRear );
262  WriteToSerial( "@1sB31\r", DescriptorRear );
263  WriteToSerial( "@0st0\r", DescriptorFront );
264  WriteToSerial( "@1st0\r", DescriptorFront );
265  WriteToSerial( "@0st0\r", DescriptorRear );
266  WriteToSerial( "@1st0\r", DescriptorRear );
267 }
268 
269 int RS160DClose(int Descriptor) {
270  int error;
271  SerDevFIFOOutputFlush(Descriptor);
272  error = SerDevClose(Descriptor);
273  if(error == -1) {
274  fprintf(stderr, "\nFailed to close device.\n");
275  return -1;
276  }
277  return 0;
278 }
279