Example Source Code:
Arduino Library plus Remote Control Arduino Sketch
For the WheelCommander WC-132


Description

This page shows how to use our WheelCommander support library for Arduino versions 1.0 to 1.05 (may work with other versions too).  We provide three example sketches:

  1. RCWC, which,  along with a third-party RC servo radio receiver library, to allows you to drive a robot platform with an RC radio transmitter, in either tank mode or velocity/rotation rate mode. It assumes the WheelCommander is connected to an Arduino using a serial connection, and that an RC servo radio receiver is connected to Arduino IO pin 8 (Timer 1 input capture). This example works with robots that use EITHER DC motors or servos to drive the wheels -- that configuration of the WheelCommander must be done ahead of time using other means, such as by using the WheelCommander Setup Wizard.
    NOTE: this example was developed using the RC transmitter and receiver from a VeX kit (the oldest version), and is set up so that in tank mode the left and right stick will independently control the left and right wheels when the sticks are moved fwd/rev. In velocity/rotation rate mode, the left stick controls forward/reverse platform motion while the right stick controls steering. The actual RC channels used are determined by the numeric parameters passed to ServoDecode.GetChannelPulseWidth(). 
  2. I2CWC, which shows how to interface the WheelCommander to the I2C port on an Arduino.  It only supports the hardware I2C lines on the Arduino, which are usually pins A4 = SDA and A5 = SCL.
  3. SerialWC, a very similar example, showing how similar use of the library is for either I2C or serial interfaces.  If you uncomment the #define MEGA line, you can use this example on a Mega 2560 Arduino and get both a serial terminal for debugging (using the base serial port) as well as interface to the WheelCommander using serial port 1.   You can easily change this code to use the other serial ports instead -- just change the second parameter to the InitSerial() call.

The easiest way to get started with this is to unzip the zip file below into your Arduino sketch folder, then restart the Arduino software. Alternatively, you could unzip it into the location on your system of the Arduino software, which would add WheelCommander support to the built-in libraries. However, this is no longer recommended with more recent versions of the Arduino environment that support local library folders. Placing such a library into the installation location of the Arduino software used to be the only way to add 3rd party libraries, but you had to keep re-doing this every time you upgraded to a newer version of the Arduino software. Using a local library folder saves you this trouble.

The folder also includes a 3rd party library, ServoDecode (included), for use by the RCWC example. To view the example within the Arduino environment, select File -> Examples -> WheelCommander -> RCWC.


 

Download

libraries/WheelCommander/examples/RCWC/RCWC.ino - Arduino Sketch
libraries/WheelCommander/examples/I2CWC/I2CWC.ino - Arduino Sketch
libraries/WheelCommander/examples/SerialWC/SerialWC.ino - Arduino Sketch
libraries/WheelCommander/WheelCommander.cpp - CPP source
libraries/WheelCommander/WheelCommander.h - H source
Arduino-WheelCommander.zip - Complete Library including Example Code

 

Source Code


libraries/WheelCommander/examples/RCWC/RCWC.ino


// WheelCommanderTest

#include <Wire.h>
#include <WheelCommander.h>
#include <ServoDecode.h>

//#define TANK_DRIVE 1

WheelCommander wc;

void setup()   // run once, when the sketch starts
{
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);
  delay(500);  
  digitalWrite(13, HIGH);  // LED off
  delay(500);  
  digitalWrite(13, LOW);
  delay(500);  
  digitalWrite(13, HIGH);  // LED off
  if (!wc.InitSerial(38400))
  {
     for(;;);
  }
  wc.SetWheelControl(WheelCommander::WC_BOTH);
  ServoDecode.begin();
  ServoDecode.setFailsafe(3,1234); // set channel 3 failsafe pulse  width
}

