_SKU_KIT0090__六足虫虫机器人高级玩法 - jimaobian/DFRobotWikiCn GitHub Wiki

#include <Servo.h>
// creating the servo objects for front, rear and mid servo
Servo frontLeg;
Servo rearLeg;
Servo midLeg;
// setting the servo angle to 90° for startup
byte frontAngle = 90;
byte rearAngle = 90;
byte midAngle = 90;
// setting the delay value
byte delayWalk = 2;
byte delayTurn = 3;
// Analog sensor pins
int distanceSensor = A1;
int lightSensorLeft = A2;
int lightSensorRight = A0;
// Analog sensor reading
int sensorValue = 0;
// Values
int sensorValueLeft = 0;
int sensorValueRight = 0;
int left = 0;
int right = 0;
// Change the following value to decrease or increase the sensitivity.
// Bigger value for lower sensitivity and smaller value for highter sensitivity
int lightDifference = 60;
// Decrease the danger value when you want to INCREASE the collision trigger distance.
// Increase the danger value when you want to DECREASE the collision trigger distance
int danger = 450;  // Increase this value when you want to DECREASE the collision trigger distance
// Arrays for sensor reading average calculation
int leftReadings[11];
int rightReadings[11];
// Booleans for decision
boolean lightLeft = false;
boolean lightRight = false;
// Setup function
void setup(){
  // serial connection for debugging
  Serial.begin(9600);
  // attaching the servos to their pins
  frontLeg.attach(9);
  rearLeg.attach(10);
  midLeg.attach(11);
  // move servos to center position -> 90°
  frontLeg.write(frontAngle);
  rearLeg.write(rearAngle);
  midLeg.write(midAngle);
  delay(2000);

}
// Light detection and filtering
void scan()
{
    int i;
        // Take 5 readings on each sensor
    for (i = 0; i < 11; i = i + 1) {
        // read the value from the left and right sensor:
                sensorValueLeft = analogRead(lightSensorLeft);
                sensorValueRight = analogRead(lightSensorRight);
                // add sensor readings of both sides to their respective array
        leftReadings[i] = sensorValueLeft;
                rightReadings[i] = sensorValueRight;
        }
// calculate an average value for left and right sensor readings
// Sorting the left light sensor values
sort(leftReadings,11);
    left = leftReadings[5];
sort(rightReadings,11);
     right = rightReadings[5];
}
// Sort the array ascending
void sort(int a[], int size) {
    for(int i=0; i<(size-1); i++) {
        for(int o=0; o<(size-(i+1)); o++) {
                if(a[o] > a[o+1]) {
                    int t = a[o];
                    a[o] = a[o+1];
                    a[o+1] = t;
                }
        }
    }
}
// Desision depending on the light values
void decision(){
// Call the scan function to provide the sensor values of the left and right light sensor
  scan();
// Compare the values and make a decision according the difference value
if (left > right){
    // subtract right from left value to get a value to work with
    left = left-right;
    // check if that previous calculated value is greater than 50
    if (left > lightDifference) // I will call this "Inside IF 1" in further comments.
    {
      lightLeft = true;
      lightRight = false;
    }
    // That happens when the left value in "Inside IF 1" is lower than 50
    else{
      /* Is the calculated value for left lower than 50 then go forward. Why? Is that value lower than 50 then the difference
         between the light on the left and right sensor are not that great. In this case it should be save to go forward.
      */
      lightLeft = true;
      lightRight = true;
    }
  }
  // That happens when left is lower than right
  else if (left < right){
    // subtract left from right value to get a value to work with
    right = right-left;
    // check if that previous calculated value is greater than 50
    if (right > lightDifference){  // I will call this "Inside IF 2" in further comments.
      lightLeft = false;
      lightRight = true;
    }
    else{
      lightLeft = true;
      lightRight = true;
    }
  }
  /* That "else" happens when none of the above conditions occur and the left and right sensor shows the same readings.
     This condition will probably never occur but we need it anyway.
  */
  else{
    // Go forward without questions
      lightLeft = true;
      lightRight = true;
  }
}
// Walk forward //////////////////////////////////////////////////////////
void forward(){

  for (midAngle = 70; midAngle < 100; midAngle +=1){
    midLeg.write(midAngle);
    delay(delayWalk);
  }
  for (frontAngle = 120; frontAngle > 50; frontAngle -= 1){
    frontLeg.write(frontAngle);
    rearLeg.write(frontAngle);
    delay(delayWalk);
  }
  for (midAngle = 100; midAngle > 70; midAngle -=1){
    midLeg.write(midAngle);
    delay(delayWalk);
  }
  for (frontAngle = 50; frontAngle < 120; frontAngle += 1){
    frontLeg.write(frontAngle);
    rearLeg.write(frontAngle);
    delay(delayWalk);
  }
}
// Walk reverse //////////////////////////////////////////////////////////
void reverse(){

  for (midAngle = 70; midAngle < 100; midAngle +=1){
    midLeg.write(midAngle);
    delay(delayWalk);
  }
  for (frontAngle = 50; frontAngle < 120; frontAngle += 1){
    frontLeg.write(frontAngle);
    rearLeg.write(frontAngle);
    delay(delayWalk);
  }

  for (midAngle = 100; midAngle > 70; midAngle -=1){
    midLeg.write(midAngle);
    delay(delayWalk);
  }
  for (frontAngle = 120; frontAngle > 50; frontAngle -= 1){
    frontLeg.write(frontAngle);
    rearLeg.write(frontAngle);
    delay(delayWalk);
  }
}
// Left Turn //////////////////////////////////////////////////////////
void leftTurn(){

  rearLeg.write(90);
  for (midAngle = 70; midAngle < 110; midAngle += 1){
    midLeg.write(midAngle);
    delay(delayTurn);
  }
  for (frontAngle = 70; frontAngle < 110; frontAngle +=1){
    frontLeg.write(frontAngle);
    delay(delayTurn);
  }
  for (rearAngle = 110; rearAngle > 70; rearAngle -=1){
    rearLeg.write(rearAngle);
    delay(delayTurn);
  }
  for (midAngle = 110; midAngle > 70; midAngle -= 1){
    midLeg.write(midAngle);
    delay(delayTurn);
  }
  for (frontAngle = 110; frontAngle > 70; frontAngle -=1){
    frontLeg.write(frontAngle);
    delay(delayTurn);
  }
  for (rearAngle = 70; rearAngle < 110; rearAngle +=1){
    rearLeg.write(rearAngle);
    delay(delayTurn);
  }
}
// Right Turn //////////////////////////////////////////////////////////
void rightTurn(){

  frontLeg.write(90);
  for (midAngle = 70; midAngle < 110; midAngle += 1){
    midLeg.write(midAngle);
    delay(delayTurn);
  }
  for (rearAngle = 70; rearAngle < 110; rearAngle +=1){
    rearLeg.write(rearAngle);
    delay(delayTurn);
  }
  for (frontAngle = 110; frontAngle > 70; frontAngle -=1){
    frontLeg.write(frontAngle);
    delay(delayTurn);
  }
  for (midAngle = 110; midAngle > 70; midAngle -= 1){
    midLeg.write(midAngle);
    delay(delayTurn);
  }
  for (rearAngle = 110; rearAngle > 70; rearAngle -=1){
    rearLeg.write(rearAngle);
    delay(delayTurn);
  }
  for (frontAngle = 70; frontAngle < 110; frontAngle +=1){
    frontLeg.write(frontAngle);
    delay(delayTurn);
  }
}
// Stop walking
void stay(){
    frontLeg.write(90);
    midLeg.write(90);
    rearLeg.write(90);
}
//////////////////////////////////////////////////////////////
void loop(){
  // First scan for the light
  scan();
  //  read the IR distance sensor value
  sensorValue = analogRead(distanceSensor);
  if (sensorValue < danger){
    decision();
  }
  else {
    frontLeg.write(90);
    rearLeg.write(90);
    midLeg.write(90);
    delay(1000);
    for (byte x=0; x<5; x+=1){
      reverse();
    }
    delay(300);
    for (byte x=1; x<5; x+=1){
      leftTurn();
    }
    delay(300);
  }
  decision();
  if (lightLeft == true && lightRight == true){
  forward();
  // Serial.println("left = right");
  delay(5);
  }
  else if (lightLeft == false && lightRight == true){
  rightTurn();
  // Serial.println("left < right");
  delay(5);
  }
  else if (lightLeft == true && lightRight == false){
  leftTurn();
  // Serial.println("left > right");
  delay(5);
  }
  else {
  stay();
  // Serial.println ("Nothing to complain");
  }
}