Ikedo Mini V2 sumo robot with auto and RC mode.

IkedoMiniV2_AutoRC.ino

/*******************************************************************************
 * THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTY AND SUPPORT
 * IS APPLICABLE TO THIS SOFTWARE IN ANY FORM. CYTRON TECHNOLOGIES SHALL NOT,
 * IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR CONSEQUENTIAL
 * DAMAGES, FOR ANY REASON WHATSOEVER.
 ********************************************************************************
 * DESCRIPTION:
 *
 * This example allow robot to operate in auto and RC mode.
 * Connection:
 * • IR Sumo Start <---> D9
 * • RC Speed      <---> GPIO1
 * • RC Steering   <---> GPIO2
 * DIP Switch:
 * • 0-6 <---> AUTO
 * • 7   <---> RC
 *
 * AUTHOR   : Kong Wai Weng / Idris Zainal Abidin
 * COMPANY  : Cytron Technologies Sdn Bhd
 * WEBSITE  : www.cytron.io
 * EMAIL    : [email protected]
 *
 *******************************************************************************/

#include "CytronMakerSumo.h"

// Edge sensor threshold sensitivity.
#define EDGE_THRESHOLD_COEF  0.5

// Constants for direction.
#define LEFT    0
#define RIGHT   1

// Maximum speed (PWM).
#define SPEED_MAX    255

// IR Sumo Start
#define IR_SUMO_START 9

#define AUTO false
#define RC   true

// Pins connected to RC receiver.
#define RC_SPEED          GPIO1
#define RC_STEERING       GPIO2

// Neutral value for RC pulse (microseconds).
#define NEUTRAL_SPEED     1500
#define NEUTRAL_STEERING  1500

// Range for RC pulse (microseconds).
// Full down/left to full up/right.
#define RANGE_SPEED       1000
#define RANGE_STEERING    1000

// Searching direction.
int searchDir = LEFT;

// Define note and duration for melody.
int melody1Pitch[] = {NOTE_C4, NOTE_G4};
int melody1Duration[] = {8, 8};

int melody2Pitch[] = {NOTE_C5, NOTE_D5, NOTE_E5};
int melody2Duration[] = {16, 16, 16};

int edgeLeftThreshold = 0;
int edgeRightThreshold = 0;
int speedLeftMax, speedRightMax;
long prevMillis = 0;
bool ledState = 0;
long count = 0;

bool startWithButton = false;
bool robot_mode = AUTO;

/*******************************************************************************
 * Initialize and setup.
 *******************************************************************************/
void setup()
{
  Serial.begin(115200);
  MakerSumo.begin();

  pinMode(IR_SUMO_START, INPUT_PULLUP);

  motorSpeedAlignment();

  // To enter motor speed alignment mode.
  // Press and hold START button, press RST button, or
  // Turn off robot, press and hold START button, turn on robot
  if (!digitalRead(START)) {
    MakerSumo.playMelody(melody2Pitch, melody2Duration, 3);
    while (!digitalRead(START)); // Wait START button to release

    while (1) {
      // Adjust the potentiometer based on the motor speed on both sides
      // If robot move forward and slightly to the right, 
      // turn the potentiometer to slightly left, and vice versa
      motorSpeedAlignment();

      // Press START button to test
      // Robot will move forward with half speed for 1s
      if (!digitalRead(START)) {
        digitalWrite(LED, HIGH);
        delay(1000);

        MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax/2);
        MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax/2);
        delay(1000);

        MakerSumo.stop();
      }
    }
  }

  // If DIP is set to 7, enter RC mode
  if (MakerSumo.readDipSwitch() == 7) {
    // Set the pins for RC receiver as input with pulled up.
    pinMode(RC_SPEED, INPUT_PULLUP);
    pinMode(RC_STEERING, INPUT_PULLUP);
    robot_mode = RC;
  }

  // Play the melody once.
  MakerSumo.playMelody(melody1Pitch, melody1Duration, 2);

  if (robot_mode == AUTO) { // Auto mode
    if (digitalRead(IR_SUMO_START)) {
      startWithButton = true;
    }

    // While waiting button pressed / IR sumo start,
    // it will display robot status on Serial Monitor
    while (true) {
      printStatus();

      if (digitalRead(START) == LOW) {
        startWithButton = true;
        break;
      }
      else if (!startWithButton && digitalRead(IR_SUMO_START)) {
        break;
      }
    }

    // Turn on the User LED.
    digitalWrite(LED, HIGH);

    if (startWithButton) {
      // Delay 5 seconds
      for (int i = 0; i < 3; i++) {
        delay(1000);
        tone(8, NOTE_C4, 200);
      }
      delay(1000);
      tone(8, NOTE_C5, 500);
      delay(950);
    }

    // Read edge sensor for black/white surface
    edgeLeftThreshold = analogRead(EDGE_L) * EDGE_THRESHOLD_COEF;
    edgeRightThreshold = analogRead(EDGE_R) * EDGE_THRESHOLD_COEF;

    // Start routine with DIP strategy
    startRoutine(MakerSumo.readDipSwitch());
  }
}

/*******************************************************************************
 * Main program loop.
 *******************************************************************************/
