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
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)
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)
temp = raw_input(
"Hit l, r, f, s or w to go left, right, forward, stop or save waypoint"
)
if (temp == "l"):
print("Going left")
drive(cruise_speed, -30)
if (temp == "r"):
print("Going right")
drive(cruise_speed, 20)
if (temp == "f"):
print("Going forward")
drive(cruise_speed, 0)
if (temp == "s"):
print("Stopping")
rc.ForwardM1(address, 0) # kill motors
rc.ForwardM2(address, 0)
if (temp == "w"):
print("Saving waypoint #", waypoint_num)
frames = pipe.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
x = data.translation.x
y = -1.000 * data.translation.z # don't ask me why, but in "VR space", y is z and it's reversed
if (waypoint_num == 0):
with open(record_file,
'w') as csvfile: # overwrite original file
recordwriter = csv.writer(csvfile,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
#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
launch_connect=2190 #Belt position for touching the upper part - 16.5 cm
"""seemingly useless encoders_ready safety"""
encoders_ready = 0 #At the beggining, the encoders are not ready
#Create an instance of the Flask class for the web app
app = Flask(__name__)
#Render HTML template
@app.route("/")
def index():
return render_template('dronelauncher_web_test.html')
#Motor controller functions
try:
rc.ForwardM2(address, rotation_speed_manual)
rc.ForwardM2(address,0) #Both commands are used to avoid rotation
except AttributeError:
print("'Roboclaw' object has no attribute '_port' -> FAILURE TO DETECT ROBOCLAW")
@app.route('/app_pitch_up', methods=['POST'])
def function_pitch_up():
rc.ForwardM1(address, pitch_speed_manual)
return (''), 204 #Returns an empty response
@app.route('/app_pitch_down', methods=['POST'])
def function_pitch_down():
rc.BackwardM1(address, pitch_speed_manual)
return (''), 204
@app.route('/app_pitch_position', methods=['POST'])
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)
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)
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
#Windows comport name
#rc = Roboclaw("COM11",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
address = 0x80
while(1):
rc.ForwardM1(address,32) #1/4 power forward
rc.BackwardM2(address,32) #1/4 power backward
time.sleep(2)
rc.BackwardM1(address,32) #1/4 power backward
rc.ForwardM2(address,32) #1/4 power forward
time.sleep(2)
rc.BackwardM1(address,0) #Stopped
rc.ForwardM2(address,0) #Stopped
time.sleep(2)
m1duty = 16
m2duty = -16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward
rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward
time.sleep(2)
m1duty = -16
m2duty = 16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power backward