moved files to 'balboa' directory

BalancerRPi @ 2c1ed22f

-Subproject commit 2c1ed22f37ee719c4a4c2a294c87ef9b0f17e700

balboa/BalancerRPi/.gitignore

+BalancerRPi.ino.with_bootloader.arduino_leonardo.hex

balboa/BalancerRPi/Balance.cpp

+#include <Wire.h>
+#include "Balance.h"
+
+int32_t gYZero;
+int32_t angle; // millidegrees
+int32_t angleRate; // degrees/s
+int32_t distanceLeft;
+int32_t speedLeft;
+int32_t driveLeft;
+int32_t distanceRight;
+int32_t speedRight;
+int32_t driveRight;
+int16_t motorSpeed;
+bool isBalancingStatus;
+bool balanceUpdateDelayedStatus;
+
+bool isBalancing()
+{
+  return isBalancingStatus;
+}
+
+bool balanceUpdateDelayed()
+{
+  return balanceUpdateDelayedStatus;
+}
+
+void balanceSetup()
+{
+  // Initialize IMU.
+  Wire.begin();
+  if (!imu.init())
+  {
+    while(true)
+    {
+      Serial.println("Failed to detect and initialize IMU!");
+      delay(200);
+    }
+  }
+  imu.enableDefault();
+  imu.writeReg(LSM6::CTRL2_G, 0b01011000); // 208 Hz, 1000 deg/s
+
+  // Wait for IMU readings to stabilize.
+  delay(1000);
+
+  // Calibrate the gyro.
+  int32_t total = 0;
+  for (int i = 0; i < CALIBRATION_ITERATIONS; i++)
+  {
+    imu.read();
+    total += imu.g.y;
+    delay(1);
+  }
+
+  gYZero = total / CALIBRATION_ITERATIONS;
+}
+
+// This function contains the core algorithm for balancing a
+// Balboa 32U4 robot.
+void balance()
+{
+  // Adjust toward angle=0 with timescale ~10s, to compensate for
+  // gyro drift.  More advanced AHRS systems use the
+  // accelerometer as a reference for finding the zero angle, but
+  // this is a simpler technique: for a balancing robot, as long
+  // as it is balancing, we know that the angle must be zero on
+  // average, or we would fall over.
+  angle = angle * 999 / 1000;
+
+  // This variable measures how close we are to our basic
+  // balancing goal - being on a trajectory that would cause us
+  // to rise up to the vertical position with zero speed left at
+  // the top.  This is similar to the fallingAngleOffset used
+  // for LED feedback and a calibration procedure discussed at
+  // the end of Balancer.ino.
+  //
+  // It is in units of millidegrees, like the angle variable, and
+  // you can think of it as an angular estimate of how far off we
+  // are from being balanced.
+  int32_t risingAngleOffset = angleRate * ANGLE_RATE_RATIO + angle;
+
+  // Combine risingAngleOffset with the distance and speed
+  // variables, using the calibration constants defined in
+  // Balance.h, to get our motor response.  Rather than becoming
+  // the new motor speed setting, the response is an amount that
+  // is added to the motor speeds, since a *change* in speed is
+  // what causes the robot to tilt one way or the other.
+  motorSpeed += (
+    + ANGLE_RESPONSE * risingAngleOffset
+    + DISTANCE_RESPONSE * (distanceLeft + distanceRight)
+    + SPEED_RESPONSE * (speedLeft + speedRight)
+    ) / 100 / GEAR_RATIO;
+
+  if (motorSpeed > MOTOR_SPEED_LIMIT)
+  {
+    motorSpeed = MOTOR_SPEED_LIMIT;
+  }
+  if (motorSpeed < -MOTOR_SPEED_LIMIT)
+  {
+    motorSpeed = -MOTOR_SPEED_LIMIT;
+  }
+
+  // Adjust for differences in the left and right distances; this
+  // will prevent the robot from rotating as it rocks back and
+  // forth due to differences in the motors, and it allows the
+  // robot to perform controlled turns.
+  int16_t distanceDiff = distanceLeft - distanceRight;
+
+  motors.setSpeeds(
+    motorSpeed + distanceDiff * DISTANCE_DIFF_RESPONSE / 100,
+    motorSpeed - distanceDiff * DISTANCE_DIFF_RESPONSE / 100);
+}
+
+void lyingDown()
+{
+  // Reset things so it doesn't go crazy.
+  motorSpeed = 0;
+  distanceLeft = 0;
+  distanceRight = 0;
+  motors.setSpeeds(0, 0);
+
+  if (angleRate > -2 && angleRate < 2)
+  {
+    // It's really calm, so we know the angles.
+    if (imu.a.z > 0)
+    {
+      angle = 80000;
+    }
+    else
+    {
+      angle = -80000;
+    }
+    distanceLeft = 0;
+    distanceRight = 0;
+  }
+}
+
+void integrateGyro()
+{
+  // Convert from full-scale 1000 deg/s to deg/s.
+  angleRate = (imu.g.y - gYZero) / 29;
+
+  angle += angleRate * UPDATE_TIME_MS;
+}
+
+void integrateEncoders()
+{
+  static int16_t lastCountsLeft;
+  int16_t countsLeft = encoders.getCountsLeft();
+  speedLeft = (countsLeft - lastCountsLeft);
+  distanceLeft += countsLeft - lastCountsLeft;
+  lastCountsLeft = countsLeft;
+
+  static int16_t lastCountsRight;
+  int16_t countsRight = encoders.getCountsRight();
+  speedRight = (countsRight - lastCountsRight);
+  distanceRight += countsRight - lastCountsRight;
+  lastCountsRight = countsRight;
+}
+
+void balanceDrive(int16_t leftSpeed, int16_t rightSpeed)
+{
+  driveLeft = leftSpeed;
+  driveRight = rightSpeed;
+}
+
+void balanceDoDriveTicks()
+{
+  distanceLeft -= driveLeft;
+  distanceRight -= driveRight;
+  speedLeft -= driveLeft;
+  speedRight -= driveRight;
+}
+
+void balanceResetEncoders()
+{
+  distanceLeft = 0;
+  distanceRight = 0;
+}
+
+void balanceUpdateSensors()
+{
+  imu.read();
+  integrateGyro();
+  integrateEncoders();
+}
+
+void balanceUpdate()
+{
+  static uint16_t lastMillis;
+  uint16_t ms = millis();
+
+  // Perform the balance updates at 100 Hz.
+  if ((uint16_t)(ms - lastMillis) < UPDATE_TIME_MS) { return; }
+  balanceUpdateDelayedStatus = ms - lastMillis > UPDATE_TIME_MS + 1;
+  lastMillis = ms;
+
+  balanceUpdateSensors();
+  balanceDoDriveTicks();
+
+  if ((imu.a.x < 1000 && imu.a.z > 0) || (imu.a.x < 7500 && imu.a.z < 0))
+  {
+    lyingDown();
+    isBalancingStatus = false;
+  }
+  else
+  {
+    balance();
+    isBalancingStatus = true;
+  }
+}

