Hindernisvermeidung funktioniert
This commit is contained in:
199
src/main.cpp
Normal file
199
src/main.cpp
Normal file
@ -0,0 +1,199 @@
|
||||
#include <Arduino.h>
|
||||
#include <Ultrasonic.h>
|
||||
|
||||
//// Adafruit Display
|
||||
#include <SPI.h>
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_GFX.h>
|
||||
#include <Adafruit_SSD1306.h>
|
||||
|
||||
#define OLED_RESET 4
|
||||
Adafruit_SSD1306 display(OLED_RESET);
|
||||
#define NUMFLAKES 10
|
||||
#define XPOS 0
|
||||
#define YPOS 1
|
||||
#define DELTAY 2
|
||||
|
||||
#if (SSD1306_LCDHEIGHT != 64)
|
||||
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
|
||||
#endif
|
||||
|
||||
const int PIN_ENA = 0;
|
||||
const int PIN_ENB = 1;
|
||||
const int PIN_IN1 = 3;
|
||||
const int PIN_IN2 = 5;
|
||||
const int PIN_IN3 = 9;
|
||||
const int PIN_IN4 = 10;
|
||||
|
||||
const int SERVO_PIN = 7;
|
||||
|
||||
Ultrasonic ultrasonic(A2,A3);
|
||||
int distance = 21;
|
||||
|
||||
|
||||
// put your setup code here, to run once:
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
pinMode(PIN_ENA,OUTPUT);
|
||||
pinMode(PIN_ENB, OUTPUT);
|
||||
pinMode(PIN_IN1, OUTPUT);
|
||||
pinMode(PIN_IN2, OUTPUT);
|
||||
pinMode(PIN_IN3, OUTPUT);
|
||||
pinMode(PIN_IN4, OUTPUT);
|
||||
pinMode(SERVO_PIN, OUTPUT);
|
||||
digitalWrite(PIN_ENA, HIGH);
|
||||
digitalWrite(PIN_ENB, HIGH);
|
||||
|
||||
// Display Initialisieren
|
||||
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
|
||||
display.clearDisplay();
|
||||
display.setTextSize(2);
|
||||
display.setTextColor(WHITE);
|
||||
display.setCursor(0,0);
|
||||
display.println("2WD China Bots");
|
||||
//display.setTextSize(3);
|
||||
display.println("HTWK-");
|
||||
display.setTextSize(2);
|
||||
display.println("Leipzig");
|
||||
display.display();
|
||||
}
|
||||
/**
|
||||
* @param leftMotor -255 bis 255, Stärke
|
||||
* @param rightMotor -255 bis 255, Stärke
|
||||
*/
|
||||
void setMotors(int leftMotor, int rightMotor) {
|
||||
if (rightMotor>=0)
|
||||
{
|
||||
analogWrite(PIN_IN2, rightMotor);
|
||||
digitalWrite(PIN_IN1, LOW);
|
||||
} else {
|
||||
analogWrite(PIN_IN2, rightMotor);
|
||||
digitalWrite(PIN_IN1, HIGH);
|
||||
}
|
||||
|
||||
if (leftMotor>=0)
|
||||
{
|
||||
analogWrite(PIN_IN4, leftMotor);
|
||||
digitalWrite(PIN_IN3, LOW);
|
||||
} else {
|
||||
analogWrite(PIN_IN4, leftMotor);
|
||||
digitalWrite(PIN_IN3, HIGH);
|
||||
}
|
||||
}
|
||||
|
||||
void stopMotors() {
|
||||
digitalWrite(PIN_IN1,LOW);
|
||||
digitalWrite(PIN_IN2,LOW);
|
||||
digitalWrite(PIN_IN3,LOW);
|
||||
digitalWrite(PIN_IN4,LOW);
|
||||
digitalWrite(PIN_ENA,LOW);
|
||||
digitalWrite(PIN_ENB,LOW);
|
||||
}
|
||||
|
||||
void enableHigh() {
|
||||
digitalWrite(PIN_ENA,HIGH);
|
||||
digitalWrite(PIN_ENB,HIGH);
|
||||
}
|
||||
|
||||
void setServo(int degrees) {
|
||||
int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 300;
|
||||
int lenMicroSecondsOfPeriod = 20 * 1000; // 20 milliseconds (ms)
|
||||
for (int i = 0; i < 200; ++i) {
|
||||
digitalWrite(SERVO_PIN, HIGH);
|
||||
// Delay for the length of the pulse
|
||||
delayMicroseconds(lenMicroSecondsOfPulse);
|
||||
|
||||
// Turn the voltage low for the remainder of the pulse
|
||||
digitalWrite(SERVO_PIN, LOW);
|
||||
|
||||
// Delay this loop for the remainder of the period so we don't
|
||||
// send the next signal too soon or too late
|
||||
delayMicroseconds(lenMicroSecondsOfPeriod - lenMicroSecondsOfPulse);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int lookLeft(){
|
||||
setServo(180);
|
||||
delay(1000);
|
||||
return ultrasonic.distanceRead();
|
||||
}
|
||||
|
||||
int lookRight(){
|
||||
setServo(0);
|
||||
delay(1000);
|
||||
return ultrasonic.distanceRead();
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
enableHigh();
|
||||
setServo(90);
|
||||
distance = ultrasonic.distanceRead();
|
||||
display.clearDisplay();
|
||||
display.setTextSize(2);
|
||||
display.setTextColor(WHITE);
|
||||
display.setCursor(0,0);
|
||||
display.println("Abstand: ");
|
||||
display.setTextSize(3);
|
||||
display.print(distance);
|
||||
display.setTextSize(2);
|
||||
display.println(" cm");
|
||||
display.display();
|
||||
Serial.println(distance);
|
||||
if (distance<30 && distance>0) {
|
||||
setMotors(0, 0);
|
||||
if(lookLeft()>lookRight()){
|
||||
setServo(90);
|
||||
while(distance<50) {
|
||||
distance = ultrasonic.distanceRead();
|
||||
display.clearDisplay();
|
||||
display.setTextSize(2);
|
||||
display.setTextColor(WHITE);
|
||||
display.setCursor(0,0);
|
||||
display.println("Abstand (turning left): ");
|
||||
display.setTextSize(3);
|
||||
display.print(distance);
|
||||
display.setTextSize(2);
|
||||
display.println(" cm");
|
||||
display.display();
|
||||
Serial.println("Turning left: ");
|
||||
Serial.println(distance);
|
||||
setMotors(-120,0);
|
||||
delay(10);
|
||||
}
|
||||
delay(500);
|
||||
} else {
|
||||
setServo(90);
|
||||
while(distance<50) {
|
||||
distance = ultrasonic.distanceRead();
|
||||
display.clearDisplay();
|
||||
display.setTextSize(2);
|
||||
display.setTextColor(WHITE);
|
||||
display.setCursor(0,0);
|
||||
display.println("Abstand (turning right): ");
|
||||
display.setTextSize(3);
|
||||
display.print(distance);
|
||||
display.setTextSize(2);
|
||||
display.println(" cm");
|
||||
display.display();
|
||||
Serial.println("Turning right: ");
|
||||
Serial.println(distance);
|
||||
setMotors(0,-120);
|
||||
delay(10);
|
||||
}
|
||||
delay(500);
|
||||
}
|
||||
|
||||
} else {
|
||||
if((distance>215)||(distance<=0)){
|
||||
setMotors(255, 255);
|
||||
} else {
|
||||
setMotors(distance+70, distance+70);
|
||||
}
|
||||
}
|
||||
delay(10);
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user