gistfile1.py

from playerc import *
import math
import time

class PID:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.previous_error = 0
        self.integral = 0
        self.integral_times = 0

    def PID(self, error):
        if self.integral_times < 10:
            self.integral = self.integral + error*self.dt
            self.integral_times += 1
        derivative = (error - self.previous_error)/self.dt
        self.previous_error = error        
        return self.Kp*error + self.Ki*self.integral + self.Kd*derivative


class RobotConnect:
    def __init__(self, name, port):
        self.c = playerc_client(None, name, port)
        if self.c.connect() != 0:
            raise playerc_error_str()

        self.p2d = playerc_position2d(self.c, 0)
        if self.p2d.subscribe(PLAYERC_OPEN_MODE) != 0:
            raise playerc_error_str()

        self.bumper = playerc_bumper(self.c, 0)
        if self.bumper.subscribe(PLAYERC_OPEN_MODE) != 0:
            raise playerc_error_str()

        

class RobotPID(RobotConnect):
    def set_dt(self, dt):
        self.dt = dt

    def set_forward_vars(self, Kp, Ki, Kd, acc):
        self.f_Kp = Kp
        self.f_Ki = Ki
        self.f_Kd = Kd
        self.f_acc = acc

    def set_angle_vars(self, Kp, Ki, Kd, acc):
        self.a_Kp = Kp
        self.a_Ki = Ki
        self.a_Kd = Kd
        self.a_acc = acc

    def forward(self, dist):
        angle = self.p2d.pa
        self.last_error = 10000
        
        self.c.read()
        goal_x = math.cos(self.p2d.pa)*dist+self.p2d.px
        goal_y = math.sin(self.p2d.pa)*dist+self.p2d.py
        
        f = PID(self.f_Kp, self.f_Ki, self.f_Kd, self.dt)
        a = PID(self.a_Kp*.7, self.a_Ki, self.a_Kd, self.dt)

        f_err = self.f_get_error(goal_x, goal_y)

        while not (self.f_close_enough(f_err) and self.a_close_enough(angle)):
            if f_err > self.last_error*1.05:
                print "BREAKING", f_err, self.last_error
                break
            print f_err, angle-self.p2d.pa
            self.c.read()

            self.p2d.set_cmd_vel(f.PID(f_err), 0.0, a.PID(angle - self.p2d.pa), 1)
            time.sleep(self.dt)
            self.last_error = f_err
            f_err = self.f_get_error(goal_x, goal_y)                   

        

    def turn(self, angle):
        self.c.read()
        angle += self.p2d.pa
                
        a = PID(self.a_Kp, self.a_Ki, self.f_Kd, self.dt)

        while not self.a_close_enough(angle):
            self.c.read()

            print "ANGLE", angle-self.p2d.pa
            self.p2d.set_cmd_vel(0, 0.0, a.PID(angle - self.p2d.pa), 1)
            time.sleep(self.dt)
        
    def f_close_enough(self, f_err):
        return f_err < self.f_acc
        
    def a_close_enough(self, goal_a):
        return abs(goal_a - self.p2d.pa) < self.a_acc

    def f_get_error(self, goalx, goaly):
        return math.sqrt((goalx-self.p2d.px)**2 + (goaly-self.p2d.py)**2)