ロボットカーメモ

//PIN Assign
#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); 
  }
  */
}