class Motor:
def __init__(self, address=RC_ADDRESS):
self.rc = Roboclaw(COMPORT, 115200)
self.rc.Open()
logger.info('Opened Roboclaw')
self.address = address
self.dir = None
self.speed = INITIAL_SPEED
self.throttle(Throttle.NEUTRAL)
self.rc.TurnRightMixed(address, 0)
def set_speed(self, speed: int) -> None:
self.speed = speed
logger.info('Set motor speed to %d' % speed)
def throttle(self, dir: Throttle) -> None:
if dir != self.dir:
self.dir = dir
logger.debug('Throttling into direction: %s' % dir)
if dir == Throttle.FORWARD:
try:
self.rc.BackwardMixed(self.address, self.speed)
except:
pass
elif dir == Throttle.NEUTRAL:
try:
self.rc.ForwardMixed(self.address, 0)
except:
pass
elif dir == Throttle.REVERSE:
try:
self.rc.ForwardMixed(self.address, self.speed)
except:
pass
def __init__(self, addr):
self.act = Roboclaw(addr, 115200)
self.currents = [0,0]
while self.act.Open()==0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
print("Opened Belt roboclaw to ",addr)
def main():
#
# get the encoder counts
#
# address of the RoboClaw as set in Motion Studio
#
address = 0x80
print(address)
#
# Creating the RoboClaw object, serial port and baudrate passed
#
robo = Roboclaw("/dev/ttymxc2", 38400)
#
# Starting communication with the RoboClaw hardware
#
opened = robo.Open()
if opened:
#
# Start motor 1 in the forward direction at half speed
#
counts = robo.ReadEncM1(address)
print("motor 1 counts: ", counts)
m1_count = counts[1]
print("motor 1 count: ", m1_count)
else:
print("port did not open")
return
def __init__(self, address=RC_ADDRESS):
self.rc = Roboclaw(COMPORT, 115200)
self.rc.Open()
logger.info('Opened Roboclaw')
self.address = address
self.dir = None
self.speed = INITIAL_SPEED
self.throttle(Throttle.NEUTRAL)
self.rc.TurnRightMixed(address, 0)
def __init__(self, addr1, addr2):
self.act1 = Roboclaw(addr1, 115200)
self.act2 = Roboclaw(addr2, 115200)
while self.act1.Open() == 0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
while self.act2.Open() == 0:
print("Failed to open actuator 2 comms, trying again")
time.sleep(1)
print("Opened Actuator roboclaw to ", addr1, "and ", addr2)
def __init__(self):
# Initialize addresses = [0x80, 0x81]
self.addresses = [0x80, 0x81]
# Connect to roboclaw
serial_port = "/dev/ttyS0"
baudrate = 38400
self.roboclaw = Roboclaw(serial_port, baudrate)
self.roboclaw.Open()
self.robotspeed = 10
def __init__(self, address=0x80, baudrate=38400, port="/dev/ttyS0"):
self.address = address
self.rc = Roboclaw(port, baudrate)
self.rc.Open()
version = self.rc.ReadVersion(self.address)
if version[0] == False:
print("GETVERSION Failed - check power supply and conections")
return
else:
print(repr(version[1]))
def __init__(self):
#setup variables
#Linux comport name
#self.rc = Roboclaw("/dev/ttyACM1",115200)
#Windows com-port name
self.rc = Roboclaw("COM8", 115200)
self.rc.Open()
#Declare variables
self.address = 0x80 #Controller 1, M1=Pitch, M2=Rotation
self.address_2 = 0x81 #Controller 2, M1=Lift, M2=Launch
self.pitch_pulses = 355000 #Encoder pulses from the linear actuator
self.pitch_length = 90.0 #Degrees
self.pitch_speed_pulses = 7000 #Pulses per second
self.pitch_speed_manual = 75 #From 0 to 127
self.pitch_ready = 70.0 #Pitch degrees for the launch (temporary)
self.rotation_pulses = 950000 #Encoder pulses from the rotation motor
self.rotation_length = 180.0 #Degrees
self.rotation_speed_pulses = 16000 #Pulses per second
self.rotation_speed_manual = 127 #From 0 to 127
self.rotation_ready = 10.0 #Rotation degress for the launch (temporary)
self.lift_pulses = 19000 #Encoder pulses from the lifting colum
self.lift_length = 130.0 #cm
self.lift_speed_pulses = 420 #Pulses per second
self.lift_speed_manual = 127 #From 0 to 127 - 7 bits
self.lift_ready = self.lift_length #Lift lenght for the launch (temporary)
self.launch_pulses = 14800 #Encoder pulses from the launch motor
self.launch_length = 111.0 #cm
self.launch_speed_pulses = 6 * 13400 #Pulses per second during launch (145000 max) (13400 pulses/m)
self.launch_speed_pulses_slow = 2500 #Pulses per second during preparation
self.launch_speed_manual = 12 #From 0 to 127
self.launch_acceleration = (
self.launch_speed_pulses**
2) / 13400 #Acceleration during launch (pulses/second2)
self.launch_max_speed = 10 #Maximum launch speed
self.launch_min_speed = 1 #Minimum launch speed
self.launch_max_acceleration = 48 #Maximum launch acceleration
self.launch_min_acceleration = 1 #Minimum launch acceleration
self.launch_standby = 8000 #Drone position during stand-by
self.launch_mount = 17000 #Drone position during mounting
self.launch_break = 21000 #Belt position during breaking
self.launch_bottom = 0 #Drone position at the back part of the capsule
self.launch_connect = 2190 #Belt position for touching the upper part
self.encoders_ready = 0 #At the beggining, the encoders are not ready
def __init__(self):
# GPIO.setmode(GPIO.BCM)
# GPIO.setwarnings(False)
self.address_front_wheels = 0x80
self.address_rear_wheels = 0x81
self.full_speed = 127
self.sleep_time = 0.005
self.roboclaw = Roboclaw("/dev/ttyS0", 38400)
self.roboclaw.Open()
print("Error")
print(self.roboclaw.ReadError(self.address_front_wheels))
self.roboclaw.SetMinVoltageMainBattery(self.address_front_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_front_wheels, 112)
self.roboclaw.SetMinVoltageMainBattery(self.address_rear_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_rear_wheels, 112)
def __init__(self):
self.rc1 = Roboclaw('/dev/roboclaw1', 115200)
self.rc2 = Roboclaw('/dev/roboclaw2', 115200)
self.currents = [0, 0, 0, 0]
while self.rc1.Open() == 0:
print('OPEN ROBOCLAW 1 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 1 COMMS')
while self.rc2.Open() == 0:
print('OPEN ROBOCLAW 2 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 2 COMMS')
print('STARTING CURRENT MONITOR LOOP')
def main():
#
# error checking follows:
#
# must have one argument
#
usage_str = "usage:\npython3 motor_forward.py XXX"
arg_count = len(sys.argv)
if (arg_count != 2):
print(usage_str)
#
# get the move speed, must be between 0 and 127
#
else:
speed = int(sys.argv[1])
#
# address of the RoboClaw as set in Motion Studio
#
address = 0x80
#
# Creating the RoboClaw object, serial port and baudrate passed
#
robo = Roboclaw("/dev/ttymxc2", 38400)
#
# Starting communication with the RoboClaw hardware
#
opened = robo.Open()
if opened:
#
# Start motor 1 in the forward direction at half speed
#
robo.ForwardM2(address, speed)
#
# pause for two seconds
#
time.sleep(2.0)
#
# stop the motor
#
robo.ForwardM2(address, 0)
else:
print("port did not open")
return
def __init__(self):
self.rc = Roboclaw("/dev/ttyACM0", 115200) # Linux comport name
self.address = 0x80
self.rc.Open()
self.ready = True
version = self.rc.ReadVersion(self.address)
if not version[0]:
print("GETVERSION Failed")
exit()
else:
print(repr(version[1]))
print("Car main battery voltage at start of script: ", self.rc.ReadMainBatteryVoltage(self.address))
for i in range(1000):
try:
self.rc.ForwardM2(self.address, i)
time.sleep(0.1)
print(i)
except Exception as e:
print(e)
class Swag:
def __init__(self):
self.rc = Roboclaw("/dev/ttyACM0", 115200) # Linux comport name
self.address = 0x80
self.rc.Open()
self.ready = True
version = self.rc.ReadVersion(self.address)
if not version[0]:
print("GETVERSION Failed")
exit()
else:
print(repr(version[1]))
print("Car main battery voltage at start of script: ", self.rc.ReadMainBatteryVoltage(self.address))
for i in range(1000):
try:
self.rc.ForwardM2(self.address, i)
time.sleep(0.1)
print(i)
except Exception as e:
print(e)
def control_speed(self, mc, adr, speed_m1, speed_m2):
# speedM1 = leftMotorSpeed, speedM2 = rightMotorSpeed
if speed_m1 > 0:
mc.ForwardM1(adr, speed_m1)
elif speed_m1 < 0:
speed_m1 = speed_m1 * (-1)
mc.BackwardM1(adr, speed_m1)
else:
mc.ForwardM1(adr, 0)
if speed_m2 > 0:
mc.ForwardM2(adr, speed_m2)
elif speed_m2 < 0:
speed_m2 = speed_m2 * (-1)
mc.BackwardM2(adr, speed_m2)
else:
mc.ForwardM2(adr, 0)
#
#formatter = logging.Formatter('%(asctime)s:%(name)s:%(levelname)s:%(message)s')
#
#file_handler = logging.FileHandler('launcher.log')
#file_handler.setLevel(logging.INFO)
#file_handler.setFormatter(formatter)
#
#logger.addHandler(file_handler)
logging.basicConfig(filename='launcher.log',level=logging.DEBUG)
#Open serial port
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
#Windows comport name
rc = Roboclaw("COM8",115200)
rc.Open()
encoders_ready = 1 # set to 1 so that the position method can be tested
origo = [90.0, 0, 0, 0]
actual_coordonates = origo[:]
case_open = 0
class motor():
def __init__(self, address, channel, pulses, length, speed_pulses, speed_manual, ready):
self.address = address
self.channel = channel
self.pulses = pulses
self.length = length
self.speed_pulses = speed_pulses
from roboclaw_3 import Roboclaw
#Windows comport name
#rc = Roboclaw("COM3",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
print("code finished")
class DriveControl:
def __init__(self):
self.rc1 = Roboclaw('/dev/roboclaw1', 115200)
self.rc2 = Roboclaw('/dev/roboclaw2', 115200)
self.currents = [0, 0, 0, 0]
while self.rc1.Open() == 0:
print('OPEN ROBOCLAW 1 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 1 COMMS')
while self.rc2.Open() == 0:
print('OPEN ROBOCLAW 2 COMMS FAILED, RETRYING...')
time.sleep(1)
print('OPENED ROBOCLAW 2 COMMS')
print('STARTING CURRENT MONITOR LOOP')
def updateCurrents(self):
con1 = self.rc1.ReadCurrents(0x80)
con2 = self.rc2.ReadCurrents(0x80)
frontLeftCurr = float(con1[1]) / 100
frontRightCurr = float(con1[2]) / 100
backLeftCurr = float(con2[1]) / 100
backRightCurr = float(con2[2]) / 100
self.currents = [
frontLeftCurr, backLeftCurr, frontRightCurr, backRightCurr
]
print(self.currents)
def readCurrents(self, i):
print(self.currents[i])
return self.currents[i]
def moveLeftSide(self, speed):
self.drive(self.rc1, 'm1', speed)
self.drive(self.rc1, 'm2', speed)
def moveRightSide(self, speed):
self.drive(self.rc2, 'm1', speed)
self.drive(self.rc2, 'm2', speed)
def moveM1(self, speed):
self.drive(self.rc1, 'm1', speed)
def moveM2(self, speed):
self.drive(self.rc1, 'm2', speed)
def moveM3(self, speed):
self.drive(self.rc2, 'm1', speed)
def moveM4(self, speed):
self.drive(self.rc2, 'm2', speed)
def drive(self, claw, motor, speed):
speed = self.ensureValidSpeed(speed)
direction = 's' # needs to be either f or b to run
scaledValue = 0
if speed <= 1800:
scaledValue = self.translateValue(speed, 1800, 0, 0, 127)
direction = 'b'
elif speed >= 2200:
scaledValue = self.translateValue(speed, 2200, 4095, 0, 127)
direction = 'f'
else:
direction = 'f'
scaledValue = 0
# forward and backward go the same direction here becuase the
# motors are reversed in the wiring
if motor == 'm1':
if direction == 'f':
claw.ForwardM1(0x80, int(scaledValue))
elif direction == 'b':
claw.BackwardM1(0x80, int(scaledValue))
else:
print('bad direction value')
elif motor == 'm2':
if direction == 'f':
claw.ForwardM2(0x80, int(scaledValue))
elif direction == 'b':
claw.BackwardM2(0x80, int(scaledValue))
else:
print('bad direction value')
else:
print('bad motor index')
self.updateCurrents()
def ensureValidSpeed(self, speed):
if speed < 0:
speed = 0
if speed > 4095:
speed = 4095
return speed
def translateValue(self, value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value - leftMin) / float(leftSpan)
return rightMin + (valueScaled * rightSpan)
import time
import paho.mqtt.client as mqtt
from flask import Flask, render_template, request
from roboclaw_3 import Roboclaw
geschwindigkeit = 60
roboclaw = Roboclaw('/dev/ttyACM0', 38400)
roboclaw.Open()
# Flask - Webseite
app = Flask(__name__)
@app.route("/", methods=['GET', 'POST'])
def index():
if request.method == 'POST':
#geschwindigkeit = request.form["geschwindigkeit"]
if request.form['steuerbefehl'] == "vor":
fahre_rover_vorwaerts(geschwindigkeit)
if request.form['steuerbefehl'] == "stop": stoppe_rover()
if request.form['steuerbefehl'] == "zurueck":
fahre_rover_rueckwaerts(geschwindigkeit)
if request.form['steuerbefehl'] == "rechts":
drehe_rover_nach_rechts(geschwindigkeit)
if request.form['steuerbefehl'] == "links":
drehe_rover_nach_links(geschwindigkeit)
# *** Before using this example the motor/controller combination must be
# *** tuned and the settings saved to the Roboclaw using IonMotion.
# *** The Min and Max Positions must be at least 0 and 50000
import time
from roboclaw_3 import Roboclaw
# Windows comport name
rc = Roboclaw("COM3", 115200)
# Linux comport name
# rc = Roboclaw("/dev/ttyACM0",115200)
def display_speed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
print("Encoder1:")
if enc1[0] == 1:
print(enc1[1])
print(format(enc1[2], '02x'))
else:
print("failed")
print("Encoder2:")
if enc2[0] == 1:
print(enc2[1])
import time
from roboclaw_3 import Roboclaw
#Windows comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
while 1:
#Get version string
version = rc.ReadVersion(0x80)
if version[0] == False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
time.sleep(1)
# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
# Start streaming with requested config
pipe.start(cfg)
roboclaw_vid = 0x03EB # VID of Roboclaw motor driver in hex
found = False
for port in comports():
if port.vid == roboclaw_vid:
roboclawport = port.device
found = True
if found == True:
print("Roboclaw port:", roboclawport)
rc = Roboclaw(roboclawport, 0x80)
rc.Open() # Start motor controller
address = 0x80
version = rc.ReadVersion(address)
tickdistanceL = 10 # number of left encoder ticks per mm traveled
tickdistanceR = 10 # number of right encoder ticks per mm traveled
if version[0] == False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
# open waypoint file
if testmode:
waypoint_file = 'waypoints_test.csv'
#***Before using this example the motor/controller combination must be
#***tuned and the settings saved to the Roboclaw using IonMotion.
#***The Min and Max Positions must be at least 0 and 50000
import time
from roboclaw_3 import Roboclaw
#Windows comport name
#rc = Roboclaw("COM3",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyS0", 38400)
rc.Open()
address = 0x80
#rc.ResetEncoders(address)
while (1):
print("Pos 50000")
rc.SpeedAccelDeccelPositionM1(address, 500, 2000, 500, -5000, 1)
for i in range(0, 80):
print("Position: %d, Setpoint: %d", rc.ReadEncM1(address), 0)
#displayspeed()
time.sleep(0.1)
time.sleep(2)
#rc.SetEncM1(address, -10000)
print("Pos 0")
rc.SpeedAccelDeccelPositionM1(address, 4000, 4000, 4000, 0, 1)
conn.send(data)
command = data.decode()
print(command)
if command.strip() == 'THROTTLE FORWARD':
rc.ForwardMixed(address, 32)
if command.strip() == 'THROTTLE REVERSE':
rc.BackwardMixed(address, 32)
if input() == 'c':
conn.close()
# Windows comport name
comport = "COM5"
rc = Roboclaw(comport, 115200)
rc.Open()
address = 0x80
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serv.bind(('10.24.178.220', 8080))
serv.listen(5)
while True:
conn, addr = serv.accept()
from_client = ''
while True:
class MecanumRobot:
def __init__(self):
# GPIO.setmode(GPIO.BCM)
# GPIO.setwarnings(False)
self.address_front_wheels = 0x80
self.address_rear_wheels = 0x81
self.full_speed = 127
self.sleep_time = 0.005
self.roboclaw = Roboclaw("/dev/ttyS0", 38400)
self.roboclaw.Open()
print("Error")
print(self.roboclaw.ReadError(self.address_front_wheels))
self.roboclaw.SetMinVoltageMainBattery(self.address_front_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_front_wheels, 112)
self.roboclaw.SetMinVoltageMainBattery(self.address_rear_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_rear_wheels, 112)
def move_backward(self):
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def move_forward(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def slide_right(self):
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def slide_left(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def rotate_left(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_front_wheels, self.full_speed)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.BackwardM2, args=(self.address_rear_wheels, self.full_speed)).start()
sleep(self.sleep_time)
def stop(self):
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_front_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM1, args=(self.address_rear_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_front_wheels, 0)).start()
sleep(self.sleep_time)
threading.Thread(target=self.roboclaw.ForwardM2, args=(self.address_rear_wheels, 0)).start()
sleep(self.sleep_time)
class BeltControl:
def __init__(self, addr):
self.act = Roboclaw(addr, 115200)
self.currents = [0,0]
while self.act.Open()==0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
print("Opened Belt roboclaw to ",addr)
############# public methods #############
def updateCurrents(self):
con1 = self.act.ReadCurrents(0x80)
digCurr = float(con1[1]) / 100
offCurr = float(con1[2]) / 100
self.currents = [digCurr, offCurr]
# method for reading the blet currents
def readCurrents(self, i):
return self.currents[i]
# control the offload belt
def offload(self, speed):
speed = self.verify_speed(speed)
direction = "s"
if speed <= 1800:
# move actuator forward
adjusted_speed = self.translate_value(speed, 1800,0,0,127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = self.translate_value(speed, 2200, 4095,0,127)
direction = "f"
else :
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f" :
self.act.ForwardM1(0x80, int(adjusted_speed))
elif direction == "b":
self.act.BackwardM1(0x80, int(adjusted_speed))
else:
self.act.ForwardM1(0x80, 0)
self.updateCurrents()
# control the digging belt
def dig(self, speed):
speed = self.verify_speed(speed)
direction = "s"
if speed <= 1800:
# move actuator forward
adjusted_speed = self.translate_value(speed, 1800,0,0,127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = self.translate_value(speed, 2200, 4095,0,127)
direction = "f"
else :
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f" :
self.act.ForwardM2(0x80, int(adjusted_speed))
elif direction == "b":
self.act.BackwardM2(0x80, int(adjusted_speed))
else:
self.act.ForwardM2(0x80, 0)
self.updateCurrents()
############# private methods #############
# verifies speed and position
def verify_speed(self, speed):
# set maximum speed that the actuator can go
maximum = 4095
# cap the speed at the max in either direction
if speed > maximum:
speed = maximum
elif speed < 0:
speed = 0
else:
speed = speed
return speed
# translates vale from left range to right range
def translate_value(self, value, leftMin, leftMax, rightMin, rightMax):
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
valueScaled = float(value-leftMin)/float(leftSpan)
return rightMin + (valueScaled * rightSpan)
#from pyfirmata import ArduinoMega, util
from time import sleep
from threading import Timer
import re
import RPi.GPIO as GPIO
import smbus
from smbus import SMBus
from PCA9685 import PWM
import serial
measWaitTime = 0
motorWaitTime = 0
ser = serial.Serial()
batteryVoltage = ""
roboclaw = Roboclaw("/dev/ttyS0", 38400)
def connectMotorSerial():
global roboclaw
motor_baudrate = 38400
print("connecting motors")
roboclaw.Open()
def connectSerial():
connectMotorSerial()
global ser
global batteryVoltage
try:
class MotorController(object):
def __init__(self):
# Initialize addresses = [0x80, 0x81]
self.addresses = [0x80, 0x81]
# Connect to roboclaw
serial_port = "/dev/ttyS0"
baudrate = 38400
self.roboclaw = Roboclaw(serial_port, baudrate)
self.roboclaw.Open()
self.robotspeed = 10
def forward(self, drivetime):
for address in self.addresses:
self.roboclaw.ForwardMixed(address, self.robotspeed)
sleep(drivetime)
for address in self.addresses:
self.roboclaw.ForwardMixed(address, 0)
def backward(self, drivetime):
for address in self.addresses:
self.roboclaw.BackwardMixed(address, self.robotspeed)
sleep(drivetime)
for address in self.addresses:
self.roboclaw.BackwardMixed(address, 0)
def right(self, drivetime):
self.roboclaw.TurnRightMixed(self.addresses[0], self.robotspeed)
self.roboclaw.TurnLeftMixed(self.addresses[1], self.robotspeed)
sleep(drivetime)
self.roboclaw.TurnRightMixed(self.addresses[0], 0)
self.roboclaw.TurnLeftMixed(self.addresses[1], 0)
def left(self, drivetime):
self.roboclaw.TurnLeftMixed(self.addresses[0], self.robotspeed)
self.roboclaw.TurnRightMixed(self.addresses[1], self.robotspeed)
sleep(drivetime)
self.roboclaw.TurnLeftMixed(self.addresses[0], 0)
self.roboclaw.TurnRightMixed(self.addresses[1], 0)
def diagonals(self, direction, drivetime):
if direction == 'northeast' or direction == "wd":
for address in self.addresses:
self.roboclaw.ForwardM1(address, self.robotspeed)
sleep(drivetime)
elif direction == 'northwest' or direction == "wa":
for address in self.addresses:
self.roboclaw.ForwardM2(address, self.robotspeed)
sleep(drivetime)
elif direction == 'southeast' or direction == "sd":
for address in self.addresses:
self.roboclaw.BackwardM2(address, self.robotspeed)
sleep(drivetime)
elif direction == 'southwest' or direction == "sa":
for address in self.addresses:
self.roboclaw.BackwardM1(address, self.robotspeed)
sleep(drivetime)
def rotate(self, direction, drivetime):
if direction == 'clockwise' or direction == 'e':
for address in self.addresses:
self.roboclaw.TurnLeftMixed(address, self.robotspeed)
sleep(drivetime)
elif direction == 'counter-clockwise' or direction == 'q':
for address in self.addresses:
self.roboclaw.TurnRightMixed(address, self.robotspeed)
sleep(drivetime)
from roboclaw_3 import Roboclaw
#Windows comport name
rc = Roboclaw("COM3",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
print(rc.Open())
#DRONE LAUNCHER
from flask import Flask, render_template, request, jsonify
from roboclaw_3 import Roboclaw
import socket
from time import sleep
#Open serial port
#Linux comport name
rc = Roboclaw("/dev/ttyACM0",115200)
"""Here it would be good to have LEDs switch on to confirm connection to each roboclaw"""
rc.Open()
#Declare variables
#Specify IP address and port for the server
##host=(([ip for ip in socket.gethostbyname_ex(socket.gethostname())[2] if not ip.startswith("127.")] or [[(s.connect(("8.8.8.8", 53)), s.getsockname()[0], s.close()) for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]]) + ["no IP found"])[0]
##port=5000
address = 0x81 #Controller 1, M1=Pitch, M2=Rotation
address_2 = 0x81 #Controller 2, M1=Lift, M2=Launch
address_3 = 0x81 #Controller 3, M1=Case Open, M2=Case Close
pitch_pulses=355000 #Encoder pulses from the linear actuator
pitch_length=90.0 #Degrees 0 -> 90 degrees (0 vertical, 90 horizontal)
pitch_speed_pulses=7000 #Pulses per second
pitch_speed_manual=127 #From 0 to 127
pitch_ready=65.0 #Pitch degrees for the launch (temporary) 0 is 90 -> 65 is 25
rotation_pulses=950000 #Encoder pulses from the rotation motor
rotation_length=180.0 #Degrees
import time
from roboclaw_3 import Roboclaw
#Windows comport name
#Linux comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
address = 0x80
for i in range(1):
rc.ForwardM1(address, 127) #1/4 power forward
time.sleep(2)
rc.BackwardM1(address, 0) #Stopped
rc.ForwardM2(address, 0) #Stopped
time.sleep(2)
#Start with Raspberry Pi
from roboclaw_3 import Roboclaw
import numpy as np
import matlab.engine
import time
address = [0x80]
roboclaw = Roboclaw("/dev/ttyS0", 38400)
roboclaw.Open()
roboclaw.ForwardM1(address[0],64) #range is 0 - 127
roboclaw.ForwardM2(address[0],64)
time.sleep(2)
def init_all_motors():
roboclaw.ForwardM1(address[0],0) #range is 0 - 127
roboclaw.ForwardM2(address[0],0)
# roboclaw.ForwardM1(address[1],0)
# roboclaw.ForwardM2(address[1],0)
def get_optimal_settings():
eng = matlab.engine.start_matlab()
eng.cd(r'C:\Users\BAB\Desktop\Fotballmaskin-master-python3\src\Fotballmaskin\Pcprogram\optimalisering')
eng.make_init(nargout=0)
#GUI
print("""What is the desired landing point?\nThe machine is in origin
and Y is shooting direction\nReply in following format:\nX,Y,Z\n""")
desired_point_string = input()
#Format input
