Code examples from final RoboSumo lecture

Simple rangefinder example:

//
// RoboSumo navigation example using rangefinder
// Written by Ted Burke - 17-4-2018
//

void setup()
{
  pinMode(2, INPUT);  // rangefinder echo
  pinMode(3, OUTPUT); // rangefinder trigger
  pinMode(4, OUTPUT); // left motor forward
  pinMode(5, OUTPUT); // left motor reverse
  pinMode(6, OUTPUT); // right motor forward
  pinMode(7, OUTPUT); // right motor reverse
  pinMode(8, OUTPUT); // blue LED
  // A0 is left colour sensor
  // A2 is right colour sensor

  // Serial.begin(9600);
}

void loop()
{
  int colour_left, colour_right, object_detect;
  
  // Read colour sensors
  colour_left = analogRead(0);  
  colour_right = analogRead(2);

  // Use rangefinder to check for object
  digitalWrite(3, HIGH);
  delayMicroseconds(20);
  digitalWrite(3, LOW);
  while(digitalRead(2) == 0); // wait for start of echo
  delayMicroseconds(2000);
  if (digitalRead(2) == 0) object_detect = 1;
  else object_detect = 0;

  // Robot behaviour
  if (object_detect == 0)
  {
    digitalWrite(8, HIGH); // LED on
    motors(1, -1);
  }
  else
  {
    digitalWrite(8, LOW); // LED off
    motors(1, 1);
  }

  delay(100);
}

void motors(int left, int right)
{
  if (left > 0)
  {
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
  }
  else if (left < 0)
  {
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
  }
  else
  {
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
  }
  
  if (right > 0)
  {
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }
  else if (right < 0)
  {
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
  }
  else
  {
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
  }
}

Final state machine example:

//
// RoboSumo navigation example using state machine
// Written by Ted Burke - 17-4-2018
//

int state = 1;

void setup()
{
  pinMode(2, INPUT);  // rangefinder echo
  pinMode(3, OUTPUT); // rangefinder trigger
  pinMode(4, OUTPUT); // left motor forward
  pinMode(5, OUTPUT); // left motor reverse
  pinMode(6, OUTPUT); // right motor forward
  pinMode(7, OUTPUT); // right motor reverse
  pinMode(8, OUTPUT); // blue LED
  // A0 is left colour sensor
  // A2 is right colour sensor

  // Serial.begin(9600);
}

void loop()
{
  int colour_left, colour_right, object_detect;
  
  // Read colour sensors
  colour_left = analogRead(0);  
  colour_right = analogRead(2);

  // Use rangefinder to check for object
  digitalWrite(3, HIGH);
  delayMicroseconds(20);
  digitalWrite(3, LOW);
  while(digitalRead(2) == 0); // wait for start of echo
  delayMicroseconds(2000);
  if (digitalRead(2) == 0) object_detect = 1;
  else object_detect = 0;

  // Robot behaviour - state machine
  if (state == 1)
  {
    // Spin and search
    motors(0, 1);
    digitalWrite(8, LOW);

    if (object_detect == 1) state = 2;
  }
  else if (state == 2)
  {
    // charge at opponent
    motors(1, 1);
    digitalWrite(8, HIGH);

    if (object_detect == 0) state = 1;
    if (colour_left > 512) state = 3;
    if (colour_right > 512) state = 4;
  }
  else if (state == 3)
  {
    // white detected on left
    motors(0, 1);

    if (colour_right > 512) state = 5;
  }
  else if (state == 4)
  {
    // white detected on right
    motors(1, 0);

    if (colour_left > 512) state = 5;
  }
  else if (state == 5)
  {
    // reverse to centre of table
    motors(-1, -1);
    delay(2500);
    state = 1;
  }

  delay(100);
}

void motors(int left, int right)
{
  if (left > 0)
  {
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
  }
  else if (left < 0)
  {
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
  }
  else
  {
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
  }
  
  if (right > 0)
  {
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }
  else if (right < 0)
  {
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
  }
  else
  {
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
  }
}
This entry was posted in Uncategorized. Bookmark the permalink.

3 Responses to Code examples from final RoboSumo lecture

  1. Hi Ted, the rangefinder example you have posted is this complete or is there tweaks needed.
    I had my robot following going forward when black detected and when white was detected the robot turned away and moved back towards the center on the ring.
    I am using one TCRT5000 and worked fine.

    To test I have connected a 2nd TCRT5000 along with the HC-SR04 and uploaded your code. but the robot moves forward and when a object detected the robot spins and then continues to move forward. it is not picking up the edge detection now and both have power supplied.

    any suggestions?

  2. batchloaf says:

    Hi Stephen,

    The code is basically a complete example, but of course it may need some tweaks to ensure the pins used are correct for your robot. Even when it’s working correctly, I wouldn’t necessarily suggest this as a good strategy for the competition – it was more of an illustration of how to implement a chosen strategy, rather than a guide to what strategy is best.

    Anyway, some practical suggestions. Firstly, try something simpler! Pare back the state machine to just two or three states and check the sensors one by one. Unless you’re certain that each sensor is working correctly, it doesn’t make much sense to debug a more complicated state machine where a lot of things are going on.

    Another practical suggestion is to try using the serial monitor to display on-screen every time the robot changes state. You’ll need to uncomment this line…

    Serial.begin(9600);
    

    …in the setup function, and then add the following line to each state…

    Serial.println(state);
    

    Then prop the robot up on something so that the wheels are off the ground and try providing appropriate input to the sensors (black / white for the TCRT5000s and object / space in front of the HC-SR04) and seeing how the robot responds. By keeping an eye on the Serial Monitor in the Arduino software, you’ll be able to see what state the program is in at all times and that might help to identify where things are going wrong.

    Finally, please make sure that the motors are both wired the right way around. You can test this using the motors function. Just put this at the end of your setup function…

    motors(1, 1);
    delay(5000);
    

    If the motors are wired correctly, you should see both wheels go forward for 5 seconds when you power up the robot. Assuming they do, you can remove those two lines again – it’s just a quick test to be sure the motors will turn the way you expect in each state.

    Ted

    • Thanks so much.
      yeah for the object of a competition I agree tweaks will be needed and plan a plan to get the best out of the robot.
      I wanted to use the code to see that it is all working correctly 1st.

      Cheers Ill try ur suggestions.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s