Go Down

Topic: Toggling auto/remote robot modes using keybord (Read 708 times) previous topic - next topic

kev0153

Hi,

Been searching around now for a few days without much luck.  I'm trying to get my head around a way to toggle between two modes for my robot.  The first mode is "remote control" where I control the robot using my keyboard sending serial commands over a xbee connection, the second mode is autonomous where actions of the robot are controlled using input from various sensors.  I have the code for each mode working separately now what I'd like to do is this:

1.  Start the robot in autonomous mode and let it do its thing.
2.  If the robot starts to get in trouble, override autonomous mode with a key press (say the 'o' key).
3.  Control robot using keyboard to get it out of trouble.
4.  Toggle back to autonomous mode and let it go

As I said I have the two separate modes working great but i can't get my head around making the switch between the two.

I have started to play around with finite states using the FSM library but have not had much luck.

A kick in the right direction would be greatly appreciated.  

Here is my FSM code I've been playing around with.  To make things easier, I've made the autonomous section of the code simple for now. It just turns the motors forward for a few seconds and then stops.  It sort of works but seems to be a bit unpredictable, I'm pretty sure I don't have the keyboard toggle setup right.

Thanks for any help
Code: [Select]

#include <FiniteStateMachine.h>

#include <NewSoftSerial.h>

NewSoftSerial mySerial(4, 3);  //int rx, int tx)

const byte NUMBER_OF_STATES = 2;

int motor_reset = 12;  //motor reset pin

//initialize states
State remote = State(remoteOn);
State automatic = State(autoOn);

FSM autoStateMachine = FSM(automatic);  //start in automatic mode

void setup()
{
 pinMode(motor_reset, OUTPUT);  //motor reset pin
 Serial.begin(9600);
 mySerial.begin(9600);
 digitalWrite(motor_reset, LOW);
 delay(50);
 digitalWrite(motor_reset, HIGH);
 delay(50);
}

void loop () {
 if (Serial.available() > 0) {
   int onByte = Serial.read();  //check for keyboard letter o press to switch states (pretty sure this doesn't work)
     if (onByte == 'o') {
     autoStateMachine.transitionTo(remote); //change to remote
     }
   else {
     autoStateMachine.transitionTo(automatic); //use automatic mode (need a toggle in here somewhere)
   }
 }
autoStateMachine.update();
}

void remoteOn() {  //remote stuff - this works
 if (Serial.available() > 0) {
   int inByte = Serial.read();
 switch (inByte) {
   case 'w':    
     motorforward();
     break;
   case 's':    
     motorreverse();
     break;
   case 'd':    
     rotateccw();
     break;
   case 'a':    
     rotatecw();
     break;
   default:
     motorstop();
     }
 }
}

void autoOn() {  //simple auto mode for now
 motorforward();
 delay(5000);
 motorstop();
}

//subroutine motor forward
void motorforward()
{
 //left motor
 unsigned char buff1[6];
 buff1[0] = 0x80;  //start byte - do not change
 buff1[1] = 0x00;  //device type byte - do not change
 buff1[2] = 0x01; //Motore number and direction byte;  motor one = 00,01
 buff1[3] = 0x7F; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff1[i], BYTE);
 }

 //right motor
 unsigned char buff2[6];
 buff2[0] = 0x80;  //start byte - do not change
 buff2[1] = 0x00;  //device type byte - do not change
 buff2[2] = 0x03; //Motore number and direction byte;  motor two = 02,03
 buff2[3] = 0x7F; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff2[i], BYTE);
 }
}    

//subroutine motor reverse

void motorreverse()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x46; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x46; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }

 //subroutine motor all stop
}  

void motorstop()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x00; //Motor speed "0 to 128" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x00; //Motor speed "0 to 128" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }
}

void rotateccw()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x01; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }
}

void rotatecw()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
 mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x03; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
 mySerial.print(buff4[i], BYTE);
 }
}





kev0153

Ok,

I got it working.  I tossed out the idea of using the keyboard as a switch for now.  I just wired up a physical button to the robot to test the toggling of different modes and got it working.  

The robot starts off in autonomous mode and when I push the button it waits for some serial input and when it receives said input it reacts.  I think I'm going to add a intermediate mode of all stop so that it works like this.

1.  Autonomous mode
2. Push Button
3.  All Stop
4. Push Button
5.  Enter remote control mode
6. Push Button
7. Enter Autonomous mode

