Robotics

Print
PDF
21
October
2012

Adventures in Control Theory: Pt. 1

Lately I've been expanding my knowledge in control theory in preparation for a complex automated process control system that I'm designing. This is the first installation in a series of articles detailing my "Adventures in Control Theory" as I progress through different types of control systems and take them from theory to practical use. Of course, I'm uploading all of this in the hopes that it will be useful to somebody else trying to learn how to use these theories properly.

Mathematically, the control function for an ideal PID control can be written as shown below:

The above equation describes a PID control loop in the continuous-time domain. In order to translate the above equation into code, we'll need to create a form in the discrete-time domain, like below:

I've implemented this discrete equation into a Python class, as seen below:

 import time 

class PID:    
    def __init__(self):
        self.Kp = 0
        self.Kd = 0
        self.Ki = 0
        self.initialize()
        
    def SetKp(self, Kp):
        self.Kp = Kp
    
    def SetKi(self, Ki):
        self.Ki = Ki
        
    def SetKd(self, Kd):
        self.Kd = Kd
        
    def SetPrevErr(self, preverr):
        self.prev_err = preverr
        
    def initialize(self):
        self.currtm = time.time()
        self.prevtm = self.currtm 
        self.prev_err = 0
        self.Cp = 0
        self.Ci = 0
        self.Cd = 0
        
    def GenOut(self, error):
        """ Performs a PID computation and returns a control value based on the
        elapased time (dt) and the error signal from a summing junction. """
        
        self.currtm = time.time()       #get t
        dt = self.currtm - self.prevtm  #get delta t
        de = error - self.prev_err      #get delta error
        
        self.Cp = self.Kp * error   #proportional term
        self.Ci += error * dt       #integral term
        
        self.Cd = 0
        if dt > 0:                  #no div by zero
            self.Cd = de/dt         #derivative term
            
        self.prevtm = self.currtm   #save t for next pass
        self.prev_err = error       #save t-1 error
        
        return self.Cp + (self.Ki * self.Ci) + (self.Kd * self.Cd) 

Read on to see how I create a simulation using Python to demonstrate the effects of the three tuning constants on the control response of the algorithm!

Print
PDF
01
April
2012

Synchronizing variables between two RoboRealm instances

This weekend I wrote a quick Python script to do a one-way synchronization of variables between two RoboRealm instances in the background. My code makes use of the RoboRealm API to access variables from the first instance and set them into the second one. Each instance must have a unique API port, which can be set via the command line when starting RoboRealm using the -api_port flag or by simply saving unique settings in each instance (preferred). I'm publishing the code here just in case it's useful for somebody else - it only took me five minutes to write but it could save somebody hours of frustration if they've never worked with the RoboRealm API before.

Feel free to modify or use my code for your own use - but feel free to give me a shout-out if you found this helpful!

 import socket,re

# socket read/write timeout in seconds
TIMEOUT = 30

############################# RR API CLASS ##################################
#precompiling all needed regular expressions
VarParReq=re.compile('<[^>]+>([^<]*)]+>')
HitWidReq=re.compile('([^<]*)([^<]*)')

class RR_API:
  def __init__(self):
    #create an INET, STREAMing socket
    self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    #only wait TIMEOUT seconds for any request
    self.sock.settimeout(TIMEOUT)

  def escape(self, str):
    str = str.replace("&", "&")
    str = str.replace("<", "<")
    str = str.replace(">", ">")
    return str

  def write(self, msg, msgLen):
    totalsent = 0
    # keep sending while there is more data to send
    while totalsent < msgLen:
      sent = self.sock.send(msg[totalsent:msgLen])
      if sent == 0:
        raise RuntimeError, "socket connection broken"
      totalsent = totalsent + sent

  # Buffered socket image read. Since we don't know how much data was read from a
  # previous socket operation we have to add in any previously read information
  # that may still be in our buffer. We detect the end of XML messages by the
  #  tag but this may require reading in part of the image data that
  # follows a message. Thus when reading the image data we have to move previously
  # read data to the front of the buffer and continuing reading in the
  # complete image size from that point. 
  def readMessage(self):
    msg = ""
    while True:
      byte = self.sock.recv(1)
      if byte == '':
        raise RuntimeError, "socket connection broken"
      msg = msg + byte
      if (msg[-11:] == ""):
        return msg

  # Initiates a socket connection to the RoboRealm server
  def Connect(self, hostname, port):
    self.sock.connect((hostname, port))

  # close the socket handle
  def close(self):
    self.sock.close()

  # Returns the value of the specified variable.
  def GetVariable(self, name):
    self.sock.send(""+str(name)+"")
    data = self.readMessage()
    m = VarParReq.match(data)
    if m:
      value = m.group(1)
    else:
      value = ""
    return value

  # Sets the value of the specified variable.
  def SetVariable(self, name, value):
    self.sock.send(""+self.escape(str(name))+""+self.escape(str(value))+"")
    if (self.readMessage() == "ok"):
      return 1
    else:
      return 0
      
def SyncVariable(instance1variable, instance2variable):
    instance2.SetVariable("%s"%instance2variable, instance1.GetVariable("%s"%instance2variable))
############################# Test program ##################################

# initialize the API class
instance1 = RR_API()
instance2 = RR_API()

#connect to RoboRealm - (host, port)
instance1.Connect("localhost", 6060)
instance2.Connect("localhost", 6061)

while True:
    # Syntax for syncing variables: SyncVariable("Variable_to_get_from_instance_1", "Variable_to_set_in_instance_2")
    SyncVariable("IMAGE_COUNT", "imagecount_instance_1")
    SyncVariable("FPS", "Frames_per_second_instance_1")

instance1.close()
instance2.close() 

You can download the .py file here (Python 2.7).

Print
PDF
27
January
2012

Computer Vision: using a camera to find distance

I've been spending a lot of my time lately mentoring my local FIRST robotics team #3142. While I am volunteering a huge chunk of time every day, it doesn't mean I haven't been able to still work on projects. In fact, I've been working on a lot of cool computer vision projects recently, mostly centered around the excellent RoboRealm software. In this year's FIRST robotics season, teams from all around the world will compete in a race to build a complete robot from scratch in only six weeks - and this year, the robot has to play basketball. The challenge of building a robot capable of shooting basketballs into a hoop 98 inches high from as far as 50 feet away is not an easy one.. and it can't be accomplished with reasonable consistency without computer assistance. I put together a quick demo showing the team how it could be done: I used a single RGB webcam mounted on a tilt mechanism. In order to find the distance from the camera to the target, all we need to know is the height difference between the camera and target and the angle from horizontal to the target. Check out the diagram below:

This image shows a vertical cross-section of the camera's view of the target. The difference in elevation z between the camera and the center of the target is known, as well as the angle a. Thus, simple trigonometry tell us that:

My demo used a PID loop to adjust the camera tilt so that the image was always in the center of the screen. Once the target was locked on, I used an accelerometer to get data on the current tilt of the camera. I easily converted this to an angle, then took the height divided by the tangent of the angle to give me a distance estimate accurate to the inch. Here's a screenshot of what my program looked like:

Print
PDF
17
November
2011

The Segway

I originally posted the theory behind the control software for my Segway back in January, over 11 months ago. As most of you expected at the time, I was indeed working on building my own Segway. I finished the darn thing back in May, and am finally getting around to uploading pictures and documentation of the project.