Program for Robotic Swarm With Potential Issues

Hi everyone :D,

A side note before we begin, I'm just a newbie at electronics, and this project was way too hard without some help, yet I first created my own program for my Robotic swarm project. The goal of the whole swarm of robots is to find the colored tape at the end utilizing the distance of their surroundings and each other. How these robots will find the distance of each other is using infrared emitters and phototransistors to measure the led intensity for every robot. As soon as the intensity reaches a certain point, the robot will turn in the opposite direction from that robot. This will effectively create a swarm that is flocking and therefore a swarm of partical swarm optimization. When I came to test this with just one of the robots to see if it actually moves, it did not. The robot stood still, and yes, before I checked each component before soldering it on onto the robot. Everything functioned as they were supposed to. The servos alone could move around, the color sensor was functional, and the Arduino could operate and what not. The Leds and Phototransistors were also functional. Heres the code:

//Libraries
#include <FreqCount.h>
#include <MD_TCS230.h>
#include <Servo.h>

//Color Sensor Pins
#define s2 2
#define s3 4
#define out 5

//Infrared Leds and Phototransistors Pins
#define flr A4 //Front Left
#define frr A2 //Front Right
#define fr A3 //Front 
#define slr A5 //Side Left
#define srr A2 //Side Right
#define blr A6 //Back Left
#define brr A1 //Back Right
#define emit 6 //Infrared Led

//Servo Pins
Servo ls; //Left Servo
Servo rs; //Right Servo

//Color values
colorData  rgb; //Custom variable to store rgb

//Servo values
int lf = 180; //Left Forward
int rf = 0; //Right Forward
int lr = 0; //Left Reverse
int rr = 180; //Right Reverse
int stall = 90; //Stop

MD_TCS230 CS(s2, s3);

void setup() {
  pinMode(fr, INPUT);
  pinMode(flr, INPUT);
  pinMode(frr, INPUT);
  pinMode(srr, INPUT);
  pinMode(slr, INPUT);
  pinMode(blr, INPUT);
  pinMode(brr, INPUT);
  pinMode(emit, OUTPUT);
  ls.attach(9);
  rs.attach(10);
  CS.begin();
}

void loop() {
  digitalWrite(emit, HIGH);
  ls.write(lf);
  rs.write(rf); 
  irobjectdetect();
  readSensor();
}

void irobjectdetect() {
  if(flr <= 150) // front left ir receiver then turn right
  {
    turnr();
  }
  else if(frr <= 150) // front right ir receiver then turn left
  {
    turnl();
  }
  else if(fr <= 150) // front receiver then turn left
  {
    turnl();
  }
  else if(blr <= 150) // back left ir receiver then turn right
  {
    turnr();
  }
  else if(brr <= 150) // back right ir receiver then turn left
  {
    turnl();
  }
  else if(srr <= 150) // side right ir receiver then turn left
  {
    turnl();
  }
  else if(slr <= 150) // side left ir receiver then turn right
  {
    turnr();
  }
  else
  {
    ls.write(lf);
    rs.write(rf);
  }
}

void turnr() {
  ls.write(lf);
    for(int i = 0; i<2; i++)
    {
      rs.writeMicroseconds(2000);
      i++;
    }
    ls.write(lf);
    rs.write(rf);
}

void turnl() {
  rs.write(rf);
    for(int i = 0; i<2; i++)
    {
      ls.writeMicroseconds(2000);
      i++;
    }
    ls.write(lf);
    rs.write(rf);
}

void readSensor()
{
  if(CS.available()==0)
  {
    CS.read();
    CS.getRGB(&rgb);
  }
  if(rgb.value[TCS230_RGB_R] == 86 && rgb.value[TCS230_RGB_G] == 87 && rgb.value[TCS230_RGB_B] == 150)
    {
      ls.write(stall);
      rs.write(stall);
    }
}

The parts used for this project are two Fitec 9 gram servos, a TCS230 color sensor, 8 Infrared emitters or leds from Sparkfun, 7 Infrared Phototransistors from Sparkfun, an Arduino nano, and Adafruit's powerboost 500 basic. That last one is kinda irrelevant.

If you want to know more about the TCS230, you're unfortunately going to have to read the documentation for it, right here: http://arduinocode.codeplex.com/releases/view/115253, as it is RGB, and it uses different frequencies to read colors with its diodes and such, hence, the Frequency library. Although there were examples for TCS230, I wanted to make my own program for it instead of copying and pasting. I just studied the examples and learnt from it.

The Fitec servo are PWM controlled, and I use the servo library, and that link is this: https://www.arduino.cc/en/reference/servo, and the infrared phototransistor kinda speaks out for itself. It measures intensity of an infrared source, whether it's closer or farther, and that data is an analog value. It has no connection to measurement, although, the larger the number, the farther away, and the smaller the number, the shorter. 0 is never achievable. Only 20 is the lowest that you will ever go. The leds are digitally controlled, high is on, and low is off.

