I had a question about a project I am working on. As of now, it is using the EasyVR shield http://download.tigal.com/veear/EasyVR_User_Manual_3.4.2.pdf to control motor and servo motion through an Arduino. The process works as expected, when a spoken command is given the motors and servos respond. My code is attached below. When the program begins, it waits for a word in group 0 to start. My starting word is "Arduino", so until it hears the starting word the loop will timeout and keep asking in the terminal for a spoken word from group 0 (which is expected).
What I am trying to debug is a motor twitch that takes place every time the loop times out. It is a small backwards motion that is magnified when I incorporate an Xbee for wireless testing. Through research, I think I have narrowed the problem down to interrupts from header files but do not have enough background to diagnose exactly where or to fix. Can anyone verify this/give recommendations on how to fix?
Thanks in advance,
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Servo.h> //Reference the Servo Library
SoftwareSerial port(12,13);
#else // Arduino 0022 - use modified NewSoftSerial
#include <WProgram.h>
// #include <NewSoftSerial.h>
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Servo.h> //Reference the Servo Library
// SoftwareSerial port(12,13);
//NewSoftSerial port(12,13);
#endif
#include <EasyVR.h>
EasyVR easyvr(port);
//Groups and Commands
enum Groups
{
GROUP_0 = 0,
GROUP_4 = 4,
};
enum Group0
{
G0_ARDUINO = 0,
};
enum Group4
{
G4_RIGHT = 0,
G4_LEFT = 1,
G4_FORWARD = 2,
G4_BACKWARD = 3,
G4_HALT = 4,
};
EasyVRBridge bridge;
int8_t group, idx;
///////My declarations
Servo myservo; // create servo object to control the steering servo
Servo mymotor; // create servo object to control the driving motor
int pos = 0; // variable to store the servo position
int val; // variable to hold motor speed/orientation
////////////////////////////////
void setup()
{
// bridge mode?
if (bridge.check())
{
cli();
bridge.loop(0, 1, 12, 13);
}
// run normally
Serial.begin(9600);
Serial.println("X-CTU Terminal Awake!");
port.begin(9600);
myservo.attach(9); // attaches the servo on pin 9 to the servo object
mymotor.attach(6); // attaches the motor on pin 6 to the servo object
if (!easyvr.detect())
{
Serial.println("EasyVR not detected!");
for (;;);
}
easyvr.setPinOutput(EasyVR::IO1, LOW);
Serial.println("EasyVR detected!");
easyvr.setTimeout(5);
easyvr.setLanguage(0);
group = EasyVR::TRIGGER; //<-- start group (customize)
}
void action();
void loop()
{
easyvr.setPinOutput(EasyVR::IO1, HIGH); // LED on (listening)
Serial.print("Say a command in Group ");
Serial.println(group);
easyvr.recognizeCommand(group);
do
{
//processing while waiting to finish
}
while (!easyvr.hasFinished());
easyvr.setPinOutput(EasyVR::IO1, LOW); // LED off
idx = easyvr.getWord();
if (idx >= 0)
{
// built-in trigger (ROBOT)
// group = GROUP_X; <-- jump to another group X
return;
}
idx = easyvr.getCommand();
if (idx >= 0)
{
// print debug message
uint8_t train = 0;
char name[32];
Serial.print("Command: ");
Serial.print(idx);
if (easyvr.dumpCommand(group, idx, name, train))
{
Serial.print(" = ");
Serial.println(name);
}
else
Serial.println();
easyvr.playSound(0, EasyVR::VOL_FULL);
// perform some action
action();
}
else // errors or timeout
{
if (easyvr.isTimeout())
Serial.println("Timed out, try again...");
int16_t err = easyvr.getError();
if (err >= 0)
{
Serial.print("Error ");
Serial.println(err, HEX);
}
}
}
void action()
{
switch (group)
{
case GROUP_0:
switch (idx)
{
case G0_ARDUINO:
// write your action code here
easyvr.playSound(1,31);
group = GROUP_4; //jump to another group X for composite commands
break;
}
break;
case GROUP_4:
switch (idx)
{
case G4_RIGHT:
easyvr.playSound(1,31);
myservo.write(178);
delay(15);
mymotor.write(84);
delay(1000);
group = GROUP_4;
break;
case G4_LEFT:
easyvr.playSound(1,31);
myservo.write(1);
delay(15);
mymotor.write(84);
delay(1000);
group = GROUP_4;
break;
case G4_FORWARD:
easyvr.playSound(1,31);
myservo.write(87);
mymotor.write(84);
delay(1000);
group = GROUP_4;
break;
case G4_BACKWARD:
easyvr.playSound(1,31);
myservo.write(93);
mymotor.write(102);
delay(1000);
group = GROUP_4;
break;
case G4_HALT:
myservo.write(90);
mymotor.write(90);
easyvr.playSound(1,31);
group = GROUP_4;
break;
}
break;
}
}