Lab 9

Introduction

For this lab, we had to make the robot mimic some cockroach-like behaviour. Certain behaviours take precedence over other behaviours.

Our initial idea:

  • Create an if, else if statement for each of the states the robot could be in, putting the higheset priority as the first if.
  • Each instruction has to be short snippet of code for the current action
  • Do that small action, and the re-check to see if anything has changed.

Code


extern long ping();
void cockroachHide() {
    //drive if distance in front of bot is less than 20cm
    if (microsecondsToCentimeters(ping())>20) {
      leftMotorSpeed=leftMotorDefaultSpeed;
      rightMotorSpeed=rightMotorDefaultSpeed;
      Forward(0);
    } 
    //stop if at wall. wait for light to come back up before leaving function
    else {
      Stop(0);
      while(analogRead(lightPin)>200);
    }
}
extern void followCurvyLine(bool);
void cockroach() {
  long now = millis();
  bool ledState = false;
  bool fed = false;
  bool line = false;
  bool obstacle= false;
  int lastRead = LOW;
  int adjustment=0;
  long lineFollowStartTime=0;
  long lastAdjustmentTime=millis();
  int defaultLSpeed=leftMotorDefaultSpeed/1.7;
  int defaultRSpeed=rightMotorDefaultSpeed/1.7;
  int turnIn = random(1000,10000);
  long paperSince = 0;
  long finishedFeed = 0;

  //moving average for PaperSensor
  int values[4];
  int total=0;
  for (byte i=0;i<4;i++) total+=values[i]=analogRead(PaperSensor2);
  int maIndex=0; //movoing average index
  int paperValue = total/4;


  while(true) {
    //get new reading for paper sensor
    total-=values[maIndex];
    total+=values[maIndex]=analogRead(PaperSensor2);
    maIndex=(maIndex+1)%4;
    paperValue = total/4;
    delay(10); //for reading between sensor values
    //first priority hide
    if (analogRead(lightPin)>200) {
      cockroachHide();
      if (cockroachState!=4) {
        cockroachState=4;
        drawCurrentMenu();
      }
    }
    
    //second priority feed
    else if (paperValue>30 && paperValue<60 && paperSince==0 && digitalRead(LineSensor==LOW)) {
      paperSince=millis();
      analogWrite(ledPin, 255);
    }
    else if (paperSince!=0 && millis()-paperSince<=100) {
      //wait for paper to clear or set
    }
    else if (millis()-paperSince>100  && paperSince!=0 && finishedFeed==0) { //confidently on paper 
      //feed
      if (cockroachState!=3) {
        cockroachState=3;
        drawCurrentMenu();
      }
      enableRotaryInterrupts();
      leftTick=0;
      leftMotorSpeed=leftMotorDefaultSpeed;
      rightMotorSpeed=-rightMotorDefaultSpeed;
      Forward(0);
      long timeSinceChange=millis();
      while(leftTick<50) {
        if (millis()-timeSinceChange>200) {
          ledState=!ledState;
          digitalWrite(ledPin,ledState);
          timeSinceChange=millis();
        }
      }
      Stop(0);
      detatchrotaryInterrupts();
      line=false;
      finishedFeed=millis();
    }
    else if (millis()-finishedFeed<=5000 && finishedFeed!=0) { 
      Stop(0);
    }
    else if (millis()-finishedFeed>5000 && finishedFeed!=0) {
      finishedFeed=0;
      paperSince=0;
      leftMotorSpeed=leftMotorDefaultSpeed*0.8;
      rightMotorSpeed=rightMotorDefaultSpeed*0.85;
      Forward(0);
      delay(2000);
      
    }
    
    //third priority avoid obstacles
    else if (microsecondsToCentimeters(ping())<40) {
      line=false;
      if (cockroachState!=2) {
        cockroachState=2;
        drawCurrentMenu();
      }
      //has apporached obstacle
      if (!obstacle) {
        Left(90);
        if (!myservo.attached()) myservo.attach(servoPin);
        myservo.write(171);    
        now = millis();
        while (millis()-now<1000);
        servoOff();
        obstacle=!obstacle;
      }
      //is following obstacle
      else  {
        leftMotorSpeed=leftMotorDefaultSpeed;
        rightMotorSpeed=rightMotorDefaultSpeed;
        Forward(0);
      }
    }

    //fourth priority follow lines
    else if (digitalRead(LineSensor)==LOW && !line && paperSince==0 && paperValue < 30) {
      //has approached line
      if (cockroachState!=9) {
        cockroachState=9; //lining up for line
        drawCurrentMenu();
      }
      leftTick=0;
      leftMotorSpeed=leftMotorDefaultSpeed*0.64;
      rightMotorSpeed=rightMotorSpeed*0.8;
      Forward(0);
      enableRotaryInterrupts();
      while (leftTick<6);
      Stop(0);
      delay(300);
      leftMotorSpeed=-leftMotorDefaultSpeed;
      rightMotorSpeed=rightMotorDefaultSpeed;;
      Forward(0);
      while(digitalRead(LineSensor)==LOW);
      leftMotorSpeed*=0.8;
      rightMotorSpeed*=0.8;
      Forward(0);
      while(digitalRead(LineSensor)==HIGH);
      Stop(0);
      leftMotorSpeed=leftMotorDefaultSpeed;
      rightMotorSpeed=rightMotorDefaultSpeed;;
      line =true;
      lineFollowStartTime=millis();
    }
    
    else if (line) {
      //is following line
      if (cockroachState!=1) {
        cockroachState=1;
        drawCurrentMenu();
      }
      if (millis()-lineFollowStartTime<5000) followCurvyLine(0,0);
      else {
        line=false;
        if (cockroachState!=0) {
          cockroachState=0;
          drawCurrentMenu();
        }
        leftMotorSpeed=leftMotorDefaultSpeed*0.8;
        rightMotorSpeed=rightMotorDefaultSpeed*0.85;
        Forward(0);
        delay(2000);
      }
    }

    //explore and find food. yum. 
    else {
      if (cockroachState!=0) {
        cockroachState=0;
        drawCurrentMenu();
      }
      if (millis()-now>turnIn) {
        Left(random(90));
        now=millis();
        turnIn = random(1000,10000);
      }
      leftMotorSpeed=leftMotorDefaultSpeed*0.8;
      rightMotorSpeed=rightMotorDefaultSpeed*0.85;
      Forward(0);
    }
    if (microsecondsToCentimeters(ping())>40 && obstacle) {
      obstacle=false;
      if (!myservo.attached()) myservo.attach(servoPin);
      myservo.write(81);    
      now = millis();
      while (millis()-now<1000);
      servoOff();
    }
    if (paperValue<30 || paperValue>60) { paperSince=0; 
      analogWrite(ledPin, 255);}  //reset paper sensor if acctidentally triggered.
    if (useSave) saveData(useSave);                     //time to log some data!! :D
  }
  currentMenu = draw;
}

Issues

As we begun with our plan, we realized that each state could be further broken down into some smaller states. For example, after the robot had detected the line, it had to turn to line up with the line. We had to differentiate between the robot turning to find the line and the robot actually following the line.

We had some issues detecting paper vs detecting lines. If we hit the line sideways, we were not sure how to distinguish between paper and line detection. As a result, we would get false positives for feeding behaviour if the robot happened to cross perpendicular to the line.

Results

Video goes here

Conclusion

In conclusion, it is easy to implement these behaviors if each behavior can be broken down into a quick check of the sensor, and the necessary code for each sensor check. Then, this goes into a loop that will go through, check each instruction, and do a quick adjustment as a result of the sensor check. For example, if in line following mode, I didn't see the line, turn to the left; if i saw the line, turn to the right. Then check to see if the overhead lights are on, if so, up the motor speeds. If the paper sensor is on a line, implement the feed mode, etc. These and other sensors were checked.

In the cases of more complex behaviors, each of those could be further broken down into simpler states. For example, when line following, there was the state of initially detecting and acquiring the line, and the state of actually following the line.