def main(): rospy.init_node('motor_node') motor = Motor() rospy.Subscriber('platform_up_vel', Float32, motor.up) rospy.spin() motor.cleanup()
class Driver(object): def __init__(self): rospy.init_node("driver") self._last_received = rospy.get_time() self._timeout = rospy.get_param("~timeout", 2) self._rate = rospy.get_param("~rate", 10) self._max_speed = rospy.get_param("~max_speed", 0.5) self._wheel_base = rospy.get_param("~wheel_base", 0.091) self._rMotor = Motor(12, 10,8) self._lMotor = Motor(7,3,5) self._rMotor_speed = 0 self._lMotor_speed = 0 rospy.Subscriber("cmd_vel", Twist, self._velocity_received_callback) def _velocity_received_callback(self, message): self._last_received = rospy.get_time() linear = message.linear.x angular = message.angular.z lSpeed = linear - angular*self._wheel_base/2 rSpeed = linear + angular*self._wheel_base/2 self._lMotor_speed = 100 * lSpeed/self._max_speed self._rMotor_speed = 100 * rSpeed/self._max_speed def run(self): rate = rospy.Rate(self._rate) while not rospy.is_shutdown(): delay = rospy.get_time() - self._last_received if delay < self._timeout: self._rMotor.move(self._rMotor_speed) self._lMotor.move(self._lMotor_speed) else: self._rMotor.stop() self._lMotor.stop() rate.sleep() def exit(self): self._rMotor.cleanup() self._lMotor.cleanup()
class Powertrain: """Represents a pair of motors. Uses the Motor class to control them. """ def __init__(self, left_wheel_forward, left_wheel_backward, left_wheel_enable, right_wheel_forward, right_wheel_backward, right_wheel_enable): """Constructor. Args: left_wheel_forward (int): GPIO pin connected to left motor's forward pin. left_wheel_backward (int): GPIO pin connected to left motor's backward pin. left_wheel_enable (int): GPIO pin connected to left motor's enable pin. right_wheel_forward (int): GPIO pin connected to right motor's forward pin. right_wheel_backward (int): GPIO pin connected to right motor's backward pin. right_wheel_enable (int): GPIO pin connected to right motor's enable pin. """ # set motors self.left = Motor(left_wheel_forward, left_wheel_backward, left_wheel_enable) self.right = Motor(right_wheel_forward, right_wheel_backward, right_wheel_enable) def forward(self, duty_cycle): """Drive the powertrain forward. Args: duty_cycle (float): The duty cycle to run the motors at. """ # apply to both motors self.left.forward(duty_cycle) self.right.forward(duty_cycle) def reverse(self, duty_cycle): """Drive the powertrain backward. Args: duty_cycle (float): The duty cycle to run the motors at. """ # apply to both motors self.left.backward(duty_cycle) self.right.backward(duty_cycle) def turn(self, left_duty_cycle, right_duty_cycle, left_forward=True, right_forward=True): """Drive motor speeds separately to turn. Args: left_duty_cyle (float): The duty cyle to run the left motor at. right_duty_cycle (float): The duty cycle to run the right motor at. left_forward (boolean, optional): Flag for the left motor to go forward. Defaults to True. right_forward (boolean, optional): Flag for the right motor to go forward. Defaults to True. """ #print("LDC: {0}, RDC: {1}".format(left_duty_cycle, right_duty_cycle)) # if-else to use forward/backward if (left_forward): self.left.forward(left_duty_cycle) else: self.left.backward(left_duty_cycle) if (right_forward): self.right.forward(right_duty_cycle) else: self.right.backward(right_duty_cycle) def turn_left(self, max_duty_cycle, intensity=50.0, forward=True): """Drive the motors to turn left. Args: max_duty_cycle (float): The duty cycle to run the right motor at. The left motor runs at a portion of this. intensity (float, optional): The intensity to turn left. The value is clamped between 100 and 0. A greater intensity turns harder. Defaults to 50.0. forward (boolean, optional): Flag for spinning motors forward. Defaults to True. """ # get min_duty_cycle min_duty_cycle = get_new_duty_cycle(max_duty_cycle, intensity) # pass to turn self.turn(min_duty_cycle, max_duty_cycle, forward, forward) def turn_right(self, max_duty_cycle, intensity=50.0, forward=True): """Drive the motors to turn right. Args: max_duty_cycle (float): The duty cycle to run the left motor at. The right motor runs at a portion of this. intensity (float, optional): The intensity to turn right. The value is clamped between 100 and 0. A greater intensity turns harder. Defaults to 50.0. forward (boolean, optional): Flag for spinning motors forward. Defaults to True. """ # get min_duty_cycle min_duty_cycle = get_new_duty_cycle(max_duty_cycle, intensity) # pass to turn self.turn(max_duty_cycle, min_duty_cycle, forward, forward) def turn_intensity(self, max_duty_cycle, intensity, forward=True): """Drive the motors to turn based on intensity and its sign. Args: max_duty_cycle (float): The duty cycle to run the faster motor at. The other motor runs at a portion of this. intensity (float): The intensity to turn. The sign of the intensity affects the turn direction (e.g. negative for left, positive for right). The absolute value is clamped between 100 and 0. A greater intensity turns harder. If intensity is 0, this function drives motors equally. forward (boolean, optional): Flag for spinning motors forward. Defaults to True. """ # need to multiply intensity by -1 if intensity is negative if (intensity < 0): self.turn_left(max_duty_cycle, intensity=(-1 * intensity), forward=forward) else: self.turn_right(max_duty_cycle, intensity=intensity, forward=forward) def pivot(self, duty_cycle, clockwise=False): """Drive the motors to run opposite of each other to pivot. Args: duty_cycle (float): The duty cycle to run the motors at. clockwise (boolean, optional): Flag for pivoting clockwise. Defaults to False. """ if (clockwise): self.turn(duty_cycle, duty_cycle, right_forward=False) else: self.turn(duty_cycle, duty_cycle, left_forward=False) def stop(self): """Stop the motors. """ # TODO should this call brake() for a short time then call off()? self.left.off() self.right.off() def cleanup(self): """Cleanup GPIO pins for the motors. Calling this releases the motors, so further use of the powertrain will not be possible. """ # apply to both motors self.left.cleanup() self.right.cleanup()
wiringpi.wiringPiSetupGpio() wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS) motor1 = Motor(25, 24) motor2 = Motor(22, 23) #motor2 = Motor(23, 22) process = subprocess.Popen( "raspivid -n -ih -vf -hf -t 0 -rot 0 -w 1280 -h 720 -fps 15 -b 1000000 -o - | nc -lkv4 5001", shell=True) loop = asyncio.get_event_loop() print("Starting UDP server") # One protocol instance will be created to serve all client requests listen = loop.create_datagram_endpoint( lambda: MotorServerProtocol([motor1, motor2]), local_addr=('0.0.0.0', 9999)) transport, protocol = loop.run_until_complete(listen) try: loop.run_forever() except KeyboardInterrupt: pass transport.close() loop.close() motor1.cleanup() motor2.cleanup() process.kill() process.wait()
z_mil = int(config['grid']['z_step_p_millimeter']) print(' xdir: ' + str(xdir) + ' xstep: ' + str(xstep)) print(' ydir: ' + str(ydir) + ' ystep: ' + str(ystep)) print(' zdir: ' + str(zdir) + ' zstep: ' + str(zstep)) print(' enable pin: ' + str(enable_pin)) print(' sleep time: ' + str(sleep_time)) print(' ---------------------') print(' x_mil: ' + str(x_mil)) print(' y_mil: ' + str(y_mil)) print(' z_mil: ' + str(z_mil)) inputok = input('- Looks good? Keep going? (y/n) ') if inputok != 'y': sys.exit('( ! ) Okay. Stopping ...') Motor.cleanup(motor) motor = Motor(xdir, xstep, ydir, ystep, zdir, zstep, enable_pin, sleep_time) Motor.setup(motor) #import code; code.interact(local=dict(globals(), **locals())) operator = FileOperator(motor, x_mil, y_mil, z_mil) print('') print('( 3 ) Choosing GCODE File') files = os.listdir('./gcodefiles/') filnum = -1 for file in files: filnum += 1 print(' ' + str(filnum) + ' : ' + file) inputfile = input('- Choose file by entering (0...' + str(filnum) + '): ')
from car_config import * left = Motor(LFP, LBP, LEP) right = Motor(RFP, RBP, REP) def test_left(): left.forward(50) time.sleep(2) left.off() time.sleep(1) left.backward(50) time.sleep(2) left.off() def test_right(): right.forward(80) time.sleep(1) right.off() time.sleep(1) right.backward(80) time.sleep(1) right.off() test_left() test_right() left.cleanup() right.cleanup()
return "OK" @app.route('/configure', methods=['GET', 'POST']) def configure(): if request.method == 'POST': data = request.get_json() with open("config.json", "r+") as jsonFile: config = json.load(jsonFile) for key in data: config[key] = data[key] jsonFile.seek(0) json.dump(data, jsonFile) jsonFile.truncate() load_config() return "OK" else: with open('config.json') as json_file: data = json.load(json_file) return jsonify(data) try: if __name__ == "__main__": motor = Motor(17, 1) # run server load_config() app.run(host='0.0.0.0', port=9002, threaded=True) except KeyboardInterrupt: motor.cleanup()