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