COLORPAL Interfacing Problem.
AliRady
Posts: 3
Me and my friends have built a linefollowing robot, it is suppose to also avoid obstacles and identify the color of the obstacles using the colorpal. However it seems that when the colorpal is interfaced with the linefollower code, one of the codes has to stop working depending on which one was placed first. We need to solve this issue badly. Either the linefollower stops "linefollowing" or the colorpal doesn't initialize properly and its LED is off.
Any suggestions why?
We are using ARDUINO UNO.
Any suggestions why?
// this code includes five sensors [R, A, B, C, L];
#include <QTRSensors.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#define NUM_SENSORS 5 // number of sensors used
// create an object for three QTR-xA sensors on analog inputs 2, 3, and 4
// L,A,B,C,R
QTRSensorsAnalog qtra((unsigned char[]) {
4,2,1,0,3}
, NUM_SENSORS);
unsigned int sensorValues[NUM_SENSORS];
int thresholdArray[NUM_SENSORS];
int bV[NUM_SENSORS];
Servo rservo;
Servo lservo;
boolean wasTurning = false;
int motorSpeed;
const int led = 13;
const int ledStriaght = 2;
// LEDS: STLeft, TLeft, Striaght, TRight, STRight
const int touchSensor = 5;
int ledsDebugging[] ={
2, 4, 13, 7, 8};
//
COLORPAL
const int sio = 2; // ColorPAL connected to pin 2
const int unused = 255; // Non-existant pin # for SoftwareSerial
const int sioBaud = 4800;
const int waitDelay = 100;
SoftwareSerial serin(sio, unused);
SoftwareSerial serout(unused, sio);
// Received RGB values from ColorPAL
int red;
int grn;
int blu;
//
void setup()
{
Serial.begin(9600);
rservo.attach(9);
lservo.attach(10);
for(int i = 0; i<5; i++){
pinMode(ledsDebugging, OUTPUT);
}
delay(500);
turnOnLed(2);
//pinMode(13, OUTPUT);
//pinMode(ledStriaght, OUTPUT);
//digitalWrite(ledStriaght, LOW);
//digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
Serial.println("Starting calibration");
moveF(0);
for (int i = 0; i < 250; i++) // make the calibration take about 10 seconds
{
qtra.calibrate(); // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
}
Serial.println("End of calibration");
//digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration
turnOnLed(-1);
delay(200);
for(int i = 0; i<NUM_SENSORS;i++){
thresholdArray = ((qtra.calibratedMaximumOn+qtra.calibratedMinimumOn)/2)-170;
}
//
COLORPAL
Serial.println("1");
reset(); // Send reset to ColorPal
Serial.println("2");
serout.begin(sioBaud);
Serial.println("3");
pinMode(sio, OUTPUT);
Serial.println("4");
serout.print("= (00 $ m) !"); // Loop print values, see ColorPAL documentation
Serial.println("5");
serout.end(); // Discontinue serial port for transmitting
Serial.println("6");
serin.begin(sioBaud); // Set up serial port for receiving
Serial.println("7");
pinMode(sio, INPUT);
pinMode(touchSensor, INPUT);
//
}
void loop(){
unsigned int position = qtra.readLine(sensorValues);
for(int i = 0; i < NUM_SENSORS; i++){
if(sensorValues > thresholdArray){
bV = 1;
}
else{
bV= 0;
}
}
String state = "";
for(int i = 0; i< NUM_SENSORS; i++){
state += bV;
}
Serial.print("Readings: ");
Serial.println( state);
if(((bV[4] == 1 && (bV[1] == 1 || bV[3] == 1)) || (bV[0] == 0 && bV[1] == 0 && bV[2] == 0 && bV[3] == 0 && bV[4] == 1)) && !wasTurning){
Serial.println("State: Right Turn");
turnOnLed(-1);
digitalWrite(ledsDebugging[4], HIGH);
digitalWrite(ledsDebugging[3], HIGH);
turnRight();
wasTurning = true;
}
else if(bV[1] == 0 && bV[2] == 1 && bV[3] == 1){
Serial.println("State: Tending to the Left");
turnOnLed(1);
tendToRight();
wasTurning = false;
}
else if( bV[1] == 0 && bV[2] == 0 && bV[3] == 1){
Serial.println("State: Tending to the Left, SHARPLY!!");
turnOnLed(0);
sTendToRight();
wasTurning = false;
}
else if(bV[1] == 1 && bV[2] == 1 && bV[3] == 0){
Serial.println("State: Tending to the Right");
turnOnLed(3);
tendToLeft();
wasTurning = false;
}
else if (bV[1] == 1 && bV[2] == 0 && bV[3] == 0){
Serial.println("State: Tending to the Right, SHARPLY!!");
turnOnLed(4);
sTendToLeft();
wasTurning = false;
}
else if(bV[0] == 0 && bV[1] == 0 && bV[2] == 0 && bV[3] == 0 && bV[4] == 0){
Serial.println("State: End of line");
turnOnLed(-1);
while(bV[2] == 1){
updateValues();
Serial.println("turning left 1 to 0");
rotate(-15);
}
while(bV[2] == 0){
updateValues();
Serial.println("turning left 0 to 1");
rotate(-15);
}
wasTurning = true;
;
}
else if(bV[0] == 1 && bV[1] == 1 && bV[2] == 1 && bV[3] == 1 && bV[4] == 1){
Serial.println("State: +");
turnOnLed(-1);
turnRight();
wasTurning = true;
}
if(((bV[0] == 1 && (bV[1] == 1 || bV[3] == 1) && bV[2] == 0 && bV[4] == 0) || (bV[0] == 1 && bV[1] == 0 && bV[2] == 0 && bV[3] == 0 && bV[4] == 0)) && !wasTurning){
Serial.println("State: Left Turn");
turnOnLed(-1);
digitalWrite(ledsDebugging[0], HIGH);
digitalWrite(ledsDebugging[1], HIGH);
turnLeft();
wasTurning = true;
}
else if(bV[1] == 1 && bV[2] == 1 && bV[3] == 1){
Serial.println("State: Straight");
turnOnLed(2);
moveF(7);
wasTurning = false;
}
else if((bV[0] == 0 && bV[1] == 1 && bV[2] == 0 && bV[3] == 1 && bV[4] == 0)||
(bV[0] == 0 && bV[1] == 0 && bV[2] == 1 && bV[3] == 0 && bV[4] == 0)||
(bV[0] == 0 && bV[1] == 0 && bV[2] == 1 && bV[3] == 0 && bV[4] == 1)||
(bV[0] == 1 && bV[1] == 0 && bV[2] == 1 && bV[3] == 0 && bV[4] == 0)){
Serial.println("Weird position");
moveF(7);
wasTurning = false;
}
else{
updateValues();
}
}
void turnRight(){
while(bV[2] == 1){
updateValues();
Serial.println("turning right 1 to 0");
rotate(15);
}
while(bV[2] == 0){
updateValues();
Serial.println("turning right 0 to 1");
rotate(15);
}
}
void turnLeft(){
while(bV[2] == 1){
updateValues();
Serial.println("turning left 1 to 0");
rotate(-10);
}
while(bV[2] == 0){
updateValues();
Serial.println("turning left 0 to 1");
rservo.write(90+(-20));
lservo.write(90+(-10));
}
}
void rotate(int s)
{
rservo.write(90+s);
lservo.write(90+s);
}
void moveF(int speed_v){
motorSpeed = speed_v;
rservo.write(90-speed_v);
lservo.write(90+speed_v);
}
void tendToRight(){
rservo.write(90-motorSpeed-1); //same value 2
lservo.write(90+motorSpeed+4); //5
}
void tendToLeft(){
rservo.write(90-motorSpeed-4);
lservo.write(90+motorSpeed+1); //same value
}
void sTendToRight(){
rservo.write(90-motorSpeed-2); //same value //4
lservo.write(90+motorSpeed+7); //10
}
void sTendToLeft(){
rservo.write(90-motorSpeed-7);// 10
lservo.write(90+motorSpeed+2); //same value // 1
}
void updateValues(){
unsigned int position = qtra.readLine(sensorValues);
for(int i = 0; i < NUM_SENSORS; i++){
if(sensorValues > thresholdArray){
bV = 1;
}
else{
bV= 0;
}
}
}
void printState(){
String state = "";
printState();
for(int i = 0; i< NUM_SENSORS; i++){
state += bV;
}
Serial.print("Readings: ");
Serial.println( state);
}
void turnOnLed(int ledIndex){
for (int i = 0; i < 5; i++){
digitalWrite(ledsDebugging, LOW);
}
if(ledIndex != -1){ // all leds off
digitalWrite(ledsDebugging[ledIndex], HIGH);
}
}
void rotateOA(int angle)
{
int time = 0.00285*angle*1000;
Serial.println("turning");
int currentTime = millis();
int nextTime = millis();
int spd;
if(angle >0)
{
spd = 40;
}
else
{
spd = -40;
}
while(currentTime + time > nextTime)
{
lservo.write(90+spd);
rservo.write(90+spd);
nextTime = millis();
}
lservo.write(90);
rservo.write(90);
}
void moveDistance(int distance)
{
//distance in cms
int time = (distance/24.5)*1000;
int currentTime = millis();
int nextTime = millis();
int spd;
if(distance > 0)
{
spd = 40;
}
else
{
spd = -40;
}
while(currentTime + time > nextTime)
{
moveF(spd);
nextTime = millis();
}
moveF(0);
}
void checkObst(String c)
{
if( digitalRead(touchSensor) == 1){
//moveF(motorSpeed);
//moveDistance(1);
Serial.println("Switch on");
if(c.equals("black")){
moveF(0);
}
else
{
if(c.equals("red"))
{
Serial.println("avoiding");
rotateOA(90);
moveDistance(15);
rotateOA(-90);
moveDistance(10);
rotateOA(-90);
moveDistance(15);
rotateOA(90);
}
else
{
if(c.equals("green"))
{
rotateOA(90);
moveDistance(5);
moveDistance(-5);
rotateOA(-90);
}
/**
* else
* if(color.equals("idk") or color.equals("white"))
* {
* checkObst();
* }
*/
}
}
}
}
String readData() {
char buffer[32];
String color;
if (serin.available() > 0) {
// Wait for a $ character, then read three 3 digit hex numbers
buffer[0] = serin.read();
if (buffer[0] == '$') {
for(int i = 0; i < 9; i++) {
while (serin.available() == 0); // Wait for next input character
buffer = serin.read();
if (buffer == '$') // Return early if $ character encountered
return "null";
}
color = checkColor(buffer);
delay(10);
}
}
return color;
}
// Parse the hex data into integers
String checkColor(char * data) {
sscanf (data, "%3x%3x%3x", &red, &grn, &blu);
if(red < 5 && grn < 5 && blu < 5)
{
return "white";
}
if(red < 40 && grn < 40 && blu < 40)
{
return "black";
}
if(red > blu && red > grn)
{
return "red";
}
if(grn > blu && grn > red)
{
return "green";
}
if(blu > red && blu > grn)
{
return "blue";
}
return "?????? Idk ???????";
}
void reset() {
delay(200);
pinMode(sio, OUTPUT);
digitalWrite(sio, LOW);
pinMode(sio, INPUT);
while (digitalRead(sio) != HIGH);
pinMode(sio, OUTPUT);
digitalWrite(sio, LOW);
delay(80);
pinMode(sio, INPUT);
delay(waitDelay);
}
We are using ARDUINO UNO.
Comments
-Phil
Just for future reference,
Also, even though I'm the designer of the ColorPAL, I'm not a C guru. Hopefully, someone more qualified will be able to step in to help.
Best regards,
-Phil
[COLOR=#333333][FONT=Arial][I]int ledsDebugging[] ={[/I][/FONT][/COLOR] [COLOR=#333333][FONT=Arial][I]2, 4, 13, 7, 8};[/I][/FONT][/COLOR] [COLOR=#333333][FONT=Arial][I]//--------------------------COLORPAL----------------------------------------------[/I][/FONT][/COLOR] [COLOR=#333333][FONT=Arial][I]const int sio = 2 [/I][/FONT][/COLOR]You're reusing the same pin D2 for the ColorPAL and the LED. I didn't bother to check if your LED code is actually being used, but if it is, that would certainly cause issues.
More ideas below if that doesn't fix it.
Since you're using the ColorPAL at 4800 baud (not the "ideal" speed, but can work), you should try connecting the ColorPAL to pins 0 and 1 and use hardware serial. That's a baud rate directly supported by the Serial library. It does mean losing your Serial Monitor debug, but you can implement other techniques for simple feedback. In any case, when the robot is running in autonomous mode, it won't use hardware serial anyway. Hardware serial doesn't use other timer hardware or interrupts in the AVR chip that's not reserved for itself.
A telltale issue may be that because you're using the analog version of the sensors, there's a lot of noise generated by the AVR's ADC each time a sensor is read. This is only a hunch, and it may not be the cause, but the ColorPAL is pretty critical of its data communications. You might try temporarily changing the code to read just a single sensor. You can't line follow with that, but it could point to a noise problem.
My eyes glazed over because of the unformatted code you posted (see Phil's pointer on how to post properly formatted code), but one approach to try would be to activate then reset the ColorPAL except when you're using it. Do that only if you're not actively reading the line sensors. It is unlikely your robot will do both functions at the same time anyway.
Finally, a word about the Arduino, especially the Uno: While it's a great little microcontroller, it is actually quite limited in what it can do. Just because you connect stuff to it, doesn't mean it can process all of it at the same time. The more complex your system, the more you need to get creative in dealing with the hardware limitations.