Exemple #1
0
    def __init__(self, name='maus', i2c_bus=0, servo_trims=[0, 0, 0, 0, 0], servo_pins=[8, 9, 10, 11, 4], pca9685_address=0x40, bno055_address=0x29):
        
        #Configuration
        self._name = name
        self._i2c_bus = i2c_bus
        self._servo_trims = servo_trims
        self._servo_pins = servo_pins
        self._pca9685_address = pca9685_address
        self._bno055_address = bno055_address
    
        #Setting up hardware
        self._bus = smbus.SMBus(self._i2c_bus)
        if not self._bus:
            raise Exception('I2C bus connection failed!')

        self.control = ServoController(self._bus, self._pca9685_address)
        self.sensor = Inclinometer(self._bus, self._bno055_address)
        self.sensor.pitch_trim=-8.5

        #Setting up OctoSnake
        self.osc = []
        self.osc.append(octosnake.Oscillator())
        self.osc.append(octosnake.Oscillator(octosnake.semiSin))
        self.osc.append(octosnake.Oscillator())
        self.osc.append(octosnake.Oscillator(octosnake.semiSin))
        self.osc.append(octosnake.Oscillator())

        self.osc[1].ref_time = self.osc[0].ref_time
        self.osc[2].ref_time = self.osc[0].ref_time
        self.osc[3].ref_time = self.osc[0].ref_time
        self.osc[4].ref_time = self.osc[0].ref_time

        #Setting up PyKDL
        self.ik = MausKinematics()

        #Setting up servo controller
        for i in range(len(self._servo_pins)):
            self.control.addServo(self._servo_pins[i], self._servo_trims[i])

        self.control.servos[self._servo_pins[1]].reverse = True
        self.control.servos[self._servo_pins[3]].reverse = True
