Pages: [1]   Go Down
Author Topic: Simultaneous sensing and movement  (Read 1246 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 360
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I just got my Arduino today, and decided to build a robot (I had the other parts ready).
The robot is made as such:
Hi-tec servo on analog pin 0, swiveling a Sharp IR Rangefinder
Tracked chassis based on Tamiya Twin-motor Gearbox
Powered by Arduino via NiCd battery, rechargeable.

My problem right now is that I can't call the scan() function, which scans, and the move() function at the same time, or nearly simultaneously.  As such, the robot starts and stops on its way aroudn the room many, many times.  If possible, could someone suggest a fix for this situation?  Source Code:
Code:
#include <Servo.h>            // include the standard servo library
Servo scanservo;              // set a variable to map the servo
int lrisk = 0;
int rrisk = 0;
int frisk = 0;

int IRpin = 1;                // analog pin for reading the IR sensor

// motors
// motor1
int m1p1 = 2;
int m1p2 = 3;

// motor2
int m2p1 = 4;
int m2p2 = 5;

/* setup the pins, servo and serial port */
void setup() {
  scanservo.attach(14);
  // initialize the serial port:
  Serial.begin(9600);
  pinMode(m1p1, OUTPUT);
  pinMode(m2p1, OUTPUT);
  pinMode(m1p2, OUTPUT);
  pinMode(m2p2, OUTPUT);
}

/* begin rotating the servo and getting sensor values */
void loop() {
  scan();
  move();
}
void move()
{
  if (/*frisk > 2000 || */frisk < 80) {
    if (lrisk < rrisk)
      turnLeft();
    else
      turnRight();
  }
  int diff = lrisk - rrisk;
  if (diff > 100)
    turnRight();
  else if (diff < -100)
    turnLeft();
  else
    forward();
}
void mstop() {
  digitalWrite(m2p1, LOW);
  digitalWrite(m2p2, LOW);
  digitalWrite(m1p1, LOW);
  digitalWrite(m1p2, LOW);
}
void turnRight() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p1, HIGH);
  delay(500);
  mstop();
}

void turnLeft() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p1, HIGH);
  delay(500);
  mstop();
}

void forward() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p2, HIGH);
  delay(500);
  mstop();
}

void reverse() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p1, HIGH);
  delay(500);
  mstop();
}
int cm (float i) {
  float volts = i*0.0048828125;
  float distance = 65*pow(volts, -1.10);
  return (int) distance;
}
int irtocm() {
  int total = 0;
  for (int i = 0; i < 10; i++)
    total += cm(analogRead(IRpin));
  return total / 10;
}
void scan() {
  frisk = rrisk = lrisk = 0;
  Serial.println("Scan Start");
  for (int i = 30; i < 150; i += 3) {
    scanservo.write(i);
    delay(10);
    if (i < 70)
      lrisk += irtocm();
    else if (i <= 110)
      frisk += irtocm();
    else
      rrisk += irtocm();
  }
  Serial.println("---");
  scanservo.write(30);
}


Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 613
Posts: 49310
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
void forward() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p2, HIGH);
  delay(500);
  mstop();
}

This function really should be called lurch.

Once you set the robot moving, leave it moving (that is, remove the stop at the end of the forward, reverse, turnLeft, and turnRight functions). The scans will occur frequently enough (especially if you remove the serial print statements from the scan function) to keep the robot from getting into trouble.
Logged

UK
Offline Offline
Faraday Member
**
Karma: 17
Posts: 2884
Gorm deficient
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
I can't call the scan() function, which scans, and the move() function at the same time

Nor will you ever be able to with all those "delay"s.
One way of fixing this would be to restructure your code as a state-machine
Logged

Per Arduino ad Astra

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 360
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

So... all the examples in the beginning seem to be done with delays.   Does it really still work without them?
Logged

Manchester (England England)
Offline Offline
Brattain Member
*****
Karma: 626
Posts: 34143
Solder is electric glue
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
Does it really still work without them?

only if you .......

Quote
restructure your code as a state-machine
Logged

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 360
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I'm not quite sure what a "state machine" is.  Does that mean that you just change its movement pattern when the scan tells it to, and otherwise keeps it running?

Modified Source Code:
Code:
#include <Servo.h>            // include the standard servo library
Servo scanservo;              // set a variable to map the servo
int lrisk = 0;
int rrisk = 0;
int frisk = 0;

