학부게시판
학부게시판
그린모빌리티 자율주행 자동차 코드
김현수
2022.12.29
조회수 1023
211.220.225.54
저작권 등 다른 사람의 권리를 침해하거나 명예를 훼손하는 게시글은 이용약관 및 관련법률에 의해 제재를 받으실 수 있습니다.
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"
const int dcMotors[] = {25,26,12,27,16,17,22,21};
const int forward[] = {HIGH,LOW};
const int backward[] = {LOW,HIGH};
const int stop[] = {LOW,LOW};
void initMotor() {
for(int i=0;i(dcMotors)/sizeof(dcMotors[0]);i++)
pinMode(dcMotors[i], OUTPUT);
}
void goForward() {
for(int i=0;i(dcMotors)/sizeof(dcMotors[0]);i++)
digitalWrite(dcMotors[i], forward[i%2]);
}
void goBackward() {
for(int i=0;i(dcMotors)/sizeof(dcMotors[0]);i++)
digitalWrite(dcMotors[i], backward[i%2]);
}
void turnLeft() {
for(int i=0;i<4;i++)
digitalWrite(dcMotors[i], backward[i%2]);
for(int i=4;i<8;i++)
digitalWrite(dcMotors[i], forward[i%2]);
}
void turnRight() {
for(int i=0;i<4;i++)
digitalWrite(dcMotors[i], forward[i%2]);
for(int i=4;i<8;i++)
digitalWrite(dcMotors[i], backward[i%2]);
}
void stopMotor() {
for(int i=0;i(dcMotors)/sizeof(dcMotors[0]);i++)
digitalWrite(dcMotors[i], stop[i%2]);
}
int trigPins[] = {13, 14};
int echoPins[] = {34, 35};
void initHCSR04() {
for(int i=0;i(trigPins)/sizeof(trigPins[0]);i++) {
pinMode(trigPins[i], OUTPUT);
pinMode(echoPins[i], INPUT);
}
}
void getDistanceCM(int &left, int &right) { // 초음파 센서 거리 측정 함수(단위:cm)
int distanceCM[2] = {0,};
for(int i=0;i(trigPins)/sizeof(trigPins[0]);i++) {
digitalWrite(trigPins[i], LOW);
delayMicroseconds(2);
digitalWrite(trigPins[i], HIGH);
delayMicroseconds(10);
digitalWrite(trigPins[i], LOW);
long duration = pulseIn(echoPins[i], HIGH);
distanceCM[i] = duration/58;
}
right = distanceCM[0];
left = distanceCM[1];
}
const int SAFEDISTANCE = 20;
void setup() {
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // brown-out reset disable
Serial.begin(115200);
initMotor();
initHCSR04();
}
const int SAMPLE_COUNT = 200;
void loop() {
// 이 부분을 완성해 주세요!
}