void loop()
{
  // Auto mode
  if (robot_mode == AUTO) {
    // Edge is detected on the left.
    if (analogRead(EDGE_L) < edgeLeftThreshold) {
      // Back off and make a U-Turn to the right.
      backoff(RIGHT);

      // Toggle the search direction.
      searchDir ^= 1;
    }
    
    // Edge is detected on the right.
    else if (analogRead(EDGE_R) < edgeRightThreshold) {
      // Back off and make a U-Turn to the left.
      backoff(LEFT);
      
      // Toggle the search direction.
      searchDir ^= 1;
    }

    // Edge is not detected.
    else {
      // Keep searching if opponent is not detected.
      if ((digitalRead(OPP_FC) == HIGH) &&
          (digitalRead(OPP_FL) == HIGH) &&
          (digitalRead(OPP_FR) == HIGH) &&
          (digitalRead(OPP_L) == HIGH)  &&
          (digitalRead(OPP_R) == HIGH) ) {
        search(searchDir);
      }
      
      // Attack if opponent is in view.
      else {
        attack();
      }
    }

    // Stop the robot after received STOP signal
    if (!startWithButton && !digitalRead(IR_SUMO_START)) {
      MakerSumo.stop();
      while (true) {
        digitalWrite(LED, HIGH);
        delay(100);
        digitalWrite(LED, LOW);
        delay(100);
      }
    }
  }
  // RC mode
  else if (robot_mode == RC) {
    // Read the speed channel from RC receiver.
    int speedPulse = pulseIn(RC_SPEED, HIGH, 100000);

    // Stop the robot if no pulse is received.
    if (speedPulse == 0) {
      digitalWrite(LED, LOW);
      MakerSumo.stop();
      return;
    }

    // Convert the pulse width to percentage (-1.0 to 1.0).
    float speedPercent = (float)(speedPulse - NEUTRAL_SPEED) / (float)(RANGE_SPEED / 2);
    speedPercent = constrain(speedPercent, -1.0f, 1.0f);

    // Add in dead band.
    if ((speedPercent > -0.1f) && (speedPercent < 0.1f)) {
      speedPercent = 0.0f;
    }

    // Read the steering channel from RC receiver.
    int steeringPulse = pulseIn(RC_STEERING, HIGH, 100000);

    // Stop the robot if no pulse is received.
    if (steeringPulse == 0) {
      digitalWrite(LED, LOW);
      MakerSumo.stop();
      return;
    }

    // Convert the pulse width to percentage (-1.0 to 1.0).
    float steeringPercent = (float)(steeringPulse - NEUTRAL_STEERING) / (float)(RANGE_STEERING / 2);
    steeringPercent = constrain(steeringPercent, -1.0f, 1.0f);

    // Add in dead band.
    if ((steeringPercent > -0.1f) && (steeringPercent < 0.1f)) {
      steeringPercent = 0.0f;
    }
    
    // Mix speed and steering together for left and right motor speed.
    float leftSpeedPercent = speedPercent + steeringPercent;
    int leftSpeed = (int)(leftSpeedPercent * 255.0f);
    leftSpeed = constrain(leftSpeed, -255, 255);

    float rightSpeedPercent = speedPercent - steeringPercent;
    int rightSpeed = (int)(rightSpeedPercent * 255.0f);
    rightSpeed = constrain(rightSpeed, -255, 255);

    // Set the speed for each motor.
    MakerSumo.setMotorSpeed(MOTOR_L, leftSpeed);
    MakerSumo.setMotorSpeed(MOTOR_R, rightSpeed);

    // Turn on LED if the speed is not 0.
    if ((leftSpeed != 0) || (rightSpeed != 0)) {
      digitalWrite(LED, HIGH);
    }
    else {
      digitalWrite(LED, LOW);
    }
  }
}

/*******************************************************************************
 * Start Routine with strategies
 * This function should be called once only when the game start.
 *******************************************************************************/
void startRoutine(int dip) {

  if (dip == 1) {
    strategy1();
  }
  else if (dip == 2) {
    strategy2();
  }
  else if (dip == 3) {
    strategy3();
  }
  else if (dip == 4) {
    strategy4();
  }
  else if (dip == 5) {
    strategy5();
  }
  else if (dip == 6) {
    strategy6();
  }
  else {
    strategy0();
  }
}

void strategy0()
{
  
}

void strategy1()
{
  // Turn right around 50 degrees.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  delay(80);

  // Go curve around the edge.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.6);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(600);

  // Turn right and face to the center of the ring.
  MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(120);

  searchDir = RIGHT;
}

void strategy2()
{
  // Go forward 30% speed.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.3);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.3);
  delay(100);

  // Go forward 60% speed.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.6);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.6);
  delay(100);

  // Go forward full speed.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(200);
}

void strategy3()
{
  // Turn right around 50 degrees.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  delay(40);

  // Go forward full speed.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.7);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(350);

  searchDir = RIGHT;
}

void strategy4()
{
  // Turn right around 50 degrees.
  MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(80);

  // Go curve around the edge.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.6);
  delay(600);

  // Turn right and face to the center of the ring.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  delay(120);

  searchDir = LEFT;
}