int IRpin = 1;                // analog pin for reading the IR sensor

// motors
// motor1
int m1p1 = 2;
int m1p2 = 3;

// motor2
int m2p1 = 4;
int m2p2 = 5;

/* setup the pins, servo and serial port */
void setup() {
  scanservo.attach(14);
  // initialize the serial port:
  Serial.begin(9600);
  pinMode(m1p1, OUTPUT);
  pinMode(m2p1, OUTPUT);
  pinMode(m1p2, OUTPUT);
  pinMode(m2p2, OUTPUT);
}

/* begin rotating the servo and getting sensor values */
void loop() {
  scan();
  move();
}

void move()
{
  if (/*frisk > 2000 || */frisk < 2400) {
    reverse();
    delay(200);

    if (lrisk < rrisk)
      turnLeft();
    else
      turnRight();
  }
  int diff = lrisk - rrisk;
  if (diff > 800)
    turnLeft();
  else if (diff < -800)
    turnRight();
  else
    forward();
}
void mstop() {
  digitalWrite(m2p1, LOW);
  digitalWrite(m2p2, LOW);
  digitalWrite(m1p1, LOW);
  digitalWrite(m1p2, LOW);
}
void turnRight() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p1, HIGH);
}

void turnLeft() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p1, HIGH);
}

void forward() {
  mstop();
  digitalWrite(m1p1, HIGH);
  digitalWrite(m2p2, HIGH);
}

void reverse() {
  mstop();
  digitalWrite(m1p2, HIGH);
  digitalWrite(m2p1, HIGH);
}
float cm (float i) {
  i = 5 * i / 1034;
  float a = 0.008271;
  float b = 939.6;
  float c = -3.398;
  float d = 17.339;
  return ((a + b * i) / ( 1 + c * i + d * i * i )) ;
}
int irtocm() {
  float total = 0;
  for (int i = 0; i < 10; i++) {
    int v = analogRead(IRpin);
    total += cm(v);
  }
  return (int) total / 10;
}
void scan() {
  frisk = 0;
  rrisk = 0;
  lrisk = 0;
  for (int i = 0; i < 180; i += 1) {
    scanservo.write(i);
    delay(2);
    int r = irtocm();
    if (i < 60) {
      lrisk -= r;
    }
    else if (i >= 60 && i <= 120) {
      frisk -= r;
    }
    else if (i > 120 && i <= 180) {
      rrisk -= r;
    }
  }

  Serial.print(" Left Risk ");
  Serial.print(lrisk);
  Serial.print(" Front Risk ");
  Serial.print(frisk);
  Serial.print(" Right Risk ");
  Serial.println(rrisk);

  scanservo.write(0);
  delay(600);
}
« Last Edit: March 30, 2010, 09:09:11 am by rbtying » Logged

Phoenix, Arizona USA
Offline Offline
Faraday Member
**
Karma: 40
Posts: 5575
Where's the beer?
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
I'm not quite sure what a "state machine" is.  Does that mean that you just change its movement pattern when the scan tells it to, and otherwise keeps it running?

Something like that, yes.

State machines are constructs (typically implemented via switch-case statements - if/then/else constructs can also be used) that are put inside a loop, with a numeric index variable to indicate the current state. The index variable is set to the default starting state (typically "0"), then the loop is run. On each pass thru the switch-case in the loop, the value of the index variable is checked against the state desired, and if it matches, then that code is run (either within the case statement, or more generally via a function call, which returns the result state, which is used on the next pass). On the final state, one can exit from the loop, or reset the state back to default, or whatever. The code itself that the current state runs can change to the next state, or to a completely different state.

Simple example, probably with lots of bugs that Grumpy_Mike or someone else will complain about:

 ;D


Code:
void setup() {
  // open the serial port at 9600 bps:
  Serial.begin(9600);
}

void loop() {
  static int state = 0;

  switch (state) {
     case 0:
        state = function0();
        break;
     case 1:
        state = function1();
        break;
     case 2:
        state = function2();
        break;
     case 3:
        state = function3();
        break;
  }
}

int function0() {
  Serial.println("Get out of");
  return (2);
}

int function1() {
  Serial.println("and");
  return (3);
}

int function2() {
  Serial.println("your seat");
  return (1);
}

int function3() {
  Serial.println("jump around!");
  return (0);
}
Logged

I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 360
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks, I will try that as soon as I get some more free time.  
Logged

Pages: [1]   Go Up
Jump to: