Hallo,
Kann sich bitte mal jemand diesen code ansehen und mir sagen ob das wirklich so funktioniert?
Ich habe vor ein RC Modellbau von mir per Arduino uno autonom fahren zu lassen.
Ich habe einen code gefunden dieser arbeitet aber mit 2 motoren und ich muss so ein motorshield usw kaufen. Das will ich nicht.
Ich hab also die fernsteuerung auseinander genommen und an den Potis für den Motor und fürs Lenken jeweils einen servo gebaut.
Im Grunde ist es also so dass das Arduino eigentlich 2 Servos stuert und einen Ultraschalsensor hat.
das orginal ist von hier http://www.riedberg-edu.eu/access-dam/5c385c60ea1e575b0624eddf/1/inline/5c385c60ea1e575b0624edd7GK_KIT_003Manual.pdf
/*******************************************************************************************************
Final Robot Program
Created 7/11/16
*******************************************************************************************************/
//----------------------------------------------------- Start
//----------------------------------------------------- Initialization
#define trig 3
#define echo 2
int thresholddistance = 30;
long duration;
float forwarddistance;
float distancefront;
float distanceArr[6];
#include <Servo.h>
#define pinServo 4
//----------------------------------------------------- Initialization: Setting up Servo Angles
int servoArray[6] = {0, 25, 50, 110, 145, 180};
Servo myServo;
//----------------------------------------------------- Initialization: Motors
int servoTurn[8] = {0, 25, 50, 90, 110, 145, 180};
int servoGas[9] = {0, 25, 50, 90, 110, 145, 180};
//----------------------------------------------------- Initialization: Setup Module
void setup() {
Serial.begin(9600);
myServo.attach(pinServo);
//Setup Motors
for (int i = 90; i < 2; i++)
{
pinMode(servoTurn[i], OUTPUT);
pinMode(servoGas[i], OUTPUT);
}
//Setup Sensor
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
}
//----------------------------------------------------- Initialization: Motor Module
// Following Module is for the Robot to brake
void brake()
{
digitalWrite(servoTurn[90], LOW);
digitalWrite(servoGas[90], LOW);
}
// Following Module is for the Robot to go forward
void drive_forward()
{
digitalWrite(servoGas[90], LOW);
delayMicroseconds(400);
digitalWrite(servoGas[180], HIGH);
delayMicroseconds(1000 - 400);
digitalWrite(servoGas[90], LOW);
delayMicroseconds(1000 - 400);
digitalWrite(servoGas[180], HIGH);
delayMicroseconds(400);
}
// Following Module is for the Robot to slightly go towards left
void slight_left()
{
digitalWrite(servoGas[90], LOW);
digitalWrite(servoGas[180], HIGH);
digitalWrite(servoTurn[90], LOW);
digitalWrite(servoTurn[135], HIGH);
delay(600);
}
// Following Module is for the Robot to slightly go towards right
void slight_right()
{
digitalWrite(servoGas[90], LOW);
digitalWrite(servoGas[180], HIGH);
digitalWrite(servoTurn[90], LOW);
digitalWrite(servoTurn[45], HIGH);
delay(600);
}
// Following Module is for the Robot to turn left
void turn_left()
{
digitalWrite(servoGas[90], LOW);
digitalWrite(servoGas[180], HIGH);
digitalWrite(servoTurn[90], LOW);
digitalWrite(servoTurn[180], HIGH);
delay(900);
}
// Following Module is for the Robot to turn right
void turn_right()
{
digitalWrite(servoGas[90], LOW);
digitalWrite(servoGas[180], HIGH);
digitalWrite(servoTurn[90], LOW);
digitalWrite(servoTurn[0], HIGH);
delay(900);
}
// Following Module is for the Robot to go forward
void drive_backward()
{
digitalWrite(servoGas[90], LOW);
delayMicroseconds(400);
digitalWrite(servoGas[0], HIGH);
delayMicroseconds(1000 - 400);
digitalWrite(servoGas[90], LOW);
delayMicroseconds(1000 - 400);
digitalWrite(servoGas[0], HIGH);
delayMicroseconds(400);
}
//----------------------------------------------------- Sight Forward
float distance(int angle) {
myServo.write(angle);
delay(500);
digitalWrite(trig, LOW);
delayMicroseconds(20);
digitalWrite(trig, HIGH);
delayMicroseconds(20);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
forwarddistance = duration / 58;
return forwarddistance;
}
//----------------------------------------------------- Sensor Array Module
void scan_around() {
for (int i = 0; i < 6; i++) {
myServo.write(servoArray[i]);
delay(100);
digitalWrite(trig, LOW);
delayMicroseconds(20);
digitalWrite(trig, HIGH);
delayMicroseconds(20);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
distanceArr[i] = duration / 58;
Serial.print("distanceArr[");
Serial.print(i);
Serial.print("] = ");
Serial.println(distanceArr[i]);
delay(100);
}
}
//----------------------------------------------------- Loop Module
void loop() {
myServo.write(75);
distancefront = distance(75);
Serial.println(distancefront);
// Serial.println(thresholddistance);
if ((distancefront > thresholddistance) && (distancefront < 1000))
{
drive_forward();
delay(1000);
Serial.println("1");
}
else {
brake();
scan_around();
if ((distanceArr[1] > 1000) && (distanceArr[2] > 1000) && (distanceArr[3] > 1000) && (distanceArr[4] > 1000) && (distanceArr[5] > 1000) && (distanceArr[6] > 1000))
{
return;
Serial.println("Restarted the Loop");
delay(100);
}
else if (((distanceArr[1]) > 30.0) && ((distanceArr[2]) > 30.0))
{
slight_right();
drive_forward();
delay(1000);
Serial.println("2");
}
else if (((distanceArr[0]) > 30.0) && ((distanceArr[1]) > 30.0))
{
turn_right();
drive_forward();
delay(1000);
Serial.println("3");
}
else if (((distanceArr[3]) > 30.0) && ((distanceArr[4]) > 30.0))
{
slight_left();
drive_forward();
delay(1000);
Serial.println("4");
}
else if (((distanceArr[4]) > 30.0) && ((distanceArr[5]) > 30.0))
{
turn_left();
drive_forward();
delay(1000);
Serial.println("5");
}
else
{
drive_backward();
brake();
delay(1000);
slight_left();
drive_forward();
delay(1000);
Serial.println("6");
}
}
// delay(100);
}
Ich hoffe jemand kann mir sagen ob das so passt und ob ich was ändern muss!
Lg