Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- static int LeftSpeed = 5;
- static int RightSpeed = 10;
- static int LeftDir = 6;
- static int RightDir = 9;
- static int LowSpeed = 50;
- static int MediumSpeed = 100;
- static int HighSpeed = 150;
- const int Sensor_0_Pin = 0;
- const int Sensor_1_Pin = 1;
- const int Sensor_2_Pin = 2;
- const int Sensor_3_Pin = 3;
- const int Sensor_4_Pin = 4;
- const int Sensor_5_Pin = 5;
- const int Sensor_6_Pin = 6;
- const int Sensor_7_Pin = 7;
- int sensor0 = 0;
- int sensor1 = 0;
- int sensor2 = 0;
- int sensor3 = 0;
- int sensor4 = 0;
- int sensor5 = 0;
- int sensor6 = 0;
- int sensor7 = 0;
- void ReadSensors() {
- // считываем состояние датчиков
- sensor0 = digitalRead(Sensor_0_Pin);
- sensor1 = digitalRead(Sensor_1_Pin);
- sensor2 = digitalRead(Sensor_2_Pin);
- sensor3 = digitalRead(Sensor_3_Pin);
- sensor4 = digitalRead(Sensor_4_Pin);
- sensor5 = digitalRead(Sensor_5_Pin);
- sensor6 = digitalRead(Sensor_6_Pin);
- sensor7 = digitalRead(Sensor_7_Pin);
- }
- int config() {
- ReadSensors();
- sensor0 = sensor0 * -1;
- sensor1 = sensor1 * -1;
- sensor2 = sensor2 * -1;
- sensor3 = sensor3 * -1;
- int position = sensor0 + sensor1 + sensor2 + sensor4 + sensor5 + sensor6 + sensor7;
- Serial.println(position);
- return position;
- }
- void Backward() {
- // Left Wheel
- analogWrite(LeftSpeed, MediumSpeed);
- digitalWrite(LeftDir, HIGH);
- // Right Wheel
- analogWrite(RightSpeed, MediumSpeed);
- digitalWrite(RightDir, HIGH);
- }
- void Forward() {
- // Left Wheel
- analogWrite(LeftSpeed, MediumSpeed);
- digitalWrite(LeftDir, LOW);
- // Right Wheel
- analogWrite(RightSpeed, MediumSpeed);
- digitalWrite(RightDir, LOW);
- }
- void Stop() {
- // Left Wheel
- analogWrite(LeftSpeed, 0);
- // Right Wheel
- analogWrite(RightSpeed, 0);
- }
- void Right() {
- // Left Wheel
- analogWrite(LeftSpeed, MediumSpeed);
- digitalWrite(LeftDir, HIGH);
- // Right Wheel
- analogWrite(RightSpeed, MediumSpeed);
- digitalWrite(RightDir, LOW);
- }
- void Left() {
- // Left Wheel
- analogWrite(LeftSpeed, MediumSpeed);
- digitalWrite(LeftDir, LOW);
- // Right Wheel
- analogWrite(RightSpeed, MediumSpeed);
- digitalWrite(RightDir, HIGH);
- }
- void RightCircle() {
- // Left Wheel
- analogWrite(LeftSpeed, HighSpeed);
- digitalWrite(LeftDir, LOW);
- // Right Wheel
- analogWrite(RightSpeed, LowSpeed);
- digitalWrite(RightDir, LOW);
- }
- void LeftCircle() {
- // Left Wheel
- analogWrite(LeftSpeed, LowSpeed);
- digitalWrite(LeftDir, LOW);
- // Right Wheel
- analogWrite(RightSpeed, HighSpeed);
- digitalWrite(RightDir, LOW);
- }
- void setup() {
- pinMode(LeftSpeed, OUTPUT);
- pinMode(LeftDir, OUTPUT);
- pinMode(RightDir, OUTPUT);
- pinMode(RightSpeed, OUTPUT);
- }
- void loop() {
- int position = config()
- if (position == 0) {
- Forward();
- }
- else if (position >= 1 && position <= 2) {
- LeftCircle();
- }
- else if (position >= 3) {
- Left();
- }
- else if (position <= -1 && position >= -2) {
- RightCircle();
- }
- else if (position <= -3) {
- Right();
- }
- else {
- Forward();
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement