moveFunc

#include <LSM6.h>
#include <Balboa32U4.h>

Balboa32U4Motors motors;
Balboa32U4ButtonA buttonA;

LSM6 imu;
char report[120];

void setup() {
  // put your setup code here, to run once:
  delay(2000);
  Serial.begin(9600);
  Serial.write("STARTING...");
  //buttonA.waitForButton();

  delay(1000);

  // setup for angle detection, magnetometer not included
  if (!imu.init()) {
    // failed to detect the LSM6
    ledRed(1);
    while (1) {
      Serial.write(F("Failed to detect the LSM6."));
      delay(100);
    }
  }

  imu.enableDefault();

  // Set the gyro full scale to 1000 dps because the default
  // value is too low, and leave the other settings the same.
  imu.writeReg(LSM6::CTRL2_G, 0b10001000);

  // Set the accelerometer full scale to 16 g because the default
  // value is too low, and leave the other settings the same.
  imu.writeReg(LSM6::CTRL1_XL, 0b10000100);
}

void loop() {
  // put your main code here, to run repeatedly:
  //  ledYellow(0);
  //  move_fwd(200);
  //  delay(5000);

  //  ledYellow(0);
  //  move_back(200);
  //  delay(5000);
  //
  //  ledYellow(0);
  //  move_left(200);
  //  delay(5000);
  //
  //  ledYellow(0);
  //  move_right(200);
  //  delay(5000);

  get_imu();
  delay(100);
  get_angle();
}

// speed: less than 300
void move_back(int speed) {
  ledYellow(1);
  motors.setLeftSpeed(speed);
  motors.setRightSpeed(-speed);
}

// speed: less than 300
void move_fwd(int speed) {
  ledYellow(1);
  motors.setLeftSpeed(-speed);
  motors.setRightSpeed(speed);
}

// speed: less than 300
void move_left(int speed) {
  ledYellow(1);
  motors.setLeftSpeed(-speed);
  motors.setRightSpeed(-speed);
}

// speed: less than 300
void move_right(int speed) {
  ledYellow(1);
  motors.setLeftSpeed(speed);
  motors.setRightSpeed(speed);
}

int32_t gYZero;
int32_t angle;
int32_t angleRate;

// get the angle of robot
void get_angle() {
  // atan2--in radians, to convert it to degrees, we multiply it with 57296=180000/pi
  angle = atan2(imu.a.z, imu.a.x) * 57296;

  int32_t total = 0;
  for (int i = 0; i < 100; i++) {
    imu.read();
    total += imu.g.y;
    delay(1);
  }
  gYZero = total / 100;

//  Serial.write(F("angle: %6d gYZero: %6d", angle, gYZero));

  snprintf_P(report, sizeof(report),
             PSTR("angle: %6d gYZero: %6d"),
             angle, gYZero);
  Serial.println(report);
}

// get the imu data
void get_imu() {
    imu.read();
    sprintf(report, "A: %6d %6d %6d   G: %6d %6d %6d ",
          imu.a.x, imu.a.y, imu.a.z,
          imu.a.x, imu.g.y, imu.g.z);
    Serial.write(report);
    delay(100);
}