void loop()    // run over and over again
{
#ifdef TANK_DRIVE
  int l;
  int r;
  int old_l = -1000;
  int old_r = -1000;
  
  if (wc.ready)
  {
    if (ServoDecode.getState() == READY_state)
    {
      digitalWrite(13, LOW);
      l = (ServoDecode.GetChannelPulseWidth(3) - 1500) / 3;
      r = (ServoDecode.GetChannelPulseWidth(2) - 1500) / 3;
      if (r != old_r)
      {
        old_r = r;
        wc.SetWheelControl(WheelCommander::WC_RIGHT);
        wc.SetVelocity(r);
        wc.Go();
      }
      if (l != old_l)
      {
        old_l = l;
        wc.SetWheelControl(WheelCommander::WC_LEFT);
        wc.SetVelocity(l);
        wc.Go();
      }
    }
    else
    {
      digitalWrite(13, HIGH);
      if (old_l || old_r)
      {
        wc.Coast();
        old_l = old_r = 0;
      }
    }
  }
#else
  int vel;
  int rot;
  int old_vel = -1000;
  int old_rot = -1000;
  
  if (wc.ready)
  {
    if (ServoDecode.getState() == READY_state)
    {
      digitalWrite(13, LOW);
      rot = (ServoDecode.GetChannelPulseWidth(1) - 1500) / 5;
      vel = ((ServoDecode.GetChannelPulseWidth(2) - 1500) * 10) / 14;
      if ((rot != old_rot) || (vel != old_vel))
      {
        old_rot = rot;
        old_vel = vel;
        wc.SetVelocity(vel);
        wc.SetRotation(rot);
        wc.Go();
      }
    }
    else
    {
      digitalWrite(13, HIGH);    
      if (old_vel || old_rot)
      {
        wc.Coast();
        old_vel = old_rot = 0;
      }
    }
  }
#endif
  delay(50);





libraries/WheelCommander/examples/I2CWC/I2CWC.ino


// WheelCommander I2C Test

#include <Wire.h> // need to include this, as WheelCommander.h needs it (and it must be included in the Sketch -- just including it in WheelCommander.h does not work)
#include <WheelCommander.h>


WheelCommander wc;

void setup()                                                                   // run once, when the sketch starts
{
   char name[4];
   int ver;

   Serial.begin(38400);
   Serial.println("WheelCommander I2C Test");

   pinMode(13, OUTPUT);
   digitalWrite(13, LOW);
   delay(500);  
   digitalWrite(13, HIGH);                                                     // LED off
   delay(500);  
   digitalWrite(13, LOW);
   delay(500);  
   digitalWrite(13, HIGH);                                                     // LED off

   if (!wc.InitI2C(0x10))
      Serial.println("Failed to Init I2C connection with WheelCommander!");

   if (wc.Sync())
      Serial.println("Synced.");
   else
      Serial.println("Failed to sync!");

   if (wc.CommTest())
      Serial.println("CommTest passed.");
   else
      Serial.println("CommTest failed!");

   if (wc.GetName(name, ver))
   {
      Serial.print("Device name: ");
      Serial.print(name);
      Serial.print(", version: ");
      Serial.println(ver);
   }
   else
      Serial.println("Failed to get device name!");

   Serial.println("WC is initialized.");

   if (!wc.ready)
   {
      Serial.println("WC is not ready!");
      for(;;);
   }
      
   WheelCommander::EEADDR addr;
   uint8_t val;
   Serial.println("EEPROM Contents:");
   for (addr = WheelCommander::A_Mode; addr < WheelCommander::A_End; )
   {
      if (wc.GetConstant(addr, val))
      {
         Serial.print(addr);
         Serial.print(" = ");
         Serial.println((int)val);
      }
      addr = (WheelCommander::EEADDR)((int)addr + 1);
   }

   if (!wc.SetWheelControl(WheelCommander::WC_BOTH))
      Serial.println("Failed to send SetWheelControl");
   if (!wc.Coast())
      Serial.println("Failed to send Coast.");
   if (!wc.Reset())
      Serial.println("Failed to send Reset.");
   if (!wc.SetVelocity(100))
      Serial.println("Failed to send SetVelocity(100)");
   if (!wc.SetRotation(0))
      Serial.println("Failed to send SetRotation(0)");
   Serial.println("Going forward at speed = 100 for 1000.");
   delay(1000);
   if (!wc.Go())
      Serial.println("Failed to send Go!");
}

void loop()                                                                    // run over and over again
{
   long dist;

   if (wc.GetDistance(dist))
   {
      if (dist > 1000)
      {
         Serial.println(dist);
         if (!wc.Coast())
           Serial.println("Failed to coast.");
         Serial.println("Done.");
         for(;;);
      }
      else
      {
         Serial.println(dist);
      }
   }
   else
      Serial.println("Failed to get distance.");
   delay(500);





libraries/WheelCommander/examples/SerialWC/SerialWC.ino


// WheelCommander Serial Test

#include <Wire.h> // need to include this, as WheelCommander.cpp needs it (and it must be included in the Sketch -- just including it in WheelCommander.h does not work)
#include <WheelCommander.h>

//#define MEGA 1
#ifdef MEGA
#define SERIAL_PORT 1
#else
#define SERIAL_PORT 0
#endif

WheelCommander wc;

void toggle_led()
{
   static bool toggle = false;
   digitalWrite(13, toggle);
   toggle = !toggle;
}

// fast blink = fail
void fail(int code)
{
#ifdef MEGA
   Serial.print("Fatal Error ");
   Serial.println(code);
#endif
   wc.Sync();
   wc.Coast();
   for (;;)
   {
      digitalWrite(13, LOW); // turn LED off
      delay(3000);
      for (int i = 0; i < code; i++)
      {
        toggle_led();
        delay(250);
        toggle_led();
        delay(250);
      }
   }
}

void setup()                                                                   // run once, when the sketch starts
{
   char name[4];
   int ver;

#if MEGA
   Serial.begin(38400);
   Serial.println("Mega Serial WheelCommander Test");
#endif

   // slow blink = start
   pinMode(13, OUTPUT);
   toggle_led();
   delay(500);  
   toggle_led();
   delay(500);  
   toggle_led();
   delay(500);  

   if (!wc.InitSerial(38400, SERIAL_PORT))
      fail(1);

   if (!wc.Sync())
      fail(2);

   if (!wc.CommTest())
      fail(3);

   if (!wc.GetName(name, ver))
      fail(4);
#if MEGA
   else
   {
      Serial.print("Device name: ");
      Serial.print(name);
      Serial.print(", version: ");
      Serial.println(ver);
   }
#endif

   if (!wc.ready)
     fail(5);
   if (!wc.SetWheelControl(WheelCommander::WC_BOTH))
     fail(6);
   if (!wc.Coast())
      fail(7);
   if (!wc.Reset())
      fail(8);
   if (!wc.SetVelocity(100))
     fail(9);
   if (!wc.SetRotation(0))
     fail(10);
   delay(1000);
   if (!wc.Go())
     fail(11);
}

void loop()                                                                    // run over and over again
{
   long dist;

   if (wc.GetDistance(dist))
   {
      toggle_led();
      if (dist > 1000)
      {
         if (!wc.Coast())
           fail(13);
         digitalWrite(13, HIGH);                                            // LED off = done
         for (;;);
      }
   }
   else
      fail(12);
   delay(500);





libraries/WheelCommander/WheelCommander.cpp


/******************************************************************************/
/* WC-132 WheelCommander Library for Arduino 0022 - 1.0                       */
/*                                                                            */
/* Copyright 2009-2012, Noetic Design, Inc.                                   */
/*                                                                            */
/******************************************************************************/

#if defined(ARDUINO) && ARDUINO >= 100
  #include <Arduino.h>
#else
  #include <WProgram.h>
#endif
#include <HardwareSerial.h>
#include <Wire.h>
#include "WheelCommander.h"

#define DEFAULT_TIMEOUT 500

#if defined(UBRR3H)
#define NUM_SERIAL_PORTS 4
#elif defined(UBRR2H)
#define NUM_SERIAL_PORTS 3
#elif defined(UBRR1H)
#define NUM_SERIAL_PORTS 2
#elif defined(UBRRH) || defined(UBRR0H)
#define NUM_SERIAL_PORTS 1
#else
#define NUM_SERIAL_PORTS 0
#endif

WheelCommander::WheelCommander()
{
   ready = FALSE;
   pSerial = NULL;
   port = 0;
}

BOOL WheelCommander::InitSerial(long baud, int serial_port)
{
   char name[3] = "  ";
   int version;

   pSerial = NULL;
   i2c = FALSE;
   little_endian = FALSE;
   if ((serial_port < 0) || (serial_port >= NUM_SERIAL_PORTS))
   {
      return FALSE;
   }
   switch(serial_port)
   {
#if defined(UBRRH) || defined(UBRR0H)
      case 0:
         pSerial = &Serial;      // support single serial port on Uno, Duemilanove, etc.
         break;
#endif
#if defined(UBRR1H)
      case 1:
         pSerial = &Serial1;     // support additional serial ports on Arduinos that offer them, such as the Mega 2560
         break;
#endif
#if defined(UBRR2H)
      case 2:
         pSerial = &Serial2;
         break;
#endif
#if defined(UBRR3H)
      case 3:
         pSerial = &Serial3;
         break;
#endif
      default:
         return FALSE;
   }
   port = serial_port;
   pSerial->begin(baud);
   flush();
   if (!Sync())
      return FALSE;
   return ready = TRUE;
}

BOOL WheelCommander::InitI2C(unsigned int slave_address)
{
   char name[3] = "  ";
   int version;

   i2c = TRUE;
   little_endian = TRUE;
   ready = TRUE;
   slave_addr = slave_address;
   Wire.begin();
   if (!Sync())
      return FALSE;
   return TRUE;
}

/**************************************************/
/* start of device-independent transport routines */

void WheelCommander::start_read(int num)
{
   delayMicroseconds(100);
   if (i2c)
      Wire.requestFrom(slave_addr, num);
}

void WheelCommander::end_read()
{
   flush();
}

void WheelCommander::start_write(void)
{
   delayMicroseconds(100);
   if (i2c)
      Wire.beginTransmission(slave_addr);
}

void WheelCommander::end_write(void)
{
   if (i2c)
      Wire.endTransmission();
   else
   {
      pSerial->write('\n');
   }
}

int WheelCommander::available(void)
{
   if (i2c)
      return (int)Wire.available();
   else
      return pSerial->available();
}

int WheelCommander::read(void)
{
   if (i2c)
#if defined(ARDUINO) && ARDUINO >= 100
      return (int)Wire.read();
#else
      return (int)Wire.receive();
#endif
   else
   {
      int data = pSerial->read();
      return data;
   }
}

void WheelCommander::write(uint8_t val)
{
   if (i2c)
#if defined(ARDUINO) && ARDUINO >= 100
      Wire.write(val);
#else
      Wire.send(val);
#endif
   else
   {
      pSerial->write(val);
   }
}

void WheelCommander::flush(void)
{
   if (i2c)
   {
      while (Wire.available())
#if defined(ARDUINO) && ARDUINO >= 100
         Wire.read();
#else
         Wire.receive();
#endif
   }
   else
      pSerial->flush();
}

int WheelCommander::calc_num(uint8_t command_bytes, uint8_t data_bytes)
{
   if (i2c)
      return command_bytes + data_bytes;
   else
      return command_bytes + data_bytes * 2 + 1; // serial takes an extra terminator character
}

/* end of device-independent transport routines   */
/**************************************************/

/**************************************************/
/* start of helper functions                      */

BOOL WheelCommander::waitFor(int timeout, int chars)
{
   unsigned long timeToStop = (unsigned long)timeout + millis();
   while (available() < chars)
   {
      if (millis() > timeToStop)
         return FALSE;
   }
   return TRUE;
}

BOOL WheelCommander::checkEOL(void)
{
   // assume the character is already there
   int val;

   if (i2c)
      return !available(); // there should be no more pending characters

   val = read();
   if ((val == '\n') || (val == '\r') || (val == '\0'))
      return TRUE;
   else
      return FALSE;
}

BOOL WheelCommander::checkAck(int timeout)
{
   int num = i2c ? 1 : 2;
   uint8_t val;

   // Arduino Wire library can't do RESTART, so can't get ack byte correctly; have to delay to allow for turnaround time in WC firmware
   // when doing a two-phase transfer like this
   delayMicroseconds(875);
   start_read(num);
   if (waitFor(timeout, num))
   {
      if ((val = read()) == WC_ACK)
      {
         if (checkEOL())
         {
            end_read();
            return TRUE;
         }
      }
   }
   end_read();
   return FALSE;
}

uint8_t WheelCommander::getASCIIHexByte(void)
{
   char msn = (char)read() - '0';
   char lsn = (char)read() - '0';

   if (msn > 9)
      msn -= 7;
   if (lsn > 9)
      lsn -= 7;
   return (uint8_t)(((msn << 4) & 0xf0) | (lsn & 0x0f));
}

uint8_t WheelCommander::getByte()
{
   if (i2c)
      return (uint8_t)read();    // i2c is natively byte-oriented
   else
      return getASCIIHexByte();  // serial is ASCII hex formatted, so each byte received represents a nibble, most significant first
}

uint16_t WheelCommander::getWord()
{
   uint16_t val, val1, val2;

   val1 = (uint16_t)getByte();
   val2 = (uint16_t)getByte();

   if (little_endian)
   {
      val = val2;
      val <<= 8;
      val += val1;
   }
   else
   {
      val = val1;
      val <<= 8;
      val += val2;
   }
   return val;
}

uint32_t WheelCommander::getDword()
{
   uint32_t val, val1, val2;

   val1 = (uint32_t)getWord();
   val2 = (uint32_t)getWord();

   if (little_endian)
   {
      val = val2;
      val <<= 16;
      val += val1;
   }
   else
   {
      val = val1;
      val <<= 16;
      val += val2;
   }
   return val;
}

void WheelCommander::putByte(uint8_t val)
{
   char msn; 
   char lsn;

   if (i2c)
   {
      write(val); // i2c is natively byte-oriented
      return;
   }

   msn = (val >> 4) & 0x0f; // serial is nibble / ASCII hex oriented
   lsn = val & 0x0f;
   if (msn > 9)
      msn += 7;
   msn += '0';
   if (lsn > 9)
      lsn += 7;
   lsn += '0';

   write(msn);
   write(lsn);
}

void WheelCommander::putWord(uint16_t val)
{
   uint8_t val1, val2;

   val1 = (uint8_t)(val >> 8);
   val2 = (uint8_t)(val & 0x0ff);

   if (little_endian)
   {
      putByte(val2);
      putByte(val1);
   }
   else
   {
      putByte(val1);
      putByte(val2);
   }
}

void WheelCommander::putDword(uint32_t val)
{
   uint16_t val1, val2;

   val1 = (uint16_t)(val >> 16);
   val2 = (uint16_t)(val & 0x0ffff);
   if (little_endian)
   {
      putWord(val2);
      putWord(val1);
   }
   else
   {
      putWord(val1);
      putWord(val2);
   }
}

BOOL WheelCommander::doWordCommand(char command, uint16_t &val, int timeout)
{
   int num = calc_num(1, 2);
   char cmd;

   start_write();
   write(command);
   end_write();

   start_read(num);
   if (!waitFor(timeout, num))
   {
      end_read();
      return FALSE;
   }
   if ((cmd = read()) != command)
   {
      end_read();
      return FALSE;
   }
   val = getWord();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   return TRUE;
}


BOOL WheelCommander::doDwordCommand(char command, uint32_t &val, int timeout)
{
   int num = calc_num(1, 4);
   char cmd;

   start_write();
   write(command);
   end_write();

   start_read(num);
   if (!waitFor(timeout, num))
   {
      end_read();
      return FALSE;
   }
   if ((cmd = read()) != command)
   {
      end_read();
      return FALSE;
   }
   val = getDword();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   return TRUE;
}

/* end of helper functions                        */
/**************************************************/

/**************************************************/
/* start of command handlers                      */

BOOL WheelCommander::Sync()
{
   for (int i = 0; i < 10; i++)
   {
      start_write();
      write(WC_SYNC);
      end_write();

      start_read(1);
      if (waitFor(50 * (i + 1), 1))
      {
         int val = read();
         end_read();
         if (val == WC_SYNC)
         {
            delay(20);
            flush();
            return TRUE;
         }
      }
      end_read();
   }
   return FALSE;
}

BOOL WheelCommander::GetName(char *name, int &version, int timeout)
{
   int cmd;
   int num = calc_num(5, 0);

   start_write();
   write(WC_NAME_CMD);
   end_write();

   start_read(num);
   if (!waitFor(timeout, num))
   {
      end_read();
      return FALSE;
   }
   if ((cmd = read()) != WC_NAME_CMD)
   {
      end_read();
      return FALSE;
   }
   if (name)
   {
      name[0] = read();
      name[1] = read();
      name[2] = '\0';
   }
   version = (int)getASCIIHexByte();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   return TRUE;
}

BOOL WheelCommander::Reset()
{
   start_write();
   write(WC_RESET_CMD);
   end_write();
   return checkAck();
}

BOOL WheelCommander::CommTest(uint8_t first, uint8_t last, uint8_t step, int timeout)
{
   int num = calc_num(1, 1);
   uint8_t echo;
   uint8_t swapped_echo;

   for (int val = first; val <= last; val += step)
   {
      start_write();
      write(WC_ECHO_CMD);
      putByte(val);
      end_write();

      start_read(num);
      if (!waitFor(timeout, num))
      {
         end_read();
         return FALSE;
      }
      if (read() != WC_ECHO_CMD)
      {
         end_read();
         return FALSE;
      }
      echo = getByte();
      if (!checkEOL())
      {
         end_read();
         return FALSE;
      }
      end_read();
      swapped_echo = ((val & 0x0f) << 4) | ((val >> 4) & 0x0f);
      if (swapped_echo != echo)
         return FALSE;
   }
   return TRUE;
}

BOOL WheelCommander::GetStatus(STAT &stat)
{
   return doWordCommand(WC_STAT_CMD, (uint16_t &)stat);
}

BOOL WheelCommander::SetMode(WC_MODE mode)
{
   return SetConstant(A_Mode, mode.data);
}

BOOL WheelCommander::SetBaud(WC_BAUD baud)
{
   long long_baud;

   if (SetConstant(A_Baud, (uint8_t)baud))
   {
      // convert single byte baud number to bits per second
      switch(baud)
      {
      case BD_1200:
         long_baud = 1200;
         break;
      case BD_4800:
         long_baud = 4800;
         break;
      case BD_9600:
         long_baud = 9600;
         break;
      case BD_19200:
         long_baud = 19200;
         break;
      case BD_38400:
         long_baud = 38400;
         break;
      case BD_57600:
         long_baud = 57600;
         break;
      case BD_115200:
         long_baud = 115200;
         break;
      case BD_230400:
         long_baud = 230400;
         break;
      case BD_460800:
         long_baud = 460800;
         break;
      default:
         return FALSE;
      }
      if (!i2c)
         pSerial->begin(long_baud); // change serial baud to match
      return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::SetI2C7BitAddr(uint8_t i2c_7bit_address)
{
   start_write();
   write(WC_I2C_ADDR_CMD);
   write(i2c_7bit_address * 2);
   end_write();
   return checkAck();
}

BOOL WheelCommander::GetI2C7BitAddr(uint8_t &i7bitadr)
{
   uint8_t i8bitadr;
   if (GetConstant(A_I2CAddr, i8bitadr))
   {
      i7bitadr = i8bitadr / 2;
      return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::SetPolarity(BOOL fwd)
{
   WC_MODE mode;
   if (GetConstant(A_Mode, mode.data))
   {
      mode.flags.polarity = fwd;
      return SetConstant(A_Mode, mode.data);
   }
   return FALSE;
}

BOOL WheelCommander::GetTrapezoidalMode(BOOL &trap)
{
   uint8_t val;
   if (GetConstant(A_ModeExt, val))
   {
      trap = (BOOL)((val & 2) != 0);
      return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::SetTrapezoidalMode(BOOL trap)
{
   uint8_t val;

   if (GetConstant(A_ModeExt, val))
   {
      if (trap)
         val |= 2;
      else
         val &= 0xfd;
      if (SetConstant(A_ModeExt, val))
         return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::SetEncoderProperties(BOOL quadrature, uint16_t ticks_per_rotation)
{
   uint8_t val;

   if (GetConstant(A_ModeExt, val))
   {
      if (quadrature)
         val |= 1;
      else
         val &= 0xfe;
      if (SetConstant(A_ModeExt, val))
         return SetConstant(A_tpr_lo, (int16_t &)ticks_per_rotation);
   }
   return FALSE;
}

BOOL WheelCommander::GetEncoderProperties(BOOL &quadrature, uint16_t &ticks_per_rotation)
{
   uint8_t val = quadrature;

   if (GetConstant(A_ModeExt, val))
   {
      quadrature = (BOOL)(val & 1);
      if (GetConstant(A_tpr_lo, (int16_t &)ticks_per_rotation))
         return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::GetLogBytes(int log_num, uint8_t *bytes, int num)
{
}

BOOL WheelCommander::Go()
{
   start_write();
   write(WC_GO_CMD);
   end_write();
   return checkAck();
}

BOOL WheelCommander::Coast()
{
   start_write();
   write(WC_COAST_CMD);
   end_write();
   return checkAck();
}

BOOL WheelCommander::Brake()
{
   start_write();
   write(WC_BRAKE_CMD);
   end_write();
   return checkAck();
}

BOOL WheelCommander::GetDistance(long &dist, int timeout)
{
   return doDwordCommand(WC_DST_CMD, (uint32_t &)dist, timeout);
}

BOOL WheelCommander::GetXDistance(long &dist, int timeout)
{
   return doDwordCommand(WC_XDST_CMD, (uint32_t &)dist, timeout);
}

BOOL WheelCommander::GetAngle(int &ang, int timeout)
{
   return doWordCommand(WC_ANG_CMD, (uint16_t &)ang, timeout);
}

BOOL WheelCommander::GetXAngle(int &ang, int timeout)
{
   return doWordCommand(WC_XANG_CMD, (uint16_t &)ang, timeout);
}

BOOL WheelCommander::GetVelocity(int &vel, int timeout)
{
   return doWordCommand(WC_VEL_CMD, (uint16_t &)vel, timeout);
}

BOOL WheelCommander::GetRotation(int &rot, int timeout)
{
   return doWordCommand(WC_ROT_CMD, (uint16_t &)rot, timeout);
}

BOOL WheelCommander::GetAcceleration(int &accel, int timeout)
{
   return doWordCommand(WC_ACCEL_CMD, (uint16_t &)accel, timeout);
}

BOOL WheelCommander::SetDistance(long int dist, int timeout)
{
   start_write();
   write(WC_DST_CMD);
   putDword(dist);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetXDistance(long int dist, int timeout)
{
   start_write();
   write(WC_XDST_CMD);
   putDword(dist);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetAngle(int ang, int timeout)
{
   start_write();
   write(WC_ANG_CMD);
   putWord(ang);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetXAngle(int ang, int timeout)
{
   start_write();
   write(WC_XANG_CMD);
   putWord(ang);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetVelocity(int vel, int timeout)
{
   start_write();
   write(WC_VEL_CMD);
   putWord(vel);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetRotation(int rot, int timeout)
{
   start_write();
   write(WC_ROT_CMD);
   putWord(rot);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::SetAcceleration(int accel, int timeout)
{
   start_write();
   write(WC_ACCEL_CMD);
   putWord(accel);
   end_write();
   return checkAck(timeout);
}

BOOL WheelCommander::Calibrate()
{
   start_write();
   write(WC_CALIBRATE_CMD);
   end_write();
   return checkAck();
}

BOOL WheelCommander::Query(CALERROR &error)
{
}

BOOL WheelCommander::SetInterrupt(INT_FLAGS flags)
{
}

BOOL WheelCommander::GetInterruptStatus(INT_FLAGS &flags, uint16_t &io_lines)
{
}

BOOL WheelCommander::SetDIOMonitorInfo(int bit, IO_FLAGS flags)
{
}

BOOL WheelCommander::SetAIOMonitorInfo(int bit, IO_FLAGS flags, uint16_t lo, uint16_t hi)
{
}

BOOL WheelCommander::ReadDIO(int bit, BOOL &value)
{
   uint16_t val;
   int num = calc_num(1, 2);

   start_write();
   write(WC_DIGITAL_IO_CMD);
   putByte(bit);
   end_write();

   start_read(num);
   if (!waitFor(DEFAULT_TIMEOUT, num))
   {
      end_read();
      return FALSE;
   }
   if (read() != WC_DIGITAL_IO_CMD)
   {
      end_read();
      return FALSE;
   }
   val = getWord();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   value = val & 0x0ff;
   if (bit != ((val >> 8) & 0x0ff))
      return FALSE;
   return TRUE;
}

BOOL WheelCommander::WriteDIO(int bit, BOOL value)
{
   uint16_t val = value & 0x01 | ((bit & 0x0ff) << 8);

   start_write();
   write(WC_DIGITAL_IO_CMD);
   putWord(val);
   end_write();
   return checkAck();
}

BOOL WheelCommander::ReadAIO(int bit, uint16_t &value)
{
   int bit_read; 
   int num = calc_num(1, 3);

   start_write();
   write(WC_ANALOG_IO_CMD);
   putByte(bit);
   end_write();

   if (!waitFor(DEFAULT_TIMEOUT, num))
   {
      end_read();
      return FALSE;
   }
   if (read() != WC_ANALOG_IO_CMD)
   {
      end_read();
      return FALSE;
   }
   bit_read = getByte();
   value = getWord();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   if (bit != bit_read)
      return FALSE;
   return TRUE;
}

BOOL WheelCommander::ReadDigitalSupply(uint16_t &voltage)
{
   uint16_t ad_val;
   if (ReadAIO(6, ad_val))
   {
      voltage = (ad_val / 5); // approximately voltage * 10
      return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::ReadServoSupply(uint16_t &voltage)
{
   uint16_t ad_val;
   if (ReadAIO(7, ad_val))
   {
      voltage = (ad_val / 2); // approximately voltage * 10
      return TRUE;
   }
   return FALSE;
}

BOOL WheelCommander::SetConstant(EEADDR addr, uint8_t const_value)
{
   start_write();
   write(WC_CONSTS_CMD);
   putByte(addr);
   putByte(const_value);
   end_write();

   if (addr != A_Baud)
      return checkAck(DEFAULT_TIMEOUT);
   else
      return TRUE;
}

BOOL WheelCommander::GetConstant(EEADDR addr, uint8_t &const_value)
{
   uint8_t raddr;
   uint8_t rconst;
   int num = calc_num(1, 2);

   start_write();
   write(WC_CONSTS_CMD);
   putByte(addr);
   end_write();

   start_read(num);
   if (!waitFor(DEFAULT_TIMEOUT, num))
   {
      end_read();
      return FALSE;
   }
   if (read() != WC_CONSTS_CMD)
   {
      end_read();
      return FALSE;
   }
   raddr = getByte();
   rconst = getByte();
   if (!checkEOL())
   {
      end_read();
      return FALSE;
   }
   end_read();
   const_value = rconst;
   if (addr != raddr)
      return FALSE;
   return TRUE;
}

BOOL WheelCommander::SetConstant(EEADDR addr, int16_t const_value)
{
   uint8_t temp = const_value & 0x0ff;
   if (SetConstant(addr, temp))
   {
      temp = (const_value >> 8) & 0x0ff;
      return SetConstant((EEADDR)((int)addr + 1), temp);
   }
   return FALSE;
}

BOOL WheelCommander::GetConstant(EEADDR addr, int16_t &const_value)
{
   uint8_t temp;
   if (GetConstant(addr, temp))
   {
      const_value = temp;
      if (GetConstant(((EEADDR)((int)addr + 1)), temp))
      {
         const_value |= ((int16_t)temp) << 8;
         return TRUE;
      }
   }
   return FALSE;
}

BOOL WheelCommander::SetWheelControl(WC_CTRL mode)
{
   start_write();
   write(WC_WHEEL_CONTROL_CMD);
   write(mode);
   end_write();
   return checkAck();
}

BOOL WheelCommander::GetOdometry(WC_CTRL odo_ch, long &odo, int timeout)
{
   int num = calc_num(1, 4);

   start_write();
   write(WC_ODO_CMD);
   write(odo_ch);
   end_write();

   start_read(num);
   if (!waitFor(timeout, num))
   {
      end_read();
      return FALSE;
   }
   if (read() != WC_ODO_CMD)
   {
      end_read();
      return FALSE;
   }
   odo = getDword();
   return checkEOL();
}

/* end of command handlers                        */
/**************************************************/



libraries/WheelCommander/WheelCommander.h


////////////////////////////////////////////////////////////////////////////////////////////////////
/// \file WheelCommander.h
///
/// \brief Declares the WheelCommander class
////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef _WHEEL_COMMANDER_H_
#define _WHEEL_COMMANDER_H_

#if defined(ARDUINO) && ARDUINO >= 100
  #include <Arduino.h>
#else
  #include <WProgram.h>
#endif
#include <HardwareSerial.h>
#include <inttypes.h>

#define TRUE 1
#define FALSE 0
typedef unsigned char BOOL;

#define WC_SYNC '.'
#define WC_ACCEL_CMD 'A'
#define WC_ANG_CMD 'W'
#define WC_VEL_CMD 'V'
#define WC_ROT_CMD 'Y'
#define WC_DST_CMD 'D'
#define WC_STAT_CMD 'S'
#define WC_ECHO_CMD 'E'
#define WC_BRAKE_CMD 'B'
#define WC_COAST_CMD 'C'
#define WC_RESET_CMD 'R'
#define WC_NAME_CMD 'N'
#define WC_I2C_ADDR_CMD 'T'
#define WC_GO_CMD 'G'
#define WC_CONSTS_CMD 'F'
#define WC_UPDATE_CMD 'U'
#define WC_DIGITAL_IO_CMD 'J'
#define WC_ANALOG_IO_CMD 'H'
#define WC_WHEEL_CONTROL_CMD 'L'
#define WC_CALIBRATE_CMD 'K'
#define WC_QUERY_CMD 'Q'
#define WC_INT_CMD 'I'
#define WC_ODO_CMD 'O'
#define WC_LOG_CMD 'M'
#define WC_XANG_CMD 'Z'
#define WC_XDST_CMD 'X'

#define WC_ACK 'a'
#define WC_NACK 'n'

#define WC_SLOW_COMMS 0x01  // -- user settable mode -- access via bytes[1]
#define WC_NOTIFY 0x02
#define WC_SHORT_MODE 0x04
#define WC_POLARITY 0x08
#define WC_PWM 0x10
#define WC_LOCKED_ANTIPHASE 0x20
#define WC_DUAL_DIR_LINES 0x40 // -- h-bridge needs two different direction lines per motor; they must be inverted with respect to eachother
#define WC_COUNTER_AXIS 0x80 // axes are facing opposite directions

#define WC_MOTOR_CONTROL (WC_PWM | WC_LOCKED_ANTIPHASE | WC_DUAL_DIR_LINES)

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \class  WheelCommander
///
/// \author Pete Skeggs
/// \date 5/6/2009
///
/// \brief 
////////////////////////////////////////////////////////////////////////////////////////////////////

class WheelCommander
{
public:

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Constructor. 
///
////////////////////////////////////////////////////////////////////////////////////////////////////

WheelCommander();


   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Initializes the WheelCommander to use the serial port. 
   /// 
   /// \param baud - the number of bits per second the WheelCommander is already configured for.
   /// \param  serial_port - 0 for most Arduino boards; Mega can take 0-3.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL InitSerial(long baud, int serial_port = 0);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Initializes the WheelCommander to use I2C using the specified 7 bit slave address.
   ///
   /// \param slave_address - 7 bit I2C address of WheelCommander board.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL InitI2C(unsigned int slave_address);


#pragma pack(1)

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \struct wc_stat  
/// \brief this defines the bitfields used to report the status of the WC-132
////////////////////////////////////////////////////////////////////////////////////////////////////

typedef struct wc_stat {
   uint16_t ang_mode : 1;      // 0
   uint16_t vel_mode : 1;      // 1
   uint16_t dst_mode : 1;      // 2
   uint16_t rot_mode : 1;      // 3

   uint16_t dst_done : 1;      // 4
   uint16_t running : 1;       // 5
   uint16_t i2c_mode : 1;      // 6
   uint16_t rs232_mode : 1;    // 7

   uint16_t motor_enable : 1;  // 8
   uint16_t cmd_overflow : 1;  // 9
   uint16_t port_overflow : 1; // 10
   uint16_t delayed_coast : 1; // 11

   uint16_t ldir : 1;          // 12
   uint16_t interrupt_asserted : 1;// 13
   uint16_t rdir : 1;          // 14
   uint16_t trapezoidal_mode : 1;// 15
} STAT;

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief this defines a union of a single byte, data, and its corresponding bit flags, used to
/// control the operating mode of the WC-132
////////////////////////////////////////////////////////////////////////////////////////////////////

typedef union _sm_mode_union
{
struct _sm_mode {
uint8_t slow_comms : 1;
uint8_t notify : 1;
uint8_t short_mode : 1;
uint8_t polarity : 1;
uint8_t pwm : 1;
uint8_t locked_antiphase : 1;
uint8_t dual_dir_lines : 1;
uint8_t counter_axis : 1;
} flags;
uint8_t data;
} WC_MODE;

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief this defines a union of a single byte, data, and its corresponding bit flags, used to
/// control the behavior of a single digital or analog I/O line 
////////////////////////////////////////////////////////////////////////////////////////////////////

typedef union _io_flags_union
{
   struct _io_flags
   {
      uint8_t level : 1;         // if digital, this is the level it should be set to
      uint8_t digital : 1;       // 1 = digital I/O; 0 = analog
      uint8_t input : 1;         // 1 = input line; 0 = output (digital only)
      uint8_t monitor : 1;       // 1 = watch for change; cause interrupt on change
      uint8_t watch_for_hi : 1;  // 1 = interrupt when hi (if digital) or above hi_limit (if analog)
      uint8_t watch_for_lo : 1;  // 1 = interrupt when lo (if digital) or above lo_limit (if analog)
      uint8_t edge : 1;          // 1 = edge-triggered
      uint8_t set : 1;           // 1 = signaled
   } flags;
   uint8_t data;
} IO_FLAGS;

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief this defines a union of a single byte, data, and its corresponding bit flags, used to
/// define the pin number and behavior of an optional host-attention interrupt output; this is useful
/// in I2C mode to make the host aware that something has happened, like the change of an I/O line
/// or the completion of a motion request
////////////////////////////////////////////////////////////////////////////////////////////////////

typedef union _int_union_
{
   struct _int_flags
   {
      uint8_t pin : 4;
      uint8_t int_active : 1;
      uint8_t dst_done_enabled : 1;
      uint8_t active_high : 1;
      uint8_t int_enabled : 1;      // interrupts are enabled
   } flags;
   uint8_t data;
} INT_FLAGS;
#pragma pack()

////////////////////////////////////////////////////////////////////////////////////////////////////
/// \enum _wc_baud
/// \brief this defines the symbolic names for possible serial baud rates
////////////////////////////////////////////////////////////////////////////////////////////////////

typedef enum _wc_baud
{
   BD_1200, BD_4800, BD_9600, BD_19200, BD_38400, BD_57600, BD_115200, BD_230400, BD_460800
} WC_BAUD;

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the status of the WC-132
   ///
   /// \param stat  - [in/out] returns the status of the WC-132
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetStatus(STAT &stat);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Calibrates the WC-132
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Calibrate();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \enum _eeprom_addrs
   ///
   /// \brief this defines the symbolic names for nonvolatile constants that can be stored and retrieved,
/// which affect the behavior of the WC-132
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   typedef enum _eeprom_addrs 
   {
         A_skip, A_Written, A_Mode, A_Baud, A_I2CAddr, 
         A_KpV, A_KiV, A_KdV, A_KfV, unused, 
         A_KpP, A_KiP, A_KdP,
         A_KDpP, A_KDiP, A_KDpV, A_KDiV, 
         A_KdH, A_KpH, A_KpvTh, A_KvTh, 
         A_Kwb_lo, A_Kwb_hi, A_Kwc_lo, A_Kwc_hi,  A_Klz_lo, A_Klz_hi, A_Krz_lo, A_Krz_hi, 
         A_Vmx_lo, A_Vmx_hi,  A_AxSgn,
         A_lm_lo,  A_lm_hi,   A_lb_lo,  A_lb_hi,
         A_pwm,    A_ModeExt, A_tpr_lo, A_tpr_hi, A_KDdP, A_KDdV,  A_KfP, 
         A_Kwc_diff_lo, A_Kwc_diff_hi,
         A_rm_lo,  A_rm_hi,   A_rb_lo,  A_rb_hi, A_KoB,
         A_End
   } EEADDR;

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \enum _cal_error
   ///
   /// \brief this defines the symbolic names for calibration status, including both normal and error
/// conditions
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   typedef enum _cal_error {
//  0       1     2          3        4          5                6                  
    E_NONE, E_OK, E_RUNNING, E_ZEROS, E_SWAPPED, E_LEFT_MAXSPEED, E_RIGHT_MAXSPEED,
//  7            8            9           10            11
    E_HALFSPEED, E_FULLSPEED, E_BADORDER, E_BADRESULTS, E_NO_CAL, E_MAX_CAL
   } CALERROR;

   #define INVERT_LEFT 0x02
   #define INVERT_RIGHT 0x01

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the operating mode of the WC-132
   ///
   /// \param mode  - the mode structure to use
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetMode(WC_MODE mode);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the WC-132 baud rate; will take effect immediately.
   ///
   /// \param baud  - The symbolic baud value to set.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetBaud(WC_BAUD baud);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the WC-132's 7 bit I2C slave address; will take effect immediately.
   ///
   /// \param i2c_7bit_address  - the address to set
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetI2C7BitAddr(uint8_t i2c_7bit_address);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the WC-132's current I2C slave address.
   ///
   /// \param i7bitadr  - [in/out] returns the current address
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetI2C7BitAddr(uint8_t &i7bitadr);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the polarity, or forward vs. reverse direction, of your robot platform.
   ///
   /// \param fwd  - true means the default motor polarity is correct; false to invert them.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetPolarity(BOOL fwd);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the current state of the trapezoidal position control mode
   ///
   /// \param trap  - [in/out] returns the current state
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetTrapezoidalMode(BOOL &trap);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets position control to use a trapezoidal motion profile (acceleration and deceleration
/// at the ends of a movement)
   ///
   /// \param trap  - true to use trapezoidal control; false for host-driven profiles
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetTrapezoidalMode(BOOL trap);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the WC-132's encoder's properties.
   ///
   /// \param quadrature          - true for a quadrature encoder (ChA, ChB); false for sign/magnitude
/// (CLK/DIR)
   /// \param ticks_per_rotation  - the number of encoder ticks corresponding to one turn of a wheel
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetEncoderProperties(BOOL quadrature, uint16_t ticks_per_rotation);    // default is FALSE, 128

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the encoder properties
   ///
   /// \param quadrature          - [in/out] true if set for quadrature; false for sign/magnitude
   /// \param ticks_per_rotation  - [in/out] returns the number of ticks per wheel rotation
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetEncoderProperties(BOOL &quadrature, uint16_t &ticks_per_rotation);    // default is FALSE, 128

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the host attention control interrupt parameters
   ///
   /// \param flags - the interrupt control bits
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetInterrupt(INT_FLAGS flags);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the reason for the host interrupt
   ///
   /// \param flags  - [in/out] the interrupt flags
   /// \param io_lines  - [in/out] the I/O line that caused the interrupt
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetInterruptStatus(INT_FLAGS &flags, uint16_t &io_lines);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets monitor information for a digital I/O line
   ///
   /// \param bit  - which digital I/O line (0 to 7) to modify
   /// \param flags - the I/O control flags to use
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetDIOMonitorInfo(int bit, IO_FLAGS flags);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets monitor information for an analog I/O line
   ///
   /// \param bit  - the analog I/O line (0 to 7) to modify
   /// \param flags - the I/O control flags to use
   /// \param lo  - the lower threshold
   /// \param hi  - the higher threshold
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetAIOMonitorInfo(int bit, IO_FLAGS flags, uint16_t lo, uint16_t hi);      // set monitor info for this analog input line

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Reads the state of a digital I/O line
   ///
   /// \param bit  - the digital line to query
   /// \param value - [in/out] the current logic level of the queried line
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL ReadDIO(int bit, BOOL &value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Writes a digital output to a specific value
   ///
   /// \param bit  - the digital line to change
   /// \param value - the logic level to change it to
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL WriteDIO(int bit, BOOL value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Reads the current analog value of an specific pin
   ///
   /// \param bit  - the analog line to read
   /// \param value - [in/out] the value (ranging from 0 to 1023)
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL ReadAIO(int bit, uint16_t &value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Reads the digital supply's voltage, in 10ths of volts
   ///
   /// \param voltage  - [in/out] the voltage
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL ReadDigitalSupply(uint16_t &voltage);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Reads the motor or servo supply voltage, in 10ths of volts
   ///
   /// \param voltage  - [in/out] the voltage
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL ReadServoSupply(uint16_t &voltage);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets a byte constant to a new value
   ///
   /// \param addr          - the address of the constant to modify
   /// \param const_value  - the new value
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetConstant(EEADDR addr, uint8_t const_value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets a constant value
   ///
   /// \param addr          - the address of the constant to query
   /// \param const_value  - [in/out] the constant's value
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetConstant(EEADDR addr, uint8_t &const_value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets a word constant to a new value 
   ///
   /// \param laddr      - the address of the constant to modify
   /// \param const_value  - the new value
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetConstant(EEADDR laddr, int16_t const_value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets a word constant's value
   ///
   /// \param laddr      - the address of the constant to query
   /// \param const_value  - [in/out] the constant's value
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetConstant(EEADDR laddr, int16_t &const_value);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Queries the status of a calibration run
   ///
   /// \param error - [in/out] the error or E_OK if completed correctly
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Query(CALERROR &error);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \enum _wheel_control
   ///
   /// \brief this defines values for changing how the WC-132 controls the two wheels (together or
/// independently)
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   typedef enum _wheel_control
   {
      WC_LEFT = '0', WC_RIGHT = '1', WC_BOTH = '2'
   } WC_CTRL;

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the WC-132's fundamental control mode for wheels (coordinated with steering control,
/// or independent)
   ///
   /// \param mode  - the wheel mode to set
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetWheelControl(WC_CTRL mode);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the current raw encoder tick count for the specified wheel
   ///
   /// \param odo_ch  - the wheel to query
   /// \param odo     - [in/out] the tick count
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetOdometry(WC_CTRL odo_ch, long &odo, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets a log bytes. 
   ///
   /// \param log_num  - a log number 
   /// \param bytes  - a pointer to a buffer to retrieve the log to
   /// \param num      - number of bytes to query
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetLogBytes(int log_num, uint8_t *bytes, int num);


   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Start a motion control operation -- begins the recently specified velocity, rotation rate,
/// distance, or angle values 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Go();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Release control of motors and terminate the motion control.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Coast();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Commands the motors to use electric feedback to stop more quickly than possible by
/// coasting.  Also terminates motion control.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Brake();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the currently specified absolute distance (if not running) or actual distance
/// (if running), from the origin in the robot's frame of reference; this can be reset to 0 by means
/// of the Reset() command (which resets the motion frame, not the microcontroller).
   ///
   /// \param dist      - [in/out] returns the current distance in the same units of measurement 
/// used to set the wheel circumference (A_Kwc_lo and A_Kwc_lo) and wheel base (A_Kwb_lo and A_Kwb_hi)
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetDistance(long &dist, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the relative distance moved; currently actually returns the same value as
/// GetDistance().
   ///
   /// \param dist      - [in/out] returns the current distance 
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetXDistance(long &dist, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the current absolute angle of the platform in the robot's frame of reference.
   ///
   /// \param ang      - [in/out] the angle
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetAngle(int &ang, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets relative angle turned; currently actually returns the same value as GetAngle(). 
   ///
   /// \param ang      - [in/out] the angle
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetXAngle(int &ang, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the measured velocity of the platform.
   ///
   /// \param vel      - [in/out] the velocity in distance units per second
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetVelocity(int &vel, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the measured rotation rate of the platform.
   ///
   /// \param rot      - [in/out] rotation rate in angle units per second
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetRotation(int &rot, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the most recently specified acceleration value to use for trapezoidal motion
/// control.  This does not return the actual measured acceleration as implied by its name.
   ///
   /// \param accel  - [in/out] the acceleration in distance units per second squared
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetAcceleration(int &accel, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the absolute distance to move (in the robot's frame of reference)
   ///
   /// \param dist      - [in/out] the distance in the same units of measurement used to set the wheel
/// circumference (A_Kwc_lo and A_Kwc_lo) and wheel base (A_Kwb_lo and A_Kwb_hi)
/// 
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetDistance(long int dist, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the relative distance to move (distance from current position)
   ///
   /// \param dist      - [in/out] the change in distance
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetXDistance(long int dist, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the absolute angle to rotate to (in the robot's frame of reference)
   ///
   /// \param ang      - [in/out] the angle in degrees
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetAngle(int ang, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the relative angle to rotate to (angle from the current pose)
   ///
   /// \param ang      - [in/out] the change in angle in degrees
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetXAngle(int ang, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the velocity; positive is forward, negative is reverse
   ///
   /// \param vel      - [in/out] the velocity in distance units per second
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetVelocity(int vel, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets a rotation rate; positive is clock-wise, negative is counter-clock-wise
   ///
   /// \param rot      - [in/out] the rotation rate in degrees per second
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetRotation(int rot, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Sets the acceleration to use for position control in trapezoidal control mode
   ///
   /// \param accel  - [in/out] the acceleration in distance units per second squared
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL SetAcceleration(int accel, int timeout = 5000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Gets the name of the device.  Used to distinguish between various Nubotics products.
   ///
   /// \param name      - If non-null, ponts to a buffer in which the name will be stored.
   /// \param version  - [in/out] the current firmware version.
   /// \param timeout  - A timeout in milliseconds. 
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL GetName(char *name, int &version, int timeout = 10000);

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Reset motion control parameters; stops any active movement, and resets the current values
/// for acceleration, velocity, rotation rate, position, and angle.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Reset();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Synchronises communication with the Unicoder.
   ///
   /// \retval true if it succeeds, false if it fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL Sync();

   ////////////////////////////////////////////////////////////////////////////////////////////////////
   /// \brief Tests communication with the WheelCommander. 
   ///
   /// \param first - The first byte value to test using the Echo command. 
   /// \param last  - The last byte value to test. 
   /// \param step  - Amount to increment by starting at first and ending at last. 
   ///
   /// \retval true if the test passes, false if the test fails. 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

   BOOL CommTest(uint8_t first = 0, uint8_t last = 255, uint8_t step = 1, int timeout = 1000);

   static const char *GetCalError(int error);

   WC_MODE curmode;
   BOOL ready;
   BOOL i2c;
   HardwareSerial *pSerial;
   int port;
   int slave_addr;
   int little_endian; // i2c buffers are little-endian, serial are big-endian

   private:
      int available(void);
      void start_read(int num);
      void end_read();
      void start_write(void);
      void end_write(void);
      int calc_num(uint8_t command_bytes, uint8_t data_bytes);
      int read(void);
      void flush(void);
      void write(uint8_t val);
      BOOL checkAck(int timeout = 5000);
      BOOL waitFor(int timeout, int chars);
      BOOL checkEOL(void);
      uint8_t getASCIIHexByte(void);
      uint8_t getByte(void);
      uint16_t getWord(void);
      uint32_t getDword(void);
      void putByte(uint8_t val);
      void putWord(uint16_t val);
      void putDword(uint32_t val);
      BOOL doWordCommand(char command, uint16_t &val, int timeout = 5000);
      BOOL doDwordCommand(char command, uint32_t &val, int timeout = 5000);
};

#endif


 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.