What I think of my own program is that to me, there are problems like the robot is unable to work because the chip is being bombarded with commands and it can't properly operate the servos and do all those loops. My second conclusion is that the arduino is stuck at a loop and is constantly trying to satisfy the parameters and statement needed to satisfy, even though it is not possible. And my last conclusion is that it is absolute trash and that it needs to be scrapped and started over. I hope not. For anyone reviewing the programming, can you please explain why it is trash before jumping to conclusions, and I hope it isn't what people will say :(.

Happy reading and reviewing.

Your project has many parts that in the end must work on their own, and finally together. You say your bot does not move. Perhaps that is a place to start. What is supposed to make your robot move?

zoomkat:
What is supposed to make your robot move?

Well pwm controls the servos, and the servos move based on what the infrared phototransistors read. The Ir leds are right next to the phototransistors, giving the robot object detection and communication with the other robots. What happens is, for both cases, if the number goes down 150, the analog value for the phototransistors, then depending on the phototransistor, there are 7, that corresponds to the turn that the robot has to make. The phototransistors are in a circle on the robot, like a crown, for the robot, and it covers multiple locations, and right next to each phototransistor is an infrared led.

Yada, yada, yada... Assuming your servos are modified for continuous rotation, have you gotten them to move as desired just controlling from the serial monitor? Most servo issues are due to inadequate power supply, or poor grounding between the servo and the arduino. Below is some servo test code you can try to see if you can get the servos to move and control via the serial monitor.

// zoomkat 3-28-14 serial servo incremental test code
// using serial monitor type a character (s to increase or a 
// to decrease) and enter to change servo position 
// (two hands required, one for letter entry and one for enter key)
// use strings like 90x or 1500x for new servo position 
// for IDE 1.0.5 and later
// Powering a servo from the arduino usually *DOES NOT WORK*.

#include<Servo.h>
String readString;
Servo myservo;
int pos=1500; //~neutral value for continuous rotation servo
//int pos=90;

void setup()
{
  myservo.attach(7, 400, 2600); //servo control pin, and range if desired
  Serial.begin(9600);
  Serial.println("serial servo incremental test code");
  Serial.println("type a character (s to increase or a to decrease)");
  Serial.println("and enter to change servo position");
  Serial.println("use strings like 90x or 1500x for new servo position");
  Serial.println();
}

void loop()
{
  while (Serial.available()) {
    char c = Serial.read();  //gets one byte from serial buffer
    readString += c; //makes the string readString
    delay(2);  //slow looping to allow buffer to fill with next character
  }
  if (readString.length() >0) {
    if(readString.indexOf('x') >0) { 
      pos = readString.toInt();
    }

    if(readString =="a"){
      (pos=pos-1); //use larger numbers for larger increments
      if(pos<0) (pos=0); //prevent negative number
    }
    if (readString =="s"){
      (pos=pos+1);
    }

    if(pos >= 400) //determine servo write method
    {
      Serial.println(pos);
      myservo.writeMicroseconds(pos);
    }
    else
    {   
      Serial.println(pos);
      myservo.write(pos); 
    }
  }
  readString=""; //empty for next input
}

I see two problems.

  1. You define A2 to be both frr and srr.

  2. You are comparing pin numbers to 150, you're not reading the pins. I think you want:

if(analogRead(flr) <= 150)

zoomkat:
Below is some servo test code you can try to see if you can get the servos to move and control via the serial monitor.

Worked swimmingly well for my robot. changed it up and down and responded quickly and adequately. Servo is not the main issue. I think the program is wrong, because i feel like that in the main program, one loop is just being repeated over and over, and can't stop because it can never reach the parameters.

if(flr <= 150) // front left ir receiver then turn right

See #2

flr is defined as A4, so that's always going to be less than 150 on any Arduino board. Same issue with all the rest of those tests.

Delta_G:

if(flr <= 150) // front left ir receiver then turn right

flr is defined as A4, so that's always going to be less than 150 on any Arduino board. Same issue with all the rest of those tests.

Fixed it. Here's that whole section revised:

 void irobjectdetect() {
  if(analogRead(flr) <= 150) // front left ir receiver then turn right
  {
    turnr();
  }
  else if(analogRead(frr) <= 150) // front right ir receiver then turn left
  {
    turnl();
  }
  else if(analogRead(fr) <= 150) // front receiver then turn left
  {
    turnl();
  }
  else if(analogRead(blr) <= 150) // back left ir receiver then turn right
  {
    turnr();
  }
  else if(analogRead(brr) <= 150) // back right ir receiver then turn left
  {
    turnl();
  }
  else if(analogRead(srr) <= 150) // side right ir receiver then turn left
  {
    turnl();
  }
  else if(analogRead(slr) <= 150) // side left ir receiver then turn right
  {
    turnr();
  }
  else
  {
    ls.write(lf);
    rs.write(rf);
  }
}

After that and plugging it into the robot, it still will not move. It just stands still. I'm thinking that the program gets stuck in this part. Reading every if loop and else if loop and looping around and around, trying to check for any changes, and the servo command is being ignored.

void turnr() 
{
  ls.write(lf);
    for(int i = 0; i<2; i++)
    {
      rs.writeMicroseconds(2000);
      i++;
    }
    ls.write(lf);
    rs.write(rf);
}

What's the for loop for? (why the double increment too?)
Are you worried "rs" will forget what you told it first time through?

AWOL:
What's the for loop for? (why the double increment too?)
Are you worried "rs" will forget what you told it first time through?

That for loop works to turn the right servo 2 times backwards for that certain degree, ratio, whatever. The left servo is just moving straight, and the right servo works together to turn it to the right. I thought it was necessary to set a certain amount of rotations since one rotation will not be enough for the servo.

That for loop works

I very much doubt it.
Have you tested it in isolation?

AWOL:
Have you tested it in isolation?

Yep, just did right now, it doesn't work.
Ok, back to the drawing board :smiley:

Here's a different approach:

rservo.writeMicroseconds(1000);
  lservo.writeMicroseconds(2000);
  delay(3000);
  rservo.writeMicroseconds(1000);
  lservo.writeMicroseconds(1500);
  delay(1200);
  rservo.writeMicroseconds(1000);
  lservo.writeMicroseconds(2000);
  delay(3000);
  rservo.writeMicroseconds(1500);
  lservo.writeMicroseconds(2000);
  delay(1200);

Just simply that.

A Servo write happens very quickly - probably quicker than even the shortest servo control pulse, certainly quicker than the 20ms servo frame time, so doing two back-to-back, and then writing a different value isn't even going to be seen by the servo.