Toggling auto/remote robot modes using keybord

system · May 24, 2010, 4:58pm

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:

Start the robot in autonomous mode and let it do its thing.
If the robot starts to get in trouble, override autonomous mode with a key press (say the 'o' key).
Control robot using keyboard to get it out of trouble.
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

#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);
  }
}

system · May 25, 2010, 3:02pm

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.

Autonomous mode
Push Button
All Stop
Push Button
Enter remote control mode
Push Button
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.

system · May 26, 2010, 12:51pm

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.

#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

Topic		Replies	Views
Remote Control - Semi-Autonomous Networking, Protocols, and Devices	2	783	May 6, 2021
Toggle between two void functions Programming Questions	7	1880	May 5, 2021
Adding an autonomous mode to a remote-controlled Robot Programming Questions	11	4262	May 5, 2021
IR robot, avoidance mode issues (Solved) Motors, Mechanics, Power and CNC	27	3486	May 6, 2021
Switching between two controlling modes code question Programming Questions	5	456	May 5, 2021

Toggling auto/remote robot modes using keybord

Related Topics