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()