Ikedo Mini V2 sumo robot with motor speed alignment, auto detection for edge and strategy (half moon) at DIP 1 and 4.

IkedoMiniV2WithStrategies.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 is for the Autonomous Sumo Robot with:
 * - motor speed alignment
 * - auto detection for edge
 * - strategy 1 and 4
 *
 * AUTHOR   : Idris Zainal Abidin
 * COMPANY  : Cytron Technologies Sdn Bhd
 * WEBSITE  : my.cytron.io
 * EMAIL    : [email protected]
 *
 *******************************************************************************/

#include "CytronMakerSumo.h"

// Edge Sensor Threshold Differences.
#define EDGE_THRESHOLD_DIFF  70

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

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

// 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 edgeBlackLeft = 0;
int edgeBlackRight = 0;
int speedLeftMax, speedRightMax;
long prevMillis = 0;
bool ledState = 0;

/*******************************************************************************
 * Setup
 * This function runs once after reset.
 *******************************************************************************/
void setup() {
  MakerSumo.begin();

  motorSpeedAlignment();

  // To enter motor speed alignment mode.
  // Press and hold START button, press RST button
  if (!digitalRead(START)) {
    MakerSumo.playMelody(melody2Pitch, melody2Duration, 3);
    while (!digitalRead(START));

    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
      motorSpeedAlignment();

      // Press START button to test
      if (!digitalRead(START)) {
        digitalWrite(LED, HIGH);
        delay(1000);

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

        MakerSumo.stop();
      }
    }
  }

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

  // Wait until start button is pressed and released.
  while (digitalRead(START));
  while (!digitalRead(START));

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

  // Read edge sensor for black surface
  edgeBlackLeft = analogRead(EDGE_L);
  edgeBlackRight = analogRead(EDGE_R);
  
  delay(5000);

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

/*******************************************************************************
 * Main program loop.
 *******************************************************************************/
void loop() {
  // Edge is detected on the left.
  if (analogRead(EDGE_L) < edgeBlackLeft - EDGE_THRESHOLD_DIFF) {
    // 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) < edgeBlackRight - EDGE_THRESHOLD_DIFF) {
    // 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 if the button is pressed.
  if (digitalRead(START) == 0) {
    // Stop the motors.
    MakerSumo.setMotorSpeed(MOTOR_L, 0);
    MakerSumo.setMotorSpeed(MOTOR_R, 0);

    // Loop forever here.
    while (1);
  }
}

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

  if (dip == 0) {
    strategy0();
  }
  else 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 if (dip == 7) {
    strategy7();
  }
}

void strategy0() {
  
}

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

  // Go curve around the edge.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax/1.6);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  delay(800);

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

  searchDir = RIGHT;
}

void strategy2() {

}

void strategy3() {

}

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

  // Go curve around the edge.
  MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
  MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax/1.6);
  delay(800);

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

  searchDir = LEFT;
}

void strategy5() {

}

void strategy6() {

}

void strategy7() {

}

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

/*******************************************************************************
 * 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) {
    MakerSumo.setMotorSpeed(MOTOR_L, speedLeftMax);
    MakerSumo.setMotorSpeed(MOTOR_R, speedRightMax);
  }

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

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

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

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

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

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

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