Here is my code:
#include <TextFinder.h>
#include <Servo.h>
Servo myservo;
TextFinder finder(Serial);
const int NUMBER_OF_FIELDS = 2;
int values[NUMBER_OF_FIELDS];
const int ledPin = 3;
const int ledPin2 = 11;
const int ledPin3 = 13;
const int leftBumperPin = 10;
const int rightBumperPin = 4;
const int alwaysON = 2;
const int analogInPin = A1;
int E1 = 6;
int E2 = 5;
int D1 = 8;
int D2 = 7;
int pos = 80;
int sensorValue;
int outputValue;
int actual;
int x = 0;
int val = 0;
int leftspeed = 230; //255 is maximum speed
int rightspeed = 230;
int rightspeed_1 = 100;
int leftspeed_1 = 100;
void setup()
{
int i;
for(i=5;i<=8;i++);
pinMode(i, OUTPUT);
pinMode(ledPin2, OUTPUT);
Serial.begin(9600);
myservo.attach(9);
pinMode(leftBumperPin, INPUT);
pinMode(rightBumperPin, INPUT);
pinMode(alwaysON, OUTPUT);
}
void loop()
{
int fieldIndex = 0;
if(finder.find("v:") )
{
while(fieldIndex < NUMBER_OF_FIELDS)
values[fieldIndex++] = finder.getValue();
Serial.println("All fields received:");
for(fieldIndex=0; fieldIndex < NUMBER_OF_FIELDS; fieldIndex++)
Serial.println(values[fieldIndex]);
//**********************************************************************************************
if(values[0] == 1)
{
analogWrite(ledPin2,0);
Serial.println("GREEN LIGHT");
if(values[1]<=0) analogWrite(ledPin,0);
else if(values[1]==1) analogWrite(ledPin,255/10);
else if(values[1]==2) analogWrite(ledPin,255/9);
else if(values[1]==3) analogWrite(ledPin,255/8);
else if(values[1]==4) analogWrite(ledPin,255/7);
else if(values[1]==5) analogWrite(ledPin,255/6);
else if(values[1]==6) analogWrite(ledPin,255/5);
else if(values[1]==7) analogWrite(ledPin,255/4);
else if(values[1]==8) analogWrite(ledPin,255/3);
else if(values[1]==9) analogWrite(ledPin,255/2);
else if(values[1]>=10) analogWrite(ledPin,255);
}
//**********************************************************************************************
else if(values[0] == 2)
{
digitalWrite(ledPin2,LOW);
if(values[1]==0) myservo.write(1);
else if(values[1]==1) myservo.write(18);
else if(values[1]==2) myservo.write(36);
else if(values[1]==3) myservo.write(54);
else if(values[1]==4) myservo.write(72);
else if(values[1]==5) myservo.write(90);
else if(values[1]==6) myservo.write(108);
else if(values[1]==7) myservo.write(126);
else if(values[1]==8) myservo.write(144);
else if(values[1]==9) myservo.write(162);
else if(values[1]==10) myservo.write(179);
}
//**********************************************************************************************
else if(values[0] == 3)
{
while(values[1] != 0)
{
digitalWrite(alwaysON, HIGH);
while(x==0)
{
delay(1000);
x++;
}
while(pos < 130)
{
pos += 3;
myservo.write(pos);
delay(15);
actual = LOW;
sensorValue = analogRead(analogInPin);
outputValue = map(sensorValue, 98, 500, 0, 180);
Serial.print("outputValue = ");
Serial.println(outputValue);
if (outputValue >= 50)
{
actual = HIGH;
Serial.println("actual is high");
delay(100);
}
digitalWrite(ledPin3, actual);
int val_left = digitalRead(leftBumperPin);
int val_right = digitalRead(rightBumperPin);
if (actual == HIGH)
{
digitalWrite(ledPin3, HIGH);
Serial.println("problem_1");
Serial.println("problem_1.1");
right (180,180);
delay(3000);
}
else if (val_left == LOW)
{
Serial.println("problem_2");
digitalWrite(ledPin3, HIGH);
Serial.println("problem_2.1");
cw_back (100,200);
delay(3200);
}
else if(val_right == LOW)
{
Serial.println("problem_3");
digitalWrite(ledPin3, HIGH);
Serial.println("problem_3.1");
ccw_back (200,100);
delay(3200);
}
else if(val_right == HIGH && val_left == HIGH && actual == LOW)
{
digitalWrite(ledPin3, LOW);
forward (230,230);
delay(100);
}
}
while(pos > 55)
{
pos -= 3;
myservo.write(pos);
delay(15);
actual = LOW;
sensorValue = analogRead(analogInPin);
outputValue = map(sensorValue, 98, 500, 0, 180);
Serial.print("outputValue = ");
Serial.println(outputValue);
if (outputValue >= 50)
{
actual = HIGH;
Serial.println("actual is high");
delay(100);
}
digitalWrite(ledPin3, actual);
{
int val_left = digitalRead(leftBumperPin);
int val_right = digitalRead(rightBumperPin);
if (actual == HIGH)
{
Serial.println("problem_1");
digitalWrite(ledPin3, HIGH);
Serial.println("problem_1.1");
left (180,180);
delay(3000);
}
else if (val_left == LOW)
{
Serial.println("problem_2");
digitalWrite(ledPin3, HIGH);
Serial.println("problem_2.1");
cw_back (100,200);
delay(3200);
}
else if(val_right == LOW)
{
Serial.println("problem_3");
digitalWrite(ledPin3, HIGH);
Serial.println("problem_3.1");
ccw_back (200,100);
delay(3200);
}
else if(val_right == HIGH && val_left == HIGH && actual == LOW)
{
digitalWrite(ledPin3, LOW);
forward (230,230);
delay(100);
}}
}}
}
//**********************************************************************************************
else if(values[0] == 4)
{
while(val != '.')
{
while (Serial.available() > 0)
{
char val = Serial.read();
Serial.print("val = " );
Serial.println(val);
//delay(500);
switch(val) // Perform an action depending on the command
{
case 'w'://Move Forward
forward (leftspeed,rightspeed);
delay(100);
break;
case 's'://Move Backwards
backwards (leftspeed,rightspeed);
delay(100);
break;
case 'a'://Turn Left
left (0,rightspeed);
delay(100);
break;
case 'd'://Turn Right
right (leftspeed,0);
delay(100);
break;
case 'q':
ccw_front (leftspeed_1,rightspeed);
delay(100);
break;
case 'e':
cw_front (leftspeed,rightspeed_1);
delay(100);
break;
case 'x':
ccw_back (leftspeed,rightspeed_1);
delay(100);
break;
case 'z':
cw_back (leftspeed_1,rightspeed);
delay(100);
break;
case '/':
halt (0,0);
delay(100);
break;
}}}}
//**********************************************************************************************
else
{
digitalWrite(ledPin2,HIGH);
digitalWrite(ledPin,LOW);
Serial.println("RED LIGHT");
}
}
}
//Subroutine:
void halt (char a,char b) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}
void forward (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,LOW);
analogWrite (E2,b);
digitalWrite(D2,LOW);
}
void left (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,HIGH);
analogWrite (E2,b);
digitalWrite(D2,LOW);
}
void right (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,LOW);
analogWrite (E2,b);
digitalWrite(D2,HIGH);
}
void ccw_front (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,LOW);
analogWrite (E2,b);
digitalWrite(D2,LOW);
}
void cw_front (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,LOW);
analogWrite (E2,b);
digitalWrite(D2,LOW);
}
void ccw_back (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,HIGH);
analogWrite (E2,b);
digitalWrite(D2,HIGH);
}
void cw_back (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,HIGH);
analogWrite (E2,b);
digitalWrite(D2,HIGH);
}
void backwards (char a,char b)
{
analogWrite (E1,a);
digitalWrite(D1,HIGH);
analogWrite (E2,b);
digitalWrite(D2,HIGH);
}
The error message only comes up when there is a 3 or 4 in the first field of the serial message sent to the board. The weird thing is that it works until the board gets an input from one of the sensors. I know this isn't a very good explanation of the problem but it's complicated to explain. Let me know if there's any additional information you need that would help you help me.
Thanks in advance!