TB1622FNGのピン配置
PWMA 12,AIN1 13,AIN2 14, STBY 3V3, GND TO ESP32
VM FROM 5V-BATTERY,VCC 5V,GND FROM 5V-BATTERY,GND TO ESP32
https://404background.com/circuit/tb6612fng/#%E5%9B%9E%E8%B7%AF%E3%82%92%E7%B5%84%E3%82%80 に詳しい説明があるがアルディーノUNO用にて注意
#include <ESP32Servo.h> // pwm freq 50hz固定
#include <BluetoothSerial.h>
// 定数の定義
#define SERVO 5 // MG996R アルディーノではpwm可能なPIN必須
#define PWMA 12 // TB6612FNGのPWMA アルディーノではpwm可能なPIN必須
#define AIN1 13 / TB6612FNGのAIN1
#define AIN2 14 // TB6612FNGのAIN2
#define LEDC_CHANNEL 6 // PWMのチャンネル for dcmotor
#define LEDC_BASE_FREQ 10000 // PWMの周波数 for dcmotor
#define MAX_SPEED 3 // 速度の最大値
#define MIN_SPEED -1 // 速度の最小値
#define ANGLE_STEP 6 // 角度の増分
#define ANGLE_MAX 18 // 角度の最大値
// Bluetoothシリアルにつける名前
const char *btname = "ESP32Car";
// MG996Rの角度調整
const int servo_offset = -5;
// 速度・角度 を保存するグローバル変数
int speed = 0;
int angle = 0;
// MG996RとBluetoothシリアルに対応するグローバル変数
Servo myServo;
BluetoothSerial SerialBT;
void setup() {
// Bluetoothシリアルの初期化
SerialBT.begin(btname);
// PWMの初期化
ledcSetup(LEDC_CHANNEL, LEDC_BASE_FREQ, 8);
// チャンネル指定と周波数、分周ビットの指定 8ビットなら0から255
ledcAttachPin(PWMA, LEDC_CHANNEL); //上記チャンネルをPINに紐付け
// MG996Rの初期化
myServo.attach(SERVO); // MG996Rのピン番号を指定
myServo.write(90 + servo_offset);
// GPIOピンの初期化
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
}
void loop() {
// Bluetoothシリアルから文字を読み込んで処理する
read_bt();
// モーターを操作する
operate_motor();
}
// Bluetoothシリアルから文字を読み込んで処理する
void read_bt() {
// Bluetoothシリアルから文字を受信しているかどうかを判断する
if (SerialBT.available()) {
// Bluetoothシリアルから1文字読み込む
int ch = SerialBT.read();
// 文字が「e」の場合
if (ch == 'e') {
// speedがMAX_SPEEDより小さければ1増やす(加速)
if (speed < MAX_SPEED) {
speed++;
}
}
// 文字が「x」の場合
else if (ch == 'x') {
// speedがMIN_SPEEDより大木れば1減らす(減速)
if (speed > MIN_SPEED) {
speed--;
}
}
// 文字が「d」の場合
else if (ch == 'd') {
// angleがANGLE_MAXより小さければANGLE_STEPだけ増やす(ハンドルを右に切る)
if (angle < ANGLE_MAX) {
angle += ANGLE_STEP;
}
}
// 文字が「s」の場合
else if (ch == 's') {
// angleが-ANGLE_MAXより大きいければANGLE_STEPだけ減らす(ハンドルを左に切る)
if (angle > -ANGLE_MAX) {
angle -= ANGLE_STEP;
}
}
// 文字が「i」の場合
else if (ch == 'i') {
// モーターを止め、ハンドルを中心に戻す
speed = 0;
angle = 0;
}
}
}
// モーターを操作する
void operate_motor() {
// speedがプラス
if (speed > 0 ) {
// モーターを正転させる
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
}
// speedがマイナスの場合
else if (speed < 0) {
// モーターを逆転させる
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
}
// その他の場合
else {
// モーターを止める
speed = 0;
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}
// モーターの回転速度を決める ledcWriteはアルディーノではanalogWriteに相当
ledcWrite(LEDC_CHANNEL, 30 + 40 * abs(speed));
// MG996Rの角度を決める
myServo.write(90 + angle + servo_offset);
}
------------------------------------------------------------------------------------------
ラズピコは以下の通り まずdig_0でニュートラルにホーンをとりつける
あとは(+)(ー)で左右に振る!
from machine import PWM, Pin
import time
IN1=Pin(1,Pin.OUT)
IN2=Pin(2,Pin.OUT)
servo1 = PWM(Pin(28))
servo1.freq(50)
dig_0 = 0.0725 #0° almost 4700
dig_90 = 0.12 #90° almost 7800
dig__90 = 0.025 #-90° almost 1600
max_duty = 65025
while True:
#servo1.duty_u16(int(max_duty*dig_90))
servo1.duty_u16(6000)
print("40")
time.sleep(3)
servo1.duty_u16(int(max_duty*dig_0))
print("0")
time.sleep(3)
#servo1.duty_u16(int(max_duty*dig__90))
servo1.duty_u16(3000)
print("-40")
time.sleep(3)