98 lines
2.2 KiB
C++
98 lines
2.2 KiB
C++
#include <thread>
|
|
#include <chrono>
|
|
#include <sstream>
|
|
#include <iostream>
|
|
|
|
#include "engine.hpp"
|
|
#include "ultrasound_sensor.hpp"
|
|
|
|
using namespace std;
|
|
|
|
enum class drive_mode
|
|
{
|
|
LEFT,
|
|
RIGHT,
|
|
FIND_TURN,
|
|
};
|
|
|
|
int main()
|
|
{
|
|
engine right(
|
|
gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
|
|
gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
|
|
);
|
|
engine left(
|
|
gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
|
|
gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
|
|
);
|
|
gpio bwright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
|
|
gpio bw1(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
|
|
gpio bw2(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
|
|
gpio bwleft(17, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
|
|
|
|
ultrasound_sensor ultrasound(23, 24);
|
|
|
|
left.set_speed(-50);
|
|
right.set_speed(-50);
|
|
while (ultrasound.get_value() > 200000)
|
|
{
|
|
cout << ultrasound.get_value() << endl;
|
|
this_thread::sleep_for(500ms);
|
|
}
|
|
left.set_speed(0);
|
|
right.set_speed(0);
|
|
|
|
/* auto stop_time = chrono::high_resolution_clock::now() + 1min;
|
|
drive_mode mode = drive_mode::LEFT;
|
|
right.set_speed(50);
|
|
left.set_speed(10);
|
|
while (stop_time > chrono::high_resolution_clock::now())
|
|
{
|
|
int found_corners = bwright.get_value() + bwleft.get_value() + bw1.get_value() + bw2.get_value();
|
|
if (mode == drive_mode::FIND_TURN)
|
|
{
|
|
if (!bwright.get_value())
|
|
{
|
|
right.set_speed(100);
|
|
left.set_speed(0);
|
|
mode = drive_mode::LEFT;
|
|
}
|
|
else if (!bwleft.get_value())
|
|
{
|
|
right.set_speed(0);
|
|
left.set_speed(100);
|
|
mode = drive_mode::RIGHT;
|
|
}
|
|
}
|
|
else if (found_corners >= 3 || (bwright.get_value() && bwleft.get_value()))
|
|
{
|
|
left.set_speed(100);
|
|
right.set_speed(100);
|
|
mode = drive_mode::FIND_TURN;
|
|
}
|
|
else if (mode == drive_mode::LEFT)
|
|
{
|
|
if (bwright.get_value())
|
|
{
|
|
left.set_speed(100);
|
|
right.set_speed(0);
|
|
mode = drive_mode::RIGHT;
|
|
}
|
|
}
|
|
else if (mode == drive_mode::RIGHT)
|
|
{
|
|
if (bwleft.get_value())
|
|
{
|
|
left.set_speed(0);
|
|
right.set_speed(100);
|
|
mode = drive_mode::LEFT;
|
|
}
|
|
}
|
|
this_thread::sleep_for(1ms);
|
|
}
|
|
right.set_speed(0);
|
|
left.set_speed(0);
|
|
*/
|
|
return 0;
|
|
}
|