#define FRONT_SENSOR 14
#define SIDE_SENSOR 15
#define RIGHT_WHEEL1 16
#define RIGHT_WHEEL2 22
#define LEFT_WHEEL1 13
#define LEFT_WHEEL2 12
#define SPEED_PWM 17
//PWM Channel
#define SPEED_PWM_CH1 0
#define LEDC_TIMER_8_BIT 8
#define LEDC_BASE_FREQ 500
//センサー用変数
const int maxCount = 2;
int count = 0;
int currentFrontSensorValue = 0;
int previousFrontSensorValue = 0;
int currentSideSensorValue = 0;
int previousSideSensorValue = 0;
void task0(void* param) {
ExecuteSensorRead();
}
void ExecuteSensorRead() {
int frontAverageValue = 0;
int sideAverageValue = 0;
while (1) {
//Sensor Read
frontAverageValue += analogRead(FRONT_SENSOR);
sideAverageValue += analogRead(SIDE_SENSOR);
count++;
if (count == maxCount) {
previousFrontSensorValue = currentFrontSensorValue;
previousSideSensorValue = currentSideSensorValue;
currentFrontSensorValue = frontAverageValue / count;
currentSideSensorValue = sideAverageValue / count;
frontAverageValue = sideAverageValue = 0;
count = 0;
Serial.print("front :");
Serial.print(currentFrontSensorValue);
Serial.print(", side :");
Serial.println(currentSideSensorValue);
}
delay(100);
}
}
//Motor Control
void MoveForward(int delayMs) {
digitalWrite(RIGHT_WHEEL1, HIGH);
digitalWrite(RIGHT_WHEEL2, LOW);
digitalWrite(LEFT_WHEEL1 , LOW);
digitalWrite(LEFT_WHEEL2 , HIGH);
delay(delayMs);
}
void MoveBack(int delayMs) {
digitalWrite(RIGHT_WHEEL1, LOW);
digitalWrite(RIGHT_WHEEL2, HIGH);
digitalWrite(LEFT_WHEEL1 , HIGH);
digitalWrite(LEFT_WHEEL2 , LOW);
delay(delayMs);
}
void TurnLeft(int delayMs) {
digitalWrite(RIGHT_WHEEL1, LOW);
digitalWrite(RIGHT_WHEEL2, HIGH);
digitalWrite(LEFT_WHEEL1 , LOW);
digitalWrite(LEFT_WHEEL2 , HIGH);
delay(delayMs);
}
void TurnRight(int delayMs) {
digitalWrite(RIGHT_WHEEL1, HIGH);
digitalWrite(RIGHT_WHEEL2, LOW);
digitalWrite(LEFT_WHEEL1 , HIGH);
digitalWrite(LEFT_WHEEL2 , LOW);
delay(delayMs);
}
void Brake(int delayMs) {
digitalWrite(RIGHT_WHEEL1, HIGH);
digitalWrite(RIGHT_WHEEL2, HIGH);
digitalWrite(LEFT_WHEEL1 , HIGH);
digitalWrite(LEFT_WHEEL2 , HIGH);
delay(delayMs);
}
void Stop(int delayMs) {
digitalWrite(RIGHT_WHEEL1, LOW);
digitalWrite(RIGHT_WHEEL2, LOW);
digitalWrite(LEFT_WHEEL1 , LOW);
digitalWrite(LEFT_WHEEL2 , LOW);
delay(delayMs);
}
void setup() {
Serial.begin(9600);
delay(100);
pinMode(FRONT_SENSOR, INPUT);
pinMode(SIDE_SENSOR , INPUT);
pinMode(RIGHT_WHEEL1, OUTPUT);
pinMode(RIGHT_WHEEL2, OUTPUT);
pinMode(LEFT_WHEEL1 , OUTPUT);
pinMode(LEFT_WHEEL2 , OUTPUT);
ledcSetup(SPEED_PWM_CH1, LEDC_BASE_FREQ, LEDC_TIMER_8_BIT);
ledcAttachPin(SPEED_PWM, SPEED_PWM_CH1);
xTaskCreatePinnedToCore(task0, "Task0", 4096, NULL, 1, NULL, 0);
Brake(5000);
}
void loop() {
for(int i=0; i<6; i++){
ledcWrite(SPEED_PWM_CH1, i*50);
MoveForward(1000);
if(i==5){
Brake(1000);
}
}
/*
//正面に障害物あり
if(currentFrontSensorValue > 2500){
Serial.println("Detect Object in Front");
Brake(200);
TurnLeft(1000);
Brake(200);
}
//側面に壁がない
else if(currentSideSensorValue < 1000){
if(previousSideSensorValue < 1000){
MoveForwarad(500);
}
Brake(100);
TurnRight(2500);
}
else if(currentSideSensorValue > 3500){
Serial.println("Detect Object On the Side");
TurnLeft(500);
MoveForward(500);
}
else{
MoveForward(500);
}
*/
}
