Computer Vision Balance Bot

Keith Young

I'm making a balancing robot that can navigate mazes. I currently have the computer vision system solving the mazes using OpenCV on a Raspberry Pi B+. I'm now trying to get the balancing bit to work...

For now I'm trying to do this with an ActivityBot with a counterweight at the front.

Here goes my first attempt at putting in code in the new forums...

Well that failed faster than I figured. Says my body is too long... I guess I'll try attaching it for now?

Here is the problem

After a while of running, the left wheel will stop while the right wheel keeps on spinning. What gives? I am setting both sides of drive_rampstep the same. In fact it's drive_rampstep(dl,dl) in order to test if I was crazy or if indeed somehow the bot was going bonkers.

Can anyone shed some light on how one wheel can be stopping while the other keeps on spinning, when both are set to the same speed?

I'm using SimpleIDE on Windows at the moment.

Thanks

PS after this is working I hope to share how I did it. For now I'm on a deadline so the Raspi install, OpenCV install, OpenCV code etc will have to wait a couple weeks.

Keith Young

After some troubleshooting I think the battery was too low, and it may have been sending the bot to revert to a different program.

It now balances but is very jittery. I'm going to start working on filtering the data and smoothing the response.

Once I have the robot balancing I'll start designing my power system for the robot. I should be receiving the actual camera I'll be using at which point I can test the realtime maze solving capabilities.

One motor does clearly go faster than the other so I'll need to use another gyroscope axis and try to correct for unwanted turning of the ActivityBot.

Hopefully I can get a decent chassis together and have it all wrapped up in time for Maker Faire Detroit. Otherwise I'll have to revert to my old simpler bots.

More to come.

Keith Young

I've now got it balancing but it is very jittery and sometimes the "training wheels" are touching the ground. Sometimes in fact for now the "training wheels" are smacking pretty hard.

Some of the information I've been using:

http://roboticssamy.blogspot.pt/2013/11/rs4-robot.html#comment-form

https://github.com/TKJElectronics/BalancingRobotArduino

http://robotics.ee.uwa.edu.au/theses/2003-Balance-Ooi.pdf

Keith Young

Do you guys know how to get consistent known loop times while sampling sensors and doing a bunch of math?

I know I could go for long pauses, which would give me a smaller percentage of error in estimating the loop time, but that would make the robot more unstable.

I'm using C in SimpleIDE.

I need the accurate loop time for a DT when estimating angle based on the gyroscope and accelerometer.

JasonDorie

The simplest way to do consistent loop timing is something like this:
int loopTime = cnt; // Get the current system counter
while (true) // Repeat forever{ // Run some code here

loopTime += 80000000 / 400; // Run at 400Hz, assuming an 80MHz clock waitcnt( loopTime );}
(note that this is untested - the names of things may be incorrect - I usually code in Spin)

Keith Young

I don't know that would work, but it does give me an idea...

while(1)
  target_cnt = cnt + clkfreq/20
  calculate a bunch of stuff
  while(cnt < target_cnt)
    pause(1)

Thanks Jason, you're a genius!

DigitalBob

Try looking on one robot . Look at dance bot, viewport etc. I forget the guys name something like Shano but he came up with this viewport and the two wheel dance bot.4

Publison

His name is Hanno Sander. Here is his web siteLhttp://onerobot.org/
and the Dancebot:http://onerobot.org/products/dancebot/

He wrote a whole chapter on the dancebot in this book:
https://www.parallax.com/product/32316

Keith Young

Yep thanks for the heads up.

Right now I'm hung up on getting halfway decent angle measurements quickly despite the Gyro and Accelerometer seeming to only put out integers. By the time I have a full degree of error it takes a fair bit of correction, let alone that by the time it's measured it might be 2 degrees with the addition of some angular velocity.

I could try a Kalman Filter but the robot is very sensative to the speed of the loop even with such a simple estimating algorithm I'm using (similar to complementary filter).

Circuitsoft

  while(cnt < target_cnt)
    pause(1)



waitcnt() does exactly that, but the cog will use less power while in waitcnt state, but it takes zero clocks for it come out of that state, so by using waitcnt, you actually get more precise timing, and it uses less power.

Keith Young

I've been looking around for ways to communicate between the Raspberry Pi and the Prop. So far I haven't found any code that seems to work without significant modification. Does anyone know of working code that:

Is in c++ on the Pi
Is in c for the Prop (SimpleIDE)
Prefer using USB rather than GPIO, but will take what I can get.

