├── .gitignore
├── README.md
├── brbrain
    ├── .gitignore
    ├── build.gradle
    └── src
    │   └── java
    │       └── brbrain
    │           ├── AX12Register.java
    │           ├── AXRegister.java
    │           ├── AXS1Register.java
    │           └── BRBrain.java
├── firmware
    ├── CBUF.h
    ├── brbrain.c
    ├── makefile
    └── makefile_AVR.inc
└── maven
    └── brbrain
        └── brbrain
            ├── 1.0
                ├── brbrain-1.0.jar
                ├── brbrain-1.0.jar.md5
                ├── brbrain-1.0.jar.sha1
                ├── brbrain-1.0.pom
                ├── brbrain-1.0.pom.md5
                └── brbrain-1.0.pom.sha1
            ├── maven-metadata.xml
            ├── maven-metadata.xml.md5
            └── maven-metadata.xml.sha1


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Compiled Object files
 2 | *.slo
 3 | *.lo
 4 | *.o
 5 | 
 6 | # Compiled Dynamic libraries
 7 | *.so
 8 | 
 9 | # Compiled Static libraries
10 | *.lai
11 | *.la
12 | *.a
13 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | BRBrain
 2 | =======
 3 | 
 4 | The **B**ioloid **R**emote **Brain**, developped by the [Northeastern University][], provides a firmware and a Java host library for controlling the [Bioloid][]. 
 5 | With BRBrain, Java code running on a host PC controls the Bioloid via a serial tether or a Bluetooth link.
 6 | 
 7 | The aim of this github repository is to centralize the code developped by the [Northeastern University][] so as to share it more easilly with the community but also to provide addionnal increments.
 8 | 
 9 | ## BrBrain Firmware
10 | 
11 | The Firmware has been updated and now supports both CM-5 and CM-510 platforms.
12 | 
13 | [Northeastern University]: http://www.ccs.neu.edu/research/gpc/BRBrain/ "Northeastern University"
14 | [Bioloid]: http://www.robotis.com/xe/bioloid_en "Bioloid"


--------------------------------------------------------------------------------
/brbrain/.gitignore:
--------------------------------------------------------------------------------
 1 | # Gradle
 2 | *.build.gradle.swp
 3 | *.gradle
 4 | 
 5 | # Eclipse
 6 | *.classpath
 7 | .project
 8 | .settings/
 9 | 
10 | # Java
11 | build
12 | 
13 | 


--------------------------------------------------------------------------------
/brbrain/build.gradle:
--------------------------------------------------------------------------------
 1 | apply plugin: 'java'
 2 | apply plugin: 'eclipse'
 3 | apply plugin: 'maven'
 4 | 
 5 | group = 'brbrain'
 6 | version = '1.0'
 7 | 
 8 | repositories {
 9 |     mavenLocal()
10 |     mavenCentral()
11 | }
12 | 
13 | dependencies {
14 |     compile group: 'org.rxtx', name: 'rxtx', version: '2.1.7'
15 | }
16 | 
17 | sourceSets {
18 |     main {
19 |         java {
20 |             srcDir 'src/java'
21 |         }
22 |         resources {
23 |             srcDir 'src/resources'
24 |         }
25 |     }
26 | }
27 | 
28 | // Github Maven deployement
29 | 
30 | def localMavenRepo = 'file://' + new File('../maven/').absolutePath
31 | 
32 | uploadArchives {
33 |     repositories {
34 |         mavenDeployer {
35 |             repository(url: localMavenRepo)
36 |         }
37 |     }
38 | }
39 | 
40 | task deploySnapshot(dependsOn: 'uploadArchives')
41 | task deployRelease(dependsOn: 'uploadArchives')
42 | 
43 | gradle.taskGraph.whenReady { tg ->
44 | 	if (tg.hasTask(':deploySnapshot')) {
45 | 		version += '-SNAPSHOT'
46 |         }
47 | }
48 | 


