By jhcho1028
#7298
The product name is "insect bot Hexa".
The source code was uploaded according to the manual.
Go forward well, but if it meets an obstacle, only turn left.
If an obstacle is in the left, I want it turn right.
How can I modify the source code?

Code: Select all

#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;
int leftEye = 0;
int rightEye = 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.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;
  }
 
  // Sorting the left light sensor values
  sort(leftReadings,11);
  left = leftReadings[5];
 
  // Sorting the right light sensor values
  sort(rightReadings,11);
  right = rightReadings[5];
 
  Serial.println(left);
  Serial.println(right);
}

// 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()
{
  scan();
 
  if (left > right)
  {
    left = left-right;
    if (left > lightDifference)
    {
      lightLeft = true;
      lightRight = false;
      //Serial.print("turn left");
    }
    else
    {
      lightLeft = true;
      lightRight = true;
      //Serial.println("go straight1");
    }
  }
  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;
      //Serial.print("turn right");
    }
    else
    {
      /* Is the calculated value for right 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;
      //Serial.println("go straight2");
    }
  }
  /* 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
  {
      lightLeft = true;
      lightRight = true;
      //Serial.println("go straight3");
  }
}

// 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()
{
  scan();
  //  read the IR distance sensor value 
  sensorValue = analogRead(distanceSensor);
//  Serial.print ("Distance: ");
//  Serial.print(sensorValue);
  if (sensorValue < danger)
  {
    decision();
  }
  else
  {
    Serial.println("sensorValue > danger");
    frontLeg.write(90);
    rearLeg.write(90);
    midLeg.write(90);
    delay(1000);
    for (byte x=0; x<5; x+=1)
    {
      Serial.println("reverse()");
      reverse();
    }
    delay(300);
    for (byte x=1; x<5; x+=1)
    {
      Serial.println("only left");
      leftTurn();
    }
    delay(300);
  }
  decision();
  if(lightLeft == true && lightRight == true)
  {
    Serial.println("forward()");
    forward();
    delay(5);
  }
  else if (lightLeft == false && lightRight == true)
  {
    Serial.println("rightTurn()");
    rightTurn();
    delay(5);
  }
  else if (lightLeft == true && lightRight == false)
  {
    Serial.println("leftTurn()");
    leftTurn();
    delay(5);
  }
  else
  {
    Serial.println("stay()");
    stay();
  }
}

By Grey.CC
#7309
Hi,

It seems that there are some offset with light sensor. You will need a zero adjustment.
int lightSensorLeft = A2;
int lightSensorRight = A0;

This are two Analog port which insect are using to read analog light value. You could use the analog simple code to read the light value.
Code: Select all
void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
}

// the loop routine runs over and over again forever:
void loop() {
  // read the input on analog pin 0:
  int sensorValue = analogRead(A0);    //Right is A0; Left is A2
  // print out the value you read:
  Serial.println(sensorValue);
  delay(100);        // delay in between reads for stability
}


Open the serial monitor,check the value, with the same light, what is their value? Record their value, and make them balance in the code. For example with same light, rightsensor value is higher 12 than leftsensor, then minus 12 on the right side.
Code: Select all
sensorValueRight = analogRead(lightSensorRight)-12;
. It should be OK