joystick.py 文件源码

def __init__(self):
        self.ser = serial.Serial('/dev/ttyACM0', 9600)

        #geometrical calibration
        self.rs = [50, 50, 50]
        self.ls = [95, 130, 95]
        self.pot_rad_per_unit = 1./3000.*math.pi
        angles = [2./3.*math.pi, 0., -2./3.*math.pi]

        #placements of the 3 joysticks
        self.placements = []
        #attach point on the ball
        self.attach_ps = []
        for r,l,a in zip(self.rs, self.ls, angles):
            p_init = pose.exp(col([0, 0, 0, 0, 0, -(r+l)]))
            p_rot = pose.exp(col([0, a, 0, 0, 0, 0]))
            placement = p_rot * p_init
            self.placements.append(placement)
            attach_p = placement * col([0, 0, l])
            self.attach_ps.append(attach_p)

        #last calculated pose in logarithmic coordinates
        self.last_x = col([0, 0, 0, 0, 0, 0])
        #definition of the numerical solver
        f = lambda x: self.getValuesFromPose(pose.exp(x))
        self.solver = solver.Solver(f)