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 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 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 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("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
from roboclaw_3 import Roboclaw
from time import sleep
#Windows comport name
rc = Roboclaw("COM6", 115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
while (True):
rc.ForwardM1(0x80, 64)
sleep(2)
rc.ForwardM1(0x80, 0)
sleep(2)
rc.BackwardM1(0x80, 64)
sleep(2)
rc.BackwardM1(0x80, 0)
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)
