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 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 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_rc():
global rc
global rc_address
# Initialise the roboclaw motorcontroller
print("logo: initialising roboclaw driver...")
from roboclaw_3 import Roboclaw
rc_address = 0x80
rc = Roboclaw("/dev/roboclaw", 115200)
rc.Open()
# Get roboclaw version to test if is attached
version = rc.ReadVersion(rc_address)
if version[0] is False:
print("logo init: roboclaw get version failed")
sys.exit()
else:
print("logo init:",repr(version[1]))
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address,240,292) # 24V min, 29.2V max
rc.SetPinFunctions(rc_address,2,0,0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("logo init: m1 p: " + str(m1pid[1]) + ", i:" + str(m1pid[2]) + ", d:" + str(m1pid[3]))
print("m2 p: " + str(m2pid[1]) + ", i:" + str(m2pid[2]) + ", d:" + str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print ("logo init: min main battery: " + str(int(minmaxv[1])/10.0) + "V")
print ("logo init: max main battery: " + str(int(minmaxv[2])/10.0) + "V")
# Print S3, S4 and S5 Modes
S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined']
S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home']
S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home']
pinfunc = rc.ReadPinFunctions(rc_address)
print ("logo init: s3 pin: " + S3mode[pinfunc[1]])
print ("logo init: s4 pin: " + S4mode[pinfunc[2]])
print ("logo init: s5 pin: " + S5mode[pinfunc[3]])
print("logo init: roboclaw motor controller initialised...")
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)
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)
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'
with open(waypoint_file
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)
#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
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 roboclaw_3 import Roboclaw
#Linux comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
print("-----------------------------------------------------------")
if rc.Open() == 0:
print("No roboclaw found on the comport\n")
else:
print("Roboclaw connected\n")
name = ['claw', 'claw_2', 'claw_3']
address = [128, 129, 130]
for i in range(3):
try:
print('\t'.join((name[i], 'set up', str(rc.ReadError(address[i])))))
except:
print('\t'.join((name[i], "not found")))
print("-----------------------------------------------------------")
def pitchStop():
rc.SpeedM1(128, 0)
def rotationStop():
rc.SpeedM2(128, 0)
class ActuatorControl:
# max and min values that the ADCInterface will send
MAX_POS = 100
MIN_POS = 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)
############# public methods #############
# set adc interface for reading the current position of the actuator
#def setInterface(self, adcObj):
#expects an adc interface object
#self.act_interface = adcObj
#self.pos = self.act_interface.readADC()
# move the actuator letting the move command specify the direction
def readCurrents(self, i):
con1 = self.act1.ReadCurrents(0x80)
con2 = self.act2.ReadCurrents(0x80)
digCurrent = (con1[1] + con2[1]) / 100
raiseCurrent = (con1[2] + con2[2]) / 100
currents = [digCurrent, raiseCurrent]
return currents[i]
def moveDig(self, speed=False):
self.moveActBinary('dig', speed)
def moveRaise(self, speed=False):
self.moveActBinary('raise', speed)
# move up with the option of specifying speed
def moveUp(self, speed=False):
#refresh position
#self.pos = self.act_interface.readADC()
if not speed:
self.moveActBinary(1799)
else:
self.moveActScalar(speed)
# move down with the option of specifying speed
def moveDown(self, speed=False):
#refresh position
#self.pos = self.act_interface.readADC()
if not speed:
self.moveActBinary(2201)
else:
self.moveActScalar(speed)
def stop(self):
self.moveActBinary(2000)
############# private methods #############
# single speed actuator movement
def moveActBinary(self, actChoice, speed):
speed = self.verify_speed(speed)
if actChoice == 'dig':
if speed <= 1800:
self.act1.ForwardM1(0x80, 118)
self.act2.ForwardM1(0x80, 127)
elif speed >= 2200:
self.act1.BackwardM1(0x80, 127)
self.act2.BackwardM1(0x80, 127)
else:
self.act1.ForwardM1(0x80, 0)
self.act2.ForwardM1(0x80, 0)
elif actChoice == 'raise':
if speed <= 1800:
self.act1.ForwardM2(0x80, 127)
self.act2.ForwardM2(0x80, 127)
elif speed >= 2200:
self.act1.BackwardM2(0x80, 127)
self.act2.BackwardM2(0x80, 127)
else:
self.act1.ForwardM2(0x80, 0)
self.act2.ForwardM2(0x80, 0)
else:
print("bad act choice, stopping both")
self.act1.ForwardM1(0x80, 0)
self.act1.ForwardM1(0x80, 0)
self.act2.ForwardM2(0x80, 0)
self.act2.ForwardM2(0x80, 0)
# variable speed actuator movement
def moveActScalar(self, actChoice, speed):
speed = self.verify_speed(speed)
if speed <= 1800:
# move actuator forward
adjusted_speed = translate_value(speed, 1800, 0, 0, 127)
direction = "b"
elif speed >= 2200:
#move actuator backward
adjusted_speed = translate_value(speed, 2200, 4095, 0, 127)
direction = "f"
else:
#do not move actuator
direction = "s"
adjusted_speed = 0
if direction == "f":
if actChoice == 'dig':
self.act1.ForwardM1(0x80, adjusted_speed)
self.act2.ForwardM1(0x80, adjusted_speed)
elif actChoice == 'raise':
self.act1.ForwardM2(0x80, adjusted_speed)
self.act2.ForwardM2(0x80, adjusted_speed)
else:
print("bad actuator choice")
elif direction == "b":
if actChoice == 'dig':
self.act1.BackwardM1(0x80, adjusted_speed)
self.act2.BackwardM1(0x80, adjusted_speed)
if actChoice == 'raise':
self.act1.BackwardM2(0x80, adjusted_speed)
self.act2.BackwardM2(0x80, adjusted_speed)
else:
print("bad actuator choice")
else:
self.act1.ForwardM1(0x80, 0)
self.act1.ForwardM2(0x80, 0)
self.act2.ForwardM1(0x80, 0)
self.act2.ForwardM2(0x80, 0)
# 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
# make sure not to drive the actuators with no more left
#if self.pos >= MAX_POS and speed > 2000:
# speed = 2000
#elif self.pos <= MIN_POS and speed < 2000:
# speed = 2000
#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)
class Launcher:
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 encoder_ready_check(self):
'''
Checks if encoder i ready.
return:
True or False
'''
if self.encoders_ready == 0:
return False
else:
return True
# ---------------------------------------------------------------------------------
# ----------------- Pitch Functions -----------------------------------------------
# ---------------------------------------------------------------------------------
def set_pitch_position(self, pitch_position):
'''
sets the pitch position of the launcher by the given pitch_position parameter.
Args:
pitch_position (float): desired pitch position for pitch motors in degrees
'''
if self.encoder_ready_check():
# Checks conditions
if pitch_position > self.pitch_length or pitch_position < 0:
raise ValueError("out of bounds")
elif pitch_position == 0:
pitch_objective = 0
else:
pitch_objective = int(self.pitch_pulses /
(self.pitch_length / pitch_position))
pitch_increment = pitch_objective - self.rc.ReadEncM1(
self.address)[1]
if pitch_increment >= 0:
self.rc.SpeedDistanceM1(
self.address, self.pitch_speed_pulses, pitch_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
else:
self.rc.SpeedDistanceM1(self.address, -self.pitch_speed_pulses,
-pitch_increment, 1)
self.rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
else:
raise EncoderError('Encoder Not Ready')
def pitch_control(self, cmd: PitchCMD):
'''
Takes in a command (up, down or stop) and controlls the pitch accordingly
Args:
cmd (Pitch.CMD): desired movement command for pitch motors (PitchCMD.up, PitchCMD.down, PitchCMD.stop)
'''
if cmd == PitchCMD.up:
self.rc.BackwardM1(self.address, self.pitch_speed_manual)
if cmd == PitchCMD.down:
self.rc.ForwardM1(self.address, self.pitch_speed_manual)
if cmd == PitchCMD.stop:
self.rc.ForwardM1(self.address, 0)
# ---------------------------------------------------------------------------------
# ------------------------ Rotation functions--------------------------------------
# ---------------------------------------------------------------------------------
def set_rotation_position(self, rotation_position):
'''
sets the rotation position of the launcher by the given rotation_position parameter.
Args:
rotation_position (float): desired rotation position for rotation motors in degrees
'''
if self.encoder_ready_check():
# Checks conditions
if rotation_position > self.lift_length or rotation_position < 0:
raise ValueError("out of bounds")
elif rotation_position == 0:
rotation_objective = 0
else:
rotation_objective = int(
(self.rotation_pulses /
(self.rotation_length / rotation_position)) / 2)
rotation_increment = rotation_objective - self.rc.ReadEncM2(
self.address)[1]
if rotation_increment >= 0:
self.rc.SpeedDistanceM2(
self.address, self.rotation_speed_pulses,
rotation_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM2(self.address, 0, 0,
0) #To avoid deceleration
else:
self.rc.SpeedDistanceM2(self.address,
-self.rotation_speed_pulses,
-rotation_increment, 1)
self.rc.SpeedDistanceM2(self.address, 0, 0,
0) #To avoid deceleration
else:
raise EncoderError('Encoder Not Ready')
def rotation_control(self, cmd: RotationCMD):
'''
Takes in a command (right, left or stop) and controlls the rotation accordingly
Args:
cmd (Rotation.CMD): desired movement command for lift motors (Rotation.right, Rotation.left, Rotation.stop)
'''
if cmd == RotationCMD.right:
self.rc.ForwardM1(self.address_2, self.rotation_speed_manual)
self.rc.ForwardM2(self.address_2, self.rotation_speed_manual)
if cmd == RotationCMD.left:
self.rc.BackwardM1(self.address_2, self.rotation_speed_manual)
self.rc.BackwardM2(self.address_2, self.rotation_speed_manual)
if cmd == RotationCMD.stop:
self.rc.ForwardM1(self.address_2, 0)
self.rc.ForwardM2(self.address_2, 0)
# ---------------------------------------------------------------------------------
# ------------------------ Lift functions--------------------------------------
# ---------------------------------------------------------------------------------
def set_lift_position(self, lift_position):
'''
sets the lift position of the launcher by the given lift_position parameter.
Args:
lift_position (float): desired lift position on lift motors in centimeters
'''
if self.encoder_ready_check():
# Checks conditions
if lift_position > self.lift_length or lift_position < 0:
raise ValueError("out of bounds")
elif lift_position == 0:
lift_objective = 0
else:
lift_objective = int(self.lift_pulses /
(self.lift_length / lift_position))
lift_increment = lift_objective - self.rc.ReadEncM1(
self.address_2)[1]
if lift_increment >= 0:
self.rc.SpeedDistanceM1(
self.address_2, self.lift_speed_pulses, lift_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM1(self.address_2, 0, 0,
0) #To avoid deceleration
# set position cases
else:
self.rc.SpeedDistanceM1(self.address_2,
-self.lift_speed_pulses,
-lift_increment, 1)
self.rc.SpeedDistanceM1(self.address_2, 0, 0,
0) #To avoid deceleration
else:
raise EncoderError('Encoder Not Ready')
def lift_control(self, cmd: LiftCMD):
'''
Takes in a command (up, down or stop) and controlls the lift accordingly
Args:
cmd (LiftCMD): desired movement command for lift motors (LiftCMD.up, LiftCMD.down, LiftCMD.stop)
'''
if cmd == LiftCMD.up:
self.rc.ForwardM1(self.address_2, self.lift_speed_manual)
if cmd == LiftCMD.down:
self.rc.BackwardM1(self.address_2, self.lift_speed_manual)
if cmd == LiftCMD.stop:
self.rc.ForwardM1(self.address_2, 0)
# ---------------------------------------------------------------------------------
# ------------------------ Launch functions--------------------------------------
# ---------------------------------------------------------------------------------
def set_launch_position(self, launch_position):
'''
sets the launch position of the launcher by the given launch_position parameter.
Args:
launch_position (float): desired launch position in centimeters
'''
if self.encoder_ready_check():
# Checks conditions
if launch_position > self.launch_length or launch_position < 0:
raise ValueError("out of bounds")
else:
launch_objective = self.launch_bottom
launch_increment = launch_objective - self.rc.ReadEncM2(
self.address_2)[1]
if launch_increment >= 0:
self.rc.SpeedDistanceM2(
self.address_2, self.launch_speed_pulses_slow,
launch_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM2(self.address_2, 0, 0,
0) #To avoid deceleration
else:
self.rc.SpeedDistanceM2(self.address_2,
-self.launch_speed_pulses_slow,
-launch_increment, 1)
self.rc.SpeedDistanceM2(self.address_2, 0, 0,
0) #To avoid deceleration
buffer_2 = (0, 0, 0)
while (buffer_2[2] !=
0x80): #Loop until all movements are completed
buffer_2 = self.rc.ReadBuffers(self.address_2)
if launch_position == 0:
launch_objective = 0
else:
launch_objective = int(self.launch_pulses /
(self.launch_length / launch_position))
launch_increment = launch_objective - self.rc.ReadEncM2(
self.address_2)[1] + self.launch_connect
if launch_increment >= 0:
self.rc.SpeedDistanceM2(
self.address_2, self.launch_speed_pulses_slow,
launch_increment, 0
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM2(self.address_2, 0, 0,
0) #To avoid deceleration
else:
self.rc.SpeedDistanceM2(self.address_2,
-self.launch_speed_pulses_slow,
-launch_increment, 0)
self.rc.SpeedDistanceM2(self.address_2, 0, 0,
0) #To avoid deceleration
else:
raise EncoderError('Encoder Not Ready')
def launch_control(self, cmd: LaunchCMD):
'''
Takes in a command (forwards, backwards or stop) and controlls the launch accordingly
Args:
cmd (LaunchCMD): desired launch movement for launch motors, (for example: Launch.CMD.forwards)
'''
if cmd == LaunchCMD.up:
self.rc.ForwardM2(self.address_2, self.launch_speed_manual)
if cmd == LaunchCMD.down:
self.rc.BackwardM2(self.address_2, self.launch_speed_manual)
if cmd == LaunchCMD.stop:
self.rc.ForwardM2(self.address_2, 0)
def stop(self):
'''
stops all motors
'''
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
self.rc.ForwardM1(self.address_2, 0)
self.rc.ForwardM2(self.address_2, 0)
def max_pitch(self):
if self.encoder_ready_check():
pitch_increment = self.pitch_pulses - self.rc.ReadEncM1(
self.address)[1]
if pitch_increment >= 0:
self.rc.SpeedDistanceM1(
self.address, self.pitch_speed_pulses, pitch_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
self.rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
else:
self.rc.SpeedDistanceM1(self.address, -self.pitch_speed_pulses,
-pitch_increment, 1)
self.rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
def min_pitch(self):
pass
def min_lift(self):
pass
def max_lift(self):
pass
def home(self):
pass
def reset_encoders(self):
#TODO: reset encoder. Either set to "1" (ready) OR make the inverse of what it is now.
#example 1:
self.encoders_ready = 1
#example 2:
pass
def battery_voltage(self):
'''
Calculates and returns battery voltage
param:
none
return:
voltage (float): represents battery voltage
'''
voltage = round(0.1 * self.rc.ReadMainBatteryVoltage(self.address)[1],
2)
return voltage
# ---------------------------------------------------------------------------------
# ------------------------ Automated functions--------------------------------------
# ---------------------------------------------------------------------------------
def standby(self):
'''
Sets pitch, rotation, lift and launch to zero, which is the home position.
'''
self.set_pitch_position(0)
self.set_rotation_position(0)
self.set_lift_position(0)
self.set_launch_position(0)
def set_launch_variables(self, pitch_position, rotation_position,
lift_position):
'''
Sets the variables before preparing the launch.
Args:
pitch_position (int): desired pitch position between values x and y
rotation_position (int): desired rotation position between x and y
lift_position (int): desired lift position in between values x and y
launch_position (int): desired launch position in between values x and y
'''
self.change_pitch(pitch_position) #Updates self.pitch_ready
self.change_rotation(rotation_position) #Updates self.rotation_ready
self.change_lift(lift_position) #Updates self.lift_ready
def prepare_launch(self):
'''
Configures the launchers pitch, rotation and lift according to the variables set in set_launch_variables().
also sets launch position to zero, since that is the start position before launch
'''
self.set_pitch_position(self.pitch_ready)
self.set_rotation_position(self.rotation_ready)
# Changed self.lift_position to self.lift_ready since there is no variable named lift_position
self.set_lift_position(self.lift_ready)
self.set_launch_position(0)
def launch(self):
'''
launch operates the launch motor + belt.
when this runs, the drone launches into the air.
'''
pass
def mount(self):
pass
def automatic_launch(self):
pass
# ---------------------------------------------------------------------------------
# ------------------------ Variable update functions-------------------------------
# ---------------------------------------------------------------------------------
def change_pitch(self, pitch_position):
if pitch_position > self.pitch_length or pitch_position < 0:
raise ValueError("Out of Bounds")
else:
self.pitch_ready = pitch_position
def change_lift(self, lift_position):
if lift_position > self.lift_length or lift_position < 0:
raise ValueError("Out of Bounds")
else:
self.lift_ready = lift_position
def change_rotation(self, rotation_position):
if rotation_position > self.rotation_length or rotation_position < 0:
raise ValueError("Out of Bounds")
else:
self.rotation_ready = rotation_position
def change_speed(self, speed):
if speed > self.launch_max_speed or speed < self.launch_min_speed:
raise ValueError("Out of Bounds")
else:
if speed > 7:
self.launch_speed_pulses = speed * 13400
self.launch_acceleration = 655360 #maximum value
else:
self.launch_speed_pulses = speed * 13400
self.launch_acceleration = (self.launch_speed_pulses**
2) / 13400
def change_acceleration(self, acceleration):
if acceleration > self.launch_max_acceleration or acceleration < self.launch_min_acceleration:
raise ValueError("Out of Bounds")
else:
self.launch_acceleration = acceleration * 13400
def disable_buttons(self):
pass
from roboclaw_3 import Roboclaw
#Windows comport name
rc = Roboclaw("COM3",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
print(rc.Open())
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)
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)
class Footballmachine:
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 has_angle_motor_stopped_moving(self):
interval = 1
first = self.rc.ReadEncM1(self.address)
sleep(interval)
second = self.rc.ReadEncM1(self.address)
while (first != second):
first = self.rc.ReadEncM1(self.address)
sleep(interval)
second = self.rc.ReadEncM1(self.address)
def init_motors(self):
print("Initializing all motors...")
self.rc.BackwardM1(self.address, 126)
self.has_angle_motor_stopped_moving()
self.rc.BackwardM1(self.address, 0)
self.rc.ResetEncoders(self.address)
print("Angle encoder:", self.rc.ReadEncM1(self.address)[1])
def displayspeed(self):
enc1 = self.rc.ReadEncM1(self.address)
enc2 = self.rc.ReadEncM2(self.address)
speed1 = self.rc.ReadSpeedM1(self.address)
speed2 = self.rc.ReadSpeedM2(self.address)
print(("Encoder1:"), end=' ')
if (enc1[0] == 1):
print(enc1[1], end=' ')
print(format(enc1[2], '02x'), end=' ')
else:
print("failed", end=' ')
print("Encoder2:", end=' ')
if (enc2[0] == 1):
print(enc2[1], end=' ')
print(format(enc2[2], '02x'), end=' ')
else:
print("failed ", end=' ')
print("Speed1:", end=' ')
if (speed1[0]):
print(speed1[1], end=' ')
else:
print("failed", end=' ')
print(("Speed2:"), end=' ')
if (speed2[0]):
print(speed2[1])
else:
print("failed ")
def speed_to_QPPS(self, speed):
radius = 0.1
encoder_pulses_per_rad = 1024 / 2
angular_speed = speed / (2 * pi * radius)
QPPS = encoder_pulses_per_rad * angular_speed
return int(QPPS)
def angle_to_QP(self, angle):
range_min = 0
range_max = 225
angle_min = 0
angle_max = 55
a1 = int(angle) - angle_min
a2 = range_max - range_min
a3 = angle_max - angle_min
angle = int((a1 * a2) / a3 + range_min)
return angle
def set_angle(self, angle):
print("Set_angle: ", angle)
angle = self.angle_to_QP(angle)
print("Target position M1:", angle)
self.rc.SpeedAccelDeccelPositionM1(self.address, 10, 10, 10, angle, 0)
self.has_angle_motor_stopped_moving()
print("Angle encoder:", self.rc.ReadEncM1(self.address)[1])
def set_speed_then_stop(self, speed):
print("Set_speed: ", speed)
speed = self.speed_to_QPPS(int(speed))
self.rc.SpeedAccelM2(self.address, 22000, speed)
sleep(4)
self.rc.SpeedAccelM2(self.address, 22000, 0)
def set_speed(self, speed):
print("Set_speed: ", speed)
speed = self.speed_to_QPPS(int(speed))
self.rc.SpeedAccelM2(self.address, 22000, speed)
for i in range(0, 50):
print(("{} # ".format(i)), end=' ')
self.displayspeed()
sleep(0.1)
def check_encoders(self, seconds):
for i in range(0, seconds):
print(("{} # ".format(i)), end=' ')
self.displayspeed()
sleep(0.1)