hummingbirdchallenge.ino

#include <SoftwareSerial.h>
#include <Hummingbird.h>

/*
 * Dalton Miner
 * Uconn ACM
 * March 2016
*/

/*
 * Create a serial connection using pins 11 and 12.
 * Note that it's important we use these pins:
 * SoftwareSerial RX on the Leonardo is limited to pins
 * 8, 9, 10, 11, 14 (MISO), 15 (SCK), and 16 (MOSI).
 * The Hummingbird library uses 8, 9, and 10 no matter what.
 * (Despite their documentation claiming only 8 is touched.)
 * Therefore RX pin is limited to 11, 14, 15 and 16.
 */
SoftwareSerial btSerial(11, 12); //RX, TX
Hummingbird robot;
int command, motorSpeed, servoPos;

void setup()
{
  Serial.begin(9600); //serial connection to computer
  //while (!Serial); //wait for serial connection
  btSerial.begin(9600); //serial to bluetooth module or other serial device
  robot.initOnlyMotorsAndServos(); //init just the servers/motors so we can use the other pins
  delay(1000);
  Serial.print("Init done\n");
}

void loop()
{
  /*
   * Reads commands as single characters and parses both the
   * desired speed and servo angle.
   * For example sending 0x00 sets the motor to stop and the server to 0
   * Sending 0x70 is motor full power forward, 0x80 full power reverse
   * 0x0f is servo 180 (not recommended; results in high CPU usage for... reasons?)
   * 0x07 is about halfway: 84. It may be better to limit input range to 0 to 14
   * in order to give us an exact halfway point.
   * 
   * Currently this whole thing assumes we're sending motor
   * and servo commands as one char.
   * This will almost certainly have to change slightly if we opt
   * to use multiple motos/servos, but the base idea is there.
   */
  if (btSerial.available())
  {
    command = btSerial.read();
    motorSpeed = parseSpeed(command);
    servoPos = parsePos(command);
    
    //printToConsole(motorSpeed, servoPos);
    
    robot.setMotor(1, motorSpeed);
    robot.setMotor(2, motorSpeed);
    robot.setServo(1, servoPos);
  }

}

/*
 * Read the upper 4 bits of the command as the motor speed in steps.
 * We may have to use something other than map() if we want
 * more granularity in the positive output than negative.
 */
int parseSpeed(char command)
{
  char step = command >> 4; //arduino automatically sign extends. yay!
    
  return map(step, -7, 7, -255, 255);
}

/*
 * Read the lower 4 bits of the command as the servo position
 */
int parsePos(char command)
{
  char step = command & 0xf;
  
  //sign-extend.
  if (step & 0x8)
    step = step | 0xf0;

  //TODO implement a trim function instead of hacking the offset in manually
  return map(step, -7, 8, 42, 132); //note that the client app should only send values -7 to 7, and NOT 8
}

/*
 * Print to console a binary representation of value
 */
void printBinaryToConsole(char value)
{
  int i;  
  for (i = 7; i >= 0; i--)
  {
    Serial.print((value >> i) & 0x1);
  }
  Serial.print("\n");
}

/*
 * Print the calculated motor speed and servo position to console.
 */
void printToConsole(int motorSpeed, int servoPos)
{
  Serial.print("Motor: ");
  Serial.print(motorSpeed);
  Serial.print("\n");
  Serial.print("Servo: ");
  Serial.print(servoPos);
  Serial.print("\n\n");
}