Exemple #2
0
class Maus(object):
    
    def __init__(self, name='maus', i2c_bus=0, servo_trims=[0, 0, 0, 0, 0], servo_pins=[8, 9, 10, 11, 4], pca9685_address=0x40, bno055_address=0x29):
        
        #Configuration
        self._name = name
        self._i2c_bus = i2c_bus
        self._servo_trims = servo_trims
        self._servo_pins = servo_pins
        self._pca9685_address = pca9685_address
        self._bno055_address = bno055_address
    
        #Setting up hardware
        self._bus = smbus.SMBus(self._i2c_bus)
        if not self._bus:
            raise Exception('I2C bus connection failed!')

        self.control = ServoController(self._bus, self._pca9685_address)
        self.sensor = Inclinometer(self._bus, self._bno055_address)
        self.sensor.pitch_trim=-8.5

        #Setting up OctoSnake
        self.osc = []
        self.osc.append(octosnake.Oscillator())
        self.osc.append(octosnake.Oscillator(octosnake.semiSin))
        self.osc.append(octosnake.Oscillator())
        self.osc.append(octosnake.Oscillator(octosnake.semiSin))
        self.osc.append(octosnake.Oscillator())

        self.osc[1].ref_time = self.osc[0].ref_time
        self.osc[2].ref_time = self.osc[0].ref_time
        self.osc[3].ref_time = self.osc[0].ref_time
        self.osc[4].ref_time = self.osc[0].ref_time

        #Setting up PyKDL
        self.ik = MausKinematics()

        #Setting up servo controller
        for i in range(len(self._servo_pins)):
            self.control.addServo(self._servo_pins[i], self._servo_trims[i])

        self.control.servos[self._servo_pins[1]].reverse = True
        self.control.servos[self._servo_pins[3]].reverse = True


    def walk(self, steps):

        left_x_amp = 20		#millimeters
        right_x_amp = 20	#millimeters
        z_amp = 30		#millimeters
        swing_amp = 70		#degrees
        T = 900                 #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
        offset = [-10, -75, -10, -75, 0]
        phase = [90, 180, 270, 0, 335+90]

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]
 
        final = time.time() +float(T*steps/1000) 
        while time.time() < final:
            try:
                roll_data=self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)

    def slowWalk(self, steps):

        left_x_amp = 10         #millimeters
        right_x_amp = 10        #millimeters
        z_amp = 20              #millimeters
        swing_amp = 15          #degrees
        T = 700                 #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
        offset = [-20, -75, -20, -75, 0]
        phase = [90, 180, 270, 0, 170]

        #self.osc[4].signal=signal.triangle

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]

        final = time.time() + float(T*steps/1000)

        modded_phase=0

        while time.time() < final:
            try:
                roll_data=0#self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output-roll_data)
                modded_phase+=0.5
                #self.osc[4].phase=modded_phase
                print self.osc[4].phase

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)



    def run(self, steps):

        left_x_amp = 10         #millimeters
        right_x_amp = 10        #millimeters
        z_amp = 20              #millimeters
        swing_amp = 0           #degrees
        T = 150                 #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
        offset = [-20, -60, -20, -60, 0]
        phase = [0, 270, 0, 270, 0]

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]

        final = time.time() +float(T*steps/1000)
        while time.time() < final:
            try:
                roll_data=self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output)

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)


    def walkBackwards(self, steps):

        left_x_amp = 20         #millimeters
        right_x_amp = 20        #millimeters
        z_amp = 30              #millimeters
        swing_amp = 70          #degrees
        T = 900                #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
        offset = [-20, -75, -20, -75, 0]
        phase = [90, 0, 270, 180, 335-90]

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]

        final = time.time() +float(T*steps/1000)
        while time.time() < final:
            try:
                roll_data=0#self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)

    def turnLeft(self, steps):

        left_x_amp = 0         #millimeters
        right_x_amp = 20        #millimeters
        z_amp = 30              #millimeters
        swing_amp = 70          #degrees
        T = 900                #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, 0, right_x_amp, z_amp, swing_amp]
        offset = [-20, -75, -20, -75, 0]
        phase = [90, 180, 270, 0, 335+90]

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]

        final = time.time() +float(T*steps/1000)
        while time.time() < final:
            try:
                roll_data=0#self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)

    def turnRight(self, steps):

        left_x_amp = 20         #millimeters
        right_x_amp = 0        #millimeters
        z_amp = 30              #millimeters
        swing_amp = 70          #degrees
        T = 900                #milliseconds 

        period = [T, T, T, T, T]
        amplitude = [left_x_amp, z_amp, right_x_amp, 0, swing_amp]
        offset = [-20, -75, -20, -75, 0]
        phase = [90, 180, 270, 0, 335+90]

        for i in range(len(self.osc)):
            self.osc[i].period = period[i]
            self.osc[i].amplitude = amplitude[i]
            self.osc[i].phase = phase[i]
            self.osc[i].offset = offset[i]

        final = time.time() +float(T*steps/1000)
        while time.time() < final:
            try:
                roll_data=0#self.sensor.getPitch()
                for i in range(len(self.osc)):
                    self.osc[i].refresh()

                left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
                right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
                self.control.move(self._servo_pins[0], left_joints[0])
                self.control.move(self._servo_pins[1], right_joints[0])
                self.control.move(self._servo_pins[2], left_joints[1])
                self.control.move(self._servo_pins[3], right_joints[1])
                self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)

            except IOError:
                self._bus = smbus.SMBus(self._i2c_bus)


    #def jump(self):

    def moveJoints(self, joint_target, execution_time=0, sample_time=18):
        if execution_time > sample_time:
            increment = []
            initial_position = []
            for i in range(len(self._servo_pins)):
                initial_position.append(self.control.getPosition(self._servo_pins[i]))
                increment.append(float(joint_target[i] - initial_position[i]) / (execution_time / sample_time))

            final_time = time.time()*1000 + execution_time

            iteration = 1
            while time.time()*1000 < final_time:
                partial_time = time.time()*1000 + sample_time
                for i in range(len(self._servo_pins)):
                    self.control.move(self._servo_pins[i], initial_position[i] + (iteration * increment[i]))

                iteration += 1
                while (time.time()*1000 < partial_time):
                    pass
        else:
            for i in range(len(self._servo_pins)):
                self.control.move(self._servo_pins[i], joint_target[i])

    def moveCart(self, x_left, z_left, x_right, z_right):
        pass

    def zero(self):
        for i in range(len(self._servo_pins)):
            self.control.move(self._servo_pins[i], 0)

    def sleep(self):
	self.control.sleep()