#include <Arduino.h>
#define holes1 27 //ホールセンサー1
#define holes2 14 //ホールセンサー2
#define holes3 13 //ホールセンサー3
#define sw1 12 //起動スイッチ
#define md1_1 23 //モータドライバ2 IN1 コイル2
#define md1_2 22 //モータドライバ2 IN2 コイル2
#define md1_3 19 //モータドライバ2 IN3 コイル3
#define md1_4 21 //モータドライバ2 IN4 コイル3
#define md2_1 18 //モータドライバ1 IN1 コイル1
#define md2_2 17 //モータドライバ1 IN2 コイル1
const int cpwm1=A4;//GPIO32 モータドライバ2 ENA
const int cpwm2=A5;//GPIO33 モータドライバ2 ENB
const int cpwm3=A18;//GPIO25 モータドライバ1 ENA
volatile int houkou=1; //0で右 1で左
volatile int sensorEvent = 0;
int flgg=0;
int countt=0;
int countc=0;
int datac[6][3]={{0,3,200},{1,3,200},{0,3,200},{1,2,200},{1,2,200},{0,5,255}}; //動作データ{方向,距離,PWM}
int datcount=0;
int teishi=1; //1で停止 0で起動
void setup() {
// put your setup code here, to run once:
// シリアル通信を開始
Serial.begin(115200);
pinMode(holes1,INPUT_PULLUP);
pinMode(holes2,INPUT_PULLUP);
pinMode(holes3,INPUT_PULLUP);
pinMode(sw1,INPUT_PULLUP);
attachInterrupt(holes1, ss1, FALLING);
attachInterrupt(holes2, ss2, FALLING);
attachInterrupt(holes3, ss3, FALLING);
pinMode(md1_1, OUTPUT);
pinMode(md1_2, OUTPUT);
pinMode(md1_3, OUTPUT);
pinMode(md1_4, OUTPUT);
pinMode(md2_1, OUTPUT);
pinMode(md2_2, OUTPUT);
ledcAttach(cpwm1, 12800,8);
ledcAttach(cpwm2, 12800,8);
ledcAttach(cpwm3, 12800,8);
}
void ss1(){
sensorEvent = 1;
}
void ss2(){
sensorEvent = 2;
}
void ss3(){
sensorEvent = 3;
}
void kidou(){
if(digitalRead(sw1)==LOW){
if (teishi==1){
teishi=0;
datcount=0;
flgg=0;
countt=0;
houkou=datac[datcount][0];
migi(2,200);
}
}
}
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
void loop() {
kidou();
if(teishi==1){
digitalWrite(md2_1, LOW);digitalWrite(md2_2, LOW);//コイル1停止
digitalWrite(md1_3, LOW);digitalWrite(md1_4, LOW);//コイル2停止
digitalWrite(md1_1, LOW);digitalWrite(md1_2, LOW);//コイル3停止
}
int ev = 0;
int cc=128;
portENTER_CRITICAL(&mux);
if (sensorEvent != 0) {
ev = sensorEvent;
sensorEvent = 0;
}
portEXIT_CRITICAL(&mux);
if(teishi==0){
if (ev != 0) {
cc=datac[datcount][2];
Serial.println(datac[datcount][1]);
if (datac[datcount][1]>=countt){
if (houkou==0) {
migi(ev,cc);
} else {
hidari(ev,cc);
}
}else{
datcount=datcount+1;
if(datcount>5){teishi=1;}
houkou=datac[datcount][0];
countt=0;
flgg=0;
}
}
}
}
void migi(int ev,int cc){
ledcWrite(cpwm1,cc);
ledcWrite(cpwm2,cc);
ledcWrite(cpwm3,cc);
switch(ev){
case 1: digitalWrite(md2_1, HIGH);digitalWrite(md2_2, LOW);//コイル1正転
digitalWrite(md1_3, LOW);digitalWrite(md1_4, HIGH);//コイル2逆転
digitalWrite(md1_1, LOW);digitalWrite(md1_2, LOW);//コイル3停止
if(flgg==0){flgg=1;}
break;
case 2: digitalWrite(md1_3, HIGH);digitalWrite(md1_4, LOW);//コイル2正転
digitalWrite(md1_1, LOW);digitalWrite(md1_2, HIGH);//コイル3逆転
digitalWrite(md2_1, LOW);digitalWrite(md2_2, LOW);//コイル1停止
if(flgg==1){
flgg=0;
countt=countt+1;
}
break;
case 3: digitalWrite(md1_1, HIGH);digitalWrite(md1_2, LOW);//コイル3正転
digitalWrite(md2_1, LOW);digitalWrite(md2_2, HIGH);//コイル1正転
digitalWrite(md1_3, LOW);digitalWrite(md1_4, LOW);//コイル2停止
break;
}
}
void hidari(int ev,int cc){
ledcWrite(cpwm1,cc);
ledcWrite(cpwm2,cc);
ledcWrite(cpwm3,cc);
switch(ev){
case 1: digitalWrite(md2_1, LOW);digitalWrite(md2_2, HIGH);//コイル1逆転
digitalWrite(md1_3, HIGH);digitalWrite(md1_4, LOW);//コイル2正転
digitalWrite(md1_1, LOW);digitalWrite(md1_2, LOW);//コイル3停止
break;
case 2: digitalWrite(md2_1, LOW);digitalWrite(md2_2, LOW);//コイル1停止
digitalWrite(md1_3, LOW);digitalWrite(md1_4, HIGH);//コイル2逆転
digitalWrite(md1_1, HIGH);digitalWrite(md1_2, LOW);//コイル3正転
if(flgg==1){
flgg=0;
countt=countt+1;
}
break;
case 3: digitalWrite(md2_1, HIGH);digitalWrite(md2_2, LOW);//コイル1正転
digitalWrite(md1_3, LOW);digitalWrite(md1_4, LOW);//コイル2停止
digitalWrite(md1_1, LOW);digitalWrite(md1_2, HIGH);//コイル3逆転
if(flgg==0){flgg=1;}
break;
}
}
.jpg)