balboa/BalancerRPi/Balance.h

+#pragma once
+
+#include <stdint.h>
+#include <LSM6.h>
+#include <Balboa32U4.h>
+
+// This code was developed for a Balboa unit using 50:1 motors
+// and 45:21 plastic gears, for an overall gear ratio of 111.
+// Adjust the ratio below to scale various constants in the
+// balancing algorithm to match your robot.
+
+#define B11 false
+#define B10 false
+//#define B11 true
+#if B11
+const int16_t GEAR_RATIO = 111; //84; //124; //b11
+#elif B10
+const int16_t GEAR_RATIO = 111; //162;
+#else
+const int16_t GEAR_RATIO = 111;// 162; //111; //b10
+#endif
+
+
+// This constant limits the maximum motor speed.  If your gear
+// ratio is lower than what we used, or if you are testing
+// changes to the code, you might want to reduce it to prevent
+// your robot from zooming away when things go wrong.
+//
+// If you want to use speeds faster than 300, you should add
+// the line "motors.allowTurbo(true);" to setup().
+const int16_t MOTOR_SPEED_LIMIT = 300; //400;
+
+// This constant relates the angle to its rate of change for a
+// robot that is falling from a nearly-vertical position or
+// rising up to that position.  The relationship is nearly
+// linear.  For example, if you have the 80mm wheels it should be
+// about 140, which means that the angle in millidegrees is ~140
+// times its rate of change in degrees per second; when the robot
+// has fallen by 90 degrees it will be moving at about
+// 90,000/140 = 642 deg/s.  See the end of Balancer.ino for one
+// way to calibrate this value.
+const int16_t ANGLE_RATE_RATIO = 130;
+
+//const int16_t ANGLE_RATE_RATIO = 135;
+
+
+// The following three constants define a PID-like algorithm for
+// balancing.  Each one determines how much the motors will
+// respond to the corresponding variable being off from zero.
+// See the code in Balance.cpp for exactly how they are used.  To
+// get it balancing from scratch, start with them all at zero and
+// adjust them as follows:
+
+// ANGLE_RESPONSE determines the response to a combination of
+// angle and angle_rate; the combination measures how far the
+// robot is from a stable trajectory.  To test this, use your
+// hand to flick the robot up from a resting position.  With a
+// value that is too low, it won't stop itself in time; a high
+// value will cause it to slam back into the ground or oscillate
+// wildly back and forth.  When ANGLE_RESPONSE is adjusted
+// properly, the robot will move just enough to stop itself.
+// However, after stopping itself, it will be moving and keep
+// moving in the same direction, usually driving faster and
+// faster until it reaches its maximum motor speed and falls
+// over.  That's where the next constants come in.
+
+const int16_t ANGLE_RESPONSE = 11;
+/*#if B11
+const int16_t ANGLE_RESPONSE = 13; //35 //40
+#else
+const int16_t ANGLE_RESPONSE = 11;//13; //35 //40
+#endif
+*/
+// DISTANCE_RESPONSE determines how much the robot resists being
+// moved away from its starting point.  Counterintuitively, this
+// constant is positive: to move forwards, the robot actually has
+// to first roll its wheels backwards, so that it can *fall*
+// forwards.  When this constant is adjusted properly, the robot
+// will no longer zoom off in one direction, but it will drive
+// back and forth a few times before falling down.
+/*#if B11
+const int16_t DISTANCE_RESPONSE = 110;
+#else
+const int16_t DISTANCE_RESPONSE = 73; //70;
+#endif
+*/
+const int16_t DISTANCE_RESPONSE = 30;
+
+
+// DISTANCE_DIFF_RESPONSE determines the response to differences
+// between the left and right motors, preventing undesired
+// rotation due to differences in the motors and gearing.  Unlike
+// DISTANCE_REPONSE, it should be negative: if the left motor is
+// lagging, we need to increase its speed and decrease the speed
+// of the right motor.  If this constant is too small, the robot
+// will spin left and right as it rocks back and forth; if it is
+// too large it will become unstable.
+const int16_t DISTANCE_DIFF_RESPONSE = -50; //-120;
+
+// SPEED_RESPONSE supresses the large back-and-forth oscillations
+// caused by DISTANCE_RESPONSE.  Increase this until these
+// oscillations die down after a few cycles; but if you increase
+// it too much it will tend to shudder or vibrate wildly.
+const int16_t SPEED_RESPONSE = 1000;
+
+/*#if B11
+const int16_t SPEED_RESPONSE = 8000;
+#else
+const int16_t SPEED_RESPONSE = 3300; // 4400;
+#endif
+*/
+
+// The balancing code is all based on a 100 Hz update rate; if
+// you change this, you will have to adjust many other things.
+const uint8_t UPDATE_TIME_MS = 10;
+
+// Take 100 measurements initially to calibrate the gyro.
+const uint8_t CALIBRATION_ITERATIONS = 100;
+
+// These variables will be accessible from your sketch.
+extern int32_t angle; // units: millidegrees
+extern int32_t angleRate; // units: degrees/s (or millidegrees/ms)
+extern int16_t motorSpeed; // current (average) motor speed setting
+
+// These variables must be defined in your sketch.
+extern LSM6 imu;
+extern Balboa32U4Motors motors;
+extern Balboa32U4Encoders encoders;
+
+// Call this in your setup() to initialize and calibrate the IMU.
+void balanceSetup();
+
+// Call this in loop() to run the full balancing algorithm.
+void balanceUpdate();
+
+// Call this function to set a driving speed in ticks/ms.  The
+// way it works is that every update cycle we adjust the robot's
+// encoder measurements, which will cause it to drive in the
+// corresponding direction.  Differing values for left and right
+// will result in a turn.
+void balanceDrive(int16_t leftSpeed, int16_t rightSpeed);
+
+// Returns true if (according to the balancing algorithm) the
+// robot is trying to balance.  When it falls down it shuts off
+// the motors, and this function will return false.  If you pick
+// the robot up, this function will start returning true again.
+bool isBalancing();
+
+// Returns true if the last update cycle was delayed to more than
+// UPDATE_TIME_MS+1 milliseconds.  This could indicate
+// computations being too long or interrupts that are delaying
+// the loop.
+bool balanceUpdateDelayed();
+
+// Sometimes you will want to take control of the motors but keep
+// updating the balancing code's encoders and angle measurements
+// so you don't lose track of the robot's position and angle.
+// Call this every 10ms (UPDATE_TIME_MS) to update the sensors,
+// and you will be able to resume balancing immediately when you
+// are done.
+void balanceUpdateSensors();
+
+// Call this function to reset the encoders.  This is useful
+// after a large motion, so that robot does not try to make a
+// huge correction to get back to "zero".
+void balanceResetEncoders();

