Hey everyone I would really appreciate some help, I am building a robot that follows a line along a course the bracket holding the sensors is connected to a servo motor so it turns as well as the robot. I am also using an h bridge and sensor shield. I am having some trouble with my code though. when I run this the servo motor just starts shaking a bunch and the right motor will not move at all, however, if I copy the code into a new workspace and run them separately it works exactly as it is intended to. So I am pretty sure it isn't a hardware problem. I would really appreciate some help with this.
Right now the only thing in the loop is trying to move the right motor but it does nothing.
#include <Servo.h>
Servo Servo1;
float voltageRead0 = A0; //left
float voltageRead1 = A1; //right
float voltageRead2 = A2; //rightmiddle
float voltageRead3 = A3; //leftmiddle
float voltageRead4 = A4; //middle
float voltage0;
float voltage1;
float voltage2;
float voltage3;
float voltage4;
int LED1 = 13;
int LED2 = 12;
int LED3 = 11;
int LED4 = 5;
int LED5 = 8;
int servoPin = 10;
int Angle = 80;
const int ENA = 9;
const int IN1 = 7;
const int IN2 = 6;
const int ENB = 3;
const int IN3 = 4;
const int IN4 = 2;
int sensor1;
int sensor2;
int sensor3;
int sensor4;
int sensor5;
int Change = 300;
int ChangeAngle ( int Angle, float volt0, float volt1, float volt2, float volt3, float volt4) {
if (volt0 > sensor1) {
digitalWrite(LED1, HIGH);
if (Angle < 110) {
Angle = Angle + 4;
return Angle;
}
}
else {
digitalWrite(LED1, LOW);
}
if (volt1 > sensor2) {
digitalWrite(LED2, HIGH);
if (Angle < 110) {
Angle = Angle + 2;
return Angle;
}
}
else {
digitalWrite(LED2, LOW);
}
if (volt2 > sensor3) {
digitalWrite(LED3, HIGH);
}
else {
digitalWrite(LED3, LOW);
}
if (volt3 > sensor4) {
digitalWrite(LED4, HIGH);
if (Angle > 50) {
Angle = Angle - 2;
return Angle;
}
}
else {
digitalWrite(LED4, LOW);
}
if (volt4 > sensor5) {
digitalWrite(LED5, HIGH);
if (Angle > 50) {
Angle = Angle - 4;
return Angle;
}
}
else {
digitalWrite(LED5, LOW);
}
return Angle;
}
void driveForward () {
analogWrite (ENA, 80);
analogWrite (ENB, 80);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void turnRight() {
analogWrite(ENA, 10);
analogWrite(ENB, 80);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void turnLeft() {
analogWrite(ENA, 100);
analogWrite(ENB, 0);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void chooseMovement( int degree) {
if (degree > 88) {
turnRight();
}
else if (degree < 70) {
turnLeft();
}
else {
driveForward();
}
}
void setup() {
Servo1.attach(servoPin);
Serial.begin(9600);
pinMode(LED1, OUTPUT);
pinMode(LED2, OUTPUT);
pinMode(LED3, OUTPUT);
pinMode(LED4, OUTPUT);
pinMode(LED5, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
Serial.print(Angle);
sensor1 = analogRead(voltageRead0) + Change;
sensor2 = analogRead(voltageRead1) + Change;
sensor3 = analogRead(voltageRead2) + Change;
sensor4 = analogRead(voltageRead3) + Change;
sensor5 = analogRead(voltageRead4) + Change;
digitalWrite(LED1, HIGH);
delay(200);
digitalWrite(LED2, HIGH);
delay(200);
digitalWrite(LED3, HIGH);
delay(200);
digitalWrite(LED4, HIGH);
delay(200);
digitalWrite(LED5, HIGH);
delay(200);
digitalWrite(LED1, LOW);
delay(200);
digitalWrite(LED2, LOW);
delay(200);
digitalWrite(LED3, LOW);
delay(200);
digitalWrite(LED4, LOW);
delay(200);
digitalWrite(LED5, LOW);
delay(200);
}
void loop() {
voltage0 = analogRead(voltageRead0);
voltage1 = analogRead(voltageRead1);
voltage2 = analogRead(voltageRead2);
voltage3 = analogRead(voltageRead3);
voltage4 = analogRead(voltageRead4);
Serial.print(voltage0, 2);
Serial.print("\t");
Serial.print(voltage1, 2);
Serial.print("\t");
Serial.print(voltage2, 2);
Serial.print("\t");
Serial.print(voltage3, 2);
Serial.print("\t");
Serial.print(voltage4, 2);
Serial.print("\t");
Serial.println(Angle);
analogWrite(ENA, 100);
analogWrite(ENB, 0);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
