class DifControl():
def __init__(self, ser):
self.motor_r = MotorControl(0, ser)
self.motor_l = MotorControl(1, ser)
self.R = 0.115
self.l = 0.5081
def set_speed(self, vX, wZ): #meteres per second / radians per second
v_r = -1*(2*vX - self.l*wZ)/(2*self.R)
v_l = (2*vX + self.l*wZ)/(2*self.R)
# print("v_r:"+str(v_r)+" v_l:"+str(v_l))
self.motor_r.goal_velocity(v_r)
self.motor_l.goal_velocity(v_l)
def get_motors_speed(self):
self.motor_r.get_status()
self.motor_l.get_status()
if self.motor_r.direction_of_turn == '0':
v_r = self.motor_r.motor_speed
else:
v_r = -self.motor_r.motor_speed
if self.motor_l.direction_of_turn == '1':
v_l = self.motor_l.motor_speed
else:
v_l = -self.motor_l.motor_speed
return v_r, v_l
def init_motors(self, holl, acc, br):
self.motor_r.holl_impulse(holl)
self.motor_r.sef_acceleration(acc)
self.motor_r.set_brake(br)
self.motor_r.turn_on()
self.motor_l.holl_impulse(holl)
self.motor_l.sef_acceleration(acc)
self.motor_l.set_brake(br)
self.motor_l.turn_on()
from MotorControl import MotorControl
if __name__ == '__main__':
ser = serial.Serial('/dev/ttyUSB0', timeout=1.0)
motor_1 = MotorControl(0, ser)
motor_2 = MotorControl(1, ser)
motor_1.holl_impulse(8)
motor_1.sef_acceleration(20)
motor_1.set_brake(20)
motor_1.set_direction(1)
motor_1.turn_on()
motor_2.holl_impulse(8)
motor_2.sef_acceleration(20)
motor_2.set_brake(20)
motor_2.set_direction(1)
motor_2.turn_on()
while True:
# controller.goal_velocity(0.5)
motor_1.goal_velocity(0)
motor_2.goal_velocity(0)
# motor_1.set_speed(0)
# motor_2.set_speed(0)
b = motor_1.get_status()
print("big motor:"+str(motor_1.motor_speed))
b = motor_2.get_status()
print("small motor:"+str(motor_2.motor_speed))
# print controller.direction_of_turn #1 - cw, 0 - ccw
sleep(0.01)
# controller.set_adress()