The robot can now solve the mazes through computer vision and I need to start sending drive commands to the Prop which is on an ActivityBot.

After Maker Faire Detroit this weekend I intend to get everything I have on here to make it easier for anyone else trying to make a similar robot in the future. Hopefully we can build on eachothers efforts and make some pretty awesome Pi/Prop Bots.

Thanks for all the help so far.

ratronic

Keith I have some C++ code for the Pi using OpenCV. With it I communicate to the Propeller using serial

through either the serial port or a USB port. I use the WiringPi library for C++ serial on the Pi.

Keith Young

Awesome! Is it something you can paste in to here?

What did you do with your robot out of curiosity?

ratronic

It is a version of the Python program I posted in the other thread. Before I post it I need to know which version

of OpenCV you are using. With that information I can post the command line to compile it with.

Edit: It doesn't solve mazes but it track's color objects and moves a servo pan/tilt camera platform to track the

object.

Keith Young

2.4.11

ratronic

Ok give me a little bit and I will post it.

ratronic

The version you are using is real close to the 3.0 version. So on the command line you could take off

the -lopencv_videoio if it gives you a problem. Also you can find the WiringPi library here -

http://wiringpi.com/

Here is the command line I use to compile -

g++ -Wall -o lgj lgj.cpp -lwiringPi -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_videoio

Edit: I am having a problem attaching the file. I can post the code if someone tells me the magic code blocks to use at the start and end.

ratronic

Until I figure out how to use the new Forum I will just paste it here.

Edit: This file is called lgj.cpp

Edit2: There is no error checking in this. You will have to make sure that you have a /dev/video0 and a /dev/ttyUSB0 before running the program.

#include <sstream>    /* track largest red object and send serial screen location/area */
#include <opencv2/highgui/highgui.hpp>  /* video A = live video plus text info and tracking square, video B = binary masked objects */
#include <opencv2/imgproc/imgproc.hpp>
#include <wiringSerial.h>

using namespace cv;
using namespace std;

int main() {
  VideoCapture capture(0);
//  int fd(serialOpen("/dev/ttyAMA0", 115200));
  int fd(serialOpen("/dev/ttyUSB0", 115200));
  int big, fps;
  Mat image, imgCopy, hsv, thresh, threshCopy;
  stringstream ss;
  namedWindow("A");
  namedWindow("B");
  moveWindow("A", 0, 100);
  moveWindow("B", 500, 100);

  while(1) {
    int t = (int)getTickCount();
    capture >> image;
    resize(image, image, Size(200, 140));
    image.copyTo(imgCopy);
    blur(image, image, Size(5, 5));
    cvtColor(image, hsv, CV_BGR2HSV);
    inRange(hsv, Scalar(0, 35, 35), Scalar(3, 255, 255), thresh);
    thresh.copyTo(threshCopy);
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    findContours(thresh, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
    vector<Moments> mu(contours.size());
    int maxArea = 0;

    for (unsigned int cnt(0); cnt < contours.size(); cnt++) {
      int area = contourArea(contours[cnt]);
      mu[cnt] = moments(contours[cnt], false);
      if (area > maxArea) {
        maxArea = area;
        big = cnt;
      }
    }

    if (maxArea > 0) {
      int x = mu[big].m10 / mu[big].m00;
      int y = mu[big].m01 / mu[big].m00;
      rectangle(imgCopy, Point(x - 5, y - 5), Point(x + 5, y + 5), Scalar(255, 255, 255), 2, 8, 0);
      ss.str("");
      ss << "FPS " << fps;
      putText(imgCopy, ss.str(), Point(0, 15), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 255), 2, 8, 0);
      ss.str("");
      ss << "X " << x;
      putText(imgCopy, ss.str(), Point(0, 35), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 255), 2, 8, 0);
      ss.str("");
      ss << "Y " << y;
      putText(imgCopy, ss.str(), Point(0, 55), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 255), 2, 8, 0);
      ss.str("");
      ss << "A " << maxArea;
      putText(imgCopy, ss.str(), Point(0, 75), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 255), 2, 8, 0);
      serialPuts(fd, "!~");
      serialPutchar(fd, x);
      serialPutchar(fd, y);
      serialPutchar(fd, maxArea & 0xff);
      serialPutchar(fd, maxArea >> 8);
    }
    imshow("A", imgCopy);
    imshow("B", threshCopy);
    if (waitKey(1) == 27) {   /* escape key exits program */
      serialClose(fd);
      capture.release();
      destroyAllWindows();
      break;
    }
    fps = getTickFrequency() / ((int)getTickCount() - t);
  }
}

