I want to make line follower robot based on tcrt5000 ( as line sensor) and L298N, after that i want to combine the program with (ultrasonic + servo program). when ultrasonic HCSR04 detect object, servo
each sub program (tcrt5000+L298N) and (HCSR04+servo) work normally.
but when i combine the program, there's no output voltage of L298N when tcrt5000.
when i delete "myservo.attach(4); " , there's output voltage from L298N, in this condition hcsr04, tcrt5000, and ultrasonic sensor work normally without servo.
#include <Wire.h>
#include "Adafruit_TCS34725.h"
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_614MS, TCS34725_GAIN_1X);
#include <Servo.h>
Servo myservo;
#include <NewPing.h>
#define TRIGGER_PIN 49
#define ECHO_PIN 51
#define MAX_DISTANCE 200
NewPing sonar(TRIGGER_PIN,ECHO_PIN,MAX_DISTANCE);
int cm;
int IN1=41;
int IN2=43;
int ENA=2; //PWM
int IN3=45;
int IN4=47;
int ENB=3; //PWM
int pwm=200; // change pwm
int LEFT=35;
int CENTER=33;
int RIGHT=31;
int count=0;
int ctrl=0;
void setup(){
myservo.attach(4);
myservo.write(100);
Serial.begin(9600);
pinMode(LEFT,INPUT);
pinMode(CENTER,INPUT);
pinMode(RIGHT,INPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(ENB,OUTPUT);
}
void Forward()
{
digitalWrite(IN1,1);
digitalWrite(IN2,0);
analogWrite(ENA,pwm);
digitalWrite(IN3,0);
digitalWrite(IN4,1);
analogWrite(ENB,pwm);
}
void TurnRight()
{
digitalWrite(IN1,0);
digitalWrite(IN2,1);
analogWrite(ENA,pwm);
digitalWrite(IN3,0);
digitalWrite(IN4,1);
analogWrite(ENB,pwm);
}
void TurnLeft()
{
digitalWrite(IN1,1);
digitalWrite(IN2,0);
analogWrite(ENA,pwm);
digitalWrite(IN3,1);
digitalWrite(IN4,0);
analogWrite(ENB,pwm);
}
void Backward()
{
digitalWrite(IN1,0);
digitalWrite(IN2,1);
analogWrite(ENA,pwm);
digitalWrite(IN3,1);
digitalWrite(IN4,0);
analogWrite(ENB,pwm);
}
void Stop()
{
digitalWrite(IN1,0);
digitalWrite(IN2,0);
digitalWrite(IN3,0);
digitalWrite(IN4,0);
}
void RUNRed()//Tracking
{
Serial.print("R\n");
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==0))
TurnLeft();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==1))
Forward();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==1))
Forward();
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==1))
TurnRight();
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==1))
TurnRight();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==0))
TurnLeft();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==0))
TurnLeft();
}
void RUNGreen()//Tracking
{
Serial.print("G\n");
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==0))
TurnRight();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==1))
Forward();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==1))
Forward();
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==1))
TurnRight();
if((digitalRead(LEFT)==0)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==1))
TurnRight();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==0)&&(digitalRead(RIGHT)==0))
TurnLeft();
if((digitalRead(LEFT)==1)&&(digitalRead(CENTER)==1)&&(digitalRead(RIGHT)==0))
TurnLeft();
}
void CS()//Color Sensor
{
uint16_t r, g, b, c, colorTemp, lux;
tcs.getRawData(&r, &g, &b, &c);
colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);
lux = tcs.calculateLux(r, g, b);
if (((2000<=colorTemp)&&(colorTemp<=5500))and((100<=lux)&&(lux<=650)))//แดง
{
ctrl=1; //detect red light
}
if (((6000<=colorTemp)&&(colorTemp<=10000))and((700<=lux)&&(lux<=1000)))//เขียว
{
ctrl=2; //detect green light
}
}
void loop()
{
CS();
while(ctrl==1)
{
cm = sonar.ping_cm();
RUNRed();
if(cm == 3)
Stop();
myservo.write(-150);
delay(3000);
Backward();
delay(1500);
RUNRed();
}
while(ctrl==2)
{
cm = sonar.ping_cm();
RUNGreen();
if(cm == 3)
Stop();
myservo.write(150);
delay(3000);
myservo.write(100);
delay(3000);
Backward();
delay(1500);
RUNGreen();
}
}