とりあえずは、5V動作のArduino Uno で動作確認。
#include <Arduino.h> #include <Wire.h> // Grove I2C MotorDriver レジスタ #define SET_PWM_AB 0x82 #define SET_FREQ 0x84 #define CHG_ADDR 0x83 #define CHANEL_SET 0xaa #define MOTOR1_SPD 0xa1 #define MOTOR2_SPD 0xa5 // Grove I2C MotorDriver アドレス static unsigned char MOTOR_DRIVER_ADDR = 0x28; // 通信終端文字コード定義。 static char CHAR_CR = 0x0d; static char CHAR_LF = 0x0a; // グローバル変数定義。 byte rcvChar; byte rcvBuf[64]; int rcvCnt; boolean isRcvCmd; void setMotor(unsigned char mortor_no, unsigned char direction, unsigned char speed) { Wire.beginTransmission(MOTOR_DRIVER_ADDR); Wire.write(mortor_no); Wire.write(direction); Wire.write(speed); Wire.endTransmission(); } void setMotorSpeed(int motor_no, int speed) { unsigned char spd; unsigned char dir; if(( motor_no < 1) && (motor_no > 2)) return; if (speed < 0) { dir = 2; spd = (unsigned char)(-speed); } else { dir = 1; spd = (unsigned char)speed; } if (motor_no == 1) { setMotor(MOTOR1_SPD, dir, spd); } else { setMotor(MOTOR2_SPD, dir, spd); } } void setup() { Serial.begin(115200); Wire.begin(); delayMicroseconds(10); Serial.println("Start..."); setMotorSpeed(1, 40); setMotorSpeed(2, -40); delay(10000); setMotorSpeed(1, 0); setMotorSpeed(2, 0); } void loop() { // シリアル受信処理。受信バッファにデータがあれば、 if (Serial.available() > 0) { /// 1文字読み込み。 rcvChar = Serial.read(); if (rcvCnt == 0 && rcvChar == 0x80) { rcvBuf[0] = rcvChar; for (int i = 1; i < 8; i++) { rcvBuf[i] = Serial.read(); rcvCnt++; } isRcvCmd = true; } } // シリアル受信処理、ここまで。 // コマンド処理部。 if (isRcvCmd) { isRcvCmd = false; rcvCnt = 0; int motor1_speed = 0; int motor2_speed = 0; byte V123 = rcvBuf[4]; byte V125 = rcvBuf[6]; motor1_speed = (64 - (int)V123) * 2; motor2_speed = (64 - (int)V125) * 2; setMotorSpeed(1, motor1_speed); setMotorSpeed(2, motor2_speed); } }
0 件のコメント:
コメントを投稿