Keith Young

Thanks will see if I can get that working shortly!

ratronic

Keith I tried a copy and paste from the forum to make sure it works. There is a problem copying from the

code I posted here. It is with the last serialPutchar instruction. It should look like this -

serialPutchar(fd, maxArea >> 8)

Edit:OK it is maxArea shifted right 8 bits.

Keith Young

Thanks a bunch. I'm currently having trouble with wiringPi so I'm trying this at the moment:

http://forums.parallax.com/discussion/145332/raspberry-pi-and-propellor

The Pi compiles and the old software runs fine. It detects the Prop. Now I'm trying to get the Prop to read values sent from the Pi.

The goal is to say something like (Turn or Straight) (Degrees or Distance).

ratronic

If you are using serial to talk to the Propeller there are other libraries you could probably use. I picked WiringPi

because of all of the access it gives to the Pi's GPIO stuff. Good luck and have fun with your project!

Keith Young

Yeah thanks ratronic. I might end up going that route I am just a bit newbish when it comes to building and making etc in Linux.

I can currently light up the rx light on the ActivityBoard. I just can't currently get my int to change apparently.

ratronic

I know all about newbish with this stuff. You can include me. Maybe you could post the code you are having a

problem with? Maybe someone can give you some help.

Keith Young

It's on my Pi and the wifi dongle is out for the count.

Currently the serial portion of my Pi code looks like this:

#include <termios.h>
#include <sys/fcntl.h>
struct termios termOptions;
char port[1024];
int ttyFid;
ttyFid = open( "/dev/ttyUSB0", O_RDWR );
tcgetattr( ttyFid, &termOptions );
cfsetispeed( &termOptions, B115200);
cfsetospeed( &termOptions, B115200);
tcsetattr( ttyFid, TCSANOW, &termOptions );
char ch;
ch = 10;
while(1)
{
  write( ttyFid, &ch, sizeof(ch) );
}

My prop is just blinking an LED on a loop trying to rx. If it gets the rx right it should blink the light much slower.

The code is currently returning a -1 at tcgetattr( ttyFid, &termOptions ) but nothing blows up. I still get the red rx light on the ActivityBoard.

Keith Young

My primary references are:

http://forums.parallax.com/discussion/145332/raspberry-pi-and-propellor/p1

Keith Young

Current Prop code:

#include "simpletools.h"
#include "abdrive.h"

#include "fdserial.h"




fdserial *comms;

//#include <math.h>




int acc = 1;



int main()

{




comms = fdserial_open( 30, 31, 0, 115200);




while(1)

{

fdserial_rxTime( acc, 100 );

//readChar(31, acc);

high(27);

pause(acc*125);

low(27);

pause(acc*125);

if( acc == -1 ) high(26);

}

}

ratronic

Keith I was interested in the example you posted. It looks like you are trying to output a char(10) linefeed

constantly to the Propeller. I made some changes to the Pi code to get it to compile.

#include <unistd.h>
#include <termios.h>
#include <sys/fcntl.h>

int main() {
  struct termios termOptions;
  char port[1024];
  int ttyFid;
  ttyFid = open("/dev/ttyUSB0", O_RDWR);
  tcgetattr(ttyFid, &termOptions);
  cfsetispeed(&termOptions, B115200);
  cfsetospeed(&termOptions, B115200);
  tcsetattr(ttyFid, TCSANOW, &termOptions);
  char ch;
  ch = 10;
  while(1) {
    write(ttyFid, &ch, sizeof(ch));
  }
}

Keith Young

Ultimately this isn't what I want. It's just an easy way to see that my blinking pause increases significantly.

I don't really see the change besides the new include, some excluded includes, and adding int main. Am I missing something? I do have an int main etc, I just have ~300 lines unrelated to serial and therefore only included the parts related to serial.

ratronic

I have never used serial this way on the Pi. I use only simple stuff. The code I posted does compile with a

warning about the unused variable 'port'. Probably because you are not receiving anything. But it does open

/dev/ttyUSB0 and outputs the character.

Keith Young

Yeah port is unused I'm just mostly copying and pasting. Maybe things are wrong on the prop end?

I've also tried:

write( ttyFid, "%d", ch );

Nothing seems to work. My light won't blink slower. The rx light on the ActivityBoard does blip red however.