Code:
#include <TimerOne.h>
const int m1 = 10; //declaring the motors
const int m2 = 11;
const int m3 = 12;
const int m4 = 13;
const int s0 = A10; //declaring the outputs of thbe color sensor
const int s1 = A11;
const int s2 = A12;
const int s3 = A13;
const int out = A14;
double previousTime=0; //declarinng of a timer variable
float red=0,green=0,blue=0; //declaring all necessary color variables
float redA[10], greenA[10], blueA[10];
float rav=0,gav=0,bav=0;
int counter=0;
int state=0;
int interval;
void setup() {
// put your setup code here, to run once:
pinMode(m1,OUTPUT); //backwards right wheel
pinMode(m2,OUTPUT); //forward right wheel
pinMode(m3,OUTPUT); //backwards left wheel
pinMode(m4,OUTPUT); //forward right wheel
pinMode(s0, OUTPUT); //setting the ports to input
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
pinMode(48,OUTPUT);
pinMode(51,OUTPUT);
digitalWrite(s0, HIGH); //frequency scaling of 20%
digitalWrite(s1, LOW);
Serial.begin(9600); //starting the Serial monitor on a frequency of 9600
}
void driveForward() //driving forward
{
analogWrite(m1,0);
analogWrite(m2,100);
analogWrite(m3,0);
analogWrite(m4,100);
}
void driveBackwards() //driving backwards
{
analogWrite(m3,100);
analogWrite(m2,0);
analogWrite(m3,100);
analogWrite(m4,0);
}
void turnRight() //turning right
{
analogWrite(m1,900);
analogWrite(m2,0);
analogWrite(m3,0);
analogWrite(m4,900);
}
void turnLeft() //turning left
{
analogWrite(m1,0);
analogWrite(m2,90);
analogWrite(m3,90);
analogWrite(m4,0);
}
void brake() //stopDriving
{
analogWrite(m1,0);
analogWrite(m2,0);
analogWrite(m3,0);
analogWrite(m4,0);
}
void colorSensorStuff()
{
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
red = pulseIn(out, LOW);
//red = map(red, 120,3000,255,0);
Serial.print(counter+1);
Serial.print(".)");
Serial.print("R= ");
Serial.print(red);
Serial.print(" ");
//delay(100);
//GREEN
digitalWrite(s2, HIGH);
digitalWrite(s3, HIGH);
green = pulseIn(out, LOW);
//green = map(green, 120,3000,255,0);
Serial.print("G= ");
Serial.print(green);
Serial.print(" ");
// delay(100);
//BLUE
digitalWrite(s2, LOW);
digitalWrite(s3, HIGH);
blue = pulseIn(out, LOW);
//blue = map(blue, 120,3000,255,0);
Serial.print("B= ");
Serial.print(blue);
Serial.println(" ");
// delay(500);
redA[counter]=red;
greenA[counter]=green;
blueA[counter]=blue;
counter++;
if(counter==9)
{
counter=0;
for(int i=0;i<10;i++)
{
rav=rav+redA[i];
gav=gav+greenA[i];
bav=bav+blueA[i];
}
rav=(rav/10);
gav=(gav/10);
bav=(bav/10);
/*
Serial.print("Average values: ");
Serial.println(" ");
Serial.print("Rav= ");
Serial.print(rav);
Serial.print(" ");
Serial.print("Gav= ");
Serial.print(gav);
Serial.print(" ");
Serial.print("Bav= ");
Serial.print(bav);
Serial.println(" ");
*/
}
}
void loop()
{
// put your main code here, to run repeatedly:
colorSensorStuff();
//delay(1000);
int currentTime=millis();
if( red>=55 && green >= 55 && blue >= 55)
{
Serial.println("Thats black! Follow the black way!");
state=0;
driveForward();
digitalWrite(51,HIGH);
digitalWrite(48,LOW);
}
else
{
interval=currentTime-previousTime;
turnRight();
digitalWrite(48,HIGH);
digitalWrite(51,LOW);
if(interval>=1000)
{
turnLeft();
}
}
}
Habt ihr einen oder mehr Tipps was ich machen kann ? 
Zitieren
Lesezeichen