void strategy5()
{
  while ((digitalRead(OPP_FL) == HIGH) &&
         (digitalRead(OPP_FR) == HIGH) &&
         (digitalRead(OPP_L) == HIGH)  &&
         (digitalRead(OPP_R) == HIGH)) {

    if (millis() - prevMillis > 2000) {
      count++;
      MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.5);
      MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.5);
      delay(50);
      MakerSumo.stop();
      prevMillis = millis();
    }

    if (count > 2) {
      break;
    }
  }
}

void strategy6()
{
  // Turn left around 50 degrees.
  MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(40);

  // Go forward full speed.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.7);
  delay(350);

  searchDir = LEFT;
}

/*******************************************************************************
 * Search
 *******************************************************************************/
void search(int dir)
{
  // Move in circular motion.
  if (dir == LEFT) {
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.35);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.3);
  } else {
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.3);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.35);
  }
}

/*******************************************************************************
 * Attack
 * Track and attack the opponent in full speed.
 * Do nothing if opponent is not found.
 *******************************************************************************/
void attack()
{
  // Opponent in front center.
  // Go straight in full speed.
  if (digitalRead(OPP_FC) == LOW) {
    count++;
    if (count > 2000) {
      MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
      MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
    }
    else {
      MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax*0.5);
      MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax*0.5);
    }
  }

  // Opponent in front left.
  // Turn left.
  else if (digitalRead(OPP_FL) == LOW) {
    MakerSumo.setMotorSpeed(MOTOR_L, 0);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  }

  // Opponent in front right.
  // Turn right.
  else if (digitalRead(OPP_FR) == LOW) {
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, 0);
  }

  // Opponent in left.
  // Turn left.
  else if (digitalRead(OPP_L) == LOW) {
    count = 0;
    MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  }

  // Opponent in right.
  // Turn right.
  else if (digitalRead(OPP_R) == LOW) {
    count = 0;
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  }
}

/*******************************************************************************
 * Back Off
 * This function should be called when the ring edge is detected.
 *******************************************************************************/
void backoff(int dir)
{
  // Reverse
  MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  delay(100);

  // Rotate..
  if (dir == LEFT) {
    // Rotate left backward.
    MakerSumo.setMotorSpeed(MOTOR_L, -speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  } else {
    // Rotate right backward.
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, -speedRightMax);
  }
  delay(120);

  // Stop the motors.
  MakerSumo.stop();
  delay(50);
}

/*******************************************************************************
 * Motor Speed Alignment
 * Do alignment for motor speed using potentiometer.
 *******************************************************************************/
void motorSpeedAlignment()
{
  // Speed alignment for left and right motor, use potentiometer
  speedLeftMax = SPEED_MAX * 1;
  speedRightMax = SPEED_MAX * 1;

  int potentiometer = analogRead(POT);

  if (potentiometer < 460) {
    speedLeftMax = SPEED_MAX * ((float)map(potentiometer, 0, 460, 50, 100)/100);
    speedRightMax = SPEED_MAX * 1;
  }
  else if (potentiometer > 563) {
    speedLeftMax = SPEED_MAX * 1;
    speedRightMax = SPEED_MAX * ((float)map(potentiometer, 563, 1023, 100, 50)/100);
  }

  if (millis() - prevMillis > 500) {
    prevMillis = millis();

    ledState = !ledState;
    digitalWrite(LED, ledState);
  }
}

void playMelody(int *melody, int *noteDurations, int notesLength)
{
  for (int thisNote = 0; thisNote < notesLength; thisNote++) {
    int noteDuration = 720 / noteDurations[thisNote];
    tone(BUZZER, melody[thisNote], noteDuration);
    int pauseBetweenNotes = noteDuration * 1.10;
    delay(pauseBetweenNotes);
    noTone(BUZZER);
  }
}

/*******************************************************************************
 * Print Status
 * Print all input status e.g. sensors, battery, mode
 *******************************************************************************/
void printStatus()
{
  if (millis() - prevMillis > 250) {

    // Print value for Opponent Sensors.
    Serial.print("OPP_L:");
    Serial.print(digitalRead(OPP_L));

    Serial.print("  OPP_FL:");
    Serial.print(digitalRead(OPP_FL));

    Serial.print("  OPP_FC:");
    Serial.print(digitalRead(OPP_FC));

    Serial.print("  OPP_FR:");
    Serial.print(digitalRead(OPP_FR));

    Serial.print("  OPP_R:");
    Serial.print(digitalRead(OPP_R));
    
    // Print value for Edge Sensors (Analog).
    Serial.print("      EDGE_L:");
    Serial.print(analogRead(EDGE_L));

    Serial.print("  EDGE_R:");
    Serial.print(analogRead(EDGE_R));

    // Print value for DIP Switch Setting.
    Serial.print("      MODE:");
    Serial.print(MakerSumo.readDipSwitch());

    // Print value for Battery Voltage.
    Serial.print("  BATT:");
    Serial.print(MakerSumo.readBatteryVoltage());

    // Print line feed.
    Serial.println("");

    prevMillis = millis();
  }
}