The final goal will be to get the physical button off the robot.   I think I can achieve this through my Xbee radio and a virtual wire.

My code is really a mess but once I get it cleaned up I'll post it.

kev0153

Ok here is my code.  Still a little glitchy, when the robot is in auto mode it sometimes goes into the obstacle avoidance for no reason.  I think it might be some noise in the system.

I'm self taught with coding so be gentle if you comment.
Code: [Select]
#include <FiniteStateMachine.h>
#include <NewSoftSerial.h>
#include <Servo2.h>
#include <TimedAction.h>
#include <math.h>

NewSoftSerial mySerial(4, 3);  //int rx, int tx)

//config timing
const byte NUMBER_OF_SCANS_PER_SECOND = 2;  //change this to suit your needs
const byte SCAN_FREQUENCY = 1000/NUMBER_OF_SCANS_PER_SECOND;

//TimedAction / 'thread' objects
TimedAction scanAction = TimedAction(SCAN_FREQUENCY,scan);

const byte NUMBER_OF_STATES = 3;

int motor_reset = 12;  //motor reset pin
int pingPin = 7;  //ping sensor pin
int ledpin = 8; //led warning pin
int xPin = 5;
int yPin = 6;

long echo = 0;
int distance = 0;  //set distance to zero
long tilt = 0;      //set tilt angle to zero

//initialize states
State remote = State(remoteOn);
State automatic = State(autoOn);
State allstop = State(motorstop);

FSM autoStateMachine = FSM(automatic);  //start in automatic mode

int switchPin = 11;

Servo myservo;  // create servo object to control a servo
int servoPosition = 10;       //start position
boolean scanIncrement = true;  //increase position?
byte servoIncrementValue = 5;
byte servoDecrementValue = 5;

int Xraw, Yraw;
double xGForce, yGForce, Xangle, Yangle;

int val;
int buttonState;
int mode = 0;  //0 = auto mode, 1 = stop mode, 2 = remote mode (yeah, it's confusing but it's my code :)

void setup()
{
 pinMode(motor_reset, OUTPUT);  //motor reset pin
 Serial.begin(9600);
 mySerial.begin(9600);

 digitalWrite(motor_reset, LOW);
 delay(50);
 digitalWrite(motor_reset, HIGH);
 delay(50);

 pinMode(switchPin, INPUT);
 buttonState = digitalRead(switchPin);

 mySerial.begin(9600);    //start software serial communication
 myservo.attach(9);  // attaches the servo on pin 9 to the servo object
 pinMode(yPin, INPUT); //read values from y axis pin
 pinMode(xPin, INPUT); //read values from x axis pin
}

void loop () {
 val = digitalRead(switchPin);
 if (val != buttonState) {          // the button state has changed!
   if (val == LOW) {                // check if the button is pressed
     if (mode == 0) {            // auto mode on?
       mode = 1;              //change to stop mode
       autoStateMachine.transitionTo(allstop);  //all stop
     }
     else {
       if (mode == 1) {  // if it is stopped
         mode = 2;
         autoStateMachine.transitionTo(remote); //change to remote
       }
       else {
         if (mode == 2) {  // if it is in remote mode    
           mode = 0;        //auto mode on
           autoStateMachine.transitionTo(automatic);   //change to auto
         }
       }
     }
   }
 }
 buttonState = val;
 //Serial.println(val);
 //Serial.println(mode);
 autoStateMachine.update();
}

void remoteOn() {  //remote stuff - this works
 if (Serial.available() > 0) {
   int inByte = Serial.read();
   switch (inByte) {
   case 'w':    
     motorforward();
     break;
   case 's':    
     motorreverse();
     break;
   case 'd':    
     rotateccw();
     break;
   case 'a':    
     rotatecw();
     break;
   default:
     motorstop();
   }
 }
}

void autoOn() {  //simple auto mode for now
 scanAction.check();
}

void scan()
{
 scanIncrement ? servoPosition+=servoIncrementValue : servoPosition-=servoDecrementValue; //increment or decrement current position

 if (servoPosition > 70){
   scanIncrement = false;
   servoPosition = 70;
 }
 else if (servoPosition < 10){
   scanIncrement = true;
   servoPosition = 10;
 }
 motorforward();
 myservo.write(servoPosition);
 long distance = ping();
 long tilt = angle();

}

