Go Down

Topic: Simultaneous sensing and movement (Read 1 time) previous topic - next topic

Aeturnalus

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: [Select]

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



PaulS

Code: [Select]
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.

Groove

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
Per Arduino ad Astra

Aeturnalus

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

Grumpy_Mike

Quote
Does it really still work without them?


only if you .......

Quote
restructure your code as a state-machine

Aeturnalus

#5
Mar 30, 2010, 03:45 pm Last Edit: Mar 30, 2010, 04:09 pm by rbtying Reason: 1
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: [Select]

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

cr0sh

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: [Select]
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);
}
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

Aeturnalus

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

Go Up