99 lines
1.8 KiB
C++
99 lines
1.8 KiB
C++
/* Define you pins here */
|
|
const int left_wheel_1 = 0;
|
|
const int left_wheel_2 = 1;
|
|
const int right_wheel_1 = 2;
|
|
const int right_wheel_2 = 3;
|
|
|
|
/* The counter */
|
|
int i;
|
|
|
|
/* Array bellow contains all the wheels */
|
|
int Wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
|
|
|
|
void setup() {
|
|
/* Setup pins */
|
|
for (i = 0; i < sizeof(Wheels) - 1; i++) {
|
|
pinMode(Wheels[i], OUTPUT);
|
|
}
|
|
}
|
|
|
|
/* Wheel actions */
|
|
|
|
/* Forward */
|
|
void move_forward_wheel_left() {
|
|
digitalWrite(left_wheel_1, HIGH);
|
|
digitalWrite(left_wheel_2, LOW);
|
|
}
|
|
|
|
void move_forward_wheel_right() {
|
|
digitalWrite(right_wheel_1, HIGH);
|
|
digitalWrite(right_wheel_2, LOW);
|
|
}
|
|
|
|
void move_forward() {
|
|
move_forward_wheel_left();
|
|
move_forward_wheel_right();
|
|
}
|
|
|
|
/* Backward */
|
|
void move_backward_wheel_left() {
|
|
digitalWrite(left_wheel_1, LOW);
|
|
digitalWrite(left_wheel_2, HIGH);
|
|
}
|
|
|
|
void move_backward_wheel_right() {
|
|
digitalWrite(right_wheel_1, LOW);
|
|
digitalWrite(right_wheel_2, HIGH);
|
|
}
|
|
|
|
void move_backward() {
|
|
move_backward_wheel_left();
|
|
move_backward_wheel_right();
|
|
}
|
|
|
|
/* Stops */
|
|
void stop_all() {
|
|
for (i = 0; sizeof(Wheels) - 1; i++) {
|
|
digitalWrite(Wheels[i], LOW);
|
|
}
|
|
}
|
|
|
|
void stop_left() {
|
|
digitalWrite(left_wheel_1, LOW);
|
|
digitalWrite(left_wheel_2, LOW);
|
|
}
|
|
|
|
void stop_right() {
|
|
digitalWrite(right_wheel_1, LOW);
|
|
digitalWrite(right_wheel_2, LOW);
|
|
}
|
|
|
|
/* Turns */
|
|
void turn_left(int delayMS) {
|
|
stop_left();
|
|
move_forward_wheel_right();
|
|
delay(delayMS);
|
|
}
|
|
|
|
void turn_right(int delayMS) {
|
|
stop_right();
|
|
move_forward_wheel_left();
|
|
delay(delayMS);
|
|
}
|
|
|
|
/* Main code */
|
|
void loop() {
|
|
move_forward();
|
|
delay(1000);
|
|
turn_left(500);
|
|
move_backward();
|
|
delay(1000);
|
|
turn_right(500);
|
|
move_forward();
|
|
turn_left(500);
|
|
turn_left(500);
|
|
turn_left(500);
|
|
move_forward();
|
|
delay(1000);
|
|
}
|