long ping() {
 pinMode(pingPin, OUTPUT); // Switch signalpin to output
 digitalWrite(pingPin, LOW); // Send low pulse
 delayMicroseconds(2); // Wait for 2 microseconds
 digitalWrite(pingPin, HIGH); // Send high pulse
 delayMicroseconds(5); // Wait for 5 microseconds
 digitalWrite(pingPin, LOW); // Holdoff
 pinMode(pingPin, INPUT); // Switch signalpin to input
 digitalWrite(pingPin, HIGH); // Turn on pullup resistor
 echo = pulseIn(pingPin, HIGH); //Listen for echo

 distance = (echo / 58.138) * .39; //convert to CM then to inches

 Serial.println(distance);

 if (distance <= 3 && distance !=0)
 {
   avoid();
 }  
}

//start tilt check
long angle() {
 Xraw = pulseIn (xPin, HIGH);
 Xraw = pulseIn (xPin, HIGH);
 Yraw = pulseIn (yPin, HIGH);
 Yraw = pulseIn (yPin, HIGH);

 // Calculate G-force in Milli-g's.

 xGForce = (( Xraw / 10 ) - 500) * 8;
 yGForce = (( Yraw / 10 ) - 500) * 8;

 // uncomment next four lines for milli-g output.

 //Serial.println(xGForce);
 //Serial.print(" ");
 //Serial.println(yGForce);
 //Serial.print(" ");

 // Calculate angle (radians) for both -x and -y axis.

 Xangle = asin ( xGForce / 1000.0 );
 Yangle = asin ( yGForce / 1000.0 );

 // Convert radians to degrees.

 Xangle = Xangle * (360 / (2*M_PI));
 Yangle = Yangle * (360 / (2*M_PI));

 if (Xangle >= 3 || Yangle >= 10 || Yangle <= -10)
 {
   avoid();
 }

 //Serial.println(Xangle);
 //Serial.print(" ");
 //Serial.println(Yangle);
}

//avoid subroutine

void avoid()
{
 motorstop();
 ledwarning();
 delay(1000);
 motorreverse();
 delay(2000);
 motorstop();
 delay(1000);
 rotateccw();
 delay(1000);
 motorstop();
 delay(1000);
 motorforward();
 delay(1000);
 motorstop();
 delay(1000);
 rotatecw();
 delay(1000);
 motorstop();
 delay(1000);
}

//fire the led pin as a warning
void ledwarning()
{
 digitalWrite(ledpin, HIGH);
 delay(1000);
 digitalWrite(ledpin, LOW);
}

//subroutine motor forward
void motorforward()
{
 //left motor
 unsigned char buff1[6];
 buff1[0] = 0x80;  //start byte - do not change
 buff1[1] = 0x00;  //device type byte - do not change
 buff1[2] = 0x01; //Motore number and direction byte;  motor one = 00,01
 buff1[3] = 0x7F; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff1[i], BYTE);
 }

 //right motor
 unsigned char buff2[6];
 buff2[0] = 0x80;  //start byte - do not change
 buff2[1] = 0x00;  //device type byte - do not change
 buff2[2] = 0x03; //Motore number and direction byte;  motor two = 02,03
 buff2[3] = 0x7F; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff2[i], BYTE);
 }
}    

//subroutine motor reverse

void motorreverse()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x46; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x46; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }

 //subroutine motor all stop
}  

void motorstop()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x00; //Motor speed "0 to 128" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x00; //Motor speed "0 to 128" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }
}

void rotateccw()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x01; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x02; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }
}

void rotatecw()
{
 //left motor
 unsigned char buff3[6];
 buff3[0] = 0x80;  //start byte - do not change
 buff3[1] = 0x00;  //device type byte - do not change
 buff3[2] = 0x00; //Motore number and direction byte;  motor one = 00,01
 buff3[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff3[i], BYTE);
 }

 //right motor
 unsigned char buff4[6];
 buff4[0] = 0x80;  //start byte - do not change
 buff4[1] = 0x00;  //device type byte - do not change
 buff4[2] = 0x03; //Motore number and direction byte;  motor two = 02,03
 buff4[3] = 0x40; //Motor speed "0 to 127" in hex
 for(int i=0;i<4;i++) {
   mySerial.print(buff4[i], BYTE);
 }
}




I use a super simple processing sketch to send serial data.  Next up is to get the remote button wired up to my xbee using "virtual" wires .

Thanks

Go Up