--------------------------------------------------------------------------------
/brbrain/src/java/brbrain/AX12Register.java:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * <p>Represents the Dynamixel AX-12 registers in a high-level way.</p>
  3 |  *
  4 |  * <p>Copyright (C) 2007 Marsette A. Vona, III</p>
  5 |  *
  6 |  * <p>This program is free software; you can redistribute it and/or modify it
  7 |  * under the terms of the GNU General Public License as published by the Free
  8 |  * Software Foundation; either version 2 of the License, or (at your option)
  9 |  * any later version.</p>
 10 |  *
 11 |  * <p>This program is distributed in the hope that it will be useful, but
 12 |  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 13 |  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 14 |  * more details.</p>
 15 |  *
 16 |  * <p>You should have received a copy of the GNU General Public License along
 17 |  * with this program; if not, write to the Free Software Foundation, Inc., 59
 18 |  * Temple Place - Suite 330, Boston, MA 02111-1307, USA.</p>
 19 |  **/
 20 | 
 21 | package brbrain;
 22 | 
 23 | import java.util.*;
 24 | import java.lang.reflect.*;
 25 | 
 26 | /**
 27 |  * <p>Represents the Dynamixel AX-12 registers in a high-level way.</p>
 28 |  *
 29 |  * <p>Copyright (C) 2008 Marsette A. Vona, III</p>
 30 |  *
 31 |  * @author Marsette (Marty) A. Vona, III
 32 |  **/
 33 | public class AX12Register extends AXRegister {
 34 |   
 35 |   /** the Dynamixel type identifier **/
 36 |   public static final String DYNAMIXEL_TYPE = "AX12";
 37 | 
 38 |   /** nominal battery voltage in Volts (8xNiMH) = (8x1.2V) = 9.6V **/
 39 |   public static final float NOMINAL_BATTERY_VOLTAGE = 9.6f;
 40 | 
 41 |   /**
 42 |    * <p>Multiplication factor taking {@link #AX12_MOVING_SPEED} raw value to
 43 |    * position counts per millisecond.</p>
 44 |    *
 45 |    * <pre>
 46 |    * Tested both @12.4V and @9.8V
 47 |    * 500-&gt;400 @ 20: 3s  (f = 100/(3000*20) = 100/60000 = 1/600)
 48 |    * 500-&gt;400 @ 10: 6s  (f = 100/(6000*10) = 100/60000 = 1/600)
 49 |    * 500-&gt;400 @  5: 11s (f = 100/(11000*5) = 100/55000 = 1/550)
 50 |    * 500-&gt;400 @  2: 25s (f = 100/(25000*2) = 100/50000 = 1/500)
 51 |    * 500-&gt;400 @  1: 49s (f = 100/(49000*1) = 100/49000 = 1/490)
 52 |    * </pre>
 53 |    **/
 54 |   public static final double MOVING_SPEED_TO_COUNTS_PER_MS =
 55 | //  AX12_MOVING_SPEED.naturalUnitsPerCount* //rev/min
 56 | //  (1.0/60000.0)*                        //min/ms
 57 | //  (1.0/AX12_GOAL_POSITION.naturalUnitsPerCount)* //count/deg
 58 | //  360.0; //deg/rev
 59 |   1.0/550.0;
 60 | 
 61 |   /** next register ordinal during class init **/
 62 |   protected static int nextOrdinal = 0;
 63 | 
 64 |   /** dynamixel model number (RO) **/
 65 |   public static final AX12Register AX12_MODEL_NUMBER =
 66 |     new AX12Register(nextOrdinal++, "model number", 0, 2);
 67 | 
 68 |   /** dynamixel firmware version (RO) **/
 69 |   public static final AX12Register AX12_FIRMWARE_VERSION =
 70 |     new AX12Register(nextOrdinal++, "firmware version", 2);
 71 | 
 72 |   /** dynamixel ID (RW) **/
 73 |   public static final AX12Register AX12_ID =
 74 |     new AX12Register(nextOrdinal++, "id", 3, 0, MAX_DYNAMIXEL_ID);
 75 |   
 76 |   /**
 77 |    * <p>Dynamixel Baud Rate (RW), natural units [kbits/sec].</p>
 78 |    *
 79 |    * <p>See table in dynamixel docs for standard rates.</p>
 80 |    *
 81 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
 82 |    **/
 83 |   public static final AX12Register AX12_BAUD_RATE =
 84 |     new AX12Register(nextOrdinal++, "baud rate", 4, 0, 254, "kbps", true) {
 85 |     
 86 |     public float toNaturalUnits(int value) {
 87 |       return (float) (2.0e3/(value+1));
 88 |     }
 89 |     
 90 |     public int fromNaturalUnits(float value) {
 91 |       return (int) Math.round((2.0e3/value)-1.0);
 92 |     }
 93 |   };
 94 | 
 95 |   /**
 96 |    * <p>Dynamixel return delay time (RW), natural units [usec].</p>
 97 |    *
 98 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
 99 |    **/
100 |   public static final AX12Register AX12_RETURN_DELAY_TIME =
101 |     new AX12Register(nextOrdinal++, "return delay time",
102 |                      5, 0, 254, 2.0f, "us", true);
103 |   
104 |   /** clockwise angle limit (RW), natural units [deg] **/
105 |   public static final AX12Register AX12_CW_ANGLE_LIMIT =
106 |     new AX12Register(nextOrdinal++, "cw angle limit",
107 |                      6, 2, 0, 1023, 300.0f/1023.0f, "deg", false);
108 |   
109 |   /** counter-clockwise angle limit (RW), natural units [deg] **/
110 |   public static final AX12Register AX12_CCW_ANGLE_LIMIT =
111 |     new AX12Register(nextOrdinal++, "ccw angle limit",
112 |                      8, 2, 0, 1023, 300.0f/1023.0f, "deg", false);
113 |   
114 |   /** limit temp (RW), natural units [deg Celcius] **/
115 |   public static final AX12Register AX12_HIGHEST_LIMIT_TEMPERATURE =
116 |     new AX12Register(nextOrdinal++, "highest limit temperature",
117 |                      11, 0, 150, 1.0f, "C", true);
118 |   
119 |   /** low limit voltage (RW), natural units [Volts] **/
120 |   public static final AX12Register AX12_LOWEST_LIMIT_VOLTAGE =
121 |     new AX12Register(nextOrdinal++, "lowest limit voltage",
122 |                      12, 50, 250, 0.1f, "V", true);
123 |   
124 |   /** high limit voltage (RW), natural units [Volts] **/
125 |   public static final AX12Register AX12_HIGHEST_LIMIT_VOLTAGE =
126 |     new AX12Register(nextOrdinal++, "highest limit voltage",
127 |                      13, 50, 250, 0.1f, "V", true);
128 |   
129 |   /**
130 |    * <p>Maximum torque (RW), natural units normalized to [0.0, 1.0].</p>
131 |    *
132 |    * <p>According to dynamixel manual, max torque 0 enables "Free Run" mode,
133 |    * whatever that is.  Also note that the dynamixel appears to copy this
134 |    * value to {@link #AX12_TORQUE_LIMIT} at boot and may not respect changes to
135 |    * it (or to {@link #AX12_TORQUE_LIMIT}?) until the next boot.</p>
136 |    *
137 |    * <p>It is unclear how the dynamixel measures "torque" and "load" (current
138 |    * sensing? or based only on servo error?), and calibration relating to
139 |    * physical units is TBD.</p>
140 |    **/
141 |   public static final AX12Register AX12_MAX_TORQUE =
142 |     new AX12Register(nextOrdinal++, "max torque",
143 |                      14, 2, 0, 1023, 1.0f/1023.0f, "", false);
144 |   
145 |   /**
146 |    * <p>Dynamixel status return level (RW).</p>
147 |    *
148 |    * <p>0 means no return packets, 1 means return packets only for
149 |    * READ_DATA, 2 means return packets always.</p>
150 |    *
151 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
152 |    **/
153 |   public static final AX12Register AX12_STATUS_RETURN_LEVEL =
154 |     new AX12Register(nextOrdinal++, "status return level", 16, 1, 0, 2);
155 |   
156 |   /** bitmask of <code>E_*</code> constants to trigger the LED (RW) **/
157 |   public static final AX12Register AX12_ALARM_LED =
158 |     new AX12Register(nextOrdinal++, "alarm led", 17, 0, 127);
159 |   
160 |   /** bitmask of <code>E_*</code> constants to trigger torque off (RW) **/
161 |   public static final AX12Register AX12_ALARM_SHUTDOWN =
162 |     new AX12Register(nextOrdinal++, "alarm shutdown", 18, 0, 127);
163 |   
164 |   /** undocumented pot calibration (RO) **/
165 |   public static final AX12Register AX12_DOWN_CALIBRATION =
166 |     new AX12Register(nextOrdinal++, "down calibration", 20, 2);
167 |   
168 |   /** undocumented pot calibration (RO) **/
169 |   public static final AX12Register AX12_UP_CALIBRATION =
170 |     new AX12Register(nextOrdinal++, "up calibration", 22, 2);
171 |   
172 |   /** enables torque generation (RW), boolean, positive logic **/
173 |   public static final AX12Register AX12_TORQUE_ENABLE =
174 |     new AX12Register(nextOrdinal++, "torque enable", 24, 0, 1);
175 |   
176 |   /** direct LED control (RW), boolean, positive logic **/
177 |   public static final AX12Register AX12_LED =
178 |     new AX12Register(nextOrdinal++, "led", 25, 0, 1);
179 |   
180 |   /**
181 |    * <p>Clockwise compliance margin (RW), natural units normalized to [0.0,
182 |    * 1.0].</p>
183 |    *
184 |    * <p>This is the angular slop allowed before current is applied.</p>
185 |    *
186 |    * <p>Calibration relating to physical units is TBD.</p>
187 |    **/
188 |   public static final AX12Register AX12_CW_COMPLIANCE_MARGIN =
189 |     new AX12Register(nextOrdinal++, "cw compliance margin",
190 |                      26, 0, 254, 1.0f/254.0f, "", false);
191 |   
192 |   /**
193 |    * <p>Counter-clockwise compliance margin (RW), natural units normalized to
194 |    * [0.0, 1.0].</p>
195 |    *
196 |    * <p>This is the angular slop allowed before current is applied.</p>
197 |    *
198 |    * <p>Calibration relating to physical units is TBD.</p>
199 |    **/
200 |   public static final AX12Register AX12_CCW_COMPLIANCE_MARGIN =
201 |     new AX12Register(nextOrdinal++, "ccw compliance margin",
202 |                      27, 0, 254, 1.0f/254.0f, "", false);
203 |   
204 |   /**
205 |    * <p>Clockwise compliance slope (RW), natural units normalized to [0.0,
206 |    * 1.0].</p>
207 |    *
208 |    * <p>This appears to be 1.0 minus the P-gain.</p>
209 |    *
210 |    * <p>Actual calibration relating to physical units of rotational stiffness
211 |    * depends on operating voltage.</p>
212 |    *
213 |    * <p>In an example, the dynamixel documentation states that only the
214 |    * integer part of the base-2 log of this value is significant.</p>
215 |    **/
216 |   public static final AX12Register AX12_CW_COMPLIANCE_SLOPE =
217 |     new AX12Register(nextOrdinal++, "cw compliance slope",
218 |                      28, 1, 254, "", false) {
219 |     
220 |     public float toNaturalUnits(int value) {
221 |       return ((float) (value-1))*1.0f/253.0f;
222 |     }
223 |     
224 |     public int fromNaturalUnits(float value) {
225 |       return (int) Math.round(value*253.0f+1.0f);
226 |     }
227 |   };
228 |   
229 |   /**
230 |    * <p>Counter-clockwise compliance slope (RW), natural units normalized to
231 |    * [0.0, 1.0].</p>
232 |    *
233 |    * <p>This appears to be 1.0 minus the P-gain.</p>
234 |    *
235 |    * <p>Actual calibration relating to physical units of rotational stiffness
236 |    * depends on operating voltage.</p>
237 |    *
238 |    * <p>In an example, the dynamixel documentation states that only the
239 |    * integer part of the base-2 log of this value is significant.</p>
240 |    **/
241 |   public static final AX12Register AX12_CCW_COMPLIANCE_SLOPE =
242 |     new AX12Register(nextOrdinal++, "ccw compliance slope",
243 |                      29, 1, 254, "", false) {
244 | 
245 |     public float toNaturalUnits(int value) {
246 |       return ((float) (value-1))*1.0f/253.0f;
247 |     }
248 | 
249 |     public int fromNaturalUnits(float value) {
250 |       return (int) Math.round(value*253.0f+1.0f);
251 |     }
252 |   };
253 | 
254 |   /**
255 |    * <p>Goal position (RW), natural units [deg].</p>
256 |    *
257 |    * <p>Servo is centered when goal position is at center of range.</p>
258 |    **/
259 |   public static final AX12Register AX12_GOAL_POSITION =
260 |     new AX12Register(nextOrdinal++, "goal position",
261 |                      30, 2, 0, 1023, 300.0f/1023.0f, "deg", false);
262 | 
263 |   /**
264 |    * <p>Moving speed (RW), natural units [rev/min].</p>
265 |    *
266 |    * <p>It appears that in position control mode this is effectively a(n
267 |    * unsigned) speed <i>limit</i>, with the limit disabled when this is set
268 |    * to zero.</p>
269 |    *
270 |    * <p>Setting both {@link #AX12_CW_ANGLE_LIMIT} and {@link
271 |    * #AX12_CCW_ANGLE_LIMIT} to zero appears to switch the servo to velocity
272 |    * control mode, where the value of this register is the <i>signed</i> goal
273 |    * velocity.</p>
274 |    **/
275 |   public static final AX12Register AX12_MOVING_SPEED =
276 |     new AX12Register(nextOrdinal++, "moving speed",
277 |                      32, 2, -1023, 1023, true, 114.0f/1023.0f, "rpm", false);
278 | 
279 |   /** See {@link #AX12_MAX_TORQUE} **/
280 |   public static final AX12Register AX12_TORQUE_LIMIT =
281 |     new AX12Register(nextOrdinal++, "torque limit",
282 |                      34, 2, 0, 1023, 1.0f/1023.0f, "", false);
283 | 
284 |   /** Current position (RO), natural units [deg] **/
285 |   public static final AX12Register AX12_PRESENT_POSITION =
286 |     new AX12Register(nextOrdinal++, "present position",
287 |                      36, 2, 300.0f/1023.0f, "deg", false);
288 | 
289 |   /** Current speed (RO), natural units [rev/min] **/
290 |   public static final AX12Register AX12_PRESENT_SPEED =
291 |     new AX12Register(nextOrdinal++, "present speed",
292 |                      38, 2, true, 114.0f/1023.0f, "rpm", false);
293 | 
294 |   /**
295 |    * <p>Current "load" (RO), natural units normalized to [0.0, 1.0].</p>
296 |    *
297 |    * <p>It is unclear whether "load" is the same as "torque" here.</p>
298 |    *
299 |    * <p>Calibration to physical units is TBD.</p>
300 |    **/
301 |   public static final AX12Register AX12_PRESENT_LOAD =
302 |     new AX12Register(nextOrdinal++, "present load",
303 |                      40, 2, true, 1.0f/1023.0f, "", false);
304 | 
305 |   /** Current voltage (RO), natural units [Volts] **/
306 |   public static final AX12Register AX12_PRESENT_VOLTAGE =
307 |     new AX12Register(nextOrdinal++, "present voltage", 42, 0.1f, "V", true);
308 | 
309 |   /** Current temperature (RO), natural units [degrees Celcius] **/
310 |   public static final AX12Register AX12_PRESENT_TEMPERATURE =
311 |     new AX12Register(nextOrdinal++, "present temperature",
312 |                      43, 1.0f, "C", true);
313 | 
314 |   /** Whether there is a registered instruction pending (RW), boolean **/
315 |   public static final AX12Register AX12_REGISTERED_INSTRUCTION =
316 |     new AX12Register(nextOrdinal++, "registered instruction", 44, 0, 1);
317 | 
318 |   /** Whether the servo is currentl moving (RO), boolean **/
319 |   public static final AX12Register AX12_MOVING =
320 |     new AX12Register(nextOrdinal++, "moving", 46);
321 | 
322 |   /**
323 |    * <p>Whether to restrict writing to registers {@link #AX12_TORQUE_ENABLE}
324 |    * through {@link #AX12_TORQUE_LIMIT} (RW), boolean.</p>
325 |    *
326 |    * <p>Once this is set it becomes immutable until power cycle.</p>
327 |    **/
328 |   public static final AX12Register AX12_LOCK =
329 |     new AX12Register(nextOrdinal++, "lock", 47, 0, 1);
330 | 
331 |   /**
332 |    * <p>Initial current to apply after the position error has exceeded the
333 |    * compliance margin (RW), natural units normalized to [0.0, 1.0].</p>
334 |    *
335 |    * <p>Calibration to physical units TBD.</p>
336 |    **/
337 |   public static final AX12Register AX12_PUNCH =
338 |     new AX12Register(nextOrdinal++, "punch",
339 |                      48, 2, 0, 1023, 1.0f/1023.0f, "", false);
340 | 
341 |   /**
342 |    * <p><i>Virtual</i> register containing the error status of the dynamixel
343 |    * (RO).</p>
344 |    *
345 |    * <p>The error status is a bitfield of the <code>E_*</code> bits.</p> 
346 |    **/
347 |   public static final AX12Register AX12_ERROR =
348 |     new AX12Register(nextOrdinal++, "error", 54);
349 | 
350 |   private static final String svnid =
351 |   "$Id: AX12Register.java 24 2008-05-02 22:08:41Z vona $";
352 | 
353 |   /** the array of {@link AX12Register}s, indexed by ordinal **/
354 |   protected static AX12Register[] registers;
355 | 
356 |   /** total number of registers **/
357 |   public static final int NUM_REGISTERS;
358 | 
359 |   /** first register in RAM **/
360 |   public static final AXRegister FIRST_REGISTER;
361 | 
362 |   /** first register in RAM **/
363 |   public static final AXRegister FIRST_RAM_REGISTER;
364 | 
365 |   static {
366 | 
367 |     ArrayList<AX12Register> regs = new ArrayList<AX12Register>();
368 | 
369 |     Field[] fields = AX12Register.class.getDeclaredFields();
370 | 
371 |     String prefix = DYNAMIXEL_TYPE.toUpperCase();
372 | 
373 |     try {
374 |       for (Field f : fields)
375 |         if ((f.getType() == AX12Register.class) &&
376 |             (f.getName().startsWith(prefix)))
377 |           regs.add((AX12Register) (f.get(null)));
378 |     } catch (IllegalAccessException e) {
379 |       throw new RuntimeException(e);
380 |     }
381 | 
382 |     Collections.sort(regs, new Comparator<AX12Register>() {
383 |         public int compare(AX12Register r1, AX12Register r2) {
384 | 
385 |           //negative integer, zero, or a positive integer as the first argument
386 |           //is less than, equal to, or greater than the second.
387 | 
388 |           if (r1.ordinal < r2.ordinal)
389 |             return -1;
390 |           else if (r1.ordinal > r2.ordinal)
391 |             return +1;
392 |           else
393 |             return 0;
394 |         }
395 |       });
396 | 
397 |     NUM_REGISTERS = regs.size();
398 |     registers = regs.toArray(new AX12Register[NUM_REGISTERS]);
399 |     
400 |     FIRST_REGISTER = registers[0];
401 | 
402 |     int ri = 0;
403 |     for (; ri < registers.length; ri++) {
404 |       if (registers[ri].startAddr >= RAM_START_ADDRESS) {
405 |         break;
406 |       }
407 |     }
408 |     
409 |     FIRST_RAM_REGISTER = registers[ri];
410 |   }
411 | 
412 |   /**
413 |    * <p>Get the stiffness of an AX-12 actuator in units of kg*cm/deg.</p>
414 |    *
415 |    * <p>Calibrated by physical experiment.</p>
416 |    *
417 |    * @param complianceSlope the compliance slope register value which
418 |    * determines the stiffness
419 |    * @param voltage the operating voltage of the servo or NaN to use {@link
420 |    * #NOMINAL_BATTERY_VOLTAGE}
421 |    **/
422 |   public static float getCalibratedStiffness(int complianceSlope,
423 |                                              float voltage) {
424 | 
425 |     if (Float.isNaN(voltage))
426 |       voltage = NOMINAL_BATTERY_VOLTAGE;
427 | 
428 |     float stiffnessAt1 = interp(voltage, 4.3f, 10.0f, 8.2f, 12.2f);
429 |     float stiffnessAt254 = 0.3f;
430 | 
431 |     return interp(complianceSlope, stiffnessAt1, 1.0f, stiffnessAt254, 254.0f);
432 |   }
433 | 
434 |   /**
435 |    * <p>Linearly interpolate between the points (abscissaAtLowEnd,
436 |    * valueAtLowEnd) and (abscissaAtHighEnd, valueAtHighEnd).</p>
437 |    **/
438 |   public static float interp(float abscissa,
439 |                              float valueAtLowEnd,
440 |                              float abscissaAtLowEnd,
441 |                              float valueAtHighEnd,
442 |                              float abscissaAtHighEnd) {
443 |     return
444 |       valueAtLowEnd+
445 |       (abscissa-abscissaAtLowEnd)*
446 |       (valueAtHighEnd-valueAtLowEnd)/(abscissaAtHighEnd-abscissaAtLowEnd);
447 |   }
448 |                              
449 | 
450 |   /** 
451 |    * <p>Covers {@link #getCalibratedStiffness(int, float)}, uses {@link
452 |    * #NOMINAL_BATTERY_VOLTAGE}.</p>
453 |    **/
454 |   public static float getCalibratedStiffness(int complianceSlope) {
455 |     return getCalibratedStiffness(complianceSlope, Float.NaN);
456 |   }
457 | 
458 |   /** writeable reg with all options **/
459 |   protected AX12Register(int ordinal, String prettyName,
460 |                          int startAddr, int width,
461 |                          int min, int max,
462 |                          boolean signMagnitude11Bit,
463 |                          float naturalUnitsPerCount,
464 |                          String naturalUnitsLabel,
465 |                          boolean useNaturalUnitsByDefault) {
466 |   super(ordinal, prettyName,
467 |         startAddr, width,
468 |         min, max,
469 |         signMagnitude11Bit, naturalUnitsPerCount, naturalUnitsLabel,
470 |         useNaturalUnitsByDefault);
471 |   }
472 | 
473 |   /** read-only reg with all options **/
474 |   protected AX12Register(int ordinal, String prettyName,
475 |                          int startAddr, int width,
476 |                          boolean signMagnitude11Bit,
477 |                          float naturalUnitsPerCount,
478 |                          String naturalUnitsLabel,
479 |                          boolean useNaturalUnitsByDefault) {
480 |   super(ordinal, prettyName,
481 |         startAddr, width,
482 |         signMagnitude11Bit, naturalUnitsPerCount, naturalUnitsLabel,
483 |         useNaturalUnitsByDefault);
484 |   }
485 | 
486 |   /** writeable unsigned reg **/
487 |   protected AX12Register(int ordinal, String prettyName,
488 |                          int startAddr, int width,
489 |                          int min, int max,
490 |                          float naturalUnitsPerCount,
491 |                          String naturalUnitsLabel,
492 |                          boolean useNaturalUnitsByDefault) {
493 |     this(ordinal, prettyName,
494 |          startAddr, width,
495 |          min, max,
496 |          false,
497 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
498 |   }
499 | 
500 |   /** writeable unsigned reg, natural units are counts **/
501 |   protected AX12Register(int ordinal, String prettyName,
502 |                          int startAddr, int width, int min, int max) {
503 |     this(ordinal, prettyName, startAddr, width, min, max, 1.0f, "", false); 
504 |   }
505 | 
506 |   /** writeable unsigned 1-byte reg, natural units are counts **/
507 |   protected AX12Register(int ordinal, String prettyName,
508 |                          int startAddr, int min, int max,
509 |                          String naturalUnitsLabel,
510 |                          boolean useNaturalUnitsByDefault) {
511 |     this(ordinal, prettyName,
512 |          startAddr, 1, min, max, 1.0f,
513 |          naturalUnitsLabel, useNaturalUnitsByDefault);
514 |   }
515 | 
516 |   /**
517 |    * <p>Read-only unsigned reg (be careful of confusion with {@link
518 |    * #AX12Register(int, String, int, int, int, String, boolean)}).</p>
519 |    **/
520 |   protected AX12Register(int ordinal, String prettyName,
521 |                          int startAddr, int width,
522 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
523 |                          boolean useNaturalUnitsByDefault) {
524 |     this(ordinal, prettyName,
525 |          startAddr, width, false,
526 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
527 |   }
528 | 
529 |   /** read-only unsigned reg, natural units are counts **/
530 |   protected AX12Register(int ordinal, String prettyName,
531 |                          int startAddr, int width) {
532 |     this(ordinal, prettyName, startAddr, width, 1.0f, "", false);
533 |   }
534 | 
535 |   /** writeable unsigned 1-byte reg **/
536 |   protected AX12Register(int ordinal, String prettyName,
537 |                          int startAddr,
538 |                          int min, int max,
539 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
540 |                          boolean useNaturalUnitsByDefault) {
541 |     this(ordinal, prettyName,
542 |          startAddr, 1, min, max,
543 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
544 |   }
545 | 
546 |   /** writeable unsigned 1-byte reg, natural units are coutns **/
547 |   protected AX12Register(int ordinal, String prettyName,
548 |                          int startAddr, int min, int max) {
549 |     this(ordinal, prettyName, startAddr, min, max, 1.0f, "", false);
550 |   }
551 | 
552 |   /** read-only unsigned 1-byte reg **/
553 |   protected AX12Register(int ordinal, String prettyName,
554 |                          int startAddr,
555 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
556 |                          boolean useNaturalUnitsByDefault) {
557 |     this(ordinal, prettyName,
558 |          startAddr, 1,
559 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
560 |   }
561 |   
562 |   /** read-only unsigned 1-byte reg, natural units are counts **/
563 |   protected AX12Register(int ordinal, String prettyName, int startAddr) {
564 |     this(ordinal, prettyName, startAddr, 1.0f, "", false);
565 |   }
566 |   
567 |   /** get the register with the given relative ordinal **/
568 |   public AXRegister getRelativeRegister(int offset) {
569 |     return registers[ordinal+offset];
570 |   }
571 | 
572 |   /** get the total number of AX12Registers **/
573 |   public int getNumRegisters() {
574 |     return NUM_REGISTERS;
575 |   }
576 | 
577 |   /** returns {@link #DYNAMIXEL_TYPE} **/
578 |   protected String getDynamixelType() {
579 |     return DYNAMIXEL_TYPE;
580 |   }
581 | 
582 |   /** get the {@link #FIRST_REGISTER} **/
583 |   public static AXRegister getFirstRegister() {
584 |     return FIRST_REGISTER;
585 |   }
586 | 
587 |   /** get the {@link #FIRST_RAM_REGISTER} **/
588 |   public static AXRegister getFirstRAMRegister() {
589 |     return FIRST_RAM_REGISTER;
590 |   }
591 | 
592 |   /** get the AX12Register at the specified ordinal **/
593 |   public static AXRegister getRegister(int ordinal) {
594 |     return registers[ordinal];
595 |   }
596 | 
597 |   /** get a copy of the ordered array of AX12Registers **/
598 |   public static AX12Register[] getAllRegisters(AX12Register[] regs,
599 |                                                int start) {
600 |     if ((regs == null) || (regs.length < (start+NUM_REGISTERS)))
601 |       regs = new AX12Register[start+NUM_REGISTERS];
602 |     System.arraycopy(registers, 0, regs, start, NUM_REGISTERS);
603 |     return regs;
604 |   }
605 | 
606 |   /** covers {@link #getAllRegisters(AX12Register[], int)}, starts at 0 **/
607 |   public static AX12Register[] getAllRegisters(AX12Register[] regs) {
608 |     return getAllRegisters(regs, 0);
609 |   }
610 | 
611 |   /** covers {@link #getAllRegisters(AX12Register[], int)}, always conses **/
612 |   public static AX12Register[] getAllRegisters() {
613 |     return getAllRegisters(null, 0);
614 |   }
615 | }
616 | 


--------------------------------------------------------------------------------
/brbrain/src/java/brbrain/AXRegister.java:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * <p>Represents the Dynamixel AX registers in a high-level way.</p>
  3 |  *
  4 |  * <p>Copyright (C) 2007 Marsette A. Vona, III</p>
  5 |  *
  6 |  * <p>This program is free software; you can redistribute it and/or modify it
  7 |  * under the terms of the GNU General Public License as published by the Free
  8 |  * Software Foundation; either version 2 of the License, or (at your option)
  9 |  * any later version.</p>
 10 |  *
 11 |  * <p>This program is distributed in the hope that it will be useful, but
 12 |  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 13 |  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 14 |  * more details.</p>
 15 |  *
 16 |  * <p>You should have received a copy of the GNU General Public License along
 17 |  * with this program; if not, write to the Free Software Foundation, Inc., 59
 18 |  * Temple Place - Suite 330, Boston, MA 02111-1307, USA.</p>
 19 |  **/
 20 | 
 21 | package brbrain;
 22 | 
 23 | /**
 24 |  * <p>Represents the Dynamixel AX registers in a high-level way.</p>
 25 |  *
 26 |  * <p>Multi-byte registers are handled as single values, write control and
 27 |  * value clamping is provided, encoding and decoding from the 11 bit sign
 28 |  * magnitude representation used by some AX-12 registers is handled, and
 29 |  * translation to and from natural fractional units is included.</p>
 30 |  *
 31 |  * <p>Copyright (C) 2008 Marsette A. Vona, III</p>
 32 |  *
 33 |  * @author Marsette (Marty) A. Vona, III
 34 |  **/
 35 | public abstract class AXRegister {
 36 | 
 37 |   private static final String svnid =
 38 |   "$Id: AXRegister.java 24 2008-05-02 22:08:41Z vona $";
 39 | 
 40 |   /** maximum Dynamixel ID **/
 41 |   public static final int MAX_DYNAMIXEL_ID = 253;
 42 | 
 43 |   /** start address of RAM **/
 44 |   public static final int RAM_START_ADDRESS = 24;
 45 | 
 46 |   /** Dynamixel error bit **/
 47 |   public static final int E_INPUT_VOLTAGE = (1<<0);
 48 | 
 49 |   /** Dynamixel error bit (goal pos out of range) **/
 50 |   public static final int E_ANGLE_LIMIT =    (1<<1);
 51 | 
 52 |   /** Dynamixel error bit **/
 53 |   public static final int E_OVERHEATING =    (1<<2);
 54 | 
 55 |   /** Dynamixel error bit (write parameter out of range) **/
 56 |   public static final int E_RANGE =          (1<<3);
 57 | 
 58 |   /** Dynamixel error bit (comm failure) **/
 59 |   public static final int E_CHECKSUM =       (1<<4);
 60 | 
 61 |   /** Dynamixel error bit (insufficient torque) **/
 62 |   public static final int E_OVERLOAD =       (1<<5);
 63 | 
 64 |   /** Dynamixel error bit (invalid instruction) **/
 65 |   public static final int E_INSTRUCTION =    (1<<6);
 66 | 
 67 |   /** verify that (start, n) is a valid span of registers **/
 68 |   public static void checkSpan(AXRegister start, int n) {
 69 | 
 70 |     if (start == null)
 71 |       throw new IllegalArgumentException("null start");
 72 | 
 73 |     if (n < 0)
 74 |       throw new IllegalArgumentException("negative n");
 75 | 
 76 |     int startOrdinal = start.ordinal;
 77 | 
 78 |     if ((startOrdinal+n) > start.getNumRegisters())
 79 |       throw new IllegalArgumentException("span beyond last register");
 80 |   }
 81 | 
 82 |   /** check whether a span of registers contains any which are read-only **/
 83 |   public static boolean containsReadOnlyRegs(AXRegister start, int n) {
 84 | 
 85 |     checkSpan(start, n);
 86 | 
 87 |     for (int i = 0; i < n; i++)
 88 |       if (!(start.getRelativeRegister(i).writeable))
 89 |         return true;
 90 |     
 91 |     return false;
 92 |   }
 93 | 
 94 |   /** duplicate first n element of from into to, reallocating as necessary **/
 95 |   public static AXRegister[] dup(AXRegister[] from, AXRegister[] to, int n) {
 96 |     to = ensureCapacity(to, n);
 97 |     System.arraycopy(from, 0, to, 0, n);
 98 |     return to;
 99 |   }
100 | 
101 |   /**
102 |    * <p>Covers {@link #dup(AXRegister[], AXRegister[], int)}, copies all.</p>
103 |    **/
104 |   public static AXRegister[] dup(AXRegister[] from, AXRegister[] to) {
105 |     return dup(from, to, from.length);
106 |   }
107 | 
108 |   /** make sure <i>a</i> is at least length <i>n</i> **/
109 |   public static AXRegister[] ensureCapacity(AXRegister[] a, int n) {
110 |     if ((a == null) || (a.length < n))
111 |       a = new AXRegister[n];
112 |     return a;
113 |   }
114 | 
115 |   /**
116 |    * <p>Return span of <i>n</i> regs from <i>start</i>.</p>
117 |    *
118 |    * <p>Span is returned in <i>span</i> iff it's big enough, else a new
119 |    * array.</p>
120 |    *
121 |    * <p>If <i>n</i> is less than 0 then the span includes all subsequent
122 |    * registers.</p>
123 |    **/
124 |   public static AXRegister[] span(AXRegister start, int n, AXRegister[] span) {
125 |     
126 |     if (n < 0)
127 |       n = start.getNumRegisters() - start.ordinal;
128 | 
129 |     span = ensureCapacity(span, n);
130 | 
131 |     for (int i = 0; i < n; i++)
132 |       span[i] = start.getRelativeRegister(i);
133 | 
134 |     return span;
135 |   }
136 | 
137 |   /** covers {@link #span(AXRegister, int, AXRegister[])}, always conses **/
138 |   public static AXRegister[] span(AXRegister start, int n) {
139 |     return span(start, n, null);
140 |   }
141 | 
142 |   /** covers {@link #span(AXRegister, int, AXRegister[])}, spans till end **/
143 |   public static AXRegister[] span(AXRegister start, AXRegister[] span) {
144 |     return span(start, -1, span);
145 |   }
146 | 
147 |   /** covers {@link #span(AXRegister, int)}, spans till end **/
148 |   public static AXRegister[] span(AXRegister start) {
149 |     return span(start, null);
150 |   }
151 | 
152 |   /**
153 |    * <p>Linearly interpolate between the points (abscissaAtLowEnd,
154 |    * valueAtLowEnd) and (abscissaAtHighEnd, valueAtHighEnd).</p>
155 |    **/
156 |   public static float interp(float abscissa,
157 |                              float valueAtLowEnd,
158 |                              float abscissaAtLowEnd,
159 |                              float valueAtHighEnd,
160 |                              float abscissaAtHighEnd) {
161 |     return
162 |       valueAtLowEnd+
163 |       (abscissa-abscissaAtLowEnd)*
164 |       (valueAtHighEnd-valueAtLowEnd)/(abscissaAtHighEnd-abscissaAtLowEnd);
165 |   }
166 |                              
167 |   /**
168 |    * <p>Compose a human-readable string of AX-12 errors from a Dynamixel error
169 |    * bitfield.</p>
170 |    *
171 |    * @param errors the bitpacked error value
172 |    * @param buf the buffer to append, or null to make one
173 |    *
174 |    * @return the buffer to which the string was appended
175 |    **/
176 |   public static final StringBuffer errorsToString(int errors,
177 |                                                   StringBuffer buf) {
178 |     if (buf == null)
179 |       buf = new StringBuffer();
180 | 
181 |     int n = 0;
182 | 
183 |     if ((errors & E_INPUT_VOLTAGE) != 0) {
184 |       if (n++ > 0)
185 |         buf.append(", ");
186 |       buf.append("INPUT_VOLTAGE");
187 |     }
188 | 
189 |     if ((errors & E_ANGLE_LIMIT) != 0) {
190 |       if (n++ > 0)
191 |         buf.append(", ");
192 |       buf.append("ANGLE_LIMIT");
193 |     }
194 | 
195 |     if ((errors & E_OVERHEATING) != 0) {
196 |       if (n++ > 0)
197 |         buf.append(", ");
198 |       buf.append("OVERHEATING");
199 |     }
200 |     
201 |     if ((errors & E_RANGE) != 0) {
202 |       if (n++ > 0)
203 |         buf.append(", ");
204 |       buf.append("RANGE");
205 |     }
206 |     
207 |     if ((errors & E_CHECKSUM) != 0) {
208 |       if (n++ > 0)
209 |         buf.append(", ");
210 |       buf.append("CHECKSUM");
211 |     }
212 |     
213 |     if ((errors & E_OVERLOAD) != 0) {
214 |       if (n++ > 0)
215 |         buf.append(", ");
216 |       buf.append("OVERLOAD");
217 |     }
218 | 
219 |     if ((errors & E_INSTRUCTION) != 0) {
220 |       if (n++ > 0)
221 |         buf.append(", ");
222 |       buf.append("INSTRUCTION");
223 |     }
224 | 
225 |     return buf;
226 |   }
227 | 
228 |   /** Covers {@link #errorsToString(int, StringBuffer)} **/
229 |   public static final String errorsToString(int errors) {
230 |     return errorsToString(errors, null).toString();
231 |   }
232 | 
233 |   /** zero-based index of this register in the register bank **/
234 |   public final int ordinal;
235 | 
236 |   /** pretty human-readable register name **/
237 |   public final String prettyName;
238 | 
239 |   /** start (byte) address of this register in the AX-12 register space **/
240 |   public final int startAddr;
241 | 
242 |   /** byte width of this register **/
243 |   public final int width;
244 | 
245 |   /** whether this register can be written **/
246 |   public final boolean writeable;
247 | 
248 |   /** minimum valid write value of this register, if writeable **/
249 |   public final int min;
250 | 
251 |   /** maximum valid write value of this register, if writeable **/
252 |   public final int max;
253 | 
254 |   /** human-readable label for the natural units, if any **/
255 |   public final String naturalUnitsLabel;
256 | 
257 |   /** whether to prefer natural or raw units by default **/
258 |   public final boolean useNaturalUnitsByDefault;
259 | 
260 |   /**
261 |    * <p>Whether this is a 2-byte little endian reg with a 10 bit magnitude
262 |    * and the sign in bit 11.</p>
263 |    *
264 |    * <p>When the sign bit is set the value is interpreted as negative or
265 |    * clocwise.</p>
266 |    **/
267 |   public final boolean signMagnitude11Bit;
268 | 
269 |   /** multiplier taking natural units to int register counts **/
270 |   public final float naturalUnitsPerCount;
271 | 
272 |   /** writeable reg with all options **/
273 |   protected AXRegister(int ordinal, String prettyName,
274 |                        int startAddr, int width,
275 |                        int min, int max,
276 |                        boolean signMagnitude11Bit,
277 |                        float naturalUnitsPerCount,
278 |                        String naturalUnitsLabel,
279 |                        boolean useNaturalUnitsByDefault) {
280 |     this.ordinal = ordinal;
281 |     this.prettyName = prettyName;
282 |     this.startAddr = startAddr;
283 |     this.width = width;
284 |     this.writeable = true;
285 |     this.min = min;
286 |     this.max = max;
287 |     this.signMagnitude11Bit = signMagnitude11Bit;
288 |     this.naturalUnitsPerCount = naturalUnitsPerCount;
289 |     this.naturalUnitsLabel = naturalUnitsLabel;
290 |     this.useNaturalUnitsByDefault = useNaturalUnitsByDefault;
291 |   }
292 | 
293 |   /** read-only reg with all options **/
294 |   protected AXRegister(int ordinal, String prettyName,
295 |                        int startAddr, int width,
296 |                        boolean signMagnitude11Bit,
297 |                        float naturalUnitsPerCount,
298 |                        String naturalUnitsLabel,
299 |                        boolean useNaturalUnitsByDefault) {
300 |     this.ordinal = ordinal;
301 |     this.prettyName = prettyName;
302 |     this.startAddr = startAddr;
303 |     this.width = width;
304 |     this.writeable = false;
305 |     this.min = Integer.MIN_VALUE;
306 |     this.max = Integer.MAX_VALUE;
307 |     this.signMagnitude11Bit = signMagnitude11Bit;
308 |     this.naturalUnitsPerCount = naturalUnitsPerCount;
309 |     this.naturalUnitsLabel = naturalUnitsLabel;
310 |     this.useNaturalUnitsByDefault = useNaturalUnitsByDefault;
311 |   }
312 | 
313 |   /** true iff register is in RAM **/
314 |   public boolean isRAM() {
315 |     return startAddr > RAM_START_ADDRESS;
316 |   }
317 | 
318 |   /**
319 |    * <p>Check if an integer write value is within the closed interval [{@link
320 |    * #min}, {@link #max}].</p>
321 |    *
322 |    * @param value the value to check
323 |    *
324 |    * @return -1 if value is too low, +1 if it's too high, 0 if it's ok
325 |    **/
326 |   public int check(int value) {
327 |     if (value < min)
328 |       return -1;
329 |     else if (value > max)
330 |       return 1;
331 |     else
332 |       return 0;
333 |   }
334 | 
335 |   /** covers {@link #check(int)}, converts {@link #fromNaturalUnits} **/
336 |   public int check(float value) {
337 |     return check(fromNaturalUnits(value));
338 |   }
339 | 
340 |   /**
341 |    * <p>Clamp an integer write value to the closed interval [{@link #min},
342 |    * {@link #max}].</p>
343 |    *
344 |    * @param value the value to clamp
345 |    *
346 |    * @return the clamped value
347 |    **/
348 |   public int clamp(int value) {
349 |     if (value < min)
350 |       return min;
351 |     else if (value > max)
352 |       return max;
353 |     else
354 |       return value;
355 |   }
356 | 
357 |   /**
358 |    * <p>Covers {@link #clamp(int)}, converts {@link #fromNaturalUnits} and
359 |    * {@link #toNaturalUnits}.</p>
360 |    **/
361 |   public float clamp(float value) {
362 |     return toNaturalUnits(clamp(fromNaturalUnits(value)));
363 |   }
364 | 
365 |   /**
366 |    * <p>Encode an integer write value into the bits to write to the
367 |    * register.</p>
368 |    *
369 |    * <p>Handles conversion to {@link #signMagnitude11Bit} as appropriate.</p>
370 |    *
371 |    * @param value the value to encode
372 |    *
373 |    * @return the encoded value
374 |    **/
375 |   public int encode(int value) {
376 |     return
377 |       (signMagnitude11Bit && (value < 0)) ? 
378 |       ((1<<10) | (-value)) : value;
379 |   }
380 | 
381 |   /**
382 |    * <p>Encode an integer read value from the register bits.</p>
383 |    *
384 |    * <p>Handles conversion from {@link #signMagnitude11Bit} as
385 |    * appropriate.</p>
386 |    *
387 |    * @param value the value to decode
388 |    *
389 |    * @return the decoded value
390 |    **/
391 |   public int decode(int value) {
392 |     return
393 |       (signMagnitude11Bit && ((value & (1<<10)) != 0)) ?
394 |       -(value & 0x3ff) : value;
395 |   }
396 |    
397 |   /**
398 |    * <p>Convert a register int value to natural units.</p>
399 |    *
400 |    * <p>Default impl just multiplies by {@link #naturalUnitsPerCount}.</p>
401 |    *
402 |    * @param value the int value to convert
403 |    *
404 |    * @return the value in natural units
405 |    **/ 
406 |   public float toNaturalUnits(int value) {
407 |     return value*naturalUnitsPerCount;
408 |   }
409 | 
410 |   /**
411 |    * <p>Convert a register int value from natural units.</p>
412 |    *
413 |    * <p>Default impl just divides by {@link #naturalUnitsPerCount}.</p>
414 |    *
415 |    * @param value the natural value to convert
416 |    *
417 |    * @return the value in int counts
418 |    **/ 
419 |   public int fromNaturalUnits(float value) {
420 |     return (int) Math.round(value/naturalUnitsPerCount);
421 |   }
422 | 
423 |   /** check if this is a boolean valued register **/
424 |   public boolean isBoolean() {
425 |     return (min == 0) && (max == 1);
426 |   }
427 | 
428 |   /** get the register with the given relative ordinal **/
429 |   public abstract AXRegister getRelativeRegister(int offset);
430 | 
431 |   /** get the total number of registers in this Dynamixel type **/
432 |   public abstract int getNumRegisters();
433 | 
434 |   /** get the next register after this one **/
435 |   public AXRegister nextRegister() {
436 |     return getRelativeRegister(+1);
437 |   }
438 | 
439 |   /** get register before this one **/
440 |   public AXRegister prevRegister() {
441 |     return getRelativeRegister(-1);
442 |   }
443 | 
444 |   /** returns {@link #prettyName} **/
445 |   public String toString() {
446 |     return prettyName;
447 |   }
448 | 
449 |   /** get an identifier for registers of this Dynamixel type **/
450 |   protected abstract String getDynamixelType();
451 | 
452 |   /** returns the Java identifier for this register **/
453 |   public String toIdentifierString() {
454 |     return
455 |       getDynamixelType().toUpperCase()+"_"+
456 |       prettyName.toUpperCase().replace(' ', '_');
457 |   }
458 | }
459 | 


--------------------------------------------------------------------------------
/brbrain/src/java/brbrain/AXS1Register.java:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * <p>Represents the Dynamixel AX-S1 registers in a high-level way.</p>
  3 |  *
  4 |  * <p>Copyright (C) 2007 Marsette A. Vona, III</p>
  5 |  *
  6 |  * <p>This program is free software; you can redistribute it and/or modify it
  7 |  * under the terms of the GNU General Public License as published by the Free
  8 |  * Software Foundation; either version 2 of the License, or (at your option)
  9 |  * any later version.</p>
 10 |  *
 11 |  * <p>This program is distributed in the hope that it will be useful, but
 12 |  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 13 |  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 14 |  * more details.</p>
 15 |  *
 16 |  * <p>You should have received a copy of the GNU General Public License along
 17 |  * with this program; if not, write to the Free Software Foundation, Inc., 59
 18 |  * Temple Place - Suite 330, Boston, MA 02111-1307, USA.</p>
 19 |  **/
 20 | 
 21 | package brbrain;
 22 | 
 23 | import java.util.*;
 24 | import java.lang.reflect.*;
 25 | 
 26 | /**
 27 |  * <p>Represents the Dynamixel AX-S1 registers in a high-level way.</p>
 28 |  *
 29 |  * <p>Copyright (C) 2008 Marsette A. Vona, III</p>
 30 |  *
 31 |  * @author Marsette (Marty) A. Vona, III
 32 |  **/
 33 | public class AXS1Register extends AXRegister {
 34 |   
 35 |   /** the Dynamixel type identifier **/
 36 |   public static final String DYNAMIXEL_TYPE = "AXS1";
 37 | 
 38 |   /** obstacle or luminosity detection flag **/
 39 |   public static final int F_RIGHT =  (1<<2);
 40 | 
 41 |   /** obstacle or luminosity detection flag **/
 42 |   public static final int F_CENTER = (1<<1);
 43 | 
 44 |   /** obstacle or luminosity detection flag **/
 45 |   public static final int F_LEFT =   (1<<0);
 46 | 
 47 |   /** special value for {@link #AXS1_BUZZER_TIME} **/
 48 |   public static final int BUZZER_START = 254;
 49 | 
 50 |   /** special value for {@link #AXS1_BUZZER_TIME} **/
 51 |   public static final int BUZZER_STOP = 0;
 52 | 
 53 |   /** special value for {@link #AXS1_BUZZER_TIME} **/
 54 |   public static final int BUZZER_MELODY = 255;
 55 | 
 56 |   /** min duration for timed {@link #AXS1_BUZZER_TIME} in units of 0.1s **/
 57 |   public static final int BUZZER_MIN_DURATION = 3;
 58 | 
 59 |   /** max duration for timed {@link #AXS1_BUZZER_TIME} in units of 0.1s **/
 60 |   public static final int BUZZER_MAX_DURATION = 50;
 61 | 
 62 |   /** next register ordinal during class init **/
 63 |   protected static int nextOrdinal = 0;
 64 | 
 65 |   /** dynamixel model number (RO) **/
 66 |   public static final AXS1Register AXS1_MODEL_NUMBER =
 67 |     new AXS1Register(nextOrdinal++, "model number", 0, 2);
 68 | 
 69 |   /** dynamixel firmware version (RO) **/
 70 |   public static final AXS1Register AXS1_FIRMWARE_VERSION =
 71 |     new AXS1Register(nextOrdinal++, "firmware version", 2);
 72 | 
 73 |   /** dynamixel ID (RW) **/
 74 |   public static final AXS1Register AXS1_ID =
 75 |     new AXS1Register(nextOrdinal++, "id", 3, 0, MAX_DYNAMIXEL_ID);
 76 |   
 77 |   /**
 78 |    * <p>Dynamixel Baud Rate (RW), natural units [kbits/sec].</p>
 79 |    *
 80 |    * <p>See table in dynamixel docs for standard rates.</p>
 81 |    *
 82 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
 83 |    **/
 84 |   public static final AXS1Register AXS1_BAUD_RATE =
 85 |     new AXS1Register(nextOrdinal++, "baud rate", 4, 0, 254, "kbps", true) {
 86 |     
 87 |     public float toNaturalUnits(int value) {
 88 |       return (float) (2.0e3/(value+1));
 89 |     }
 90 |     
 91 |     public int fromNaturalUnits(float value) {
 92 |       return (int) Math.round((2.0e3/value)-1.0);
 93 |     }
 94 |   };
 95 | 
 96 |   /**
 97 |    * <p>Dynamixel return delay time (RW), natural units [usec].</p>
 98 |    *
 99 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
100 |    **/
101 |   public static final AXS1Register AXS1_RETURN_DELAY_TIME =
102 |     new AXS1Register(nextOrdinal++, "return delay time",
103 |                      5, 0, 254, 2.0f, "us", true);
104 |   
105 |   /** limit temp (RW), natural units [deg Celcius] **/
106 |   public static final AXS1Register AXS1_HIGHEST_LIMIT_TEMPERATURE =
107 |     new AXS1Register(nextOrdinal++, "highest limit temperature",
108 |                      11, 0, 150, 1.0f, "C", true);
109 |   
110 |   /** low limit voltage (RW), natural units [Volts] **/
111 |   public static final AXS1Register AXS1_LOWEST_LIMIT_VOLTAGE =
112 |     new AXS1Register(nextOrdinal++, "lowest limit voltage",
113 |                      12, 50, 250, 0.1f, "V", true);
114 |   
115 |   /** high limit voltage (RW), natural units [Volts] **/
116 |   public static final AXS1Register AXS1_HIGHEST_LIMIT_VOLTAGE =
117 |     new AXS1Register(nextOrdinal++, "highest limit voltage",
118 |                      13, 50, 250, 0.1f, "V", true);
119 |   
120 |   /**
121 |    * <p>Dynamixel status return level (RW).</p>
122 |    *
123 |    * <p>0 means no return packets, 1 means return packets only for
124 |    * READ_DATA, 2 means return packets always.</p>
125 |    *
126 |    * <p>Changing this will probably make the CM-5 firmware unhappy.</p>
127 |    **/
128 |   public static final AXS1Register AXS1_STATUS_RETURN_LEVEL =
129 |     new AXS1Register(nextOrdinal++, "status return level", 16, 1, 0, 2);
130 | 
131 |   /**
132 |    * <p>Default for {@link #AXS1_OBSTACLE_DETECTED_COMPARE} (RW).</p>
133 |    **/
134 |   public static final AXS1Register AXS1_OBSTACLE_DETECTED_COMPARE_VALUE =
135 |     new AXS1Register(nextOrdinal++, "obstacle detected compare value",
136 |                      20, 0, 255);
137 | 
138 |   /**
139 |    * <p>Default for {@link #AXS1_LIGHT_DETECTED_COMPARE} (RW).</p>
140 |    **/
141 |   public static final AXS1Register AXS1_LIGHT_DETECTED_COMPARE_VALUE =
142 |     new AXS1Register(nextOrdinal++, "light detected compare value",
143 |                      21, 0, 255);
144 | 
145 |   /**
146 |    * <p>Left IR distance reading (RO).</p>
147 |    **/
148 |   public static final AXS1Register AXS1_LEFT_IR_SENSOR_DATA =
149 |     new AXS1Register(nextOrdinal++, "left ir sensor data", 26);
150 | 
151 |   /**
152 |    * <p>Center IR distance reading (RO).</p>
153 |    **/
154 |   public static final AXS1Register AXS1_CENTER_IR_SENSOR_DATA =
155 |     new AXS1Register(nextOrdinal++, "center ir sensor data", 27);
156 | 
157 |   /**
158 |    * <p>Right IR distance reading (RO).</p>
159 |    **/
160 |   public static final AXS1Register AXS1_RIGHT_IR_SENSOR_DATA =
161 |     new AXS1Register(nextOrdinal++, "right ir sensor data", 28);
162 | 
163 |   /**
164 |    * <p>Left IR luminosity reading (RO).</p>
165 |    **/
166 |   public static final AXS1Register AXS1_LEFT_LUMINOSITY =
167 |     new AXS1Register(nextOrdinal++, "left luminosity", 29);
168 | 
169 |   /**
170 |    * <p>Center IR luminosity reading (RO).</p>
171 |    **/
172 |   public static final AXS1Register AXS1_CENTER_LUMINOSITY =
173 |     new AXS1Register(nextOrdinal++, "center luminosity", 30);
174 | 
175 |   /**
176 |    * <p>Right IR luminosity reading (RO).</p>
177 |    **/
178 |   public static final AXS1Register AXS1_RIGHT_LUMINOSITY =
179 |     new AXS1Register(nextOrdinal++, "right luminosity", 31);
180 | 
181 |   /**
182 |    * <p>IR obstacle detection bitmask, see {@link #F_RIGHT}, {@link #F_CENTER},
183 |    * {@link #F_LEFT} (RO).</p>
184 |    **/
185 |   public static final AXS1Register AXS1_OBSTACLE_DETECTION_FLAG =
186 |     new AXS1Register(nextOrdinal++, "obstacle detection flag", 32);
187 | 
188 |   /**
189 |    * <p>IR luminosity detection bitmask, see {@link #F_RIGHT}, {@link
190 |    * #F_CENTER}, {@link #F_LEFT} (RO).</p>
191 |    **/
192 |   public static final AXS1Register AXS1_LUMINOSITY_DETECTION_FLAG =
193 |     new AXS1Register(nextOrdinal++, "luminosity detection flag", 33);
194 | 
195 |   /**
196 |    * <p>Sound pressure (RW).</p>
197 |    *
198 |    * <p>~127-128 is quiet.</p>
199 |    **/
200 |   public static final AXS1Register AXS1_SOUND_DATA =
201 |     new AXS1Register(nextOrdinal++, "sound data", 35, 0, 255);
202 | 
203 |   /**
204 |    * <p>Loudest sound pressure heard (RW).</p>
205 |    *
206 |    * <p>Write to 0 to force update.</p>
207 |    **/
208 |   public static final AXS1Register AXS1_SOUND_DATA_MAX_HOLD =
209 |     new AXS1Register(nextOrdinal++, "sound data max hold", 36, 0, 255);
210 | 
211 |   /**
212 |    * <p>Recent count of loud sounds (RW).</p>
213 |    **/
214 |   public static final AXS1Register AXS1_SOUND_DETECTED_COUNT =
215 |     new AXS1Register(nextOrdinal++, "sound detected count", 37, 0, 255);
216 | 
217 |   /**
218 |    * <p>Timestamp of most recent loud sound detection (RW), natrual units
219 |    * [ms].</p>
220 |    **/
221 |   public static final AXS1Register AXS1_SOUND_DETECTED_TIME =
222 |     new AXS1Register(nextOrdinal++, "sound detected time",
223 |                      38, 2, 0, 65536, false, 4.096f/65536.0f, "ms", true);
224 | 
225 |   /**
226 |    * <p>Buzzer note (RW).</p>
227 |    **/
228 |   public static final AXS1Register AXS1_BUZZER_INDEX =
229 |     new AXS1Register(nextOrdinal++, "buzzer index", 40, 0, 51);
230 | 
231 |   /**
232 |    * <p>Buzzer duration, natural units [s] (RW).</p>
233 |    *
234 |    * <p>{@link #BUZZER_START}, {@link #BUZZER_STOP}, {@link #BUZZER_MELODY},
235 |    * {@link #BUZZER_MIN_DURATION}, {@link #BUZZER_MAX_DURATION} are special
236 |    * values.</p>
237 |    **/
238 |   public static final AXS1Register AXS1_BUZZER_TIME =
239 |     new AXS1Register(nextOrdinal++, "buzzer time",
240 |                      41, 0, 255, 0.1f, "s", true);
241 | 
242 |   /** Current voltage (RO), natural units [Volts] **/
243 |   public static final AXS1Register AXS1_PRESENT_VOLTAGE =
244 |     new AXS1Register(nextOrdinal++, "present voltage", 42, 0.1f, "V", true);
245 | 
246 |   /** Current temperature (RO), natural units [degrees Celcius] **/
247 |   public static final AXS1Register AXS1_PRESENT_TEMPERATURE =
248 |     new AXS1Register(nextOrdinal++, "present temperature",
249 |                      43, 1.0f, "C", true);
250 | 
251 |   /** Whether there is a registered instruction pending (RW), boolean **/
252 |   public static final AXS1Register AXS1_REGISTERED_INSTRUCTION =
253 |     new AXS1Register(nextOrdinal++, "registered instruction", 44, 0, 1);
254 | 
255 |   /** Remocon data is ready when value is 2 (RO). **/
256 |   public static final AXS1Register AXS1_IR_REMOCON_ARRIVED =
257 |     new AXS1Register(nextOrdinal++, "ir remocon arrived", 46);
258 | 
259 |   /**
260 |    * <p>This appears to be an AX-12 vestige.</p>
261 |    **/
262 |   public static final AXS1Register AXS1_LOCK =
263 |     new AXS1Register(nextOrdinal++, "lock", 47, 0, 1);
264 | 
265 |   /**
266 |    * <p>Value of received remocon data (RO).</p>
267 |    *
268 |    * <p>{@link #AXS1_IR_REMOCON_ARRIVED} == 2 indicates that data is ready.</p>
269 |    *
270 |    * <p>This will reset to 0 once read.</p>
271 |    **/
272 |   public static final AXS1Register AXS1_IR_REMOCON_RX_DATA =
273 |     new AXS1Register(nextOrdinal++, "ir remocon rx data", 48, 2);
274 | 
275 |   /**
276 |    * <p>Remocon data to send (RW).</p>
277 |    *
278 |    * <p>Transmission is triggered on register write.</p>
279 |    **/
280 |   public static final AXS1Register AXS1_IR_REMOCON_TX_DATA =
281 |     new AXS1Register(nextOrdinal++, "ir remocon tx data", 50, 2, 0, 65536);
282 | 
283 |   /**
284 |    * <p>IR obstacle detection threshold (RW).</p>
285 |    *
286 |    * <p>Initialized from {@link #AXS1_OBSTACLE_DETECTED_COMPARE_VALUE}.</p>
287 |    **/
288 |   public static final AXS1Register AXS1_OBSTACLE_DETECTED_COMPARE =
289 |     new AXS1Register(nextOrdinal++, "obstacle detected compare",
290 |                      52, 0, 255);
291 | 
292 |   /**
293 |    * <p>IR detection threshold (RW).</p>
294 |    *
295 |    * <p>Initialized from {@link #AXS1_LIGHT_DETECTED_COMPARE_VALUE}.</p>
296 |    **/
297 |   public static final AXS1Register AXS1_LIGHT_DETECTED_COMPARE =
298 |     new AXS1Register(nextOrdinal++, "light detected compare",
299 |                      53, 0, 255);
300 | 
301 |   /**
302 |    * <p><i>Virtual</i> register containing the error status of the dynamixel
303 |    * (RO).</p>
304 |    *
305 |    * <p>The error status is a bitfield of the <code>E_*</code> bits.</p> 
306 |    **/
307 |   public static final AXS1Register AXS1_ERROR =
308 |     new AXS1Register(nextOrdinal++, "error", 54);
309 | 
310 |   private static final String svnid =
311 |   "$Id: AXS1Register.java 25 2008-05-02 22:11:43Z vona $";
312 | 
313 |   /** the array of {@link AXS1Register}s, indexed by ordinal **/
314 |   protected static AXS1Register[] registers;
315 | 
316 |   /** total number of registers **/
317 |   public static final int NUM_REGISTERS;
318 | 
319 |   /** first register in RAM **/
320 |   public static final AXRegister FIRST_REGISTER;
321 | 
322 |   /** first register in RAM **/
323 |   public static final AXRegister FIRST_RAM_REGISTER;
324 | 
325 |   static {
326 | 
327 |     ArrayList<AXS1Register> regs = new ArrayList<AXS1Register>();
328 | 
329 |     Field[] fields = AXS1Register.class.getDeclaredFields();
330 | 
331 |     String prefix = DYNAMIXEL_TYPE.toUpperCase();
332 | 
333 |     try {
334 |       for (Field f : fields)
335 |         if ((f.getType() == AXS1Register.class) &&
336 |             (f.getName().startsWith(prefix)))
337 |           regs.add((AXS1Register) (f.get(null)));
338 |     } catch (IllegalAccessException e) {
339 |       throw new RuntimeException(e);
340 |     }
341 | 
342 |     Collections.sort(regs, new Comparator<AXS1Register>() {
343 |         public int compare(AXS1Register r1, AXS1Register r2) {
344 | 
345 |           //negative integer, zero, or a positive integer as the first argument
346 |           //is less than, equal to, or greater than the second.
347 | 
348 |           if (r1.ordinal < r2.ordinal)
349 |             return -1;
350 |           else if (r1.ordinal > r2.ordinal)
351 |             return +1;
352 |           else
353 |             return 0;
354 |         }
355 |       });
356 | 
357 |     NUM_REGISTERS = regs.size();
358 |     registers = regs.toArray(new AXS1Register[NUM_REGISTERS]);
359 |     
360 |     FIRST_REGISTER = registers[0];
361 | 
362 |     int ri = 0;
363 |     for (; ri < registers.length; ri++) {
364 |       if (registers[ri].startAddr >= RAM_START_ADDRESS) {
365 |         break;
366 |       }
367 |     }
368 |     
369 |     FIRST_RAM_REGISTER = registers[ri];
370 |   }
371 | 
372 |   /** writeable reg with all options **/
373 |   protected AXS1Register(int ordinal, String prettyName,
374 |                          int startAddr, int width,
375 |                          int min, int max,
376 |                          boolean signMagnitude11Bit,
377 |                          float naturalUnitsPerCount,
378 |                          String naturalUnitsLabel,
379 |                          boolean useNaturalUnitsByDefault) {
380 |   super(ordinal, prettyName,
381 |         startAddr, width,
382 |         min, max,
383 |         signMagnitude11Bit, naturalUnitsPerCount, naturalUnitsLabel,
384 |         useNaturalUnitsByDefault);
385 |   }
386 | 
387 |   /** read-only reg with all options **/
388 |   protected AXS1Register(int ordinal, String prettyName,
389 |                          int startAddr, int width,
390 |                          boolean signMagnitude11Bit,
391 |                          float naturalUnitsPerCount,
392 |                          String naturalUnitsLabel,
393 |                          boolean useNaturalUnitsByDefault) {
394 |   super(ordinal, prettyName,
395 |         startAddr, width,
396 |         signMagnitude11Bit, naturalUnitsPerCount, naturalUnitsLabel,
397 |         useNaturalUnitsByDefault);
398 |   }
399 | 
400 |   /** writeable unsigned reg **/
401 |   protected AXS1Register(int ordinal, String prettyName,
402 |                          int startAddr, int width,
403 |                          int min, int max,
404 |                          float naturalUnitsPerCount,
405 |                          String naturalUnitsLabel,
406 |                          boolean useNaturalUnitsByDefault) {
407 |     this(ordinal, prettyName,
408 |          startAddr, width,
409 |          min, max,
410 |          false,
411 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
412 |   }
413 | 
414 |   /** writeable unsigned reg, natural units are counts **/
415 |   protected AXS1Register(int ordinal, String prettyName,
416 |                          int startAddr, int width, int min, int max) {
417 |     this(ordinal, prettyName, startAddr, width, min, max, 1.0f, "", false); 
418 |   }
419 | 
420 |   /** writeable unsigned 1-byte reg, natural units are counts **/
421 |   protected AXS1Register(int ordinal, String prettyName,
422 |                          int startAddr, int min, int max,
423 |                          String naturalUnitsLabel,
424 |                          boolean useNaturalUnitsByDefault) {
425 |     this(ordinal, prettyName,
426 |          startAddr, 1, min, max, 1.0f,
427 |          naturalUnitsLabel, useNaturalUnitsByDefault);
428 |   }
429 | 
430 |   /**
431 |    * <p>Read-only unsigned reg (be careful of confusion with {@link
432 |    * #AXS1Register(int, String, int, int, int, String, boolean)}).</p>
433 |    **/
434 |   protected AXS1Register(int ordinal, String prettyName,
435 |                          int startAddr, int width,
436 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
437 |                          boolean useNaturalUnitsByDefault) {
438 |     this(ordinal, prettyName,
439 |          startAddr, width, false,
440 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
441 |   }
442 | 
443 |   /** read-only unsigned reg, natural units are counts **/
444 |   protected AXS1Register(int ordinal, String prettyName,
445 |                          int startAddr, int width) {
446 |     this(ordinal, prettyName, startAddr, width, 1.0f, "", false);
447 |   }
448 | 
449 |   /** writeable unsigned 1-byte reg **/
450 |   protected AXS1Register(int ordinal, String prettyName,
451 |                          int startAddr,
452 |                          int min, int max,
453 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
454 |                          boolean useNaturalUnitsByDefault) {
455 |     this(ordinal, prettyName,
456 |          startAddr, 1, min, max,
457 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
458 |   }
459 | 
460 |   /** writeable unsigned 1-byte reg, natural units are coutns **/
461 |   protected AXS1Register(int ordinal, String prettyName,
462 |                          int startAddr, int min, int max) {
463 |     this(ordinal, prettyName, startAddr, min, max, 1.0f, "", false);
464 |   }
465 | 
466 |   /** read-only unsigned 1-byte reg **/
467 |   protected AXS1Register(int ordinal, String prettyName,
468 |                          int startAddr,
469 |                          float naturalUnitsPerCount, String naturalUnitsLabel,
470 |                          boolean useNaturalUnitsByDefault) {
471 |     this(ordinal, prettyName,
472 |          startAddr, 1,
473 |          naturalUnitsPerCount, naturalUnitsLabel, useNaturalUnitsByDefault);
474 |   }
475 |   
476 |   /** read-only unsigned 1-byte reg, natural units are counts **/
477 |   protected AXS1Register(int ordinal, String prettyName, int startAddr) {
478 |     this(ordinal, prettyName, startAddr, 1.0f, "", false);
479 |   }
480 |   
481 |   /** get the register with the given relative ordinal **/
482 |   public AXRegister getRelativeRegister(int offset) {
483 |     return registers[ordinal+offset];
484 |   }
485 | 
486 |   /** get the total number of AXS1Registers **/
487 |   public int getNumRegisters() {
488 |     return NUM_REGISTERS;
489 |   }
490 | 
491 |   /** returns {@link #DYNAMIXEL_TYPE} **/
492 |   protected String getDynamixelType() {
493 |     return DYNAMIXEL_TYPE;
494 |   }
495 | 
496 |   /** get the {@link #FIRST_REGISTER} **/
497 |   public static AXRegister getFirstRegister() {
498 |     return FIRST_REGISTER;
499 |   }
500 | 
501 |   /** get the {@link #FIRST_RAM_REGISTER} **/
502 |   public static AXRegister getFirstRAMRegister() {
503 |     return FIRST_RAM_REGISTER;
504 |   }
505 | 
506 |   /** get the AXS1Register at the specified ordinal **/
507 |   public static AXRegister getRegister(int ordinal) {
508 |     return registers[ordinal];
509 |   }
510 | 
511 |   /** get a copy of the ordered array of AXS1Registers **/
512 |   public static AXS1Register[] getAllRegisters(AXS1Register[] regs,
513 |                                                int start) {
514 |     if ((regs == null) || (regs.length < (start+NUM_REGISTERS)))
515 |       regs = new AXS1Register[start+NUM_REGISTERS];
516 |     System.arraycopy(registers, 0, regs, start, NUM_REGISTERS);
517 |     return regs;
518 |   }
519 | 
520 |   /** covers {@link #getAllRegisters(AXS1Register[], int)}, starts at 0 **/
521 |   public static AXS1Register[] getAllRegisters(AXS1Register[] regs) {
522 |     return getAllRegisters(regs, 0);
523 |   }
524 | 
525 |   /** covers {@link #getAllRegisters(AXS1Register[], int)}, always conses **/
526 |   public static AXS1Register[] getAllRegisters() {
527 |     return getAllRegisters(null, 0);
528 |   }
529 | }
530 | 


--------------------------------------------------------------------------------
/brbrain/src/java/brbrain/BRBrain.java:
--------------------------------------------------------------------------------
   1 | /**
   2 |  * <p>Bioloid Remote Brain library.</p>
   3 |  *
   4 |  * <p>Copyright (C) 2007 Marsette A. Vona, III</p>
   5 |  *
   6 |  * <p>This program is free software; you can redistribute it and/or modify it
   7 |  * under the terms of the GNU General Public License as published by the Free
   8 |  * Software Foundation; either version 2 of the License, or (at your option)
   9 |  * any later version.</p>
  10 |  *
  11 |  * <p>This program is distributed in the hope that it will be useful, but
  12 |  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  13 |  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  14 |  * more details.</p>
  15 |  *
  16 |  * <p>You should have received a copy of the GNU General Public License along
  17 |  * with this program; if not, write to the Free Software Foundation, Inc., 59
  18 |  * Temple Place - Suite 330, Boston, MA 02111-1307, USA.</p>
  19 |  **/
  20 | 
  21 | package brbrain;
  22 | 
  23 | import static brbrain.AX12Register.*;
  24 | 
  25 | import gnu.io.*;
  26 | import java.util.*;
  27 | import java.io.*;
  28 | 
  29 | /**
  30 |  * <p>Bioloid Remote Brain library.</p>
  31 |  *
  32 |  * <p>This is a Java host library, which, combined with custom C firmware,
  33 |  * implements <i>remote brain</i> functionality for the <a
  34 |  * href="http://www.robotis.com/html/main.php">Robotis</a> <a
  35 |  * href="http://www.robotis.com/html/sub.php?sub=2&menu=1">Bioloid</a>.
  36 |  *
  37 |  * <p>High level documentation and instructions for installing the custom <a
  38 |  * href="http://www.robotis.com/html/sub.php?sub=2&menu=3">CM-5</a> firmware
  39 |  * are available from the <a
  40 |  * href="http://www.mit.edu/~vona/BRBrain/BRBrain-info.html">BRBrain
  41 |  * homepage</a>.</p>
  42 |  *
  43 |  * <h2>Java Code Runs Off-Board</h2>
  44 |  *
  45 |  * <p>This system allows you to write Java code to control Bioloid
  46 |  * constructions.  The Java code always runs on a host PC and communicates with
  47 |  * the CM-5 on your Bioloid hardware via a serial link.  Sensor and actuator
  48 |  * data is exchanged in <i>near</i> real time between the CM-5 and a host
  49 |  * workstation, allowing high-level control code to run (and be debugged)
  50 |  * directly on the workstation.</p>
  51 |  *
  52 |  * <p>Again, BRBrain does not allow you to run Java code directly on the
  53 |  * CM-5. You run Java code on a host PC that communicates with the CM-5 over
  54 |  * either the standard RS232 serial cable or an optional non-standard bluetooth
  55 |  * link that you can add to your CM-5 <a
  56 |  * href="http://www.mit.edu/~vona/BRBrain/BRBrain-info.html#BlueSMiRF">as
  57 |  * described here</a>.</p>
  58 |  *
  59 |  * <h2>Establishing Communications</h2>
  60 |  *
  61 |  * <p>Several constructors are provided to allow the actual communication link
  62 |  * between the host PC and the CM-5 to be implemented in different ways:<ul>
  63 |  *
  64 |  * <li>over a serial port accessed as a regular file</li>
  65 |  * <li>over a serial port accessed with the RXTX library</li>
  66 |  *
  67 |  * </ul>The RXTX codepath has the advantage of internally setting the
  68 |  * communications parameters (115.2kbps, 8N1, no flow control).  The regular
  69 |  * file codepath will require the serial port to be externally configured
  70 |  * (e.g. using <tt>stty</tt> on Linux).  The serial port must be 8-bit clean.
  71 |  * This is automatically guaranteed when using RXTX.  For the regular file
  72 |  * codepath it is part of the external port configuration. For example, on
  73 |  * Linux the terminal driver associated with the port device needs to be taken
  74 |  * out of "cooked" mode (the default) and placed in "raw" mode (again using
  75 |  * <tt>stty</tt>).</p>
  76 |  *
  77 |  * <p>{@link #listPorts} may be invoked to get a list of the port names that
  78 |  * the RXTX library believes to be available.</p>
  79 |  *
  80 |  * <h2>Talking to the Bioloid</h2>
  81 |  *
  82 |  * <p>The CM-5 running the custom firmware acts as a relay between the register
  83 |  * banks of the connected network of Dynamixel AX modules and the host PC.  The
  84 |  * {@link AX12Register} and {@link AXS1Register} classes enumerate the
  85 |  * Dynamixel registers that you can access.  Refer to the <a
  86 |  * href="http://www.robotis.com/hb/hisboard.php?id=bbs1_1_eng&group_no=1&category=&bd_no=27&bd_step=0&bd_group=19&bd_float=1900&mode=view&position=3&search=&find=">respective</a>
  87 |  * <a
  88 |  * href="http://www.robotis.com/hb/hisboard.php?id=bbs1_1_eng&group_no=1&category=&bd_no=31&bd_step=0&bd_group=23&bd_float=2300&mode=view&position=3&search=&find=">manuals</a>
  89 |  * for Dynamixel register descriptions.</p>
  90 |  *
  91 |  * <p>For performance, you first inform BRBrain of the set of particular
  92 |  * registers on particular Dynamixels in which you are interested.  Such a
  93 |  * specificiation is called a <i>format</i>; you specify separate formats for
  94 |  * data that you are writing from the host PC to the Dynamixels and data that
  95 |  * you are reading from the Dynamixels to the host PC with the {@link
  96 |  * #setWriteFormat} and {@link #setReadFormat} APIs, respectively.</p>
  97 |  *
  98 |  * <p>Once you have specified your read and write formats, you read and write
  99 |  * blocks of data from and to the Dynamixels by calling the {@link
 100 |  * #read(int[])}, {@link #read(float[])}, and {@link #write(int[])} and {@link
 101 |  * #write(float[])} APIs.  The integer versions of these APIs read and write
 102 |  * raw integer values from/to the Dynamixels; the float versions automatically
 103 |  * convert <i>natural units</i> for the register from/to the raw register
 104 |  * values.  For example, the natural units for the {@link
 105 |  * AX12Register#AX12_PRESENT_POSITION} register are degrees.</p>
 106 |  *
 107 |  * <p>To keep format specification simple, you can only specify one contiguous
 108 |  * block of registers to read and a separate contiguous block of registers to
 109 |  * write per Dynamixel.  The blocks can be different on different Dynamixels,
 110 |  * and you can specify an arbitrary set of Dynamixel IDs to which you will be
 111 |  * communicating; this set need not include all of the Dynamixels that are
 112 |  * actually connected to your CM-5.  On a write, all registers are written in
 113 |  * synchrony with the REG_WRITE/ACTION instructions supported by the
 114 |  * Dynamixels.</p>
 115 |  *
 116 |  * <p>A cache of most-recently read data is maintained and may be queried with
 117 |  * {@link #getCachedValue}.</p>
 118 |  *
 119 |  * <p>Every method which involves actual communication with the CM-5 is
 120 |  * synchronized.  Synchronize on the BRBrain object to perform multiple
 121 |  * communications in a single uninterrupted transaction.</p>
 122 |  *
 123 |  * <h2>Usage Example</h2>
 124 |  *
 125 |  * <p>First, get a list of the available serial ports:
 126 |  *
 127 |  * <code><pre>
 128 |  * BRBrain.listPorts();
 129 |  * </code></pre>
 130 |  *
 131 |  * The output will depend on your operating system and the serial port hardware
 132 |  * actually available on your computer.  You'll need to interpret it and figure
 133 |  * out the port to which your CM-5 is actually connected.  For example, on
 134 |  * Linux this might be {@code /dev/ttyS0} or {@code /dev/ttyUSB0} if the CM-5
 135 |  * is connected to the first built-in serial port or to the first USB-to-serial
 136 |  * adapter, respectively.  On windows it may be something like {@code
 137 |  * COM1}.</p>
 138 |  *
 139 |  * <p>Next, create a BRBrain object.  This establishes communications:
 140 |  *
 141 |  * <code><pre>
 142 |  * BRBrain b = new BRBrain("/dev/ttyS0"); //or whatever port you selected
 143 |  * </pre></code></p>
 144 |  *
 145 |  * <p>Now set read and write formats.  Let's say you want to read the five
 146 |  * AX-12 registers starting at {@link AX12Register#AX12_PRESENT_POSITION}
 147 |  * (these also include {@link AX12Register#AX12_PRESENT_SPEED}, {@link
 148 |  * AX12Register#AX12_PRESENT_LOAD}, {@link AX12Register#AX12_PRESENT_VOLTAGE},
 149 |  * and {@link AX12Register#AX12_PRESENT_TEMPERATURE}), write only the {@link
 150 |  * AX12Register#AX12_GOAL_POSITION}, and that you want to do this on two AX-12s
 151 |  * with IDs 3 and 7:
 152 |  *
 153 |  * <code><pre>
 154 |  * int status =
 155 |  *   b.setReadFormat(new int[] {3, 7}, //id
 156 |  *                   new AXRegister{AX12Register.AX12_PRESENT_POSITION, //start
 157 |  *                                  AX12Register.AX12_PRESENT_POSITION},
 158 |  *                   new int{5, 5}); //num
 159 |  * b.verifyStatus(status, "set read format");
 160 |  *
 161 |  * status = 
 162 |  *   b.setWriteFormat(new int[] {3, 7}, //id
 163 |  *                   new AXRegister{AX12Register.AX12_GOAL_POSITION,  //start
 164 |  *                                  AX12Register.AX12_GOAL_POSITION},
 165 |  *                   new int{1, 1}); //num
 166 |  * b.verifyStatus(status, "set write format");
 167 |  * </pre></code></p>
 168 |  *
 169 |  * <p>Finally, you can set some goal positions and then monitor the state of
 170 |  * your two servos:
 171 |  *
 172 |  * <code><pre>
 173 |  * //sets servo 3 to goal position 100 degrees, servo 7 to 200 degrees
 174 |  * status = b.write(new float[] {100.0f, 200.0f});
 175 |  * b.verifyStatus(status, "set goal positions");
 176 |  *
 177 |  * float[] state = new float[2*5];
 178 |  * status = b.read(state);
 179 |  * b.verifyStatus(status, "read state");
 180 |  * </pre></code></p>
 181 |  *
 182 |  * <h2>Extra Utilities</h2>
 183 |  *
 184 |  * <p>You can use the BRBrain class alone (well, {@link AXRegister} and its
 185 |  * subclasses are also required) in your programs.  Some additional utilities
 186 |  * are also included in this package which can help you implement recording and
 187 |  * playing back motion sequences, or just displaying a GUI with little
 188 |  * read/write boxes for all the Dynamixel register values:<ul>
 189 |  *
 190 |  * <li>a {@link Pose} represents the values of a set of registers for a set of
 191 |  * Dynamixels, including provisions to read/write the data both from/to the
 192 |  * hardware and from/to output/input streams</li>
 193 |  *
 194 |  * <li>a {@link PoseSequence} is an ordered sequence of {@link Pose}s</li>
 195 |  *
 196 |  * <li>a {@link PoseGUI} displays an array of read/write boxes that corresponds
 197 |  * to a {@link Pose}</li>
 198 |  *
 199 |  * <li>a {@link PoseSequenceGUI} similarly allows you to edit a {@link
 200 |  * PoseSequence}</li>
 201 |  *
 202 |  * <li>a {@link SequencePlaybackController} allows you to play back a {@link
 203 |  * PoseSequence} on the Bioloid hardware, possibly making arbitrary control
 204 |  * flow decisions</li>
 205 |  *
 206 |  * <li>{@link BRBrainShell} implements a REPL-style shell from which you can
 207 |  * access most of the above functionality from a higher-level and more dynamic
 208 |  * Scheme-language environment</li>
 209 |  *
 210 |  * </ul></p>
 211 |  *
 212 |  * <h2>Under the Hood</h2>
 213 |  *
 214 |  * <p>This section documents the variable-length packet communication protocol
 215 |  * that BRBrain uses to communicate with the custom firmware.  If you are just
 216 |  * using BRBrain to write Java control programs for your Bioloid, and you are
 217 |  * not intending to modify the firmware, you can skip this section.</p>
 218 |  *
 219 |  * <p>All packets sent in either direction follow the basic format<pre>
 220 |  *
 221 |  * instruction
 222 |  * [data...]
 223 |  * checksum
 224 |  * 
 225 |  * </pre>where "instruction" is one byte long, data is zero or more bytes, and
 226 |  * checksum is computed as the bitwise inverse of the unsigned byte sum of the
 227 |  * data and the instruction.</p>
 228 |  *
 229 |  * <p>The following instructions are used for packets from the host to the
 230 |  * CM-5:<ul>
 231 |  *
 232 |  * <li>0xF0: {@link Instruction#I_PING} either the CM-5 itself or one of the
 233 |  * connected dynamixels.  Data is one byte with either the address of the
 234 |  * dynaxmixel to ping in the range [0, {@link AXRegister#MAX_DYNAMIXEL_ID}] or
 235 |  * 255 to ping the CM-5.  The CM-5 responds with a {@link Instruction#I_STATUS}
 236 |  * packet.</li>
 237 |  *
 238 |  * <li>0xF1: {@link Instruction#I_SET_READ_FORMAT}.  Data is in the following
 239 |  * form:<pre>
 240 |  *
 241 |  * num dynamixels in the range [0, {@link #MAX_DYNAMIXELS}]
 242 |  * dynamixel 0 id
 243 |  * dynamixel 0 start byte
 244 |  * dynamixel 0 num bytes
 245 |  * dynamixel 1 id
 246 |  * dynamixel 1 start byte
 247 |  * dynamixel 1 num bytes
 248 |  * ...
 249 |  * dynamixel (n-1) id
 250 |  * dynamixel (n-1) start byte
 251 |  * dynamixel (n-1) num bytes
 252 |  * 
 253 |  * </pre>The CM-5 responds with a {@link Instruction#I_STATUS} packet. If any
 254 |  * dynamixel id is outside the closed interval [0, {@link
 255 |  * AXRegister#MAX_DYNAMIXEL_ID}], or if the span of registers for any given
 256 |  * dynamixel extends beyond the last register, the CM-5 will respond with an
 257 |  * error code and the read format will be indeterminate until it is
 258 |  * successfully re-set.</li>
 259 |  *
 260 |  * <li>0xF2: {@link Instruction#I_SET_WRITE_FORMAT}.  Data is in the following
 261 |  * form:<pre>
 262 |  *
 263 |  * num dynamixels in the range [0, {@link #MAX_DYNAMIXELS}]
 264 |  * dynamixel 0 id
 265 |  * dynamixel 0 start byte
 266 |  * dynamixel 0 num bytes
 267 |  * ...
 268 |  * dynamixel (<i>n</i>-1) id
 269 |  * dynamixel (<i>n</i>-1) start byte
 270 |  * dynamixel (<i>n</i>-1) num bytes
 271 |  * 
 272 |  * </pre>The CM-5 responds with a {@link Instruction#I_STATUS} packet. If any
 273 |  * dynamixel id is outside the closed interval [0, {@link
 274 |  * AXRegister#MAX_DYNAMIXEL_ID}], or if the span of registers for any given
 275 |  * dynamixel extends beyond the last register or includes any which are
 276 |  * read-only, the CM-5 will respond with an error code and the write format
 277 |  * will be indeterminate until it is successfully re-set.</li>
 278 |  *
 279 |  * <li>0xF3: {@link Instruction#I_READ_DATA}.  No data is sent.  The CM-5
 280 |  * responds with a {@link Instruction#I_DATA} packet.</li>
 281 |  *
 282 |  * <li>0xF4: {@link Instruction#I_WRITE_DATA}.  Data is in the following
 283 |  * form:<pre>
 284 |  *
 285 |  * dynamixel 0 data bytes
 286 |  * ...
 287 |  * dynamixel (<i>n</i>-1) data bytes
 288 |  *
 289 |  * </pre>Where <i>n</i> is the number of dynamixels specified in the most
 290 |  * recent {@link Instruction#I_SET_WRITE_FORMAT}, and for each dynamixel the
 291 |  * number of data bytes sent is again specified by the most recent {@link
 292 |  * Instruction#I_SET_WRITE_FORMAT}.  The CM-5 responds with a {@link
 293 |  * Instruction#I_STATUS} packet.</li>
 294 |  *
 295 |  * </ul></p>
 296 |  *
 297 |  * <p>The following instructions are used for packets from the CM-5 to the
 298 |  * host:<ul>
 299 |  *
 300 |  * <li>0xFA: {@link Instruction#I_STATUS}.  Data is five bytes.  The first byte
 301 |  * is zero if the previous operation succeeded, or a nonzero errorcode if there
 302 |  * was a failure.  The second byte is the total number of dynamixel bus packet
 303 |  * retries that were incurred during execution of the previous operation.  The
 304 |  * remaining three bytes are the ADC channel raw values as of the last read
 305 |  * (reads occur about every 5s) in the order pos, neg, therm.</li>
 306 |  *
 307 |  * <li>0xFB: {@link Instruction#I_DATA}.  Data is in the following form:<pre>
 308 |  *
 309 |  * dynamixel 0 data bytes
 310 |  * ...
 311 |  * dynamixel (<i>n</i>-1) data bytes
 312 |  * status byte
 313 |  * retry count byte
 314 |  * ADC pos channel
 315 |  * ADC neg channel
 316 |  * ADC therm channel
 317 |  *
 318 |  * </pre>Where <i>n</i> is the number of dynamixels specified in the most
 319 |  * recent {@link Instruction#I_SET_READ_FORMAT}, and for each dynamixel the
 320 |  * number of data bytes sent is again specified by the most recent {@link
 321 |  * Instruction#I_SET_READ_FORMAT}.  If there are errors acquiring any or all
 322 |  * data bytes, the bytes are still sent by the CM-5 to the host but with value
 323 |  * 0xFF.  The final status, retry count, and ADC bytes are always sent and have
 324 |  * the same semantics as the payload of an {@link Instruction#I_STATUS}
 325 |  * packet.</li>
 326 |  *
 327 |  * </ul></p>
 328 |  *
 329 |  * <p>Copyright (C) 2008 Marsette A. Vona, III</p>
 330 |  *
 331 |  * @author Marsette (Marty) A. Vona, III
 332 |  **/
 333 | public class BRBrain {
 334 | 
 335 |   private static final String svnid =
 336 |   "$Id: BRBrain.java 28 2008-09-29 16:24:46Z vona $";
 337 | 
 338 |   /** whether to show protocol debug messages **/
 339 |   public boolean debug = false;
 340 | 
 341 |   /** whether to enable debug log of failed recv packets **/
 342 |   public boolean enableRecvPacketDebug = true;
 343 | 
 344 |   /** whether to log to {@link #recvPacketDebugBuffer} **/
 345 |   protected boolean recvPacketDebug = enableRecvPacketDebug;
 346 | 
 347 |   /** debug log for recv packets **/
 348 |   protected List<Byte> recvPacketDebugBuffer = new ArrayList<Byte>();
 349 | 
 350 |   /** a cached register value **/
 351 |   public class CachedValue {
 352 | 
 353 |     /** the cached value **/
 354 |     protected int value;
 355 | 
 356 |     /** the last update nanotime **/
 357 |     protected long lastUpdateNS;
 358 | 
 359 |     /** get the cached value **/
 360 |     public int getValue() {
 361 |       return value;
 362 |     }
 363 | 
 364 |     /** get the last update nanotime **/
 365 |     public long getLastUpdateNS() {
 366 |       return lastUpdateNS;
 367 |     }
 368 |   }
 369 | 
 370 |   /** cache of most recently read data **/
 371 |   protected Map<AXRegister, Map<Integer, CachedValue>> cache =
 372 |     new HashMap<AXRegister, Map<Integer, CachedValue>>();
 373 | 
 374 |   /** RXTX port owner name **/
 375 |   public static final String RXTX_PORT_OWNER_NAME = "BRBrain";
 376 | 
 377 |   /** timeout in ms to wait to open a port with RXTX **/
 378 |   public static final int RXTX_OPEN_TIMEOUT_MS = 1000;
 379 | 
 380 |   /** default receive timout in ms **/
 381 |   public static final int DEF_TIMEOUT_MS = 1000;
 382 | 
 383 |   /** receive poll time in ms **/
 384 |   public static final int RECV_POLL_MS = 1;
 385 | 
 386 |   /** ms to wait before draining recv buffer in {@link #recover} **/
 387 |   public static final int RECOVER_MS = 500;
 388 | 
 389 |   /** default baudrate for RXTX **/
 390 |   public static final int RXTX_DEF_BAUD_RATE = 115200;
 391 | 
 392 |   /**
 393 |    * NOTE: do not reference any RXTX classes in static initializers to avoid
 394 |    * runtime NoClassDefFoundError when using the ADMIN jar without RXTX on
 395 |    * systems with no RXTX jar.
 396 |    **/
 397 | 
 398 |   /** CM-5 status bit **/
 399 |   public static final int S_PC_TIMEOUT =                  (1<<0);
 400 | 
 401 |   /** CM-5 status bit **/
 402 |   public static final int S_DYNAMIXEL_TIMEOUT =           (1<<1);
 403 | 
 404 |   /** CM-5 status bit **/
 405 |   public static final int S_INVALID_PC_COMMAND =          (1<<2);
 406 | 
 407 |   /** CM-5 status bit **/
 408 |   public static final int S_INVALID_DYNAMIXEL_RESPONSE =  (1<<3);
 409 | 
 410 |   /** CM-5 status bit **/
 411 |   public static final int S_PC_RX_OVERFLOW =              (1<<4);
 412 | 
 413 |   /** CM-5 status bit **/
 414 |   public static final int S_DYNAMIXEL_RX_OVERFLOW =       (1<<5);
 415 | 
 416 |   /** CM-5 status bit **/
 417 |   public static final int S_PC_CHECKSUM_ERROR =           (1<<6);
 418 | 
 419 |   /** CM-5 status bit **/
 420 |   public static final int S_DYNAMIXEL_CHECKSUM_ERROR =    (1<<7);
 421 | 
 422 |   /** maximum number of dynamixels in a format **/
 423 |   public static final int MAX_DYNAMIXELS = 32;
 424 | 
 425 |   /** read format **/
 426 |   protected static final int F_READ = 0;
 427 | 
 428 |   /** write format **/
 429 |   protected static final int F_WRITE = 1;
 430 | 
 431 |   /** set format instructions **/
 432 |   protected static final Instruction[] FMT_INSTRUCTION =
 433 |     new Instruction[] {
 434 |     Instruction.I_SET_READ_FORMAT,
 435 |     Instruction.I_SET_WRITE_FORMAT
 436 |   };
 437 | 
 438 |   /** baudrate at which the CM-5 bootloader likes to talk **/
 439 |   public static final int CM5_BOOTLOADER_BAUDRATE = 57600;
 440 | 
 441 |   /** timeout for response from the CM-5 during {@link #flashCM5} **/
 442 |   public static final double FLASH_TIMEOUT_MS = 10e3;
 443 | 
 444 |   /** delay between stages of the {@link #flashCM5} procedure **/
 445 |   public static final int FLASH_DELAY_MS = 1000;
 446 | 
 447 |   /** CM-5 bootloader welcome message **/
 448 |   public static final String CM5_BOOTLOADER_MSG =
 449 |     "SYSTEM O.K. (CM5 Boot loader";
 450 | 
 451 |   /** ADC channel connected to the positive battery terminal **/
 452 |   public static final int CHANNEL_POS = 0;
 453 | 
 454 |   /** ADC channel connected to the negative battery terminal **/
 455 |   public static final int CHANNEL_NEG = 1;
 456 | 
 457 |   /** ADC channel connected to the thermistor **/
 458 |   public static final int CHANNEL_THERM = 2;
 459 |  
 460 |   /** most recently read raw 8-bit ADC values **/
 461 |   protected int adcValue[] = new int[3];
 462 | 
 463 |   /**
 464 |    * <p>Compose a human-readable string of CM-5 status from a CM-5 status
 465 |    * byte.</p>
 466 |    *
 467 |    * @param status the bitpacked status value
 468 |    * @param buf the buffer to append, or null to make one
 469 |    *
 470 |    * @return the buffer to which the string was appended
 471 |    **/
 472 |   public static final StringBuffer statusToString(int status,
 473 |                                                   StringBuffer buf) {
 474 |     if (buf == null)
 475 |       buf = new StringBuffer();
 476 | 
 477 |     int n = 0;
 478 | 
 479 |     if ((status & S_PC_TIMEOUT) != 0) {
 480 |       if (n++ > 0)
 481 |         buf.append(", ");
 482 |       buf.append("PC_TIMEOUT");
 483 |     }
 484 | 
 485 |     if ((status & S_DYNAMIXEL_TIMEOUT) != 0) {
 486 |       if (n++ > 0)
 487 |         buf.append(", ");
 488 |       buf.append("DYNAMIXEL_TIMEOUT");
 489 |     }
 490 | 
 491 |     if ((status & S_INVALID_PC_COMMAND) != 0) {
 492 |       if (n++ > 0)
 493 |         buf.append(", ");
 494 |       buf.append("INVALID_PC_COMMAND");
 495 |     }
 496 |     
 497 |     if ((status & S_INVALID_DYNAMIXEL_RESPONSE) != 0) {
 498 |       if (n++ > 0)
 499 |         buf.append(", ");
 500 |       buf.append("INVALID_DYNAMIXEL_RESPONSE");
 501 |     }
 502 |     
 503 |     if ((status & S_PC_RX_OVERFLOW) != 0) {
 504 |       if (n++ > 0)
 505 |         buf.append(", ");
 506 |       buf.append("PC_RX_OVERFLOW");
 507 |     }
 508 |     
 509 |     if ((status & S_DYNAMIXEL_RX_OVERFLOW) != 0) {
 510 |       if (n++ > 0)
 511 |         buf.append(", ");
 512 |       buf.append("DYNAMIXEL_RX_OVERFLOW");
 513 |     }
 514 | 
 515 |     if ((status & S_PC_CHECKSUM_ERROR) != 0) {
 516 |       if (n++ > 0)
 517 |         buf.append(", ");
 518 |       buf.append("PC_CHECKSUM_ERROR");
 519 |     }
 520 | 
 521 |     if ((status & S_DYNAMIXEL_CHECKSUM_ERROR) != 0) {
 522 |       if (n++ > 0)
 523 |         buf.append(", ");
 524 |       buf.append("DYNAMIXEL_CHECKSUM_ERROR");
 525 |     }
 526 | 
 527 |     if ((status&0xff00) != 0) {
 528 |       buf.append(" (");
 529 |       buf.append(Integer.toString((status&0xff00)>>8));
 530 |       buf.append(" dynamixel retries)");
 531 |     }
 532 | 
 533 |     return buf;
 534 |   }
 535 | 
 536 |   /** Covers {@link #statusToString(int, StringBuffer)} **/
 537 |   public static final String statusToString(int status) {
 538 |     return statusToString(status, null).toString();
 539 |   }
 540 | 
 541 |   /**
 542 |    * <p>Convenience method to query the list of available ports according to
 543 |    * RXTX.</p>
 544 |    *
 545 |    * @param printStream the stream on which to print the list,
 546 |    * e.g. <code>System.out</code>
 547 |    *
 548 |    * @return printStream
 549 |    **/
 550 |   public static PrintStream listPorts(PrintStream printStream) {
 551 |    
 552 |     //do this here as it prints stuff
 553 |     Enumeration e = CommPortIdentifier.getPortIdentifiers();
 554 | 
 555 |     printStream.println("\nAvailable Ports:\n");
 556 | 
 557 |     while(e.hasMoreElements()) {
 558 |       
 559 |       CommPortIdentifier id = (CommPortIdentifier) (e.nextElement());
 560 | 
 561 |       printStream.println("name: " + id.getName());
 562 |       printStream.println("  type: " +
 563 |                           ((id.getPortType()==CommPortIdentifier.PORT_SERIAL) ?
 564 |                            "serial" : "parallel"));
 565 |       printStream.println("  current owner: " + id.getCurrentOwner());
 566 |     }
 567 | 
 568 |     return printStream;
 569 |   }
 570 | 
 571 |   /** covers {@link #listPorts(PrintStream)}, uses System.out **/
 572 |   public static PrintStream listPorts() {
 573 |     return listPorts(System.out);
 574 |   }
 575 | 
 576 |   /**
 577 |    * <p>{@link BRBrain} protocol packet instructions, see {@link BRBrain} class
 578 |    * header doc for details.</p>
 579 |    **/
 580 |   public static enum Instruction {
 581 | 
 582 |     I_PING(0xf0),
 583 |     I_SET_READ_FORMAT(0xf1),
 584 |     I_SET_WRITE_FORMAT(0xf2),
 585 |     I_READ_DATA(0xf3),
 586 |     I_WRITE_DATA(0xf4),
 587 |     I_STATUS(0xfa),
 588 |     I_DATA(0xfb);
 589 | 
 590 |     public final int code;
 591 | 
 592 |     Instruction(int code) {
 593 |       this.code = code;
 594 |     }
 595 |   }
 596 | 
 597 |   /**
 598 |    * <p>Setup an BRBrain talking to a CM-5 on the specified serial port at the
 599 |    * specified baud rate.</p>
 600 |    *
 601 |    * <p>Note that this method does no transmission on the port, and does not
 602 |    * verify the presence of a CM-5 running the correct firmware.</p>
 603 |    *
 604 |    * <p>A {@link #recover} is performed.</p>
 605 |    *
 606 |    * @param portName an RXTX serial port name, see {@link #listPorts} 
 607 |    * @param baudRate the baud rate in bits per second
 608 |    *
 609 |    * @exception IOException if there was a problem opening the port
 610 |    * @exception InterruptedException if interrupted during {@link #recover}
 611 |    * @exception IllegalStateException if the specified port is not recognized
 612 |    * by RXTX as a serial port, or if RXTX silently failed to open the port 
 613 |    **/
 614 |   public BRBrain(String portName, int baudRate)
 615 |     throws IOException, InterruptedException {
 616 | 
 617 |     if (portName == null)
 618 |       throw new IllegalArgumentException("null port name");
 619 | 
 620 |     try {
 621 | 
 622 |       CommPortIdentifier id = CommPortIdentifier.getPortIdentifier(portName);
 623 |       CommPort port = id.open(RXTX_PORT_OWNER_NAME, RXTX_OPEN_TIMEOUT_MS);
 624 | 
 625 |       if (!(port instanceof SerialPort))
 626 |         throw new IllegalStateException("RXTX port \"" + portName +
 627 |                                         "\" is not a SerialPort");
 628 | 
 629 |       serialPort = (SerialPort) port;
 630 | 
 631 |       serialPort.disableReceiveFraming();
 632 |       serialPort.disableReceiveThreshold();
 633 |       serialPort.disableReceiveTimeout();
 634 | 
 635 |       setBaudRate(baudRate);
 636 | 
 637 |       serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
 638 | 
 639 |       toCM5 = serialPort.getOutputStream();
 640 |       fromCM5 = serialPort.getInputStream();
 641 | 
 642 |       Runtime.getRuntime().addShutdownHook(new Thread() {
 643 |           public void run() {
 644 |             close();
 645 |           } });
 646 |       
 647 | /*
 648 | 
 649 |   NOTE: Do not directly reference RXTX exceptions, by doing so you would
 650 |   require the RXTX jar to be present at runtime or a NoClassDefFoundError is
 651 |   thrown.  Instead a generic Exception clause handles these exceptions below.
 652 | 
 653 |     } catch (NoSuchPortException e) {
 654 | 
 655 |       IOException ioe = new IOException("no such port "+portName);
 656 |       ioe.initCause(e);
 657 |       throw ioe;
 658 | 
 659 |     } catch (PortInUseException e) {
 660 | 
 661 |       IOException ioe = new IOException("port "+portName+" in use");
 662 |       ioe.initCause(e);
 663 |       throw ioe;
 664 | 
 665 |     } catch (UnsupportedCommOperationException e) {
 666 | 
 667 |       IOException ioe = new IOException("unsupported comm operation");
 668 |       ioe.initCause(e);
 669 |       throw ioe;
 670 | 
 671 |     } 
 672 | */
 673 | 
 674 |     } catch (Exception e) {
 675 | 
 676 |       IOException ioe = new IOException("error opening RXTX port");
 677 |       ioe.initCause(e);
 678 | 
 679 |       throw ioe;
 680 |     }
 681 | 
 682 |     if (fromCM5 == null)
 683 |       throw new IllegalStateException("RXTX failed to provide input stream");
 684 | 
 685 |     if (toCM5 == null)
 686 |       throw new IllegalStateException("RXTX failed to provide output stream");
 687 | 
 688 |     recover();
 689 |   }
 690 | 
 691 |   /** covers {@link #BRBrain(String, int)} uses {@link #RXTX_DEF_BAUD_RATE} **/
 692 |   public BRBrain(String portName)
 693 |     throws IOException, InterruptedException {
 694 |     this(portName, RXTX_DEF_BAUD_RATE);
 695 |   }
 696 | 
 697 |   /**
 698 |    * <p>Same as {@link #BRBrain(String)} but connect to CM-5 via a file instead
 699 |    * of with RXTX.</p>
 700 |    **/
 701 |   public BRBrain(File port) throws IOException, InterruptedException {
 702 | 
 703 |     if (port == null)
 704 |       throw new IllegalArgumentException("null port");
 705 |     
 706 |     toCM5 = new FileOutputStream(port);
 707 |     fromCM5 = new FileInputStream(port);
 708 |   }
 709 |  
 710 |   /** waits {@link #RECOVER_MS} and then drains recv buf **/
 711 |   public synchronized void recover() throws IOException, InterruptedException {
 712 |     Thread.sleep(RECOVER_MS);
 713 |     drainFromCM5();
 714 |     checksum = 0;
 715 |   }
 716 | 
 717 |   /** drain {@link #fromCM5} **/
 718 |   protected synchronized void drainFromCM5()
 719 |     throws IOException, InterruptedException {
 720 |     while (fromCM5.available() != 0)
 721 |       recvByte(false);
 722 |   }
 723 | 
 724 |   /** attempt to set the baud rate, works only if using RXTX **/
 725 |   protected synchronized void setBaudRate(int baudRate) throws IOException {
 726 | 
 727 |     if (serialPort == null)
 728 |       throw new IllegalStateException("no serial port -- not using RXTX?");
 729 | 
 730 |     try {
 731 |       //NOTE: Do not make the databits, stopbits, parity, or flowcontrol
 732 |       //settings static class constants.  By doing so you would require the
 733 |       //RXTX jar to be present at runtime or a NoClassDefFoundError is thrown.
 734 |       serialPort.setSerialPortParams(baudRate,
 735 |                                      SerialPort.DATABITS_8,
 736 |                                      SerialPort.STOPBITS_1,
 737 |                                      SerialPort.PARITY_NONE);
 738 |     } catch (UnsupportedCommOperationException e) {
 739 |       IOException ioe = new IOException("unsupported comm operation");
 740 |       ioe.initCause(e);
 741 |       throw ioe;
 742 |     }
 743 |   }
 744 | 
 745 |   /** 
 746 |    * <p>Interact with the user and the CM-5 bootloader to flash new firmware to
 747 |    * the CM-5.</p>
 748 |    *
 749 |    * @param binary an InputStream from which the new firmware binary is read
 750 |    * @param log an output stream to which progress and prompt messages are
 751 |    * displayed
 752 |    *
 753 |    * @return the number of bytes flashed, negative of that if verify failed
 754 |    *
 755 |    * @exception IOException if there was an input our output error
 756 |    * @exception InterruptedException if there was a timeout
 757 |    **/
 758 |   public synchronized int flashCM5(InputStream binary, PrintStream log)
 759 |     throws IOException, InterruptedException {
 760 |     
 761 |     int bytesSent = 0;
 762 |     int b;
 763 |  
 764 |     if (binary == null)
 765 |       throw new IllegalArgumentException("null binary");
 766 | 
 767 |     if (log != null) {
 768 |       log.println("waiting for CM5 reset, press MODE");
 769 |       log.flush();
 770 |     }
 771 | 
 772 |     drainFromCM5();
 773 | 
 774 |     int baudRateWas = -1;
 775 | 
 776 |     if (serialPort != null) {
 777 | 
 778 |       baudRateWas = serialPort.getBaudRate();
 779 |       
 780 |       if (baudRateWas != CM5_BOOTLOADER_BAUDRATE) {
 781 |         
 782 |         if (log != null) {
 783 |           log.println("switching from "+baudRateWas+
 784 |                       "bps to "+CM5_BOOTLOADER_BAUDRATE+"bps");
 785 |           log.flush();
 786 |         }
 787 |         
 788 |         setBaudRate(CM5_BOOTLOADER_BAUDRATE);
 789 |       }
 790 |     } else {
 791 |       if (log != null) {
 792 |         log.println(
 793 |           "not using RXTX so no control of baudrate, please ensure it is "+
 794 |           CM5_BOOTLOADER_BAUDRATE+"bps");
 795 |         log.flush();
 796 |       }
 797 |     }
 798 | 
 799 |     double deadline = System.nanoTime() + FLASH_TIMEOUT_MS*1e6;
 800 | 
 801 |     int nextMsgChar = 0;
 802 |     while (nextMsgChar < CM5_BOOTLOADER_MSG.length()) {
 803 |       
 804 |       if (System.nanoTime() > deadline)
 805 |         throw new IOException("timout waiting for CM-5 reset");
 806 |       
 807 |       if (nextMsgChar == 0) {
 808 |         sendByte('#', false);
 809 |         Thread.sleep(50); //important
 810 |       }
 811 |       
 812 |       while ((fromCM5.available() != 0) &&
 813 |              (nextMsgChar < CM5_BOOTLOADER_MSG.length())) {
 814 |         if (recvByte(false) == CM5_BOOTLOADER_MSG.charAt(nextMsgChar))
 815 |           nextMsgChar++;
 816 |         else
 817 |           nextMsgChar = 0;
 818 |       }
 819 |     }
 820 | 
 821 |     if (log != null) {
 822 |       log.println("detected reset to CM-5 bootloader");
 823 |       log.println("initiating binary upload to address 0");
 824 |       log.flush();
 825 |     }
 826 | 
 827 |     Thread.sleep(FLASH_DELAY_MS);
 828 | 
 829 |     drainFromCM5();
 830 | 
 831 |     sendByte('\n', false);
 832 |     sendByte('l', false);
 833 |     sendByte('\n', false);
 834 | 
 835 |     Thread.sleep(FLASH_DELAY_MS);
 836 | 
 837 |     drainFromCM5();
 838 | 
 839 |     if (log != null) {
 840 |       log.print("uploading binary");
 841 |       log.flush();
 842 |     }
 843 | 
 844 |     ByteArrayOutputStream verifyStream = new ByteArrayOutputStream(128*1024);
 845 | 
 846 |     for (b = binary.read(); b >= 0; b = binary.read(), bytesSent++) {
 847 | 
 848 |       sendByte(b, false);
 849 | 
 850 |       verifyStream.write(b);
 851 | 
 852 |       if ((log != null) && (bytesSent > 0) && (bytesSent%100 == 0)) {
 853 |         log.print(".");
 854 |         log.flush();
 855 |       }
 856 |     }
 857 | 
 858 |     if (log != null) {
 859 |       log.println("\nflashed "+bytesSent+" bytes");
 860 |       log.println("upload complete");
 861 |       log.flush();
 862 |     }
 863 | 
 864 |     if (log != null) {
 865 |       log.println("verifying");
 866 |       log.flush();
 867 |     }
 868 | 
 869 |     Thread.sleep(FLASH_DELAY_MS);
 870 | 
 871 |     drainFromCM5();
 872 | 
 873 |     sendByte('\n', false);
 874 |     sendByte('u', false);
 875 |     sendByte('p', false);
 876 |     sendByte(' ', false);
 877 |     sendByte('0', false);
 878 |     sendByte(',', false);
 879 |     sendByte(' ', false);
 880 |     String hexBytesSent = Integer.toHexString(bytesSent);
 881 |     for (int i = 0; i < hexBytesSent.length(); i++)
 882 |       sendByte(hexBytesSent.charAt(i), false);
 883 |     sendByte('\n', false);
 884 | 
 885 |     Thread.sleep(FLASH_DELAY_MS);
 886 | 
 887 |     drainFromCM5();
 888 | 
 889 |     if (log != null) {
 890 |       log.println("press START to initiate verify");
 891 |       log.flush();
 892 |     }
 893 | 
 894 |     byte[] verifyArray = verifyStream.toByteArray();
 895 | 
 896 |     int verifyFailedAt = -1;
 897 | 
 898 |     for (int i = 0; i < bytesSent; i++) {
 899 |       
 900 |       deadline = System.nanoTime() + FLASH_TIMEOUT_MS*1e6;
 901 |       while (fromCM5.available() == 0) 
 902 |         if (System.nanoTime() > deadline)
 903 |           throw new IOException("timout waiting for verify data");
 904 | 
 905 |       if ((log != null) && (i == 0)) {
 906 |         log.print("verifying");
 907 |         log.flush();
 908 |       }
 909 | 
 910 |       if ((log != null) && (i > 0) && (i%100 == 0)) {
 911 |         log.print(".");
 912 |         log.flush();
 913 |       }
 914 |         
 915 |       b = recvByte(false);
 916 | 
 917 |       if ((verifyFailedAt < 0) && ((b&0xff) != (verifyArray[i]&0xff)))
 918 |         verifyFailedAt = i;
 919 |     }
 920 | 
 921 |     if (log != null) {
 922 |       log.print("\n");
 923 |       log.flush();
 924 |     }
 925 | 
 926 |     if (log != null) {
 927 | 
 928 |       if (verifyFailedAt >= 0) {
 929 |         log.println("warning, verify failed at byte "+verifyFailedAt);
 930 |         log.println("value should have been "+
 931 |                     Integer.toHexString(verifyArray[verifyFailedAt]&0xff));
 932 |       } else {
 933 |         log.println("verified "+bytesSent+" bytes");
 934 |       }
 935 |       
 936 |       log.flush();
 937 |     }
 938 | 
 939 |     Thread.sleep(FLASH_DELAY_MS);
 940 | 
 941 |     drainFromCM5();
 942 | 
 943 |     if (serialPort != null) {
 944 |       if (baudRateWas != CM5_BOOTLOADER_BAUDRATE) {
 945 |         
 946 |         if (log != null) {
 947 |           log.println("returning to "+baudRateWas+"bps");
 948 |           log.flush();
 949 |         }
 950 |         
 951 |         setBaudRate(baudRateWas);
 952 |       }
 953 |     }
 954 | 
 955 |     if (log != null) {
 956 |       log.println("waiting for CM5 reset, press MODE");
 957 |       log.flush();
 958 |     }
 959 | 
 960 |     deadline = System.nanoTime() + FLASH_TIMEOUT_MS*1e6;
 961 | 
 962 |     while (fromCM5.available() == 0)
 963 |       if (System.nanoTime() > deadline)
 964 |         throw new IOException("timout waiting for CM-5 reset");
 965 | 
 966 |     b = recvByte(false);
 967 | 
 968 |     if (b != 0xff) {
 969 | 
 970 |       if (log != null) {
 971 |         log.println(
 972 |           "warning reset byte was 0x"+Integer.toHexString(b)+" not 0xff");
 973 |         log.flush();
 974 |       }
 975 | 
 976 |     } else {
 977 | 
 978 |       if (log != null) {
 979 |         log.println("detected CM-5 reset to user code");
 980 |         log.flush();
 981 |       }
 982 |     }
 983 | 
 984 |     if (verifyFailedAt >= 0)
 985 |       bytesSent = -bytesSent;
 986 | 
 987 |     return bytesSent;
 988 |   }
 989 | 
 990 |   /** covers {@link #flashCM5(InputStream, PrintStream)} **/
 991 |   public synchronized int flashCM5(File binary, PrintStream log)
 992 |     throws IOException, InterruptedException {
 993 |     return flashCM5(new FileInputStream(binary), System.out);
 994 |   }
 995 | 
 996 |   /** covers {@link #flashCM5(File, PrintStream)} **/
 997 |   public synchronized int flashCM5(String binary, PrintStream log)
 998 |     throws IOException, InterruptedException {
 999 |     return flashCM5(new File(binary), System.out);
1000 |   }
1001 | 
1002 |   /** covers {@link #flashCM5(String, PrintStream)}, uses System.out **/
1003 |   public synchronized int flashCM5(String binary)
1004 |     throws IOException, InterruptedException {
1005 |     return flashCM5(binary, System.out);
1006 |   }
1007 | 
1008 |   /**
1009 |    * <p>Issue {@link Instruction#I_PING} packet requesting ping of the
1010 |    * indicated dynamixel.</p>
1011 |    *
1012 |    * @param id the id of the dynamixel to ping in the closed interval [0,
1013 |    * {@link AXRegister#MAX_DYNAMIXEL_ID}] or 255 to ping the CM-5 itself
1014 |    *
1015 |    * @return the CM-5 status and retry bytes as the 0th and 1st byte of the
1016 |    * returned int
1017 |    *
1018 |    * @exception IOException if there was a communication error
1019 |    * @exception InterruptedException if the calling thread was interrupted
1020 |    * while waiting for response bytes from the CM-5
1021 |    **/
1022 |   public synchronized int pingDynamixel(int id)
1023 |     throws IOException, InterruptedException {
1024 | 
1025 |     if ((AX12_ID.check(id) != 0) && (id != 255))
1026 |       throw new IllegalArgumentException("invalid id "+id);
1027 | 
1028 |     startSendPacket(Instruction.I_PING);
1029 |     sendByte(id);
1030 |     endSendPacket();
1031 | 
1032 |     return recvStatus();
1033 |   }
1034 | 
1035 |   /** covers {@link #pingDynamixel}, uses id 255 to ping the CM-5 itself **/
1036 |   public synchronized int pingCM5()
1037 |     throws IOException, InterruptedException {
1038 |     return pingDynamixel(255);
1039 |   }
1040 | 
1041 |   /**
1042 |    * <p>Scan for presence of dynamixels with IDs in the closed interval [0,
1043 |    * <code>maxID</code>].</p>
1044 |    *
1045 |    * @param dynamixels presence written here, (re)consed if null or too short 
1046 |    *
1047 |    * @return the array of dynamixel presence
1048 |    **/
1049 |   public synchronized boolean[] scan(boolean[] dynamixels, int maxID)
1050 |     throws IOException, InterruptedException {
1051 | 
1052 |     if (maxID > MAX_DYNAMIXEL_ID)
1053 |       throw new IllegalArgumentException("maxID can be at most "+
1054 |                                          MAX_DYNAMIXEL_ID);
1055 | 
1056 |     if ((dynamixels == null) || (dynamixels.length < (maxID+1)))
1057 |       dynamixels = new boolean[maxID+1];
1058 | 
1059 |     for (int i = 0; i <= maxID; i++)
1060 |       dynamixels[i] = (pingDynamixel(i) == 0);
1061 | 
1062 |     return dynamixels;
1063 |   }
1064 | 
1065 |   /** covers {@link #scan(boolean[], int)}, always conses **/
1066 |   public synchronized boolean[] scan(int maxID)
1067 |     throws IOException, InterruptedException {
1068 |     return scan(null, maxID);
1069 |   }
1070 |   
1071 |   /**
1072 |    * <p>Set the read format as described in the class header doc.</p>
1073 |    *
1074 |    * <p>The format is both cached for further use on the host and is
1075 |    * transmitted to the CM-5.</p>
1076 |    *
1077 |    * @param id the id of each dynamixel to read, in order.  The number of
1078 |    * dynamixels in the read format is considered to be the number of contiguous
1079 |    * valid dynamixel IDs (i.e. ids in the interval [0, {@link
1080 |    * AXRegister#MAX_DYNAMIXEL_ID}]) starting with the zeroth entry in the id
1081 |    * array.  A copy is made.
1082 |    * @param start the start register on each dynamixel in the read format, must
1083 |    * either be length 1, implying same start reg for all dynamixels, or have at
1084 |    * least as many non-null initial entries as the number of read dynamixels.
1085 |    * A copy is made.
1086 |    * @param num the number of registers to read on each dynamixel in the read
1087 |    * format, must either be length 1, implying same num regs for all
1088 |    * dynamixels, or have at least as many valid initial entries as the number
1089 |    * of read dynamixels.  A copy is made.
1090 |    *
1091 |    * @return the returned {@link Instruction#I_STATUS} byte from the CM-5
1092 |    *
1093 |    * @exception IllegalArgumentException if the number of dynamixels in the
1094 |    * format exceeds {@link #MAX_DYNAMIXELS}, if a dynamixel ID is used more
1095 |    * than once in the format, if the latter args are too short, if the number
1096 |    * of registers is negative, or if the span of registers extends beyond the
1097 |    * last register
1098 |    * @exception IOException if there was a communication error
1099 |    * @exception InterruptedException if the calling thread was interrupted
1100 |    * while waiting for response bytes from the CM-5
1101 |    **/
1102 |   public synchronized int setReadFormat(int[] id,
1103 |                                         AXRegister[] start, int[] num) 
1104 |     throws IOException, InterruptedException {
1105 |     return setFormat(F_READ, id, start, num);
1106 |   }
1107 | 
1108 |   /** 
1109 |    * <p>Similar to {@link #setReadFormat}.</p>
1110 |    *
1111 |    * @exception IllegalArgumentException if any register block {@link
1112 |    * AXRegister#containsReadOnlyRegs}
1113 |    **/
1114 |   public synchronized int setWriteFormat(int[] id,
1115 |                                          AXRegister[] start, int[] num)
1116 |     throws IOException, InterruptedException {
1117 |     return setFormat(F_WRITE, id, start, num);
1118 |   }
1119 |    
1120 |   /** verify the first <i>n</i> dynamixel ids in the given array are unique **/
1121 |   public static void checkAXIDs(int[] id, int n) {
1122 | 
1123 |     Set<Integer> axIDSet = new LinkedHashSet<Integer>();
1124 | 
1125 |     for (int i = 0; i < n; i++) {
1126 |       if (axIDSet.contains(id[i]))
1127 |         throw new IllegalArgumentException(
1128 |           "dynamixel ID "+id[i]+" used more than once");
1129 |       else
1130 |         axIDSet.add(id[i]);
1131 |     }
1132 |   }
1133 | 
1134 |   /** covers {@link #checkAXIDs(int[], int)}, checks all **/
1135 |   public static void checkAXIDs(int[] id) {
1136 |     checkAXIDs(id, id.length);
1137 |   }
1138 | 
1139 |   /**
1140 |    * <p>Common impl of {@link #setReadFormat} and {@link #setWriteFormat}.</p>
1141 |    *
1142 |    * @param f the format
1143 |    * @param id the dynamixel ids to set
1144 |    * @param start the start regs to set, either length 1 and all start regs set
1145 |    * the same or at least as long as the number of dynamixels in the format
1146 |    * @param num the reg numbers to set, either length 1 and all start regs set
1147 |    * the same or at least as long as the number of dynamixels in the format
1148 |    *
1149 |    * @return the CM-5 status
1150 |    **/
1151 |   protected synchronized int setFormat(int f,
1152 |                                        int[] id,
1153 |                                        AXRegister[] start, int[] num)
1154 |     throws IOException, InterruptedException {
1155 | 
1156 |     int n;
1157 |     for (n = 0; (n < MAX_DYNAMIXELS) && (id != null) && (n < id.length); n++)
1158 |       if (AX12_ID.check(id[n]) != 0)
1159 |         break;
1160 | 
1161 |     checkAXIDs(id, n);
1162 | 
1163 |     if (((start.length > 1) && (start.length < n)) || 
1164 |         ((num.length > 1) && (num.length < n)))
1165 |       throw new IllegalArgumentException("latter args insufficient length");
1166 | 
1167 |     for (int i = 0; i < n; i++) {
1168 | 
1169 |       AXRegister s = (start.length > 1) ? start[i] : start[0];
1170 |       int u = (num.length > 1) ? num[i] : num[0];
1171 | 
1172 |       if (((s == null) && (u > 0)) ||
1173 |           (u < 0) ||
1174 |           ((s.ordinal+u) > s.getNumRegisters()) ||
1175 |           ((f == F_WRITE) && containsReadOnlyRegs(s, u)))
1176 |         throw new IllegalArgumentException("latter args invalid at index "+i);
1177 |     }
1178 | 
1179 |     axID[f] = dup(id, axID[f], n);
1180 | 
1181 |     if (start.length > 1) {
1182 |       startReg[f] = AXRegister.dup(start, startReg[f], n);
1183 |     } else {
1184 |       if (startReg[f].length < n)
1185 |         startReg[f] = new AXRegister[n];
1186 |       for (int i = 0; i < n; i++)
1187 |         startReg[f][i] = start[0];
1188 |     }
1189 | 
1190 |     if (num.length > 1) {
1191 |       numReg[f] = dup(num, numReg[f], n);
1192 |     } else {
1193 |       if (numReg[f].length < n)
1194 |         numReg[f] = new int[n];
1195 |       for (int i = 0; i < n; i++)
1196 |         numReg[f][i] = num[0];
1197 |     }
1198 | 
1199 |     numDynamixels[f] = n;
1200 | 
1201 |     startSendPacket(FMT_INSTRUCTION[f]);
1202 | 
1203 |     sendByte(n);
1204 | 
1205 |     totalNumRegs[f] = 0;
1206 | 
1207 |     for (int i = 0; i < n; i++) {
1208 | 
1209 |       sendByte(id[i]);
1210 | 
1211 |       if (numReg[f][i] > 0) {
1212 | 
1213 |         sendByte(startReg[f][i].startAddr);
1214 | 
1215 |         AXRegister lastReg =
1216 |           startReg[f][i].getRelativeRegister(numReg[f][i]-1);
1217 | 
1218 |         int nb = (lastReg.startAddr+lastReg.width)-startReg[f][i].startAddr;
1219 | 
1220 | //        System.err.println("nb: "+nb);
1221 | 
1222 |         sendByte(nb);
1223 | 
1224 |       } else {
1225 |         sendByte(0);
1226 |         sendByte(0);
1227 |       }
1228 | 
1229 |       totalNumRegs[f] += numReg[f][i];
1230 |     }
1231 | 
1232 |     endSendPacket();
1233 | 
1234 |     return recvStatus();
1235 |   }
1236 | 
1237 |   /**
1238 |    * <p>Get a copy of the most recently {@link #setReadFormat}, see which.</p>
1239 |    *
1240 |    * <p>Arg <i>id</i> must either be null or at least as long as the number of
1241 |    * dynamixels in the format.</p>
1242 |    *
1243 |    * <p>Args <i>star</i> and <i>num</i> must either be null, length 1 and all
1244 |    * data in format equal, or at least as long as the number of dynamixels in
1245 |    * the format.</p>
1246 |    *
1247 |    * @return the number of dynamixels in the current read format
1248 |    **/
1249 |   public synchronized int getReadFormat(int[] id,
1250 |                                         AXRegister[] start, int[] num) {
1251 |     return getFormat(F_READ, id, start, num);
1252 |   }
1253 | 
1254 |   /** get the number of dynamixels in the current read format **/
1255 |   public synchronized int getNumReadDynamixels() {
1256 |     return numDynamixels[F_READ];
1257 |   }
1258 | 
1259 |   /** get the total number of registers in the current read format **/
1260 |   public synchronized int getTotalNumReadRegs() {
1261 |     return totalNumRegs[F_READ];
1262 |   }
1263 | 
1264 |   /**
1265 |    * <p>Get a copy of the most recently {@link #setWriteFormat}, see which.</p>
1266 |    *
1267 |    * <p>Args handled similar to {@link #getReadFormat}.</p>
1268 |    *
1269 |    * @return the number of dynamixels in the current write format
1270 |    **/
1271 |   public synchronized int getWriteFormat(int[] id,
1272 |                                          AXRegister[] start, int[] num)  {
1273 |     return getFormat(F_WRITE, id, start, num);
1274 |   }
1275 | 
1276 |   /** get the number of dynamixels in the current write format **/
1277 |   public synchronized int getNumWriteDynamixels() {
1278 |     return numDynamixels[F_WRITE];
1279 |   }
1280 | 
1281 |   /** get the total number of registers in the current write format **/
1282 |   public synchronized int getTotalNumWriteRegs() {
1283 |     return totalNumRegs[F_WRITE];
1284 |   }
1285 | 
1286 |   /**
1287 |    * <p>Common impl of {@link #getReadFormat} and {@link #getWriteFormat}.</p>
1288 |    *
1289 |    * @param f the format
1290 |    * @param id the dynamixel ids to get
1291 |    * @param start the start regs to get, length 1 and all start regs same
1292 |    * through format or length geq number of dynamixels in format
1293 |    * @param num the reg numbers to get, length 1 and all start regs same
1294 |    * through format or length geq number of dynamixels in format
1295 |    *
1296 |    * @return the number of dynamixels in the format
1297 |    **/
1298 |   protected synchronized int getFormat(int f,
1299 |                                        int[] id,
1300 |                                        AXRegister[] start, int[] num) {
1301 |     int n = numDynamixels[f];
1302 | 
1303 |     if (id != null)
1304 |       System.arraycopy(axID[f], 0, id, 0, n);
1305 |     
1306 |     if (start != null) {
1307 |       if (start.length == 1) {
1308 |         checkFmtSame(startReg[f], numDynamixels[f]);
1309 |         start[0] = startReg[f][0];
1310 |       } else {
1311 |         System.arraycopy(startReg[f], 0, start, 0, n);
1312 |       }
1313 |     }
1314 | 
1315 |     if (num != null) {
1316 |       if (num.length == 1) {
1317 |         checkFmtSame(numReg[f], numDynamixels[f]);
1318 |         num[0] = numReg[f][0];
1319 |       } else {
1320 |         System.arraycopy(numReg[f], 0, num, 0, n);
1321 |       }
1322 |     }
1323 |     
1324 |     return n;
1325 |   }
1326 | 
1327 |   /** ensure that all initial <i>n</i> elements of <i>start</i> are equal **/
1328 |   protected void checkFmtSame(AXRegister[] start, int n) {
1329 |     AXRegister v = start[0];
1330 |     for (int i = 0; i < n; i++)
1331 |       if (start[i] != v)
1332 |         throw new IllegalStateException(
1333 |           "start registers not the same across all dynamixels");
1334 |   }
1335 | 
1336 |   /** ensure that all initial <i>n</i> elements of <i>num</i> are equal **/
1337 |   protected void checkFmtSame(int[] num, int n) {
1338 |     int v = num[0];
1339 |     for (int i = 0; i < n; i++)
1340 |       if (num[i] != v)
1341 |         throw new IllegalStateException(
1342 |           "num registers not the same across all dynamixels");
1343 |   }
1344 | 
1345 |   /**
1346 |    * <p>Covers {@link #setReadFormat}, uses {@link #parseFormatArgs}, for
1347 |    * jscheme API convenience.</p>
1348 |    **/
1349 |   public int setReadFormat(Object[] args)
1350 |     throws IOException, InterruptedException {
1351 |     args = parseFormatArgs(args);
1352 |     return setReadFormat((int[]) args[0],
1353 |                          (AXRegister[]) args[1],
1354 |                          (int[]) args[2]);
1355 |   }
1356 | 
1357 |   /**
1358 |    * <p>Covers {@link #setWriteFormat}, uses {@link #parseFormatArgs}, for
1359 |    * jscheme API convenience.</p>
1360 |    **/
1361 |   public int setWriteFormat(Object[] args)
1362 |     throws IOException, InterruptedException {
1363 |     args = parseFormatArgs(args);
1364 |     return setWriteFormat((int[]) args[0],
1365 |                           (AXRegister[]) args[1],
1366 |                           (int[]) args[2]);
1367 |   }
1368 | 
1369 |   /** 
1370 |    * <p>Parse read or write format from a contiguous array, for jscheme API
1371 |    * convienience.</p>
1372 |    *
1373 |    * @param args an integer multiple of &lt;Integer, {@link AXRegister},
1374 |    * Integer&gt; triples
1375 |    *
1376 |    * @return a three-element array consisting of an array of int, an array of
1377 |    * {@link AXRegister}, and an array of int, giving the dynamixel id, start
1378 |    * register and number of registers respectively in the format
1379 |    **/
1380 |   protected Object[] parseFormatArgs(Object[] args) {
1381 | 
1382 |     if ((args != null) && ((args.length%3) != 0))
1383 |       throw new IllegalArgumentException(
1384 |         "args must be an integer multiple of <int, AXRegister, int> triples");
1385 | 
1386 |     int n = (args != null) ? args.length/3 : 0;
1387 | 
1388 |     int[] id = new int[n];
1389 |     AXRegister[] start = new AXRegister[n];
1390 |     int[] num = new int[n];
1391 | 
1392 |     for (int i = 0; i < n; i++) {
1393 |       id[i] = ((Integer) (args[3*i])).intValue();
1394 |       start[i] = (AXRegister) (args[3*i+1]);
1395 |       num[i] = ((Integer) (args[3*i+2])).intValue();
1396 |     }
1397 |     
1398 |     return new Object[] {id, start, num};
1399 |   }
1400 | 
1401 |   /**
1402 |    * <p>Covers {@link #getReadFormat}, uses {@link #packFormat}, for jscheme
1403 |    * API convenience.</p>
1404 |    **/
1405 |   public Object[] getReadFormat() {
1406 | 
1407 |     int n = getNumReadDynamixels();
1408 | 
1409 |     int[] id = new int[n];
1410 |     AXRegister[] start = new AXRegister[n];
1411 |     int[] num = new int[n];
1412 | 
1413 |     getReadFormat(id, start, num);
1414 | 
1415 |     return packFormat(n, id, start, num);
1416 |   }
1417 | 
1418 |   /**
1419 |    * <p>Covers {@link #getWriteFormat}, uses {@link #packFormat}, for jscheme
1420 |    * API convenience.</p>
1421 |    **/
1422 |   public Object[] getWriteFormat() {
1423 | 
1424 |     int n = getNumWriteDynamixels();
1425 | 
1426 |     int[] id = new int[n];
1427 |     AXRegister[] start = new AXRegister[n];
1428 |     int[] num = new int[n];
1429 | 
1430 |     getWriteFormat(id, start, num);
1431 | 
1432 |     return packFormat(n, id, start, num);
1433 |   }
1434 | 
1435 |   /** 
1436 |    * <p>Pack read or write format from a contiguous array, for convienience of
1437 |    * the scheme API.</p>
1438 |    *
1439 |    * @param n the number of dynamixels in the format
1440 |    * @param id the dynamixel ids
1441 |    * @param start the start registers
1442 |    * @param num the number of registers for each dynamixel
1443 |    *
1444 |    * @return a flat array containing <code>n</code> &lt;Integer, {@link
1445 |    * AXRegister}, Integer&gt; triples giving the dynamixel id, start register,
1446 |    * and number of registers for each dynamixel in the format
1447 |    **/
1448 |   protected Object[] packFormat(int n,
1449 |                                 int[] id,
1450 |                                 AXRegister[] start, int[] num) {
1451 |     
1452 |     Object[] ret = new Object[3*n];
1453 |     
1454 |     for (int i = 0; i < n; i++) {
1455 |       ret[3*i] = new Integer(id[i]);
1456 |       ret[3*i+1] = start[i];
1457 |       ret[3*i+2] = new Integer(num[i]);
1458 |     }
1459 | 
1460 |     return ret;
1461 |   }
1462 | 
1463 |   /**
1464 |    * <p>Read int register values and CM-5 status into an array, for jscheme API
1465 |    * convenience.</p>
1466 |    *
1467 |    * @return an array of {@link #totalNumRegs}[F_READ]+1 values, with the last
1468 |    * set to the CM-5 status/retries
1469 |    **/
1470 |   public synchronized int[] read() throws IOException, InterruptedException {
1471 |     int n = totalNumRegs[F_READ];
1472 |     int[] ret = new int[n+1];
1473 |     ret[n] = read(ret);
1474 |     return ret;
1475 |   }
1476 | 
1477 |   /**
1478 |    * <p>Read natural register values and CM-5 status into an array, for jscheme
1479 |    * API convenience.</p>
1480 |    *
1481 |    * @return an array of {@link #totalNumRegs}[F_READ]+1 values, with the last
1482 |    * set to the CM-5 status/retries
1483 |    **/
1484 |   public synchronized float[] readNatural()
1485 |     throws IOException, InterruptedException {
1486 |     int n = totalNumRegs[F_READ];
1487 |     float[] ret = new float[n+1];
1488 |     ret[n] = read(ret);
1489 |     return ret;
1490 |   }
1491 | 
1492 |   /**
1493 |    * <p>Read data from dynamixels in natural units according to the current
1494 |    * read format.</p>
1495 |    *
1496 |    * @param data the read data is stored here, must have at least as many
1497 |    * entries as the total number of registers in the current read format.  If
1498 |    * there were problems reading any particular bytes they will be returned as
1499 |    * 0xff.
1500 |    *
1501 |    * @return the CM-5 status and retry bytes as the 0th and 1st byte of the
1502 |    * returned int
1503 |    *
1504 |    * @exception IOException if there was a communication error
1505 |    * @exception InterruptedException if the calling thread was interrupted
1506 |    * while waiting for response bytes from the CM-5
1507 |    **/
1508 |   public synchronized int read(float[] data)
1509 |     throws IOException, InterruptedException {
1510 |     return read((Object) data);
1511 |   }
1512 | 
1513 |   /** same as {@link #read(float[])} but reads register ints directly **/
1514 |   public synchronized int read(int[] data)
1515 |     throws IOException, InterruptedException {
1516 |     return read((Object) data);
1517 |   }
1518 | 
1519 |   /** common impl of {@link #read(float[])} and {@link #read(int[])} **/
1520 |   protected synchronized int read(Object data)
1521 |     throws IOException, InterruptedException {
1522 | 
1523 |     int[] intData = null;
1524 |     float[] naturalData = null;
1525 | 
1526 |     if (data instanceof int[]) {
1527 |       intData = (int[]) data;
1528 |       if (intData.length < totalNumRegs[F_READ])
1529 |         throw new IllegalArgumentException(
1530 |           "must pass an array of at least length "+totalNumRegs[F_READ]);
1531 |     } else if (data instanceof float[]) {
1532 |       naturalData = (float[]) data;
1533 |       if (naturalData.length < totalNumRegs[F_READ])
1534 |         throw new IllegalArgumentException(
1535 |           "must pass an array of at least length "+totalNumRegs[F_READ]);
1536 |     } else {
1537 |       throw new IllegalArgumentException("unsupported data type");
1538 |     }
1539 | 
1540 |     startSendPacket(Instruction.I_READ_DATA);
1541 |     endSendPacket();
1542 | 
1543 |     startRecvPacket(Instruction.I_DATA);
1544 | 
1545 |     int k = 0;
1546 | 
1547 |     for (int i = 0; i < numDynamixels[F_READ]; i++) {
1548 |       for (int j = 0; j < numReg[F_READ][i]; j++) {
1549 | 
1550 |         int value = 0;
1551 | 
1552 |         AXRegister reg = startReg[F_READ][i].getRelativeRegister(j);
1553 | 
1554 | //        System.err.println("reading "+reg+" ("+reg.width+" bytes)");
1555 | 
1556 |         for (int b = 0; b < reg.width; b++)
1557 |           value |= recvByte()<<(8*b);
1558 | 
1559 |         value = reg.decode(value);
1560 | 
1561 |         if (intData != null)
1562 |           intData[k++] = value;
1563 |         else
1564 |           naturalData[k++] = reg.toNaturalUnits(value);
1565 | 
1566 |         updateCachedValue(axID[F_READ][i], reg, value);
1567 |       }
1568 |     }
1569 | 
1570 |     int status = recvByte();
1571 | 
1572 |     status |= recvByte()<<8;
1573 | 
1574 |     recvADCs();
1575 | 
1576 |     endRecvPacket();
1577 | 
1578 |     return status;
1579 |   }
1580 | 
1581 |   /** update {@link #cache} **/
1582 |   protected synchronized void updateCachedValue(int axID,
1583 |                                                 AXRegister register,
1584 |                                                 int value) {
1585 | 
1586 |     Map<Integer, CachedValue> cvs = cache.get(register);
1587 | 
1588 |     if (cvs == null) {
1589 |       cvs = new LinkedHashMap<Integer, CachedValue>();
1590 |       cache.put(register, cvs);
1591 |     }
1592 | 
1593 |     CachedValue cv = cvs.get(axID);
1594 | 
1595 |     if (cv == null) {
1596 |       cv = new CachedValue();
1597 |       cvs.put(axID, cv);
1598 |     }
1599 | 
1600 |     cv.value = value;
1601 |     cv.lastUpdateNS = System.nanoTime();
1602 |   }
1603 | 
1604 |   /**
1605 |    * <p>Look up the most recent {@link CachedValue} of of the specified reg,
1606 |    * null if none.</p>
1607 |    **/
1608 |   public synchronized CachedValue getCachedValue(int axID,
1609 |                                                  AXRegister register) {
1610 |     Map<Integer, CachedValue> cvs = cache.get(register);
1611 |     return (cvs == null) ? null : cvs.get(axID);
1612 |   }
1613 | 
1614 |   /**
1615 |    * <p>Write data to dynamixels in natural units according to the current
1616 |    * write format.</p>
1617 |    *
1618 |    * @param data the data to write, must have at least as many entries as the
1619 |    * total number of registers in the current write format 
1620 |    *
1621 |    * @return the CM-5 status and retry bytes as the 0th and 1st byte of the
1622 |    * returned int
1623 |    *
1624 |    * @exception IOException if there was a communication error
1625 |    * @exception InterruptedException if the calling thread was interrupted
1626 |    * while waiting for response bytes from the CM-5
1627 |    **/
1628 |   public synchronized int write(float[] data)
1629 |     throws IOException, InterruptedException {
1630 |     return write((Object) data);
1631 |   }
1632 | 
1633 |   /** same as {@link #write(float[])} but writes register ints directly **/
1634 |   public synchronized int write(int[] data)
1635 |     throws IOException, InterruptedException {
1636 |     return write((Object) data);
1637 |   }
1638 | 
1639 |   /** common impl of {@link #write(float[])} and {@link #write(int[])} **/
1640 |   public synchronized int write(Object data)
1641 |     throws IOException, InterruptedException {
1642 | 
1643 |     int[] intData = null;
1644 |     float[] naturalData = null;
1645 | 
1646 |     if (data instanceof int[]) {
1647 |       intData = (int[]) data;
1648 |       if (intData.length < totalNumRegs[F_WRITE])
1649 |         throw new IllegalArgumentException(
1650 |           "must pass an array of at least length "+totalNumRegs[F_WRITE]);
1651 |     } else if (data instanceof float[]) {
1652 |       naturalData = (float[]) data;
1653 |       if (naturalData.length < totalNumRegs[F_WRITE])
1654 |         throw new IllegalArgumentException(
1655 |           "must pass an array of at least length "+totalNumRegs[F_WRITE]);
1656 |     } else {
1657 |       throw new IllegalArgumentException("unsupported data type");
1658 |     }
1659 | 
1660 |     startSendPacket(Instruction.I_WRITE_DATA);
1661 | 
1662 |     int k = 0;
1663 | 
1664 |     for (int i = 0; i < numDynamixels[F_WRITE]; i++) {
1665 |       for (int j = 0; j < numReg[F_WRITE][i]; j++) {
1666 | 
1667 |         int value = 0;
1668 | 
1669 |         AXRegister reg = startReg[F_WRITE][i].getRelativeRegister(j);
1670 | 
1671 |         if (intData != null)
1672 |           value = intData[k++];
1673 |         else
1674 |           value = reg.fromNaturalUnits(naturalData[k++]);
1675 | 
1676 |         value = reg.encode(value);
1677 | 
1678 |         for (int b = 0; b < reg.width; b++)
1679 |           sendByte(value>>(8*b));
1680 |       }
1681 |     }
1682 | 
1683 |     endSendPacket();
1684 | 
1685 |     return recvStatus();
1686 |   }
1687 | 
1688 |   /** set the timeout for a response from the CM-5 in ms, returns old value **/
1689 |   public synchronized double setTimeoutMS(double timeoutMS) {
1690 |     double timeoutMSWas = this.timeoutMS;
1691 |     this.timeoutMS = timeoutMS;
1692 |     return timeoutMSWas;
1693 |   } 
1694 | 
1695 |   /** get the current timeout for a response from the CM-5 in ms **/
1696 |   public synchronized double getTimeoutMS() {
1697 |     return timeoutMS;
1698 |   }
1699 | 
1700 |   /**
1701 |    * <p>Receive a byte from the CM-5.</p>
1702 |    *
1703 |    * @param addToChecksum whether to add the value of the received byte to the
1704 |    * current {@link #checksum} in progress
1705 |    *
1706 |    * @return the received byte
1707 |    *
1708 |    * @exception IOException if there was a communication error
1709 |    * @exception InterruptedException if the calling thread was interrupted
1710 |    * while waiting for response bytes from the CM-5
1711 |    **/
1712 |   protected synchronized int recvByte(boolean addToChecksum)
1713 |     throws IOException, InterruptedException {
1714 | 
1715 |     double deadline = System.nanoTime() + timeoutMS*1e6;
1716 | 
1717 |     while (fromCM5.available() == 0) {
1718 |       
1719 |       if (System.nanoTime() > deadline) {
1720 | 
1721 |         if (recvPacketDebug) {
1722 |           int i = 0;
1723 |           for (byte b : recvPacketDebugBuffer)
1724 |             dbg("RP "+(i++), b);
1725 |         }
1726 | 
1727 |         recvPacketDebug = false;
1728 | 
1729 |         throw new IOException("timeout waiting for response from CM-5");
1730 |       }
1731 |       
1732 |       Thread.sleep(RECV_POLL_MS);
1733 |     }
1734 | 
1735 |     int b = fromCM5.read();
1736 | 
1737 |     if (addToChecksum)
1738 |       checksum += b;
1739 | 
1740 |     if (debug)
1741 |       dbg("R", b);
1742 |     
1743 |     if (recvPacketDebug)
1744 |       recvPacketDebugBuffer.add((byte) (b&0xff));
1745 |     
1746 |     return b;
1747 |   }
1748 | 
1749 |   /** print a debug message for a byte **/
1750 |   protected void dbg(String msg, int b) {
1751 |     b &= 0xff;
1752 |     System.err.println(
1753 |       msg+": 0x"+Integer.toHexString(b)+" = \'"+((char) b)+"\' = "+b);
1754 |   }
1755 | 
1756 |   /** covers {@link #recvByte(boolean)}, always adds to checksum **/
1757 |   protected synchronized int recvByte()
1758 |     throws IOException, InterruptedException {
1759 |     return recvByte(true);
1760 |   }
1761 | 
1762 |   /**
1763 |    * <p>Send a byte to the CM-5.</p>
1764 |    *
1765 |    * @param b the byte to send
1766 |    * @param addToChecksum whether to add the value of the sent byte to the
1767 |    * current {@link #checksum} in progress, after sending the byte
1768 |    *
1769 |    * @exception IOException if there was a communication error
1770 |    **/
1771 |   protected synchronized void sendByte(int b, boolean addToChecksum)
1772 |     throws IOException {
1773 | 
1774 |     b = b&0xff;
1775 | 
1776 |     toCM5.write(b);
1777 | 
1778 |     if (addToChecksum)
1779 |       checksum += b;
1780 | 
1781 |     if (debug)
1782 |       dbg("W", b);
1783 |   }
1784 | 
1785 |   /** covers {@link #sendByte(int, boolean)}, always adds to checksum **/
1786 |   protected synchronized void sendByte(int b)
1787 |     throws IOException {
1788 |     sendByte(b, true);
1789 |   }
1790 | 
1791 |   /** start an outgoing packet with the given instruction **/
1792 |   protected synchronized void startSendPacket(Instruction instruction) 
1793 |     throws IOException {
1794 |     checksum = 0;
1795 |     sendByte(instruction.code);
1796 |   }
1797 | 
1798 |   /** end an outgoing packet, sending checksum **/
1799 |   protected synchronized void endSendPacket() throws IOException {
1800 |     sendByte((~checksum)&0xff);
1801 |     toCM5.flush();
1802 |   }
1803 | 
1804 |   /** start an incoming packet expecting the given instruction **/
1805 |   protected synchronized void startRecvPacket(Instruction instruction) 
1806 |     throws IOException, InterruptedException {
1807 | 
1808 |     recvPacketDebugBuffer.clear();
1809 |     recvPacketDebug = enableRecvPacketDebug;
1810 | 
1811 |     checksum = 0;
1812 |     int b = recvByte();
1813 |     if (b != instruction.code)
1814 |       throw new IOException(
1815 |         "expected "+instruction+" return packet, got 0x"+
1816 |         Integer.toHexString(b));
1817 |   }
1818 | 
1819 |   /** end an incoming packet, validating checksum **/
1820 |   protected synchronized void endRecvPacket() 
1821 |     throws IOException, InterruptedException {
1822 | 
1823 |     int b = recvByte(false);
1824 | 
1825 |     recvPacketDebug = false;
1826 | 
1827 |     if (b != ((~checksum)&0xff))
1828 |       throw new IOException(
1829 |         "invalid checksum 0x"+Integer.toHexString(b)+
1830 |         ", should be 0x"+Integer.toHexString((~checksum)&0xff));
1831 |   }
1832 | 
1833 |   /** receive a {@link Instruction#I_STATUS} packet, return payload **/
1834 |   protected synchronized int recvStatus() 
1835 |     throws IOException, InterruptedException {
1836 |     startRecvPacket(Instruction.I_STATUS);
1837 |     int status = recvByte();
1838 |     status |= recvByte()<<8;
1839 |     recvADCs();
1840 |     endRecvPacket();
1841 |     return status;
1842 |   }
1843 | 
1844 |   /** receive and store the ADC channel readings in {@link #adcValue} **/
1845 |   protected synchronized void recvADCs()
1846 |     throws IOException, InterruptedException {
1847 |     adcValue[CHANNEL_POS] = recvByte();
1848 |     adcValue[CHANNEL_NEG] = recvByte();
1849 |     adcValue[CHANNEL_THERM] = recvByte();
1850 |   }
1851 | 
1852 |   /** convert a raw ADC reading to V at the input of the 3.3k/10k divider **/
1853 |   public static float adcToVolts(int value) {
1854 |     return (((float) value)/255.0f)*5.0f*((10.0f+3.3f)/3.3f);
1855 |   }
1856 | 
1857 |   /** get the most recent ADC reading for the given channel **/
1858 |   public synchronized int getADC(int channel) {
1859 | 
1860 |     if ((channel < 0) || (channel > adcValue.length))
1861 |       throw new IllegalArgumentException("unknown channel "+channel);
1862 | 
1863 |     return adcValue[channel];
1864 |   }
1865 | 
1866 |   /** close the serial port, no further comms possible **/
1867 |   public synchronized void close() {
1868 |     serialPort.close();
1869 |   }
1870 | 
1871 |   /** {@link #close}s **/
1872 |   protected void finalize() {
1873 |     close();
1874 |   }
1875 | 
1876 |   /**
1877 |    * <p>Verify a CM-5 return status code/retry count.</p>
1878 |    *
1879 |    * <p>Emits a warning message on stderr if the retry count is non-zero, but
1880 |    * only throws an exception when the status code is non-zero.</p>
1881 |    *
1882 |    * @param status the CM-5 return status code/retry count
1883 |    * @param operation a string describing the operation for which the status
1884 |    * applies
1885 |    * @param warnOnly if set then a warning msg is printed instead of any
1886 |    * exception
1887 |    *
1888 |    * @return true if status was ok
1889 |    *
1890 |    * @exception IOException if not warnOnly and the status code, but not
1891 |    * necessarily the retry count, is non-zero
1892 |    **/
1893 |   public static boolean verifyStatus(int status, String operation,
1894 |                                      boolean warnOnly)
1895 |     throws IOException {
1896 | 
1897 |     int numRetries = (status&0xff00)>>8;
1898 | 
1899 |     if (numRetries > 0)
1900 |       System.err.println(
1901 |         "W: "+operation+" required "+numRetries+" dynamixel retries");
1902 | 
1903 |     if ((status&0xff) != 0) {
1904 | 
1905 |       String msg = operation+" failed with status "+statusToString(status);
1906 | 
1907 |       if (warnOnly)
1908 |         System.err.println("W: "+msg);
1909 |       else
1910 |         throw new IOException(msg);
1911 | 
1912 |       return false;
1913 |     }
1914 | 
1915 |     return true;
1916 |   }
1917 | 
1918 |   /** covers {@link #verifyStatus(int, String, boolean)}, not warn **/
1919 |   public static void verifyStatus(int status, String operation) 
1920 |     throws IOException {
1921 |     verifyStatus(status, operation, false);
1922 |   }
1923 | 
1924 |   /** duplicate first n element of from into to, reallocating as necessary **/
1925 |   public static int[] dup(int[] from, int[] to, int n) {
1926 |     to = ensureCapacity(to, n);
1927 |     System.arraycopy(from, 0, to, 0, n);
1928 |     return to;
1929 |   }
1930 | 
1931 |   /** covers {@link #dup(int[], int[], int)}, copies all **/
1932 |   public static int[] dup(int[] from, int[] to) {
1933 |     return dup(from, to, from.length);
1934 |   }
1935 | 
1936 |   /** duplicate first n element of from into to, reallocating as necessary **/
1937 |   public static boolean[] dup(boolean[] from, boolean[] to, int n) {
1938 |     to = ensureCapacity(to, n);
1939 |     System.arraycopy(from, 0, to, 0, n);
1940 |     return to;
1941 |   }
1942 | 
1943 |   /** covers {@link #dup(boolean[], boolean[], int)}, copies all **/
1944 |   public static boolean[] dup(boolean[] from, boolean[] to) {
1945 |     return dup(from, to, from.length);
1946 |   }
1947 | 
1948 |   /** duplicate first n element of from into to, reallocating as necessary **/
1949 |   public static String[] dup(String[] from, String[] to, int n) {
1950 |     to = ensureCapacity(to, n);
1951 |     System.arraycopy(from, 0, to, 0, n);
1952 |     return to;
1953 |   }
1954 | 
1955 |   /** covers {@link #dup(String[], String[], int)}, copies all **/
1956 |   public static String[] dup(String[] from, String[] to) {
1957 |     return dup(from, to, from.length);
1958 |   }
1959 | 
1960 |   /** make sure <i>a</i> is at least length <i>n</i> **/
1961 |   public static int[] ensureCapacity(int[] a, int n) {
1962 |     if ((a == null) || (a.length < n))
1963 |       a = new int[n];
1964 |     return a;
1965 |   }
1966 | 
1967 |   /** make sure <i>a</i> is at least length <i>n</i> **/
1968 |   public static boolean[] ensureCapacity(boolean[] a, int n) {
1969 |     if ((a == null) || (a.length < n))
1970 |       a = new boolean[n];
1971 |     return a;
1972 |   }
1973 | 
1974 |   /** make sure <i>a</i> is at least length <i>n</i> **/
1975 |   public static String[] ensureCapacity(String[] a, int n) {
1976 |     if ((a == null) || (a.length < n))
1977 |       a = new String[n];
1978 |     return a;
1979 |   }
1980 | 
1981 |   /** serial port talking to the CM-5 **/
1982 |   protected SerialPort serialPort;
1983 | 
1984 |   /** output stream to CM-5 **/
1985 |   protected OutputStream toCM5; 
1986 | 
1987 |   /** input stream from CM-5 **/
1988 |   protected InputStream fromCM5;
1989 | 
1990 |   /** timout in ms to wait for a response byte from the CM-5 **/
1991 |   protected double timeoutMS = DEF_TIMEOUT_MS;
1992 | 
1993 |   /** checksum in progress **/
1994 |   protected int checksum = 0;
1995 | 
1996 |   /** num dynamixels in current {@link #F_READ} and {@link #F_WRITE} **/
1997 |   protected int[] numDynamixels = new int[] {0, 0};
1998 | 
1999 |   /** total num regs in current {@link #F_READ} and {@link #F_WRITE} **/
2000 |   protected int[] totalNumRegs = new int[] {0, 0};
2001 | 
2002 |   /** dynamixel ids in current {@link #F_READ} and {@link #F_WRITE} **/
2003 |   protected int[][] axID = 
2004 |     new int[][] {
2005 |     new int[numDynamixels[0]],
2006 |     new int[numDynamixels[1]]
2007 |   };
2008 | 
2009 |   /** start regs in current {@link #F_READ} and {@link #F_WRITE} **/
2010 |   protected AXRegister[][] startReg =
2011 |     new AXRegister[][] {
2012 |     new AXRegister[numDynamixels[0]],
2013 |     new AXRegister[numDynamixels[1]]
2014 |   };
2015 | 
2016 |   /** num regs in current {@link #F_READ} and {@link #F_WRITE} **/
2017 |   protected int[][] numReg = 
2018 |     new int[][] {
2019 |     new int[numDynamixels[0]],
2020 |     new int[numDynamixels[1]]
2021 |   };
2022 | }
2023 | 
2024 | 


--------------------------------------------------------------------------------
/firmware/CBUF.h:
--------------------------------------------------------------------------------
  1 | /****************************************************************************
  2 | *
  3 | *   Since this code originated from code which is public domain, I
  4 | *   hereby declare this code to be public domain as well.
  5 | *
  6 | ****************************************************************************/
  7 | /**
  8 | *
  9 | *   @file   CBUF.h
 10 | *
 11 | *   @brief  This file contains global definitions for circular buffer
 12 | *           manipulation.
 13 | *
 14 | *   These macros implement a circular buffer which employs get and put
 15 | *   pointers, in such a way that mutual exclusion is not required
 16 | *   (assumes one reader & one writer).
 17 | *
 18 | *   It requires that the circular buffer size be a power of two, and the
 19 | *   size of the buffer needs to smaller than the index. So an 8 bit index
 20 | *   supports a circular buffer upto ( 1 << 7 ) = 128 entries, and a 16 bit index
 21 | *   supports a circular buffer upto ( 1 << 15 ) = 32768 entries.
 22 | *
 23 | *   The basis for these routines came from an article in Jack Ganssle's
 24 | *   Embedded Muse: http://www.ganssle.com/tem/tem110.pdf
 25 | *
 26 | *   In order to offer the most amount of flexibility for embedded environments
 27 | *   you need to define a macro for the size.
 28 | *
 29 | *   First, you need to name your circular buffer. For this example, we'll
 30 | *   call it myQ.
 31 | *
 32 | *   The size macro that needs to be defined will be the name of the
 33 | *   circular buffer followed by _SIZE. The size must be a power of two
 34 | *   and it needs to fit in the get/put indicies. i.e. if you use an
 35 | *   8 bit index, then the maximum supported size would be 128.
 36 | *
 37 | *   The structure which defines the circular buffer needs to have 3 members
 38 | *   m_getIdx, m_putIdx, and m_entry.
 39 | *
 40 | *   m_getIdx and m_putIdx need to be unsigned integers of the same size.
 41 | *
 42 | *   m_entry needs to be an array of xxx_SIZE entries, or a pointer to an
 43 | *   array of xxx_SIZE entries. The type of each entry is entirely up to the
 44 | *   caller.
 45 | *
 46 | *   #define myQ_SIZE    64
 47 | *   
 48 | *   volatile struct
 49 | *   {
 50 | *       uint8_t     m_getIdx;
 51 | *       uint8_t     m_putIdx;
 52 | *       uint8_t     m_entry[ myQ_SIZE ];
 53 | *
 54 | *   } myQ;
 55 | *
 56 | *   You could then use
 57 | *
 58 | *       CBUF_Push( myQ, 'x' );
 59 | *
 60 | *   to add a character to the circular buffer, or
 61 | *
 62 | *       ch = CBUF_Pop( myQ );
 63 | *
 64 | *   to retrieve an element from the buffer.
 65 | *
 66 | *   If you happen to prefer to use C++ instead, there is a templatized
 67 | *   version which requires no macros. You just declare 3 template parameters:
 68 | *
 69 | *       - The type that should be used for the index
 70 | *       - The size of the circular buffer
 71 | *       - The type that should be used for the entry
 72 | *
 73 | *   For example:
 74 | *
 75 | *       CBUF< uint8_t, 64, char >   myQ;
 76 | *
 77 | ****************************************************************************/
 78 | 
 79 | #if !defined( CBUF_H )
 80 | #define CBUF_H       /**< Include Guard                          */
 81 | 
 82 | /* ---- Include Files ---------------------------------------------------- */
 83 | 
 84 | /* ---- Constants and Types ---------------------------------------------- */
 85 | 
 86 | /**
 87 | *   Initializes the circular buffer for use.
 88 | */ 
 89 | 
 90 | #define CBUF_Init( cbuf )       cbuf.m_getIdx = cbuf.m_putIdx = 0
 91 | 
 92 | /**
 93 | *   Returns the number of elements which are currently contained in the 
 94 |  *  circular buffer.
 95 | */
 96 | 
 97 | #define CBUF_Len( cbuf )        ((typeof( cbuf.m_putIdx ))(( cbuf.m_putIdx ) - ( cbuf.m_getIdx )))
 98 | 
 99 | /**
100 | *   Appends an element to the end of the circular buffer
101 | */
102 | 
103 | #define CBUF_Push( cbuf, elem ) (cbuf.m_entry)[ cbuf.m_putIdx++ & (( cbuf##_SIZE ) - 1 )] = (elem)
104 | 
105 | /**
106 | *   Retrieves an element from the beginning of the circular buffer
107 | */
108 | 
109 | #define CBUF_Pop( cbuf )        (cbuf.m_entry)[ cbuf.m_getIdx++ & (( cbuf##_SIZE ) - 1 )]
110 | 
111 | /**
112 | *   Retrieves the i'th element from the beginning of the circular buffer
113 | */
114 | 
115 | #define CBUF_Get( cbuf, idx )        (cbuf.m_entry)[( cbuf.m_getIdx + idx ) & (( cbuf##_SIZE ) - 1 )]
116 | 
117 | /**
118 | *   Retrieves the i'th element from the end of the circular buffer
119 | */
120 | 
121 | #define CBUF_GetEnd( cbuf, idx )        (cbuf.m_entry)[( cbuf.m_putIdx - idx - 1 ) & (( cbuf##_SIZE ) - 1 )]
122 | 
123 | /**
124 | *   Determines if the circular buffer is empty
125 | */
126 | 
127 | #define CBUF_IsEmpty( cbuf )    ( CBUF_Len( cbuf ) == 0 )
128 | 
129 | /**
130 | *   Determines if the circular buffer is full.
131 | */
132 | 
133 | #define CBUF_IsFull( cbuf )     ( CBUF_Len( cbuf ) == ( cbuf##_SIZE ))
134 | 
135 | /**
136 | *   Determines if the circular buffer is currenly overflowed or underflowed.
137 | */
138 | 
139 | #define CBUF_Error( cbuf )      ( CBUF_Len( cbuf ) > cbuf##_SIZE )
140 | 
141 | #if defined( __cplusplus )
142 | 
143 | template < class IndexType, unsigned Size, class EntryType >
144 | class CBUF
145 | {
146 | public:
147 | 
148 |     CBUF()
149 |     {
150 |         m_getIdx = m_putIdx = 0;
151 |     }
152 | 
153 |     IndexType Len() const   { return m_putIdx - m_getIdx; }
154 | 
155 |     bool IsEmpty() const    { return Len() == 0; }
156 |     bool IsFull() const     { return Len() == Size; }
157 |     bool Error() const      { return Len() > Size; }
158 | 
159 |     void Push( EntryType val )   
160 |     {
161 |         m_entry[ m_putIdx++ & ( Size - 1 )] = val;
162 |     }
163 | 
164 |     EntryType Pop()
165 |     {
166 |         return m_entry[ m_getIdx++ & ( Size - 1 )];
167 |     }
168 | 
169 | private:
170 | 
171 |     volatile IndexType  m_getIdx;
172 |     volatile IndexType  m_putIdx;
173 |     EntryType           m_entry[ Size ];
174 | 
175 | };
176 | 
177 | #endif  // __cplusplus
178 | 
179 | /* ---- Variable Externs ------------------------------------------------- */
180 | /* ---- Function Prototypes ---------------------------------------------- */
181 | 
182 | /** @} */
183 | 
184 | #endif // CBUF_H
185 | 


--------------------------------------------------------------------------------
/firmware/brbrain.c:
--------------------------------------------------------------------------------
   1 | /****************************************************************************
   2 | *
   3 | *   Bioloid Remote Brain CM-5 firmware
   4 | *
   5 | *   Copyright (c) 2007 Marsette Vona
   6 | *
   7 | *   This program is free software; you can redistribute it and/or modify
   8 | *   it under the terms of the GNU General Public License as published by
   9 | *   the Free Software Foundation; either version 2 of the License, or
  10 | *   (at your option) any later version.
  11 | *
  12 | *   This program is distributed in the hope that it will be useful,
  13 | *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 | *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 | *   GNU General Public License for more details.
  16 | *
  17 | *   You should have received a copy of the GNU General Public License
  18 | *   along with this program; if not, write to the Free Software
  19 | *   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 | *
  21 | *   $Id: brbrain.c 24 2008-05-02 22:08:41Z vona $
  22 | ****************************************************************************/
  23 | 
  24 | #include <inttypes.h>
  25 | #include <avr/io.h>
  26 | #include <avr/interrupt.h>
  27 | #include <util/delay.h>
  28 | #include "CBUF.h"
  29 | 
  30 | #define DBG(c) txPC(c, 0)
  31 | //#define DBG(c)
  32 | 
  33 | /* comms baud rate constants */
  34 | #define BAUDRATE_1000000 1
  35 | #define BAUDRATE_38400   51 /* at 16MHz (0.2% error) */
  36 | #define BAUDRATE_57600   34 /* at 16MHz (-0.8% error) */
  37 | #define BAUDRATE_76800   25 /* at 16MHz (0.2% error) */
  38 | #define BAUDRATE_115200  16 /* at 16MHz (2.1% error) */
  39 | #define BAUDRATE_250000  7  /* at 16MHz (0.0% error) */
  40 | 
  41 | /* dynamixel bus on UART0 */
  42 | #define DYNAMIXEL_BAUDRATE BAUDRATE_1000000
  43 | 
  44 | /* pc link on UART1 */
  45 | #define PC_BAUDRATE BAUDRATE_115200
  46 | /* #define PC_BAUDRATE BAUDRATE_57600 */
  47 | /* #define PC_BAUDRATE BAUDRATE_38400 */
  48 | 
  49 | /* max non-broadcast dynamixel ID */
  50 | #define MAX_DYNAMIXEL_ID 253 
  51 | 
  52 | /* broadcast dynamixel ID */
  53 | #define ID_BCAST 0xfe
  54 | 
  55 | /* max num dynamixels in a format */
  56 | #define MAX_DYNAMIXELS  32
  57 | 
  58 | /* virtual dynamixel register holding error code */
  59 | #define VIRTUAL_ERROR_REG_ADDR 54
  60 | 
  61 | /* address of last dynamixel register, including the virtual one */
  62 | #define MAX_START_ADDR 54
  63 | 
  64 | /* total number of bytes in dynamixel register bank including virtual */
  65 | #define NUM_REG_BYTES  55
  66 | 
  67 | /* max number of bytes readable in one block from a dynamixel */
  68 | #define MAX_DYNAMIXEL_BLOCK_READ 20
  69 | 
  70 | /* the formats */
  71 | #define F_READ  0
  72 | #define F_WRITE 1
  73 | 
  74 | static uint8_t numDynamixels[2] = { 0, 0 };
  75 | static uint16_t totalNumBytes[2] = { 0, 0 };
  76 | static uint8_t axID[2][MAX_DYNAMIXELS];
  77 | static uint8_t startAddr[2][MAX_DYNAMIXELS];
  78 | static uint8_t numBytes[2][MAX_DYNAMIXELS];
  79 | 
  80 | /* buffer for params to/from dynamixel */
  81 | static uint8_t dynamixelParams[64];
  82 | 
  83 | /* buffer sizes WARNING these must be a power of 2 */
  84 | #define dynamixelRxBuffer_SIZE 128 
  85 | #define dynamixelTxBuffer_SIZE 128 
  86 | #define pcRxBuffer_SIZE 128
  87 | #define pcTxBuffer_SIZE 128
  88 | 
  89 | /* dynamixel CBUFs */
  90 | #if (dynamixelRxBuffer_SIZE > 128)
  91 | typedef uint16_t dynamixelRxIndex_t;
  92 | #else
  93 | typedef uint8_t dynamixelRxIndex_t;
  94 | #endif
  95 | volatile struct {
  96 |   dynamixelRxIndex_t m_getIdx;
  97 |   dynamixelRxIndex_t m_putIdx;
  98 |   uint8_t m_entry[dynamixelRxBuffer_SIZE];
  99 | } dynamixelRxBuffer;
 100 | 
 101 | #if (dynamixelTxBuffer_SIZE > 128)
 102 | typedef uint16_t dynamixelTxIndex_t;
 103 | #else
 104 | typedef uint8_t dynamixelTxIndex_t;
 105 | #endif
 106 | volatile struct {
 107 |   dynamixelTxIndex_t m_getIdx;
 108 |   dynamixelTxIndex_t m_putIdx;
 109 |   uint8_t m_entry[dynamixelTxBuffer_SIZE];
 110 | } dynamixelTxBuffer;
 111 | 
 112 | /* pc CBUFs */
 113 | #if (pcRxBuffer_SIZE > 128)
 114 | typedef uint16_t pcRxIndex_t;
 115 | #else
 116 | typedef uint8_t pcRxIndex_t;
 117 | #endif
 118 | volatile struct {
 119 |   pcRxIndex_t m_getIdx;
 120 |   pcRxIndex_t m_putIdx;
 121 |   uint8_t m_entry[pcRxBuffer_SIZE];
 122 | } pcRxBuffer;
 123 | 
 124 | #if (pcTxBuffer_SIZE > 128)
 125 | typedef uint16_t pcTxIndex_t;
 126 | #else
 127 | typedef uint8_t pcTxIndex_t;
 128 | #endif
 129 | volatile struct {
 130 |   pcTxIndex_t m_getIdx;
 131 |   pcTxIndex_t m_putIdx;
 132 |   uint8_t m_entry[pcTxBuffer_SIZE];
 133 | } pcTxBuffer;
 134 | 
 135 | /* tx/rx checksums */
 136 | static uint8_t checksumTxDynamixel = 0;
 137 | static uint8_t checksumRxDynamixel = 0;
 138 | static uint8_t checksumTxPC = 0;
 139 | static uint8_t checksumRxPC = 0;
 140 | 
 141 | #define DYNAMIXEL_INSTRUCTION_CHECKSUM_ERROR (1<<4)
 142 | #define MAX_DYNAMIXEL_TRIES 4
 143 | static uint8_t numDynamixelRetries = 0;
 144 | 
 145 | /* port pins */
 146 | 
 147 | #define BIT_DYNAMIXEL_TXD              PE2
 148 | #define BIT_DYNAMIXEL_RXD              PE3
 149 | 
 150 | #define BIT_ZIGBEE_LED                 PD1
 151 | #define BIT_ZIGBEE_RESET               PD4  /* out : default 1 */
 152 | #define BIT_ENABLE_RXD_LINK_PC         PD5  /* out : default 1 */
 153 | #define BIT_ENABLE_RXD_LINK_ZIGBEE     PD6  /* out : default 0 */
 154 | #define BIT_LINK_PLUGIN                PD7  /* in, no pull up */
 155 | #define BIT_CHARGE                     PB5  /* out, 0 = charge */
 156 | 
 157 | #define BIT_BLUETOOTH_RTS             BIT_ZIGBEE_LED
 158 | #define BIT_BLUETOOTH_CTS             BIT_ZIGBEE_RESET
 159 | #define BIT_ENABLE_RXD_LINK_BLUETOOTH BIT_ENABLE_RXD_LINK_ZIGBEE
 160 | 
 161 | #define BLUETOOTH_ACTIVE (PORTD&_BV(BIT_ENABLE_RXD_LINK_BLUETOOTH))
 162 | 
 163 | /* dynamixel bus is half-duplex TTL */
 164 | #define DYNAMIXEL_RXD \
 165 |   PORTE &= ~_BV(BIT_DYNAMIXEL_TXD),PORTE |= _BV(BIT_DYNAMIXEL_RXD)
 166 | #define DYNAMIXEL_TXD \
 167 |   PORTE &= ~_BV(BIT_DYNAMIXEL_RXD),PORTE |= _BV(BIT_DYNAMIXEL_TXD)
 168 | 
 169 | #define DYNAMIXEL_TXD_DONE (!(UCSR0B&_BV(UDRIE0)))
 170 | 
 171 | /* ADC channels */
 172 | #define CHANNEL_POS   0
 173 | #define CHANNEL_NEG   1
 174 | #define CHANNEL_THERM 2
 175 | #define CHANNEL_GND   31
 176 | 
 177 | /* most recently read ADC values */
 178 | static volatile uint8_t adcValue[] = {0, 0, 0};
 179 | 
 180 | /*
 181 |  * define this to enable battery charging 
 182 |  *
 183 |  * WARNING charge code is currently EXPERIMENTAL and does not yet incorporate
 184 |  * temperature monitoring
 185 |  *
 186 |  * work-around is to temporarily flash standard firmware from robotis, charge
 187 |  * battery, and then re-flash brbrain firmware
 188 |  *
 189 |  * improper battery charging could damage battery and robot or even cause fire
 190 |  *
 191 |  * see full DISCLAIMER.txt included in orginal package download
 192 |  */
 193 | /* #define ENABLE_CHARGING */
 194 | 
 195 | /* charging starts when enabled and neg voltage is above 1.10V */
 196 | #define CHARGE_START_THRESHOLD 13
 197 | 
 198 | /* charging ends when neg voltage is below 0.95V */
 199 | #define CHARGE_COMPLETE_THRESHOLD 11
 200 | 
 201 | /* do an ADC conversion, result in ADCH */
 202 | #define CONVERT_AND_WAIT(channel) {             \
 203 |     ADMUX &= 0xf0;                              \
 204 |     ADMUX |= channel & 0x0f;                    \
 205 |     ADCSRA |= (1 <<ADSC);                       \
 206 |     while (ADCSRA & (1<<ADSC))                  \
 207 |       ;                                         \
 208 |   }
 209 | 
 210 | /* LEDs */
 211 | #define LED_PWR     _BV(PC0)
 212 | #define LED_TXD     _BV(PC1)
 213 | #define LED_RXD     _BV(PC2)
 214 | #define LED_AUX     _BV(PC3)
 215 | #define LED_MANAGE  _BV(PC4)
 216 | #define LED_PROGRAM _BV(PC5)
 217 | #define LED_PLAY    _BV(PC6)
 218 | #define LED_ALL     0x7f
 219 | 
 220 | #define LED_OFF(mask) PORTC |= mask
 221 | #define LED_ON(mask)  PORTC &= ~mask
 222 | 
 223 | /* LED mapping */
 224 | #define LED_RXD_DYNAMIXEL    LED_MANAGE
 225 | #define LED_TXD_DYNAMIXEL    LED_PROGRAM
 226 | #define LED_RXD_PC           LED_RXD
 227 | #define LED_TXD_PC           LED_TXD
 228 | #define LED_ERROR            LED_PLAY
 229 | #define LED_IDLE             LED_AUX
 230 | #define LED_BLUETOOTH_ACTIVE LED_MANAGE
 231 | #define LED_BLUETOOTH_RTS    LED_PROGRAM
 232 | 
 233 | /* pushbuttons */
 234 | #define PB_UP    _BV(PE4)
 235 | #define PB_DOWN  _BV(PE5)
 236 | #define PB_LEFT  _BV(PE6)
 237 | #define PB_RIGHT _BV(PE7)
 238 | #define PB_START _BV(PD0)
 239 | 
 240 | #define PB_ALL (PB_UP|PB_DOWN|PB_LEFT|PB_RIGHT|PB_START)
 241 | 
 242 | /* mask is composed of PB_* constants, return mask is 1 for each btn pressed */
 243 | #define PB(mask) (mask&(((~PINE)&0xf0)|((~PIND)&0x01)))
 244 | 
 245 | /* button mapping */
 246 | #define PB_TOGGLE_RX_BLUETOOTH_PC PB_START
 247 | #define PB_TOGGLE_CHARGE_ENABLE   PB_UP
 248 | 
 249 | /* CM-5 to PC packet instructions */
 250 | #define I_STATUS 0xfa
 251 | #define I_DATA   0xfb
 252 | 
 253 | /* CM-5 to dynamixel packet insructions */
 254 | #define D_I_PING       0x01
 255 | #define D_I_READ_DATA  0x02
 256 | #define D_I_WRITE_DATA 0x03
 257 | #define D_I_REG_WRITE  0x04
 258 | #define D_I_ACTION     0x05
 259 | #define D_I_RESET      0x06
 260 | #define D_I_SYNC_WRITE 0x83
 261 | 
 262 | /* CM-5 status flags */
 263 | #define S_PC_TIMEOUT                 (1<<0)
 264 | #define S_DYNAMIXEL_TIMEOUT          (1<<1)
 265 | #define S_INVALID_PC_COMMAND         (1<<2)
 266 | #define S_INVALID_DYNAMIXEL_RESPONSE (1<<3)
 267 | #define S_PC_RX_OVERFLOW             (1<<4)
 268 | #define S_DYNAMIXEL_RX_OVERFLOW      (1<<5)
 269 | #define S_PC_CHECKSUM_ERROR          (1<<6)
 270 | #define S_DYNAMIXEL_CHECKSUM_ERROR   (1<<7)
 271 | 
 272 | /* CM-5 status, bitmask of S_* flags */
 273 | static uint8_t status = 0;
 274 | 
 275 | /* ~10ms ticks to go until timeout */
 276 | static volatile uint8_t ticksToGo = 0;
 277 | 
 278 | #define RX_PC_TIMEOUT_TICKS        100
 279 | #define RX_DYNAMIXEL_TIMEOUT_TICKS 10
 280 | #define TX_DYNAMIXEL_TIMEOUT_TICKS 100
 281 | #define RX_CLEAR_DELAY_TICKS       50
 282 | 
 283 | static uint8_t enableRxPCTimeout = 1;
 284 | static uint8_t enableRxDynamixelTimeout = 1;
 285 | 
 286 | static volatile uint8_t bluetoothTogglePending = 0;
 287 | 
 288 | static volatile uint8_t chargeEnableTogglePending = 0;
 289 | static volatile uint8_t chargeEnabled = 0;
 290 | static volatile uint8_t charging = 0;
 291 | 
 292 | #define CHARGE_PHASE 0
 293 | #define MEASURE_PHASE 1
 294 | 
 295 | /* #define CHARGE_PHASE_TICKS  3200 */
 296 | #define CHARGE_PHASE_TICKS  400 
 297 | #define MEASURE_PHASE_TICKS 100
 298 | #define MEASURE_TICKS       50
 299 | 
 300 | static volatile uint8_t chargePhase = CHARGE_PHASE;
 301 | static volatile uint16_t chargePhaseTicksToGo = CHARGE_PHASE_TICKS;
 302 | 
 303 | /* set a read or write format */
 304 | static void setFormat(uint8_t f);
 305 | 
 306 | /* command handlers */
 307 | static void handlePing();
 308 | static void handleSetReadFormat();
 309 | static void handleSetWriteFormat();
 310 | static void handleReadData();
 311 | static void handleWriteData();
 312 | static void handleInvalid();
 313 | 
 314 | /* pointer to a function taking no parameters and returning nothing */
 315 | typedef void (*handler_t)();
 316 | 
 317 | /* command handlers indexed by low nybble of instruction */
 318 | static handler_t commandHandler[] = {
 319 |   /* 0 */ handlePing,           /* I_PING */
 320 |   /* 1 */ handleSetReadFormat,  /* I_SET_READ_FORMAT */
 321 |   /* 2 */ handleSetWriteFormat, /* I_SET_WRITE_FORMAT */
 322 |   /* 3 */ handleReadData,       /* I_READ_DATA */
 323 |   /* 4 */ handleWriteData,      /* I_WRITE_DATA */
 324 |   /* 5 */ handleInvalid,
 325 |   /* 6 */ handleInvalid,
 326 |   /* 7 */ handleInvalid,
 327 |   /* 8 */ handleInvalid,
 328 |   /* 9 */ handleInvalid,
 329 |   /* a */ handleInvalid,
 330 |   /* b */ handleInvalid,
 331 |   /* c */ handleInvalid,
 332 |   /* d */ handleInvalid,
 333 |   /* e */ handleInvalid,
 334 |   /* f */ handleInvalid
 335 | };
 336 | 
 337 | /* handle commands indefinitely */
 338 | static void commandLoop();
 339 | 
 340 | /* set a flag in the status byte, with interrupts disabled */
 341 | static void setStatusFlag(uint8_t flag);
 342 | 
 343 | /* clear a flag in the status byte, with interrupts disabled */
 344 | static void clearStatusFlag(uint8_t flag);
 345 | 
 346 | /* rx checksum from PC and validate it, return 0 on timeout */
 347 | static uint8_t endRXPacketPC();
 348 | 
 349 | /* send instruction to pc */
 350 | static void startTXPacketPC(uint8_t instruction);
 351 | 
 352 | /* send checksum to pc */
 353 | static void endTXPacketPC();
 354 | 
 355 | /* tx a status packet to the PC */
 356 | static void txStatusPC();
 357 | 
 358 | /* tx the adc values to the PC */
 359 | static void txADCValuesPC();
 360 | 
 361 | /* rx packet header from dynamixel, return 0 on timeout */
 362 | static uint8_t startRXPacketDynamixel(uint8_t *id,
 363 |                                       uint8_t *numParams,
 364 |                                       uint8_t *error);
 365 | 
 366 | /* rx checksum from dynamixel and validate it, return 0 on timeout */
 367 | static uint8_t endRXPacketDynamixel();
 368 | 
 369 | /* receive a packet from dynamixel bus, return 0 on timeout */
 370 | static uint8_t rxPacketDynamixel(uint8_t *id,
 371 |                                  uint8_t *numParams,
 372 |                                  uint8_t *error,
 373 |                                  uint8_t *params,
 374 |                                  uint8_t numParamsExpected);
 375 | 
 376 | /* send packet header to dynamixel */
 377 | static void startTXPacketDynamixel(uint8_t id,
 378 |                                    uint8_t numParams,
 379 |                                    uint8_t instruction);
 380 | 
 381 | /* send checksum to dynamixel, wait for all tx complete, return 0 on timeout */
 382 | static uint8_t endTXPacketDynamixel();
 383 | 
 384 | /* clr stat flg & ret 1 iff either checksum fail and still have tries left **/
 385 | static uint8_t tryDynamixelAgain(uint8_t dynamixelError);
 386 | 
 387 | /* setup peripherals */
 388 | static void initHardware();
 389 | 
 390 | /* tx to dynamixel bus, blocking */
 391 | static void txDynamixel(uint8_t byte, uint8_t addToChecksum);
 392 | 
 393 | /* tx to pc, blocking */
 394 | static void txPC(uint8_t byte, uint8_t addToChecksum);
 395 | 
 396 | /* rx from dx bus, blocking, does _not_ control bus dir, ret 0 on timeout */
 397 | static uint8_t rxDynamixel(uint8_t *byte, uint8_t addToChecksum);
 398 | 
 399 | /* rx from pc, blocking, return 0 on timeout */
 400 | static uint8_t rxPC(uint8_t *byte, uint8_t addToChecksum);
 401 | 
 402 | /* check number of bytes waiting in rx buf from dynamixel bus */
 403 | /* static dynamixelRxIndex_t numWaitingDynamixel(); */
 404 | 
 405 | /* check number of bytes waiting in rx buf from pc */
 406 | /* static pcRxIndex_t numWaitingPC(); */
 407 | 
 408 | /* clear rx buf from dynamixel bus */
 409 | static void clearRxBufDynamixel();
 410 | 
 411 | /* read the relevant ADC channels into adcValue */
 412 | static void readADCs();
 413 | 
 414 | /* clear rx buf from pc */
 415 | static void clearRxBufPC();
 416 | 
 417 | int main(void) {
 418 | 
 419 |   initHardware();
 420 | 
 421 |   LED_ON(LED_PWR);
 422 | 
 423 |   sei();
 424 | 
 425 |   commandLoop();
 426 | 
 427 |   return 0;
 428 | }
 429 | 
 430 | static void handlePing() {
 431 | 
 432 |   uint8_t addr;
 433 | 
 434 |   if (rxPC(&addr, 1) && endRXPacketPC()) {
 435 |     
 436 |     if (addr <= 253) {
 437 | 
 438 |       uint8_t rxID = 0xff;
 439 |       uint8_t rxError = 0xff;
 440 | 
 441 |       /* retry loop */
 442 |       do {
 443 | 
 444 |         startTXPacketDynamixel(addr, 0, D_I_PING);
 445 |         
 446 |         if (!endTXPacketDynamixel())
 447 |           break;
 448 |           
 449 |       } while (rxPacketDynamixel(&rxID, 0, &rxError, 0, 0) &&
 450 |                tryDynamixelAgain(rxError));
 451 | 
 452 |       if (rxID != addr)
 453 |         setStatusFlag(S_INVALID_DYNAMIXEL_RESPONSE);
 454 |     }
 455 |     
 456 |     if (addr == 254)
 457 |       setStatusFlag(S_INVALID_PC_COMMAND);
 458 | 
 459 |     /* addr == 255 means ping the CM-5 itself */
 460 |   }
 461 | 
 462 |   txStatusPC();
 463 | }
 464 | 
 465 | static void handleSetReadFormat() {
 466 |   setFormat(F_READ);
 467 | }
 468 | static void handleSetWriteFormat() {
 469 |   setFormat(F_WRITE);
 470 | }
 471 | 
 472 | static void setFormat(uint8_t f) {
 473 | 
 474 |   uint8_t n;
 475 |   uint16_t totalBytes = 0;
 476 | 
 477 |   numDynamixels[f] = 0;
 478 |   totalNumBytes[f] = 0;
 479 | 
 480 |   if (!rxPC(&n, 1))
 481 |     goto DONE;
 482 | 
 483 |   if (n > MAX_DYNAMIXELS) {
 484 |     setStatusFlag(S_INVALID_PC_COMMAND);
 485 |     goto DONE;
 486 |   }
 487 | 
 488 |   for (uint8_t i = 0; i < n; i++) {
 489 | 
 490 |     uint8_t id, start, n;
 491 | 
 492 |     if (!rxPC(&id, 1))
 493 |       goto DONE;
 494 | 
 495 |     if (id > MAX_DYNAMIXEL_ID) {
 496 |       setStatusFlag(S_INVALID_PC_COMMAND);
 497 |       goto DONE;
 498 |     }
 499 | 
 500 |     if (!rxPC(&start, 1))
 501 |       goto DONE;
 502 |     
 503 |     if (start > MAX_START_ADDR) {
 504 |       setStatusFlag(S_INVALID_PC_COMMAND);
 505 |       goto DONE;
 506 |     }
 507 | 
 508 |     if (!rxPC(&n, 1))
 509 |       goto DONE;
 510 |     
 511 |     if (n > (NUM_REG_BYTES-start)) {
 512 |       setStatusFlag(S_INVALID_PC_COMMAND);
 513 |       goto DONE;
 514 |     }
 515 | 
 516 |     axID[f][i] = id;
 517 |     startAddr[f][i] = start;
 518 |     numBytes[f][i] = n;
 519 | 
 520 |     totalBytes += n;
 521 |   }
 522 | 
 523 |   totalNumBytes[f] = totalBytes;
 524 |   numDynamixels[f] = n;
 525 | 
 526 |   endRXPacketPC();
 527 |   
 528 |  DONE:
 529 |   txStatusPC();
 530 | }
 531 | 
 532 | static void handleReadData() {
 533 | 
 534 |   uint8_t rxPCok = endRXPacketPC();
 535 | 
 536 |   uint16_t bytesToGo = totalNumBytes[F_READ];
 537 | 
 538 |   startTXPacketPC(I_DATA);
 539 | 
 540 |   if (!rxPCok)
 541 |     goto DONE;
 542 |     
 543 |   for (uint8_t i = 0; i < numDynamixels[F_READ]; i++) {
 544 | 
 545 |     uint8_t id = axID[F_READ][i];
 546 |     uint8_t start = startAddr[F_READ][i];
 547 |     uint8_t n = numBytes[F_READ][i];
 548 | 
 549 |     uint8_t rxID, rxN, rxError;
 550 | 
 551 |     uint8_t returnError = 0;
 552 | 
 553 |     uint8_t thisN;
 554 | 
 555 |     /* read in blocks of up to MAX_DYNAMIXEL_BLOCK_READ bytes */
 556 | 
 557 |     while (n > 0) {
 558 | 
 559 |       thisN = (n <= MAX_DYNAMIXEL_BLOCK_READ) ? n : MAX_DYNAMIXEL_BLOCK_READ;
 560 |      
 561 |       /* do we need to read the virtual reg? */
 562 |       if ((start+thisN) == VIRTUAL_ERROR_REG_ADDR) {
 563 |         returnError = 1;
 564 |         thisN--;
 565 |       }
 566 | 
 567 |       for (uint8_t j = 0; j < thisN; j++)
 568 |         dynamixelParams[j] = 0xff;
 569 |       
 570 |       rxID = rxN = rxError = 0xff;
 571 |         
 572 |       /* retry loop */
 573 |       do {
 574 | 
 575 |         /* send read request packet */
 576 |         
 577 |         if (thisN > 0) {
 578 |           
 579 |           /* still need to read some non-virtual regs */
 580 |           
 581 |           startTXPacketDynamixel(id, 2, D_I_READ_DATA);
 582 |           txDynamixel(start, 1);
 583 |           txDynamixel(thisN, 1);
 584 |           
 585 |         } else {
 586 |           
 587 |           /* only reading the virtual reg (error) */
 588 |           
 589 |           startTXPacketDynamixel(id, 0, D_I_PING);
 590 |         }
 591 |         
 592 |         if (!endTXPacketDynamixel())
 593 |           goto DONE;
 594 |         
 595 |       } while (rxPacketDynamixel(&rxID, &rxN, &rxError, dynamixelParams, thisN)
 596 |                && tryDynamixelAgain(rxError));
 597 | 
 598 |       /* fwd read data */
 599 | 
 600 |       if ((rxID == id) && (rxN == thisN)) {
 601 |         
 602 |         /* recv pkt ok, though maybe timed out during params */
 603 |         
 604 |         for (uint8_t j = 0; j < thisN; j++) {
 605 |           txPC(dynamixelParams[j], 1);
 606 |           bytesToGo--;
 607 |         }
 608 | 
 609 |       } else {
 610 |         
 611 |         /* recv pkt from dynamixel bad, stuff return pkt */
 612 |         
 613 |         setStatusFlag(S_INVALID_DYNAMIXEL_RESPONSE);
 614 |         
 615 |         for (uint8_t j = 0; j < thisN; j++) {
 616 |           txPC(0xff, 1);
 617 |           bytesToGo--;
 618 |         }
 619 |       }
 620 | 
 621 |       /* return the virtual reg? */
 622 |       if (returnError) {
 623 |         txPC(rxError, 1);
 624 |         bytesToGo--;
 625 |         n--;
 626 |         start++;
 627 |       }
 628 | 
 629 |       n -= thisN;
 630 |       start += thisN;
 631 | 
 632 |     } /* for each block */
 633 |   } /* for each dynamixel */
 634 |  
 635 |  DONE: 
 636 | 
 637 |   /* stuff the return pkt as necessary */
 638 |   while (bytesToGo)
 639 |     txPC(0xff, 1);
 640 | 
 641 |   txPC(status, 1);
 642 |   txPC(numDynamixelRetries, 1);
 643 | 
 644 |   txADCValuesPC();
 645 |   
 646 |   endTXPacketPC();
 647 | }
 648 | 
 649 | static void handleWriteData() {
 650 | 
 651 |   for (uint8_t i = 0; i < numDynamixels[F_WRITE]; i++) {
 652 | 
 653 |     uint8_t id = axID[F_WRITE][i];
 654 |     uint8_t start = startAddr[F_WRITE][i];
 655 |     uint8_t n = numBytes[F_WRITE][i];
 656 | 
 657 |     uint8_t rxID = 0xff;
 658 |     uint8_t rxError = 0xff;
 659 | 
 660 |     for (uint8_t j = 0; j < n; j++) {
 661 |       if (!rxPC(&(dynamixelParams[j]), 1))
 662 |         goto DONE;
 663 |     }
 664 | 
 665 |     /* retry loop (requires dynamixel status return level = 2) */
 666 |     do {
 667 | 
 668 |       startTXPacketDynamixel(id, n+1, D_I_REG_WRITE);
 669 |       
 670 |       txDynamixel(start, 1);
 671 | 
 672 |       for (uint8_t j = 0; j < n; j++) 
 673 |         txDynamixel(dynamixelParams[j], 1);
 674 |       
 675 |       if (!endTXPacketDynamixel())
 676 |         goto DONE;
 677 | 
 678 |     } while (rxPacketDynamixel(&rxID, 0, &rxError, 0, 0) &&
 679 |              tryDynamixelAgain(rxError));
 680 | 
 681 |     if (rxID != id)
 682 |       setStatusFlag(S_INVALID_DYNAMIXEL_RESPONSE);
 683 |   }
 684 | 
 685 |   /* TBD do this redundantly since we can't verify checksums? */
 686 |   startTXPacketDynamixel(ID_BCAST, 0, D_I_ACTION);
 687 |   if (!endTXPacketDynamixel())
 688 |     goto DONE;
 689 |   
 690 |   endRXPacketPC();
 691 | 
 692 |  DONE:
 693 |   txStatusPC();
 694 | }
 695 | 
 696 | static void handleInvalid() {
 697 | 
 698 |   setStatusFlag(S_INVALID_PC_COMMAND);
 699 |   txStatusPC();
 700 | 
 701 |   ticksToGo = RX_CLEAR_DELAY_TICKS;
 702 |   while (ticksToGo)
 703 |     ;
 704 | 
 705 |   clearRxBufPC();
 706 |   clearRxBufDynamixel();
 707 | }
 708 | 
 709 | static void commandLoop() {
 710 | 
 711 |   uint8_t instruction;
 712 |   handler_t handler;
 713 | 
 714 |   for (;;) {
 715 | 
 716 |     LED_ON(LED_IDLE);
 717 | 
 718 |     cli();
 719 |     status = 0;
 720 |     sei();
 721 | 
 722 |     numDynamixelRetries = 0;
 723 |     checksumRxPC = 0;
 724 |     enableRxPCTimeout = 0;
 725 | 
 726 |     while (!rxPC(&instruction, 1))
 727 |       ;
 728 | 
 729 |     enableRxPCTimeout = 1;
 730 | 
 731 |     LED_OFF(LED_IDLE);
 732 | 
 733 |     handler = handleInvalid;
 734 | 
 735 |     if ((instruction&0xf0) == 0xf0)
 736 |       handler = commandHandler[instruction&0x0f];
 737 | 
 738 |     handler();
 739 |   }
 740 | }
 741 | 
 742 | static void setStatusFlag(uint8_t flag) {
 743 |   cli();
 744 |   status |= flag;
 745 |   sei();
 746 | }
 747 | 
 748 | static void clearStatusFlag(uint8_t flag) {
 749 |   cli();
 750 |   status &= ~flag;
 751 |   sei();
 752 | }
 753 | 
 754 | static uint8_t endRXPacketPC() {
 755 | 
 756 |   uint8_t checksum;
 757 | 
 758 |   if (!rxPC(&checksum, 0))
 759 |     return 0;
 760 | 
 761 |   //THIS IS BORKED
 762 |   //http://www.nongnu.org/avr-libc/user-manual/FAQ.html#faq_intpromote
 763 |   //if (checksum ^ (~checksumRxPC))
 764 |   if (checksum != ((uint8_t)(~checksumRxPC)))
 765 |     setStatusFlag(S_PC_CHECKSUM_ERROR);
 766 | 
 767 |   return 1;
 768 | }
 769 | 
 770 | static void startTXPacketPC(uint8_t instruction) {
 771 |   checksumTxPC = 0;
 772 |   txPC(instruction, 1);
 773 | }
 774 | 
 775 | static void endTXPacketPC() {
 776 |   txPC(~checksumTxPC, 0);
 777 | }
 778 | 
 779 | static void txStatusPC() {
 780 | 
 781 |   startTXPacketPC(I_STATUS);
 782 | 
 783 |   txPC(status, 1);
 784 |   txPC(numDynamixelRetries, 1);
 785 | 
 786 |   txADCValuesPC();
 787 | 
 788 |   endTXPacketPC();
 789 | }
 790 | 
 791 | static void txADCValuesPC() {
 792 |   cli();
 793 |   txPC(adcValue[CHANNEL_POS], 1);
 794 |   txPC(adcValue[CHANNEL_NEG], 1);
 795 |   txPC(adcValue[CHANNEL_THERM], 1);
 796 |   sei();
 797 | }
 798 | 
 799 | static uint8_t startRXPacketDynamixel(uint8_t *id,
 800 |                                       uint8_t *numParams,
 801 |                                       uint8_t *error) {
 802 |   checksumRxDynamixel = 0;
 803 | 
 804 |   uint8_t byte;
 805 | 
 806 |   for (uint8_t i = 0; i < 2; i++) {
 807 | 
 808 |     if (!rxDynamixel(&byte, 0))
 809 |       return 0;
 810 |     
 811 |     if (byte != 0xff)
 812 |       setStatusFlag(S_INVALID_DYNAMIXEL_RESPONSE);
 813 |   }
 814 | 
 815 |   if (!rxDynamixel(&byte, 1))
 816 |     return 0;
 817 |   
 818 |   if (id)
 819 |     *id = byte;
 820 |   
 821 |   if (!rxDynamixel(&byte, 1))
 822 |     return 0;
 823 |  
 824 |   if (numParams)
 825 |     *numParams = byte-2;
 826 | 
 827 |   if (!rxDynamixel(&byte, 1))
 828 |     return 0;
 829 | 
 830 |   if (error)
 831 |     *error = byte;
 832 | 
 833 |   return 1;
 834 | }
 835 | 
 836 | static uint8_t endRXPacketDynamixel() {
 837 | 
 838 |   uint8_t checksum;
 839 | 
 840 |   if (!rxDynamixel(&checksum, 0))
 841 |     return 0;
 842 | 
 843 |   //THIS IS BORKED
 844 |   //http://www.nongnu.org/avr-libc/user-manual/FAQ.html#faq_intpromote
 845 |   //if (checksum != (~checksumRxPC))
 846 |   if (checksum != ((uint8_t)~checksumRxDynamixel))
 847 |     setStatusFlag(S_DYNAMIXEL_CHECKSUM_ERROR);
 848 | 
 849 |   return 1;
 850 | }
 851 | 
 852 | static uint8_t rxPacketDynamixel(uint8_t *id,
 853 |                                  uint8_t *numParams,
 854 |                                  uint8_t *error,
 855 |                                  uint8_t *params,
 856 |                                  uint8_t numParamsExpected) {
 857 |   uint8_t actualNumParams;
 858 | 
 859 |   if (!startRXPacketDynamixel(id, &actualNumParams, error))
 860 |     return 0;
 861 | 
 862 |   if (numParams)
 863 |     *numParams = actualNumParams;
 864 | 
 865 |   uint8_t byte;
 866 | 
 867 |   for (uint8_t i = 0; i < actualNumParams; i++) {
 868 | 
 869 |     if (!rxDynamixel(&byte, 1))
 870 |       return 0;
 871 | 
 872 |     if (params && (i < numParamsExpected))
 873 |       params[i] = byte;
 874 |   }
 875 | 
 876 |   if (!endRXPacketDynamixel())
 877 |     return 0;
 878 | 
 879 |   if (actualNumParams != numParamsExpected)
 880 |     setStatusFlag(S_INVALID_DYNAMIXEL_RESPONSE);
 881 | 
 882 |   return 1;
 883 | }
 884 | 
 885 | static void startTXPacketDynamixel(uint8_t id,
 886 |                                    uint8_t numParams,
 887 |                                    uint8_t instruction) {
 888 |   checksumTxDynamixel = 0;
 889 | 
 890 |   txDynamixel(0xff, 0);
 891 |   txDynamixel(0xff, 0);
 892 | 
 893 |   txDynamixel(id, 1);
 894 |   txDynamixel(numParams+2, 1);
 895 |   txDynamixel(instruction, 1);
 896 | }
 897 | 
 898 | static uint8_t endTXPacketDynamixel() {
 899 | 
 900 |   txDynamixel(~checksumTxDynamixel, 0);
 901 | 
 902 |   ticksToGo = TX_DYNAMIXEL_TIMEOUT_TICKS;
 903 | 
 904 |   while (!CBUF_IsEmpty(dynamixelTxBuffer) || !DYNAMIXEL_TXD_DONE) {
 905 |     if (!ticksToGo) {
 906 |       setStatusFlag(S_DYNAMIXEL_TIMEOUT);
 907 |       LED_ON(LED_ERROR);
 908 |       return 0;
 909 |     }
 910 |   }
 911 | 
 912 |   return 1;
 913 | }
 914 | 
 915 |  static uint8_t tryDynamixelAgain(uint8_t dynamixelError) {
 916 | 
 917 |    if (((dynamixelError&DYNAMIXEL_INSTRUCTION_CHECKSUM_ERROR) ||
 918 |         (status&S_DYNAMIXEL_CHECKSUM_ERROR)) &&
 919 |        (numDynamixelRetries < (MAX_DYNAMIXEL_TRIES-1))) {
 920 |     numDynamixelRetries++;
 921 |     clearStatusFlag(S_DYNAMIXEL_CHECKSUM_ERROR);
 922 |     return 1; 
 923 |   } 
 924 | 
 925 |   return 0;
 926 | }
 927 | 
 928 | static void initHardware() {
 929 | 
 930 |   /* NOTE: even if we don't think we're using some of this hardware, we should
 931 |    * probably still init the port pins for it to be on the safe side.
 932 |    */
 933 | 
 934 |   /* inputs */
 935 | 
 936 |   DDRA = DDRB = DDRC = DDRD = DDRE = DDRF = 0;  /* set all ports to input */
 937 |   PORTB = PORTC = PORTD = PORTE = PORTF = PORTG = 0x00;
 938 | 
 939 | #if defined (CM5)
 940 |   SFIOR &= ~_BV(PUD); //enable pullups
 941 | #elif defined(CM510)
 942 |   MCUCR &= ~_BV(PUD); //enable pullups
 943 | #else
 944 |   #error "MCU has an unrecognized value."
 945 | #endif
 946 |   
 947 |   /* pullups on pushbuttons */
 948 |   PORTE |= PB_UP|PB_DOWN|PB_LEFT|PB_RIGHT;
 949 |   PORTD |= PB_START;
 950 | 
 951 |   /* no pullup on BIT_LINK_PLUGIN */
 952 | 
 953 |   /* outputs */
 954 | 
 955 |   DDRC |= LED_ALL;
 956 |   LED_OFF(LED_ALL);
 957 | 
 958 |   DDRE |= _BV(BIT_DYNAMIXEL_RXD)|_BV(BIT_DYNAMIXEL_TXD);
 959 |   DDRD |= _BV(BIT_ENABLE_RXD_LINK_PC)|_BV(BIT_ENABLE_RXD_LINK_ZIGBEE);
 960 | 
 961 |   /* zigbee led bit is now bluetooth rts signal, and it's an input */
 962 |   DDRD |= _BV(BIT_ZIGBEE_RESET)/*_BV(BIT_ZIGBEE_LED)*/; 
 963 | 
 964 |   /* zigbee reset is now bluetooth cts signal, and it needs to be low */
 965 |   /*   PORTD |= _BV(BIT_ZIGBEE_RESET); */
 966 | 
 967 | #ifdef INITIAL_RXD_BLUETOOTH
 968 | 
 969 |   /* initially listen to the bluetooth */
 970 |   PORTD |= _BV(BIT_ENABLE_RXD_LINK_BLUETOOTH);
 971 |   LED_ON(LED_BLUETOOTH_ACTIVE);
 972 | 
 973 | #else
 974 | 
 975 |   /* initially listen to the pc */
 976 |   PORTD |= _BV(BIT_ENABLE_RXD_LINK_PC);
 977 | 
 978 | #endif
 979 | 
 980 |   DYNAMIXEL_RXD; /* initially listen on the dynamixel bus */
 981 | 
 982 |   DDRB |= _BV(BIT_CHARGE);
 983 |   PORTB |= _BV(BIT_CHARGE); /* not charging */
 984 | 
 985 |   /* external AREF (connected to 5V), left adj result so ADCH is high byte **/
 986 |   ADMUX = _BV(ADLAR);
 987 | 
 988 |   /* enable ADC and divide 16MHz by 128 for 125kHz adc clock (~0.1ms conv) */
 989 |   ADCSRA = _BV(ADEN)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
 990 | 
 991 |   /* do a dummy conversion */
 992 |   CONVERT_AND_WAIT(CHANNEL_GND);
 993 | 
 994 |   readADCs();
 995 | 
 996 |   /* UART0: half-duplex dynamixel bus */
 997 |   UBRR0H = 0;
 998 |   UBRR0L = DYNAMIXEL_BAUDRATE;
 999 |   UCSR0A = _BV(U2X0); /* double speed */
1000 |   UCSR0B = _BV(TXEN0)|_BV(RXEN0)|_BV(RXCIE0);
1001 |   UCSR0C = _BV(UCSZ01)|_BV(UCSZ00); /* 8N1 */
1002 | 
1003 |   /* UART1: pc */
1004 |   UBRR1H = 0;
1005 |   UBRR1L = PC_BAUDRATE;
1006 |   UCSR1A = _BV(U2X1); /* double speed */
1007 |   UCSR1B = _BV(TXEN1)|_BV(RXEN1)|_BV(RXCIE1);
1008 |   UCSR1C = _BV(UCSZ11)|_BV(UCSZ10); /* 8N1 */
1009 | 
1010 |   /* counter 0 interrupt every ~10ms */
1011 | #if defined(CM5)
1012 |   TCCR0 = _BV(WGM01); /* mode 2, Clear Timer on Compare match */
1013 |   TCCR0 |= _BV(CS02)|_BV(CS01)|_BV(CS00); /* divide by 1024 */
1014 |   TIMSK |= _BV(OCIE0); /* OCR interrupt enable */
1015 |   OCR0 = 156; /* 16M/1024 = 15625Hz; 15625/156 = 100.16Hz */
1016 | #elif defined(CM510)
1017 |   TCCR0A = _BV(WGM01); /*Use mode 2, Clear Timer on Compare match */
1018 |   TCCR0B |= _BV(CS02)|~_BV(CS01)|_BV(CS00); /* divide by 1024 */
1019 |   TIMSK0 |= _BV(OCIE0A);/*OCRA interrupt enable,use the CCT and outPut compare match */
1020 |   OCR0A = 156; /* 16M/1024 = 15625Hz; 15625/156 = 100.16Hz */
1021 | #else
1022 |   #error "MCU has an unrecognized value."
1023 | #endif
1024 | }
1025 | 
1026 | static void txDynamixel(uint8_t byte, uint8_t addToChecksum) {
1027 | 
1028 |   DYNAMIXEL_TXD;
1029 | 
1030 |   LED_ON(LED_TXD_DYNAMIXEL);
1031 | 
1032 |   while (CBUF_IsFull(dynamixelTxBuffer))
1033 |     ;
1034 | 
1035 |   CBUF_Push(dynamixelTxBuffer, byte);
1036 | 
1037 |   UCSR0B |= _BV(UDRIE0);
1038 | 
1039 |   //  /* wait until tx reg is empty */
1040 |   //  loop_until_bit_is_set(UCSR0A, UDRE0);
1041 |   //
1042 |   //  UDR0 = byte;
1043 | 
1044 |   if (addToChecksum)
1045 |     checksumTxDynamixel += byte;
1046 | }
1047 | 
1048 | static void txPC(uint8_t byte, uint8_t addToChecksum) {
1049 | 
1050 |   LED_ON(LED_TXD_PC);
1051 | 
1052 |   while (CBUF_IsFull(pcTxBuffer))
1053 |     ;
1054 | 
1055 |   CBUF_Push(pcTxBuffer, byte);
1056 | 
1057 |   UCSR1B |= _BV(UDRIE1);
1058 | 
1059 |   //  /* wait until tx reg is empty */
1060 |   //  loop_until_bit_is_set(UCSR1A, UDRE1);
1061 |   //
1062 |   //  UDR1 = byte;
1063 | 
1064 |   if (addToChecksum)
1065 |     checksumTxPC += byte;
1066 | }
1067 | 
1068 | static uint8_t rxDynamixel(uint8_t *byte, uint8_t addToChecksum) {
1069 | 
1070 |   DYNAMIXEL_RXD;
1071 | 
1072 |   ticksToGo = RX_DYNAMIXEL_TIMEOUT_TICKS;
1073 | 
1074 |   while (CBUF_IsEmpty(dynamixelRxBuffer)) {
1075 | 
1076 |     if (enableRxDynamixelTimeout && !ticksToGo) {
1077 |       setStatusFlag(S_DYNAMIXEL_TIMEOUT);
1078 |       return 0;
1079 |     }
1080 |   }
1081 |   
1082 |   uint8_t b = CBUF_Pop(dynamixelRxBuffer);
1083 | 
1084 |   if (byte)
1085 |     *byte = b;
1086 | 
1087 |   if (addToChecksum)
1088 |     checksumRxDynamixel += b;
1089 | 
1090 |   return 1;
1091 | }
1092 | 
1093 | static uint8_t rxPC(uint8_t *byte, uint8_t addToChecksum) {
1094 | 
1095 |   ticksToGo = RX_PC_TIMEOUT_TICKS;
1096 | 
1097 |   while (CBUF_IsEmpty(pcRxBuffer)) {
1098 |     if (enableRxPCTimeout && !ticksToGo) {
1099 |       setStatusFlag(S_PC_TIMEOUT);
1100 |       return 0;
1101 |     }
1102 |   }
1103 |   
1104 |   uint8_t b = CBUF_Pop(pcRxBuffer);
1105 | 
1106 |   if (byte)
1107 |     *byte = b;
1108 | 
1109 |   if (addToChecksum)
1110 |     checksumRxPC += b;
1111 | 
1112 |   return 1;
1113 | }
1114 | 
1115 | /*
1116 | static dynamixelRxIndex_t numWaitingDynamixel() {
1117 |   return CBUF_Len(dynamixelRxBuffer);
1118 | }
1119 | 
1120 | static pcRxIndex_t numWaitingPC() {
1121 |   return CBUF_Len(pcRxBuffer);
1122 | }
1123 | */
1124 | 
1125 | static void clearRxBufDynamixel() {
1126 |   CBUF_Init(dynamixelRxBuffer);
1127 | }
1128 | 
1129 | static void clearRxBufPC() {
1130 |   CBUF_Init(pcRxBuffer);
1131 | }
1132 | 
1133 | static void readADCs() {
1134 | 
1135 |   CONVERT_AND_WAIT(CHANNEL_POS);
1136 |   adcValue[CHANNEL_POS] = ADCH;
1137 | 
1138 |   CONVERT_AND_WAIT(CHANNEL_NEG);
1139 |   adcValue[CHANNEL_NEG] = ADCH;
1140 | 
1141 |   CONVERT_AND_WAIT(CHANNEL_THERM);
1142 |   adcValue[CHANNEL_THERM] = ADCH;
1143 | }
1144 | 
1145 | /* rx from dynamixel */
1146 | #if defined(CM5)
1147 |   ISR(SIG_UART0_RECV) {
1148 | #elif defined(CM510)
1149 |   ISR(SIG_USART0_RECV) {
1150 | #else
1151 |   #error "MCU has an unrecognized value."
1152 | #endif
1153 |   LED_ON(LED_RXD_DYNAMIXEL);
1154 | 
1155 |   uint8_t byte = UDR0;
1156 | 
1157 |   if (!CBUF_IsFull(dynamixelRxBuffer)) {
1158 |     CBUF_Push(dynamixelRxBuffer, byte);
1159 |   } else {
1160 |     status |= S_DYNAMIXEL_RX_OVERFLOW;
1161 |     LED_ON(LED_ERROR);
1162 |   }
1163 | }
1164 | 
1165 | /* tx buf to dynamixel empty */
1166 | #if defined(CM5)
1167 |   ISR(SIG_UART0_DATA) {
1168 | #elif defined(CM510)
1169 |   ISR(SIG_USART0_DATA) {
1170 | #else
1171 |   #error "MCU has an unrecognized value."
1172 | #endif
1173 |   if (CBUF_IsEmpty(dynamixelTxBuffer)) {
1174 | 
1175 |     UCSR0B &= ~_BV(UDRIE0);
1176 | 
1177 |   } else {
1178 | 
1179 |     UDR0 = CBUF_Pop(dynamixelTxBuffer);
1180 | 
1181 |   }
1182 | }
1183 | 
1184 | /* rx from PC */
1185 | #if defined(CM5)
1186 |   ISR(SIG_UART1_RECV) {
1187 | #elif defined(CM510)
1188 |   ISR(SIG_USART1_RECV) {
1189 | #else
1190 |   #error "MCU has an unrecognized value."
1191 | #endif
1192 |   LED_ON(LED_RXD_PC);
1193 | 
1194 |   uint8_t byte = UDR1;
1195 | 
1196 |   if (!CBUF_IsFull(pcRxBuffer)) {
1197 |     CBUF_Push(pcRxBuffer, byte);
1198 |   } else {
1199 |     status |= S_PC_RX_OVERFLOW;
1200 |     LED_ON(LED_ERROR);
1201 |   }
1202 | }
1203 | 
1204 | /* tx buf to pc empty */
1205 | #if defined(CM5)
1206 |   ISR(SIG_UART1_DATA) {
1207 | #elif defined(CM510)
1208 |   ISR(SIG_USART1_DATA) {
1209 | #else
1210 |   #error "MCU has an unrecognized value."
1211 | #endif
1212 |   if (CBUF_IsEmpty(pcTxBuffer)) {
1213 | 
1214 |     UCSR1B &= ~_BV(UDRIE1);
1215 | 
1216 |   } else {
1217 | 
1218 |     if (!BLUETOOTH_ACTIVE || !(PORTD&_BV(BIT_BLUETOOTH_RTS)))
1219 |       UDR1 = CBUF_Pop(pcTxBuffer);
1220 | 
1221 |     /* if we skip sending this byte because the bt module had its RTS raised,
1222 |        we will try again either at the next txPC() or at the next 10ms tick
1223 |        that occurs with RTS low */
1224 |   }
1225 | }
1226 | 
1227 | /** we get this interrupt every ~10 ms **/
1228 | #if defined(CM5)
1229 |   ISR(SIG_OUTPUT_COMPARE0) {
1230 | #elif defined(CM510)
1231 |   ISR(SIG_OUTPUT_COMPARE0A) {
1232 | #else
1233 |   #error "MCU has an unrecognized value."
1234 | #endif
1235 |   if (ticksToGo)
1236 |     ticksToGo--;
1237 | 
1238 |   if (CBUF_IsEmpty(pcRxBuffer))
1239 |     LED_OFF(LED_RXD_PC);
1240 |   
1241 |   if (CBUF_IsEmpty(pcTxBuffer))
1242 |       LED_OFF(LED_TXD_PC);
1243 | 
1244 |   if (!BLUETOOTH_ACTIVE) {
1245 |     
1246 |     if (CBUF_IsEmpty(dynamixelRxBuffer))
1247 |       LED_OFF(LED_RXD_DYNAMIXEL);
1248 |     
1249 |     if (CBUF_IsEmpty(dynamixelTxBuffer))
1250 |       LED_OFF(LED_TXD_DYNAMIXEL);
1251 | 
1252 |   }  else {
1253 | 
1254 |     if (PORTD&_BV(BIT_BLUETOOTH_RTS))
1255 |       LED_ON(LED_BLUETOOTH_RTS);
1256 |     else
1257 |       LED_OFF(LED_BLUETOOTH_RTS);
1258 | 
1259 |   }
1260 | 
1261 |   if (PB(PB_TOGGLE_RX_BLUETOOTH_PC) && !bluetoothTogglePending)
1262 |     bluetoothTogglePending = 1; /* btn pushed */
1263 | 
1264 |   if (!PB(PB_TOGGLE_RX_BLUETOOTH_PC) && (bluetoothTogglePending == 2))
1265 |     bluetoothTogglePending = 0; /* toggle done & btn released */
1266 | 
1267 |   if ((bluetoothTogglePending == 1) &&
1268 |       CBUF_IsEmpty(pcTxBuffer) && CBUF_IsEmpty(pcRxBuffer) &&
1269 |       !(UCSR1B&_BV(UDRIE1))) {
1270 | 
1271 |     if (BLUETOOTH_ACTIVE) {
1272 |       PORTD &= ~_BV(BIT_ENABLE_RXD_LINK_BLUETOOTH);
1273 |       PORTD |= _BV(BIT_ENABLE_RXD_LINK_PC);
1274 |       LED_OFF(LED_BLUETOOTH_ACTIVE);
1275 |     } else {
1276 |       PORTD &= ~_BV(BIT_ENABLE_RXD_LINK_PC);
1277 |       PORTD |= _BV(BIT_ENABLE_RXD_LINK_BLUETOOTH);
1278 |       LED_ON(LED_BLUETOOTH_ACTIVE);
1279 |     }
1280 | 
1281 |     bluetoothTogglePending = 2;
1282 |   }
1283 | 
1284 |   if (BLUETOOTH_ACTIVE &&
1285 |       !CBUF_IsEmpty(pcTxBuffer) &&
1286 |       !(PORTD&_BV(BIT_BLUETOOTH_RTS)))
1287 |     UCSR1B |= _BV(UDRIE1);
1288 | 
1289 | #ifdef ENABLE_CHARGING
1290 | 
1291 |   if (PB(PB_TOGGLE_CHARGE_ENABLE) && !chargeEnableTogglePending)
1292 |     chargeEnableTogglePending = 1; /* btn pushed */
1293 | 
1294 |   if (!PB(PB_TOGGLE_CHARGE_ENABLE) && (chargeEnableTogglePending == 2))
1295 |     chargeEnableTogglePending = 0; /* toggle done & btn released */
1296 | 
1297 |   if (chargeEnableTogglePending == 1) {
1298 |     chargeEnabled = !chargeEnabled;
1299 |     chargeEnableTogglePending = 2;
1300 |   }
1301 | 
1302 |   if (chargePhaseTicksToGo) {
1303 | 
1304 |     chargePhaseTicksToGo--;
1305 | 
1306 |   } else if (chargePhase == CHARGE_PHASE) {
1307 | 
1308 |     chargePhase = MEASURE_PHASE;
1309 |     chargePhaseTicksToGo = MEASURE_PHASE_TICKS;
1310 | 
1311 |     if (charging) {
1312 |       PORTB |= _BV(BIT_CHARGE); /* stop charging */
1313 |       LED_OFF(LED_PWR);
1314 |     }
1315 | 
1316 |   } else {
1317 | 
1318 |     chargePhase = CHARGE_PHASE;
1319 |     chargePhaseTicksToGo = CHARGE_PHASE_TICKS;
1320 | 
1321 |     if (charging) {
1322 |       PORTB &= ~_BV(BIT_CHARGE); /* start charging */
1323 |       LED_ON(LED_PWR);
1324 |     }
1325 |   }
1326 | 
1327 |   if ((chargePhase == MEASURE_PHASE) &&
1328 |       (chargePhaseTicksToGo == MEASURE_TICKS)) {
1329 | 
1330 |     readADCs();
1331 | 
1332 |     if (charging &&
1333 |         (!chargeEnabled ||
1334 |          (adcValue[CHANNEL_NEG] < CHARGE_COMPLETE_THRESHOLD))) {
1335 | 
1336 |       charging = 0;
1337 |       chargeEnabled = 0;
1338 | 
1339 |       PORTB |= _BV(BIT_CHARGE); /* stop charging */
1340 | 
1341 |       LED_ON(LED_PWR);
1342 |     }
1343 | 
1344 |     if (chargeEnabled && (adcValue[CHANNEL_NEG] > CHARGE_START_THRESHOLD))
1345 |       charging = 1;
1346 |   }
1347 | 
1348 | #endif /* ENABLE_CHARGING */
1349 | 
1350 | }
1351 | 
1352 | /* default interrupt vector lights LEDs and hangs (should not get here) */
1353 | ISR(__vector_default) {
1354 | 
1355 |   LED_ON(LED_ALL);
1356 | 
1357 |   for (;;)
1358 |     ;
1359 | }
1360 | 
1361 | 


--------------------------------------------------------------------------------
/firmware/makefile:
--------------------------------------------------------------------------------
  1 | # Hey Emacs, this is a -*- makefile -*-
  2 | 
  3 | # common definition
  4 | TARGET = brbrain
  5 | SRC = $(wildcard *.c)
  6 | ASRC = 
  7 | OPT = s
  8 | 
  9 | # CM dependent definition
 10 | ifeq ($(cm),cm510)
 11 | 	MCU = atmega2561
 12 | 	CPU_HZ = 16E6
 13 | 	CM = -DCM510
 14 | else
 15 | 	MCU = atmega128
 16 | 	CPU_HZ = 16E6
 17 | 	CM = -DCM5
 18 | endif
 19 | 
 20 | # set this nonzero to define the cpp symbol EN_DBG at compile time (this may be
 21 | # used to enable runtime debug code, and is orthogonal to compiling with debug
 22 | # info).
 23 | #EN_DBG = 1
 24 | 
 25 | # PRINTF_LIB_MIN - Minimalistic printf version
 26 | # PRINTF_LIB_FLOAT - Floating point printf version (requires MATH_LIB = -lm)
 27 | #PRINTF_LIB = $(PRINTF_LIB_MIN)
 28 | 
 29 | # SCANF_LIB_MIN - Minimalistic scanf version
 30 | # SCANF_LIB_FLOAT - Floating point + %[ scanf version (requires MATH_LIB = -lm)
 31 | #SCANF_LIB = $(SCANF_LIB_MIN)
 32 | 
 33 | #MATH_LIB = -lm
 34 | 
 35 | # AVRDUDE_PROGRAMMER = stk500v2
 36 | # AVRDUDE_PORT = /dev/ttyS0
 37 | 
 38 | # Use these values for the new avr programmers which have a usb interface.
 39 | # They are mkii programmers instead of stk500, although this is mostly
 40 | # irrelevant.
 41 | # -B command sets sck in microseconds, default may be very slow, see
 42 | # http://arcknowledge.com/hardware.avr.avrdude.devel/2006-11/msg00004.html
 43 | AVRDUDE_PROGRAMMER = avrisp2 -B 5
 44 | AVRDUDE_PORT = usb
 45 | 
 46 | # note: you need privileged access to the usb port to do this so you either
 47 | # have to run the program command as root or you can remount the usb file
 48 | # system to give yourself permission:
 49 | #
 50 | # sudo mount -t usbfs none /proc/bus/usb -o remount,devgid=`id -g`,devmode=664
 51 | #
 52 | # A similar line could be put in your /etc/fstab to make this happen
 53 | # automatically at boot
 54 | 
 55 | # Uncomment the following if you do /not/ wish a verification to be
 56 | # performed after programming the device.
 57 | # AVRDUDE_NO_VERIFY = -V
 58 | 
 59 | # Define [FUSE_BYTE | (LFUSE_BYTE HFUSE_BYTE [EFUSE_BYTE])] and [LOCK_BYTE]
 60 | # in hex to have them programmed by the program-fuses and program-lock
 61 | # targets
 62 | # FUSE_BYTE =
 63 | # LFUSE_BYTE =
 64 | # HFUSE_BYTE =
 65 | # EFUSE_BYTE =
 66 | # LOCK_BYTE =
 67 | 
 68 | # Compiler flag to set the C Standard level.
 69 | # c89   - "ANSI" C
 70 | # gnu89 - c89 plus GCC extensions
 71 | # c99   - ISO C99 standard (not yet fully implemented)
 72 | # gnu99 - c99 plus GCC extensions
 73 | CSTANDARD = -std=gnu99
 74 |  
 75 | # Place -D or -U options here
 76 | CDEFS = $(CM)
 77 | 
 78 | # Place -I options here
 79 | CINCS = -I/usr/local/avr/include
 80 | 
 81 | # choose one for dead-code elimination (or choose none for no DCE)
 82 | #DCE = compile-time
 83 | # 
 84 | # broken until binutils-avr-2.17? http://lists.gnu.org/archive/html/avr-gcc-list/2006-08/msg00055.html
 85 | # DCE = link-time 
 86 | 
 87 | default:
 88 | 	@echo "building BrBrain firmware for $(MCU)..."
 89 | 	$(MAKE) clean
 90 | 	$(MAKE) all TARGET=$(TARGET)-initial-rs232
 91 | 	@echo "done"
 92 | 	
 93 | bluetooth:
 94 | 	$(MAKE) clean
 95 | 	$(MAKE) all TARGET=$(TARGET)-initial-bt CDEFS="$(CDEFS) -DINITIAL_RXD_BLUETOOTH"
 96 | 	
 97 | realclean-all:
 98 | 	@echo "cleaning..."
 99 | 	$(MAKE) realclean TARGET=$(TARGET)-initial-rs232
100 | 	$(MAKE) realclean TARGET=$(TARGET)-initial-bt
101 | 	@echo "done"
102 | 
103 | -include ./makefile_AVR.inc
104 | 
105 | 


--------------------------------------------------------------------------------
/firmware/makefile_AVR.inc:
--------------------------------------------------------------------------------
  1 | # Hey Emacs, this is a -*- makefile -*-
  2 | 
  3 | ######################################################################################
  4 | # generic parts of AVR-GCC Makefile, derived from the WinAVR template (which
  5 | # is public domain), believed to be neutral to any flavor of "make"
  6 | # (GNU make, BSD make, SysV make)
  7 | ######################################################################################
  8 | # specific makefiles in each dir should include the following block:
  9 | 
 10 | # MCU = atmega8
 11 | # TARGET = foo 
 12 | # SRC = $(wildcard *.c)
 13 | # ASRC = 
 14 | # OPT = s
 15 | # CPU_HZ = 4E6
 16 | # 
 17 | # # set this nonzero to define the cpp symbol EN_DBG at compile time (this may
 18 | # # be used to enable runtime debug code, and is orthogonal to compiling with
 19 | # # debug info).
 20 | # #EN_DBG = 1
 21 | #
 22 | # # PRINTF_LIB_MIN - Minimalistic printf version
 23 | # # PRINTF_LIB_FLOAT - Floating point printf version (requires MATH_LIB = -lm)
 24 | # PRINTF_LIB = $(PRINTF_LIB_FLOAT)
 25 | #
 26 | # # SCANF_LIB_MIN - Minimalistic scanf version
 27 | # # SCANF_LIB_FLOAT - Floating point + %[ scanf version (requires MATH_LIB = -lm)
 28 | # SCANF_LIB = $(SCANF_LIB_FLOAT)
 29 | #
 30 | # MATH_LIB = -lm
 31 | #
 32 | # AVRDUDE_PROGRAMMER = stk500v2
 33 | # AVRDUDE_PORT = /dev/ttyS0
 34 | #
 35 | # # Use these values for the new avr programmers which have a usb interface.
 36 | # # They are mkii programmers instead of stk500, although this is mostly
 37 | # # irrelevant.
 38 | # # AVRDUDE_PROGRAMMER = avrisp2
 39 | # # AVRDUDE_PORT = usb
 40 | #
 41 | # # note: you need privileged access to the usb port to do this so you either
 42 | # # have to run the program command as root or you can remount the usb file
 43 | # # system to give yourself permission:
 44 | # #
 45 | # # sudo mount -t usbfs none /proc/bus/usb -o remount,devgid=`id -g`,devmode=664
 46 | # #
 47 | # # A similar line could be put in your /etc/fstab to make this happen
 48 | # # automatically at boot
 49 | #
 50 | # # Uncomment the following if you do /not/ wish a verification to be
 51 | # # performed after programming the device.
 52 | # AVRDUDE_NO_VERIFY = -V
 53 | # 
 54 | # # Define [FUSE_BYTE | (LFUSE_BYTE HFUSE_BYTE [EFUSE_BYTE])] and [LOCK_BYTE]
 55 | # # in hex to have them programmed by the program-fuses and program-lock
 56 | # # targets
 57 | # # FUSE_BYTE =
 58 | # # LFUSE_BYTE =
 59 | # # HFUSE_BYTE =
 60 | # # EFUSE_BYTE =
 61 | # # LOCK_BYTE =
 62 | #
 63 | # # Compiler flag to set the C Standard level.
 64 | # # c89   - "ANSI" C
 65 | # # gnu89 - c89 plus GCC extensions
 66 | # # c99   - ISO C99 standard (not yet fully implemented)
 67 | # # gnu99 - c99 plus GCC extensions
 68 | # CSTANDARD = -std=gnu99
 69 | # 
 70 | # # Place -D or -U options here
 71 | # CDEFS =
 72 | # 
 73 | # # Place -I options here
 74 | # CINCS = 
 75 | # 
 76 | # # choose one for dead-code elimination (or choose none for no DCE)
 77 | # # DCE = compile-time
 78 | # 
 79 | # # broken until binutils-avr-2.17? http://lists.gnu.org/archive/html/avr-gcc-list/2006-08/msg00055.html
 80 | # # DCE = link-time
 81 | #
 82 | # -include makefile_AVR.inc 
 83 | 
 84 | ######################################################################################
 85 | ######################################################################################
 86 | 
 87 | 
 88 | 
 89 | # The rest of this file is the generic stuff
 90 | 
 91 | SVN_VERSION := $(shell svnversion -n | cut -f2 -d:)
 92 | BUILD_DATE := $(shell date +%Y-%m-%d)
 93 | 
 94 | # Debugging format.
 95 | # Native formats for AVR-GCC's -g are stabs [default], or dwarf-2.
 96 | # AVR (extended) COFF requires stabs, plus an avr-objcopy run.
 97 | DEBUG = stabs
 98 | 
 99 | #CDEBUG = -g$(DEBUG)
100 | CDBG = $(if $(EN_DBG),-DEN_DBG,)
101 | CWARN = -Wall #-Wstrict-prototypes
102 | CTUNING = -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums
103 | #CEXTRA = -Wa,-adhlns=$(<:.c=.lst)
104 | 
105 | #compile-time DCE compiler options
106 | CDCEC = -combine -fwhole-program
107 | 
108 | #link-time DCE compiler options
109 | CDCEL = -ffunction-sections
110 | 
111 | #link-time DCE linker options
112 | LDCEL = -Wl,--gc-sections
113 | 
114 | #ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs 
115 | 
116 | CFLAGS += $(CDEBUG) $(CDBG) $(CDEFS) $(CINCS) -O$(OPT) $(CWARN) $(CSTANDARD) -DF_CPU=$(CPU_HZ) -DSVN_VERSION=\"$(SVN_VERSION)\" -DBUILD_DATE=\"$(BUILD_DATE)\" $(CEXTRA)
117 | 
118 | # used for "make depend"
119 | DEPENDFILE = makefile.depend
120 | 
121 | #Additional libraries.
122 | 
123 | # Minimalistic printf version
124 | PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min
125 | 
126 | # Floating point printf version (requires MATH_LIB = -lm below)
127 | PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt
128 | 
129 | 
130 | # Minimalistic scanf version
131 | SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min
132 | 
133 | # Floating point + %[ scanf version (requires MATH_LIB = -lm below)
134 | SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt
135 | 
136 | # External memory options
137 | 
138 | # 64 KB of external RAM, starting after internal RAM (ATmega128!),
139 | # used for variables (.data/.bss) and heap (malloc()).
140 | #EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff
141 | 
142 | # 64 KB of external RAM, starting after internal RAM (ATmega128!),
143 | # only used for heap (malloc()).
144 | #EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff
145 | 
146 | EXTMEMOPTS =
147 | 
148 | #LDMAP = $(LDFLAGS) -Wl,-Map=$(TARGET).map,--cref
149 | LDFLAGS = $(EXTMEMOPTS) $(LDMAP) $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)
150 | 
151 | # Programming support using avrdude.
152 | 
153 | AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex
154 | #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
155 | AVRDUDE_WRITE_FUSES = $(if $(FUSE_BYTE),-U fuse:w:0x$(FUSE_BYTE):m,$(if $(LFUSE_BYTE),-U lfuse:w:0x$(LFUSE_BYTE):m) $(if $(HFUSE_BYTE),-U hfuse:w:0x$(HFUSE_BYTE):m) $(if $(EFUSE_BYTE),-U efuse:w:0x$(EFUSE_BYTE):m))
156 | AVRDUDE_WRITE_LOCK = $(if $(LOCK_BYTE),-U lock:w:$(LOCK_BYTE):m)
157 | 
158 | AVRDUDE_READBACK_FILE = .avrdude-readback
159 | 
160 | DUMP_AVRDUDE_READBACK_FILE = echo; echo $@; echo 76543210; xxd -b $(AVRDUDE_READBACK_FILE) | cut -d ' ' -f 2; xxd $(AVRDUDE_READBACK_FILE) | cut -d ' ' -f 2; rm $(AVRDUDE_READBACK_FILE)
161 | 
162 | # Uncomment the following if you want avrdude's erase cycle counter.
163 | # Note that this counter needs to be initialized first using -Yn,
164 | # see avrdude manual.
165 | #AVRDUDE_ERASE_COUNTER = -y
166 | 
167 | # Increase verbosity level.  Please use this when submitting bug
168 | # reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> 
169 | # to submit bug reports.
170 | #AVRDUDE_VERBOSE = -v -v
171 | 
172 | AVRDUDE_BASIC = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
173 | AVRDUDE_FLAGS = $(AVRDUDE_BASIC) $(AVRDUDE_NO_VERIFY) $(AVRDUDE_VERBOSE) $(AVRDUDE_ERASE_COUNTER) 
174 | 
175 | CC = avr-gcc
176 | OBJCOPY = avr-objcopy
177 | OBJDUMP = avr-objdump
178 | SIZE = avr-size
179 | NM = avr-nm
180 | AVRDUDE = avrdude
181 | REMOVE = rm -f
182 | MV = mv -f
183 | 
184 | # Define all object files.
185 | OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) 
186 | 
187 | # Define all listing files.
188 | LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)
189 | 
190 | # Combine all necessary flags and optional flags.
191 | # Add target processor to flags.
192 | ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS)
193 | ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
194 | 
195 | 
196 | # Default target.
197 | all: build
198 | 	@echo $(CFLAGS)
199 | 
200 | build: depend elf hex bin eep lss
201 | 
202 | package: build
203 | package.recursive: build
204 | 
205 | elf: $(TARGET).elf
206 | hex: $(TARGET).hex
207 | bin: $(TARGET).bin
208 | eep: $(TARGET).eep
209 | lss: $(TARGET).lss 
210 | sym: $(TARGET).sym
211 | 
212 | 
213 | # Program the device.  
214 | program: $(TARGET).hex $(TARGET).eep
215 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
216 | 
217 | program-fuses: 
218 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FUSES)
219 | 
220 | program-lock: 
221 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_LOCK)
222 | 
223 | read-fuse:
224 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) -U fuse:r:$(AVRDUDE_READBACK_FILE):r
225 | 	$(DUMP_AVRDUDE_READBACK_FILE)
226 | 
227 | read-lfuse:
228 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) -U lfuse:r:$(AVRDUDE_READBACK_FILE):r
229 | 	$(DUMP_AVRDUDE_READBACK_FILE)
230 | 
231 | read-hfuse:
232 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) -U hfuse:r:$(AVRDUDE_READBACK_FILE):r
233 | 	$(DUMP_AVRDUDE_READBACK_FILE)
234 | 
235 | read-efuse:
236 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) -U efuse:r:$(AVRDUDE_READBACK_FILE):r
237 | 	$(DUMP_AVRDUDE_READBACK_FILE)
238 | 
239 | read-lock:
240 | 	$(AVRDUDE) $(AVRDUDE_FLAGS) -U lock:r:$(AVRDUDE_READBACK_FILE):r
241 | 	$(DUMP_AVRDUDE_READBACK_FILE)
242 | 
243 | # Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.
244 | COFFCONVERT=$(OBJCOPY) --debugging \
245 | --change-section-address .data-0x800000 \
246 | --change-section-address .bss-0x800000 \
247 | --change-section-address .noinit-0x800000 \
248 | --change-section-address .eeprom-0x810000 
249 | 
250 | 
251 | coff: $(TARGET).elf
252 | 	$(COFFCONVERT) -O coff-avr $(TARGET).elf $(TARGET).cof
253 | 
254 | 
255 | extcoff: $(TARGET).elf
256 | 	$(COFFCONVERT) -O coff-ext-avr $(TARGET).elf $(TARGET).cof
257 | 
258 | 
259 | .SUFFIXES: .elf .hex .eep .lss .sym .cee .bin
260 | 
261 | .hex.bin:
262 | 	$(OBJCOPY) -I ihex -O binary -R .eeprom $< $@
263 | 
264 | .elf.hex:
265 | 	$(OBJCOPY) -O ihex -R .eeprom $< $@
266 | 
267 | .elf.eep:
268 | 	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
269 | 	--change-section-lma .eeprom=0 -O elf $< $@
270 | 
271 | 
272 | # Create extended listing file from ELF output file.
273 | .elf.lss:
274 | 	$(OBJDUMP) -h -S $< > $@
275 | 
276 | 
277 | # Create a symbol table from ELF output file.
278 | .elf.sym:
279 | 	$(NM) -n $< > $@
280 | 
281 | 
282 | # Link: create ELF output file from object files.
283 | ifeq ($(DCE),compile-time)
284 | $(TARGET).elf: $(SRC)
285 | 	$(CC) $(ALL_CFLAGS) $(CDCEC) $(SRC) --output $@ $(LDFLAGS)
286 | endif
287 | ifeq ($(DCE),link-time)
288 | $(TARGET).elf: $(OBJ)
289 | 	$(CC) $(ALL_CFLAGS) --output $@ $(LDFLAGS) $(LDCEL) 
290 | endif
291 | ifeq ($(DCE),)
292 | $(TARGET).elf: $(OBJ)
293 | 	$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
294 | endif
295 | 
296 | # macro Expand: run the c preprocessor
297 | .c.cee:
298 | 	$(CC) $(ALL_CFLAGS) -E $< > $@
299 | 
300 | 
301 | # Compile: create object files from C source files.
302 | ifeq ($(DCE),link-time)
303 | .c.o:
304 | 	$(CC) -c $(ALL_CFLAGS) $(CDCEL) $< -o $@ 
305 | else
306 | .c.o:
307 | 	$(CC) -c $(ALL_CFLAGS) $< -o $@ 
308 | endif
309 | 
310 | 
311 | # Compile: create assembler files from C source files.
312 | .c.s:
313 | 	$(CC) -S $(ALL_CFLAGS) $< -o $@
314 | 
315 | 
316 | # Assemble: create object files from assembler source files.
317 | .S.o:
318 | 	$(CC) -c $(ALL_ASFLAGS) $< -o $@
319 | 
320 | # Target: clean project.
321 | clean:
322 | 	$(REMOVE) \
323 | 	$(TARGET).cof $(TARGET).elf $(TARGET).map $(TARGET).sym $(TARGET).lss \
324 | 	$(OBJ) $(LST) $(SRC:.c=.s) $(SRC:.c=.d) $(SRC:.c=.cee)
325 | 
326 | realclean: clean
327 | 	$(REMOVE) $(TARGET).hex $(TARGET).bin $(TARGET).eep $(DEPENDFILE)
328 | 
329 | depend:
330 | 	$(CC) -M -mmcu=$(MCU) $(CDEFS) $(CINCS) $(SRC) $(ASRC) > $(DEPENDFILE)
331 | 
332 | .PHONY:	all build package package.recursive elf hex eep lss sym program coff extcoff clean realclean depend
333 | 
334 | #use -include so that build does not fail if dependfile is not present
335 | -include $(DEPENDFILE)
336 | 
337 | 


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.jar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/barraq/BRBrain/721ab8662cfcbf2ddcdb76e50f3e2f25d78e2045/maven/brbrain/brbrain/1.0/brbrain-1.0.jar


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.jar.md5:
--------------------------------------------------------------------------------
1 | 4084cf2b7ea5d395f9609afadb59bb06


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.jar.sha1:
--------------------------------------------------------------------------------
1 | e6055efcf6bf4370f85918da626f4357b384319b


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.pom:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <project xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd" xmlns="http://maven.apache.org/POM/4.0.0"
 3 |     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
 4 |   <modelVersion>4.0.0</modelVersion>
 5 |   <groupId>brbrain</groupId>
 6 |   <artifactId>brbrain</artifactId>
 7 |   <version>1.0</version>
 8 |   <dependencies>
 9 |     <dependency>
10 |       <groupId>org.rxtx</groupId>
11 |       <artifactId>rxtx</artifactId>
12 |       <version>2.1.7</version>
13 |       <scope>compile</scope>
14 |     </dependency>
15 |   </dependencies>
16 | </project>
17 | 


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.pom.md5:
--------------------------------------------------------------------------------
1 | 3e1ea147ba87d4e00b7ff74dd24a8e9a


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/1.0/brbrain-1.0.pom.sha1:
--------------------------------------------------------------------------------
1 | ffb439cd6121e1cbeaa3e77ac680180c947580d1


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/maven-metadata.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <metadata>
 3 |   <groupId>brbrain</groupId>
 4 |   <artifactId>brbrain</artifactId>
 5 |   <version>1.0</version>
 6 |   <versioning>
 7 |     <versions>
 8 |       <version>1.0</version>
 9 |     </versions>
10 |     <lastUpdated>20120710120056</lastUpdated>
11 |   </versioning>
12 | </metadata>
13 | 


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/maven-metadata.xml.md5:
--------------------------------------------------------------------------------
1 | 0ed32ab9aec6268f6aa20e78de399e75


--------------------------------------------------------------------------------
/maven/brbrain/brbrain/maven-metadata.xml.sha1:
--------------------------------------------------------------------------------
1 | 03ac1ac88f57e3becc105a9336d8966c289205e6


--------------------------------------------------------------------------------