#define BLYNK_PRINT Serial
#include <BlynkSimpleRedBearLab_BLE_Nano.h>
#include <BLE_API.h>
#include <Wire.h>
#include <Servo.h>
Servo myservo;
const int DRV8830 = 0x64;
int power = 0;
float angle = 0;
int rot = 0;
int Speed = 0;
int angleServo_ini = 35;
int angleServo = angleServo_ini;
// You should get Auth Token in the Blynk App.
char auth[] = "YourAuthTokenを入力";
uint8_t device_name[] = "Blynk";
//モータドライバ I2C制御 motor driver I2C
//Reference
//http://makers-with-myson.blog.so-net.ne.jp/2014-05-15
void writeMotorResister(byte vset, byte data1){
int vdata = vset << 2 | data1;
Wire.beginTransmission(DRV8830);
Wire.write(0x00);
Wire.write(vdata);
Wire.endTransmission(true);
}
void setup() {
Wire.begin(D3, D2, 400000L);
Serial.begin(250000);
writeMotorResister(0x00, 0x00);
myservo.attach(D4);
myservo.write(angleServo);
delay(100);
ble.init();
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
device_name, sizeof(device_name) - 1);
ble.gap().setDeviceName(device_name);
ble.gap().setTxPower(4);
// Add Blynk service...
Blynk.begin(auth);
ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.gap().setAdvertisingInterval(Gap::MSEC_TO_GAP_DURATION_UNITS(1000));
ble.gap().setAdvertisingTimeout(0);
ble.startAdvertising();
Serial.println("Waiting for connections...");
}
//ジョイスティック値受信
BLYNK_WRITE(V1) {
float x = param[0].asInt() - 128.0;
float y = param[1].asInt() - 128.0;
//モータスピード
power = sqrt(x*x + y*y);
if(power > 128){
power = 128;
}
Speed = map(power, 0, 128, 5, 19);
//サーボ角度
angle = atan(y/x) * 180.0 / PI;
if(x < 0 && y > 0){
angle = angle * -1;
}else if(x > 0 && y >= 0){
angle = 180.0 - angle;
}else if(x >= 0 && y < 0){
angle = 180.0 + angle;
}else if(x == 0 && y == 0 || power >= 0 && power < 57){
angle = 90;
}
angleServo = map(angle, 180, 0, 0, angleServo_ini * 2);
//モータ回転方向
if(y >= 0){
rot = 2; //前進
} else {
rot = 1; //後進
}
//モーター、サーボ実行
if (power == 0){
writeMotorResister(0x00, 0x00);
}else {
writeMotorResister((byte)Speed, (byte)rot);
}
myservo.write(angleServo);
//delay(15);
Serial.print("x: ");
Serial.print(x);
Serial.print(" y: ");
Serial.print(y);
Serial.print(" power: ");
Serial.print(power);
Serial.print(" Speed: ");
Serial.print(Speed);
Serial.print(" angleServo: ");
Serial.print(angleServo);
Serial.print(" rot: ");
Serial.println(rot);
}
void loop() {
Blynk.run();
}