class Leg:
#Length in mm
COXA = 46.13
FEMUR = 70.61
TIBIA = 146.0
def __init__(self, leg_number, pwm):
#Servo controller
self.pwm = pwm
#last leg position
self.last_x = 0
self.last_y = 0
self.last_z = 0
#normal leg position
self.normal_x = 0
self.normal_y = 130
self.angle_afwijking = 0
self.angle_afwijking_knee = 0
#leg number
self.leg_number = leg_number
if leg_number == 1:
self.hip = Servo(0, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
if leg_number == 3:
self.hip = Servo(13, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
elif leg_number == 2 or leg_number == 4:
self.hip = Servo(6, pwm)
self.height = Servo(7, pwm)
self.knee = Servo(15, pwm)
#set hip
def set_hip(self, degree):
self.hip.set_servo(degree - self.angle_afwijking)
#get hip
def get_hip(self):
return self.hip.get_servo()
#set height
def set_height(self, degree):
self.height.set_servo(degree)
#get height
def get_height(self):
return self.height.get_servo()
#set knee
def set_knee(self, degree):
self.knee.set_servo(degree + self.angle_afwijking_knee)
#get knee
def get_knee(self):
return self.knee.get_servo()
