const int leftdirectionPin = 7; // Pin nummer voor het richtingssignaal
const int rightdirectionPin = 8; // Pin nummer voor het richtingssignaal
const int leftpwmPin = 6; // Pin nummer voor het PWM signaal ~Motor control~
const int rightpwmPin = 9; // Pin nummer voor het PWM signaal
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const int followLeft = 3; // Fototransitor ~Line followers~
const int followRight = 2; // Fototransitor
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
int speaker = 13; // Luidspreker
int led = 12; // Zwaailicht
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
int sensorIR[5] = {
0, 1, 2, 3, 4}; // IR sensors
int threshold = 120; // Minimum storings waarde die hij ziet in open lucht
int sensorValue[5] = {
0, 0, 0, 0, 0};
int valueIR; // Stokage ruimte voor de hoogste IR waarde
int sampleTime = 50; // Tijd nodig om de sensors in te lezen
int i = 0;
void setup(){
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
pinMode(followRight, INPUT); //
pinMode(followLeft, INPUT); //
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
pinMode(leftdirectionPin, OUTPUT); //
pinMode(rightdirectionPin, OUTPUT); // ~Motor control~
pinMode(leftpwmPin, OUTPUT); //
pinMode(rightpwmPin, OUTPUT); //
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
pinMode(speaker, OUTPUT);
pinMode(led, OUTPUT);
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
[s] for(i = 0; i < 4; i++){ // Instellen van de afstandssensoren als input
pinMode(sensorIR[i], INPUT);
} [/s]
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Serial.begin(9600);
buggyStart(); // Melding dat de buggy opgestart is
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~ - Void Loop - Main Programme - ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void loop(){
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=====================================================================
//= Read the IRsensor values =
//=====================================================================
if(i == 5){
i = 0; // Resetten van het array
}
for(i = 0; i < 5; i++){ // Overlopen van de afstandssensoren
sensorValue[i] = analogRead(sensorIR[i]);
if(sensorValue[i] > valueIR){ // Opslagen van de hoogste waarde in de stokage plaats
valueIR = sensorValue[i];
}
if((threshold > valueIR) || (valueIR == 0)){ // Controleren of de huidige waarde hoger is dan de 'Safe' waarde
}
if(sensorValue[i] <= threshold){ // Rij vooruit indien hij in de 'Safe' zone zit
digitalWrite(speaker, LOW);
digitalWrite(led, LOW);
lineFollower();
}
if((sensorValue[i] > threshold) && (sensorValue[i] <= 150)){
digitalWrite(speaker, LOW);
digitalWrite(led, HIGH); // Rij vooruit en flikker met lamp indien hij in 'Warn' zone zit
lineFollower();
delay(100);
digitalWrite(led, LOW);
lineFollower();
delay(100);
}
if((sensorValue[i] >= 150) && (sensorValue[i] < 500)){
digitalWrite(speaker, HIGH);
digitalWrite(led, HIGH);
delay(100); // Loei sirene en flikker met lamp indien hij in 'Danger' zone zit
digitalWrite(led, LOW);
delay(100);
avoidObject();
}
if(sensorValue[i] >= 500){
digitalWrite(speaker, LOW); // Doe niks in de 'Crash' zone
digitalWrite(led, LOW);
}
delay(sampleTime);
Serial.print(i);
Serial.print(": ");
Serial.print("\t");
Serial.print(sensorValue[i]);
Serial.println("");
}
}
//=====================================================================
//= Backriding decision =
//=====================================================================
void avoidObject(){
boolean breakOut = false;
do{
sensorValue[i] = analogRead(sensorIR[i]);
if(sensorValue[i] > 250){
setmotor('R', 0, 0);
setmotor('L', 0, 0);
digitalWrite(speaker, HIGH);
digitalWrite(led, HIGH);
delay(100); // Loei sirene en flikker met lamp indien hij in 'Danger' zone zit
digitalWrite(led, LOW);
delay(100);
}
if(sensorValue[i] <= 120){
breakOut = true;
}
}
while(breakOut == false);
}
//=====================================================================
//= Motor Setting =
//=====================================================================
void setmotor(char motor,int directionM, int speedM){
if(motor == 'R'){ // Controleer of motor rechts moet worden gestuurd
digitalWrite(rightdirectionPin, directionM); // Schrijf naar de richtingspin
if(directionM == 0){ // Controleer toestand vooruit (0) of achteruit (1)
analogWrite(rightpwmPin, speedM); // Schrijf naar de PWM pin
}
else{
speedM = 255 - speedM; // Draai het PWM signaal om indien achteruit (Turning the PWM signal for the L298)
analogWrite(rightpwmPin, speedM); // Schrijf naar de PWM pin
}
}
if(motor == 'L'){ // Controleer of motor links moet worden gestuurd
digitalWrite(leftdirectionPin, directionM); // Schrijf naar de richtingspin
if(directionM == 0){ // Controleer toestand vooruit (0) of achteruit (1)
analogWrite(leftpwmPin, speedM); // Schrijf naar de PWM pin
}
else{
speedM = 255 - speedM; // Draai het PWM signaal om indien achteruit
analogWrite(leftpwmPin, speedM); // Schrijf naar de PWM pin
}
}
}
//=====================================================================
//= Set buggy start =
//=====================================================================
void buggyStart(){
Serial.println("Buggy start");
digitalWrite(speaker, HIGH);
digitalWrite(led, HIGH);
delay(1000);
digitalWrite(led, LOW);
delay(1000);
digitalWrite(speaker, LOW);
}
void lineFollower(){
boolean leftlineState = digitalRead(followLeft);
boolean rightlineState = digitalRead(followRight);
if(leftlineState == true && rightlineState == true){
setmotor('R', 0, 100);
setmotor('L', 0, 0);
Serial.println("left");
}
if(leftlineState == false && rightlineState == false){
setmotor('R', 0, 0);
setmotor('L', 0, 100);
Serial.println("right");
}
if(leftlineState != rightlineState){
setmotor('R', 0, 70);
setmotor('L', 0, 70);
Serial.println("forward");
}
}
//=====================================================================
//= End =
//=====================================================================
In this code I try to maneuver a buggy that is had build, this buggy need to drive alone on a black line and has to stop if there are obstacles in front of it.
This is the code I had written for it but there is a small delay on one of the motors which cause that he won't follow the line correctly. I think this is created by the array which also gives some problems with it. I think the array won't work fine because he see a signal of 500 on one sensor but on the other he see only 100 or something like that and this will cause that he won't sound the speaker.
Can someone check this code for me on faults?