Balloon Collaboration
September 21, 2019, 01:16:05 PM *
Welcome, Guest. Please login or register.

Login with username, password and session length
News: DaddyB, Larry and Dan are your administrators...Private message for any problems or issues with the Forum//barntekadmin
 
   Home   Help Search gallery Calendar Login Register  
Pages: [1]
  Print  
Author Topic: Well, I changed my code again...  (Read 2249 times)
Larry
Administrator
Hero Member
*****
Posts: 1167



View Profile
« on: July 17, 2013, 07:22:23 PM »

I kinda learned how to write a class in python (from copying the PID class that I am using), so I was curious if I could do the same for the servo control and the encoder reading.  Well, it turns out that I guess you can, because I did.  The way I am running my software, now, is I run the theta_in.py program.  The main role program plays is to get new angle info from the user and pass it to the main program.  It also loads up the NC.py program, which is the main number crunching program.

theta.py
Quote
#!/usr/bin/python

import os
import memcache
shared = memcache.Client(['127.0.0.1:11211'], debug=0)
shared.set('New_Pos',0)

def ClrScreen():
    # Clears the Screen
    if os.name == "posix":
        # Unix/Linux/MacOS/BSD/etc
        os.system('clear')
    elif os.name in ("nt", "dos", "ce"):
        # DOS/Windows
        os.system('CLS')

ClrScreen()
os.system('sudo python NC.py &')  #Run the NC.py program in the background, returning terminal control back to me

while 1:
    print '\033[9;1HEnter New Angle =>         '
    New_Pos=int(raw_input('\033[9;20H'))
    shared.set('New_Pos',New_Pos)
    if New_Pos==999:exit()

memcache is the module that allows me to talk between 2 programs.


NC.py
Quote
#!/usr/bin/python

from time import sleep
import PID
import ENC
import SERVO
import memcache
shared = memcache.Client(['127.0.0.1:11211'], debug=0)
shared.set('New_Pos',0)
pid=PID.PID()
enc=ENC.ENC()
svo=SERVO.Servo()

####################################################################
###   Servo Drive Setup Code
####################################################################
c=0
servoMin = 250  # Min pulse length out of 4096
servoMax = 550  # Max pulse length out of 4096

####################################################################
###   PID Setup
####################################################################
Kp=0.07
Ki=0.0
Kd=0.005
pid.SetKp(Kp)
pid.SetKd(Kd)
pid.SetKi(Ki)
outv=0

####################################################################
###   Encoder Reader Setup Code
####################################################################
enc.WriteMDR0(99)

svo.SetServoPos(servoMin)
c=servoMin

while 1:    
    count=enc.ReadCountReg()
    print '\033[2;5HServo => %3.3f' %c
    print '\033[3;5HAngle => %2.0F' %count
    print '\033[7;0Houtv => %2.3f' %outv
    New_Pos=int(shared.get('New_Pos'))
    if New_Pos==999:exit()
    while New_Pos<>count:               # loop while error exists
        count=enc.ReadCountReg()      # read encoder
        err=New_Pos-count                 # calculate error magnitude
        outv=pid.GenOut(err)              # Generate PID output value based on error input
        c=c+outv                                # Add error output to servo position
        if c<servoMin:                         # test for absolute limits
          c=servoMin                           # limit servo
          break                                    # drop out of loop so it can obtain new position if availible
        if c>servoMax:                        # test for absolute limits
          c=servoMax                           # limit servo
          break                                    # drop out of loop so it can obtain new position if availible
        svo.SetServoPos(c)                  # Send servo to new servo position
        print '\033[2;5HServo => %3.3f' %c  
        print '\033[3;5HAngle => %2.0F' %count
        print '\033[7;0Houtv => %2.3f' %outv
    sleep(.01)

At the beginning, you see it Imports PID ENC and SERVO.

PID.py
Quote
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)

ENC.py
Quote
import spidev
spi=spidev.SpiDev()


class ENC:
    
    def __init__(self):
        spi.open(0,0)
        self.stuff=0
    
    def ReadStatusReg(self):
        STR=spi.xfer2([0b01110000,0])
        return STR

    def ClearStatusReg(self):
        spi.xfer2([0b00110000])

    def ClearCountReg(self):
        spi.xfer2([0b00100000])

    def WriteMDR0(self,x):
        spi.xfer2([0b10001000,x])

    def WriteMDR1(self,x):
        spi.xfer2([0b10010000,x])

    def WriteDataReg(self,x):
        spi.xfer2([0b10011000,x])
        
    def ReadCountReg(self):
        CNTR=spi.xfer2([0b01100000,0,0])
        count=(CNTR[1]<<Cool+CNTR[2]
        if count>32768: count-=65536
        return count

and SERVO.py
Quote
from Adafruit_PWM_Servo_Driver import PWM
pwm = PWM(0x40, debug=True)

class Servo:

    def __init__(self):
      self.servoCH=6
      self.servoMin = 200  # Min pulse length out of 4096
      self.servoMax = 600  # Max pulse length out of 4096
      pwm.setPWMFreq(50)                        # Set frequency to 50 Hz

    def setServoPulse(channel, pulse):
      pulseLength = 1000000                   # 1,000,000 us per second
      pulseLength /= 50                       # 60 Hz
      print "%d us per period" % pulseLength
      pulseLength /= 4096                     # 12 bits of resolution
      print "%d us per bit" % pulseLength
      pulse *= 1000
      pulse /= pulseLength
      pwm.setPWM(channel, 0, pulse)
 
    def SetServoPos(self,x):
      if x<self.servoMin:x==self.servoMin
      if x>self.servoMax:x==self.servoMax
      pwm.setPWM(self.servoCH, 0, int(x))

I still have some coding to do before these modules would be ready for general use.  Right now, the pretty much just do what I need.
« Last Edit: July 17, 2013, 07:24:46 PM by Larry » Logged
Pages: [1]
  Print  
 
Jump to:  

Powered by MySQL Powered by PHP Powered by SMF 1.1.11 | SMF © 2006-2009, Simple Machines LLC Valid XHTML 1.0! Valid CSS!