SPRESENSEで２つのモーターを制御してみた

SPRESENSEで倒立振子を作ってみようかと思い、２つのモーターを制御してみました。ジャイロセンサーには、スイッチサイエンスのBMI160搭載されたSPRESENSE用アドオンボードを使う予定です。

BMI160のライブラリは、次のGitHubからダウンロードして使っています。サンプルが少ないので使いこなすには、自分でソースを読み解く必要がありそうです。

hanyazou/BMI160-Arduino
https://github.com/hanyazou/BMI160-Arduino

次のようなスケッチを書いてBMI160とモーターの動作を確認をしてみました。電圧が低い状態だとトルク不足で車輪が回らないので、駆動時の頭で車輪が回らない程度の短い時間トルクをフルでかけて車輪を回す工夫をしています。（十分な出力容量がある電池なら不要かもしれません）


#include &ltWire.h>
#include &ltBMI160Gen.h>

#define AIN01 25
#define AIN02 26
#define AIN11 18
#define AIN12 19
#define DELAY 3000
#define DASH 5


float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767

  float g = (gRaw * 250.0) / 32768.0;
  return g;
}


void print_gyro() {
  int gxRaw, gyRaw, gzRaw;         // raw gyro values
  float gx, gy, gz;

  // read raw gyro measurements from device
  BMI160.readGyro(gxRaw, gyRaw, gzRaw);

  // convert the raw gyro data to degrees/second
  gx = convertRawGyro(gxRaw);
  gy = convertRawGyro(gyRaw);
  gz = convertRawGyro(gzRaw);

  // display tab-separated gyro x/y/z values
  Serial.print("g:\t");
  Serial.print(gx);
  Serial.print("\t");
  Serial.print(gy);
  Serial.print("\t");
  Serial.print(gz);
  Serial.println();

  delay(500);  
}

void motor_drive(uint8_t i, bool cw) {
#ifdef DASH
  if (cw) {
    analogWrite(AIN01, 255);
    analogWrite(AIN02, 0);
    analogWrite(AIN11, 255);
    analogWrite(AIN12, 0);
  }  else  {
    analogWrite(AIN01, 0);
    analogWrite(AIN02, 255);
    analogWrite(AIN11, 0);
    analogWrite(AIN12, 255);
  }
  delay(DASH);
#endif
  int n = i*ADJUST;
  if (cw) {
    analogWrite(AIN01, i);
    analogWrite(AIN02, 0);
    analogWrite(AIN11, i);
    analogWrite(AIN12, 0);

  } else {
    analogWrite(AIN01, 0);
    analogWrite(AIN02, i);
    analogWrite(AIN11, 0);
    analogWrite(AIN12, i);
  }
  Serial.println(i);
  delay(DELAY);  
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  BMI160.begin(BMI160GenClass::I2C_MODE);
  uint8_t dev_id = BMI160.getDeviceID();
  Serial.print("DEVICE ID: ");
  Serial.println(dev_id, HEX);  
  BMI160.setGyroRange(250);
  Serial.println("Initializing IMU device...done.");
}

void loop() {
  int i;

  for (i = 0; i <= 250; i+=10) {
    motor_drive(i, true);
    print_gyro();
  }

  for (i = 250; i >= 0; i-=10) {
    motor_drive(i, true);
    print_gyro();
  }

  for (i = 0; i <= 250; i+=10) {
    motor_drive(i, false);
    print_gyro();
  }

  for (i = 250; i >= 0; i-=10) {
    motor_drive(i, false);
    print_gyro();
  }

  while(1);

}

どうも左右のモーターの特性が異なるようで、回転数に明らかな差がでてしまっています。モーターはアナログなものなので特性に差が出るのは仕方ないけど、倒立振子がきちんと動くのか心配です。

その前に、これらのパーツを組み上げるための台を３Dプリンタでデザインしないと…。久しぶりのCAD使えるかな。
（ｰ'`ｰ;）

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

Tenflyer 6軸レートジャイロBMI160 6Dof重力加速度センサーモジュール

出版社/メーカー: Tenflyer
メディア: ホーム&キッチン

HiLetgo 3pcs DRV8833デュアルモータードライバー

出版社/メーカー: HiLetgo
メディア: おもちゃ＆ホビー

SPRESENSEで２つのモーターを制御してみた

関連する記事