balboa/BalancerRPi/BalancerRPi.ino

+// This example shows how to make a Balboa balance on its two
+// wheels and drive around while balancing.
+//
+// To run this demo, you will need to install the LSM6 library:
+//
+// https://github.com/pololu/lsm6-arduino
+//
+// To use this demo, place the robot on the ground with the
+// circuit board facing up, and then turn it on.  Be careful to
+// not move the robot for a few seconds after powering it on,
+// because that is when the gyro is calibrated.  During the gyro
+// calibration, the red LED is lit.  After the red LED turns off,
+// turn the robot so that it is standing up.  It will detect that
+// you have turned it and start balancing.
+//
+// Alternatively, you can press the A button while the robot is
+// lying down and it will try to use its motors to kick up into
+// the balancing position.
+//
+// This demo is tuned for the 50:1 high-power gearmotor with
+// carbon brushes, 45:21 plastic gears, and 80mm wheels; you will
+// need to adjust the parameters in Balance.h for your robot.
+//
+// After you have gotten the robot balance well, you can
+// uncomment some lines in loop() to make it drive around and
+// play a song.
+
+#include <Balboa32U4.h>
+#include <Wire.h>
+#include <LSM6.h>
+#include <LIS3MDL.h>
+#include "Balance.h"
+
+LSM6 imu;
+LIS3MDL mag;
+Balboa32U4Motors motors;
+Balboa32U4Encoders encoders;
+Balboa32U4Buzzer buzzer;
+Balboa32U4ButtonA buttonA;
+Balboa32U4ButtonB buttonB;
+uint16_t lastSent = millis();
+bool musicOn = false;
+bool controlled = true;
+uint32_t lastCommand = millis();
+int16_t lastEncoderLeft = 0, lastEncoderRight = 0;
+
+String commandString = "";
+char accValues[40], gyroValues[40], magValues[40], odomValues[40], debugTelemetry[40];
+void initMag() {
+  if (!mag.init())
+  {
+    Serial.println("E: Failed to detect and initialize magnetometer!");
+    while (1);
+  }
+
+  mag.enableDefault();
+}
+
+void setup()
+{
+  // Uncomment these lines if your motors are reversed.
+  // motors.flipLeftMotor(true);
+  // motors.flipRightMotor(true);
+  //motors.allowTurbo(true);
+  ledYellow(0);
+  ledRed(1);
+  balanceSetup();
+  initMag();
+  ledRed(0);
+  Serial.begin(115200);
+}
+
+const char song[] PROGMEM =
+  "!O6 T240"
+  "l32ab-b>cl8r br b-bb-a a-r gr g-4 g4"
+  "a-r gr g-gg-f er e-r d4 e-4"
+  "gr msd8d8ml d-4d4"
+  "l32efg-gl8r msd8d8ml d-4d4"
+  "<bcd-d e-efg- ga-ab- a4 gr";
+
+void playSong()
+{
+  if (!buzzer.isPlaying())
+  {
+    buzzer.playFromProgramSpace(song);
+  }
+}
+
+int8_t setAngularVelocity(float angularVelocity) {
+  int8_t wheelSpeed = angularVelocity * GEAR_RATIO * 12 * 4 / 6 / 1000 / 10;
+  return wheelSpeed;
+}
+
+int8_t setVelocity(float velocity) {
+  int8_t wheelSpeed = velocity * GEAR_RATIO * 12 / 4 / 1000;
+  return wheelSpeed;
+}
+
+void driveAround()
+{
+  uint16_t time = millis() % 8192;
+  uint16_t leftSpeed, rightSpeed;
+  if (time < 1900)
+  {
+    leftSpeed = 20;
+    rightSpeed = 20;
+  }
+  else if (time < 4096)
+  {
+    leftSpeed = 25;
+    rightSpeed = 15;
+  }
+  else if (time < 4096 + 1900)
+  {
+    leftSpeed = 20;
+    rightSpeed = 20;
+  }
+  else
+  {
+    leftSpeed = 15;
+    rightSpeed = 25;
+  }
+
+  balanceDrive(leftSpeed/8, rightSpeed/8);
+}
+
+void standUp()
+{
+  motors.setSpeeds(0, 0);
+  buzzer.play("!>grms>a16>a16>g2");
+  ledGreen(1);
+  ledRed(1);
+  ledYellow(1);
+  while (buzzer.isPlaying());
+  motors.setSpeeds(-MOTOR_SPEED_LIMIT, -MOTOR_SPEED_LIMIT);
+  delay(200);
+  motors.setSpeeds(MOTOR_SPEED_LIMIT, MOTOR_SPEED_LIMIT);
+  for (uint8_t i = 0; i < 30; i++)
+  {
+    delay(UPDATE_TIME_MS);
+    balanceUpdateSensors();
+    if(angle < 50000)
+    {
+      Serial.println("D: Start balancing!");
+      break;
+    }
+  }
+  motorSpeed = MOTOR_SPEED_LIMIT; //150;
+  balanceResetEncoders();
+}
+
+void layDown() {
+  motors.setSpeeds(-100, -100);
+  delay(300);
+  motors.setSpeeds(0, 0);
+  delay(2000);
+  return;
+}
+
+void executeCommand() {
+  Serial.print("D: command received: ");
+  Serial.println(commandString);
+  if (commandString[0] == 'S') {
+    Serial.println("D: standUp");
+    standUp();
+  } else if (commandString[0] == 'L') {
+    Serial.println("D: layDown");
+    layDown();
+  } else {
+    String velocityStrings[2];
+    int j=0;
+    for (int i = 0; i<commandString.length(); i++) {
+      char sign = commandString[i];
+      if (sign == '\n') {
+        break;
+      } else if (sign == ' ') {
+        j++;
+      } else if (isDigit(sign) || (sign == '-' && velocityStrings[j].length() == 0)) {
+        velocityStrings[j] += sign;
+      } else {
+        Serial.println("E: Error in command parsing. Sign unknown.");
+        return;
+      }
+      if (j > 1) {
+        Serial.println("E: Error in command parsing. Too many spaces.");
+        return;
+      }
+    }
+    if (j < 1) {
+      Serial.println("E: Error in command parsing. Too few values.");
+      return;
+    }
+    int velocities[2];
+    for (int i = 0; i<2; i++) {
+      velocities[i] = velocityStrings[i].toInt();
+    }
+    int16_t speedL, speedR, angularVelocity, velocity;
+    angularVelocity = velocities[0];
+    velocity = velocities[1];
+    speedL = -setAngularVelocity(angularVelocity) - setVelocity(velocity);
+    speedR = setAngularVelocity(angularVelocity) - setVelocity(velocity);
+    balanceDrive(speedL, speedR);
+  }
+  lastCommand = millis();
+}
+
+void stop() {
+  balanceDrive(0, 0);
+}
+
+char readSerial() {
+  while (Serial.available() > 0) {
+    char inSign = Serial.read();
+    commandString += inSign;
+    if (inSign == '\n') {
+      executeCommand();
+      commandString = "";
+    }
+  }
+}
+
+void publishIMU() {
+  snprintf(accValues, sizeof(accValues), "A: %6d %6d %6d",
+    imu.a.x, imu.a.y, imu.a.z);
+  Serial.println(accValues);
+
+  snprintf(gyroValues, sizeof(gyroValues), "G: %6d %6d %6d",
+    imu.g.x, imu.g.y, imu.g.z);
+  Serial.println(gyroValues);
+
+  snprintf(magValues, sizeof(magValues), "M: %6d %6d %6d",
+    mag.m.x, mag.m.y, mag.m.z);
+  Serial.println(magValues);
+}
+
+void publishOdometry() {
+  int16_t encoderLeft = encoders.getCountsLeft();
+  int16_t encoderRight = encoders.getCountsRight();