Hello, I am building a robot using arduino uno, a motor shield and the sensor colorPAL. I've got colorPAL demo code from http://learn.parallax.com/colorpal-arduino-demo, and it worked properly. Then I made a code for the motors, and robot went forward. All ok till now.
Then I wrote an if statement, using data obtained from colorPAL. Unfortunately, motorE, connected in port M1 of motor shield, stopped working. Could someone explain for me why if statement is causing that trouble for M1 port? Motor shield uses digital pins 3,4,5,6,7,8,9,10,11,12 of arduino board.
Here's the code:
#include <AFMotor.h>
AF_DCMotor motorE(1);
AF_DCMotor motorD(2);
#include <SoftwareSerial.h>
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 = 200;
// Received RGB values from ColorPAL
int red;
int grn;
int blu;
// Set up two software serials on the same pin.
SoftwareSerial serin(sio, unused);
SoftwareSerial serout(unused, sio);
void setup() {
Serial.begin(9600);
reset(); // Send reset to ColorPal
serout.begin(sioBaud);
pinMode(sio, OUTPUT);
serout.print("= (00 $ m) !"); // Loop print values, see ColorPAL documentation
serout.end(); // Discontinue serial port for transmitting
serin.begin(sioBaud); // Set up serial port for receiving
pinMode(sio, INPUT);
}
void loop() {
readData();
motorD.run(FORWARD);
motorE.run(FORWARD);
motorD.setSpeed(180);
motorE.setSpeed(180);
if((red>130)&&(red<150)&&(grn>110)&&(grn<130)&&(blu>150)&&(blu<170)){
motorD.run(BACKWARD);
motorE.run(BACKWARD);
motorD.setSpeed(180);
motorE.setSpeed(180);
delay(2000);}
}
// Reset ColorPAL; see ColorPAL documentation for sequence
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);
}
void readData() {
char buffer[32];
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;*
* }*
* parseAndPrint(buffer);*
* delay(10);*
* }*
* }*
}
// Parse the hex data into integers
void parseAndPrint(char * data) {
* sscanf (data, "%3x%3x%3x", &red, &grn, &blu);*
* char buffer[32];*
* sprintf(buffer, "R%4.4d G%4.4d B%4.4d", red, grn, blu);*
* Serial.println(buffer);}*