def __init__(self, addr1, addr2):
self.act1 = Roboclaw(addr1, 115200)
self.act2 = Roboclaw(addr2, 115200)
while self.act1.Open() == 0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
while self.act2.Open() == 0:
print("Failed to open actuator 2 comms, trying again")
time.sleep(1)
print("Opened Actuator roboclaw to ", addr1, "and ", addr2)
def __init__(self, addr):
self.act = Roboclaw(addr, 115200)
self.currents = [0,0]
while self.act.Open()==0:
print("Failed to open actuator comms, trying again.")
time.sleep(1)
print("Opened Belt roboclaw to ",addr)
def main():
#
# get the encoder counts
#
# address of the RoboClaw as set in Motion Studio
#
address = 0x80
print(address)
#
# Creating the RoboClaw object, serial port and baudrate passed
#
robo = Roboclaw("/dev/ttymxc2", 38400)
#
# Starting communication with the RoboClaw hardware
#
opened = robo.Open()
if opened:
#
# Start motor 1 in the forward direction at half speed
#
counts = robo.ReadEncM1(address)
print("motor 1 counts: ", counts)
m1_count = counts[1]
print("motor 1 count: ", m1_count)
else:
print("port did not open")
return
def __init__(self):
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 __init__(self, address=RC_ADDRESS):
self.rc = Roboclaw(COMPORT, 115200)
self.rc.Open()
logger.info('Opened Roboclaw')
self.address = address
self.dir = None
self.speed = INITIAL_SPEED
self.throttle(Throttle.NEUTRAL)
self.rc.TurnRightMixed(address, 0)
def __init__(self, address=0x80, baudrate=38400, port="/dev/ttyS0"):
self.address = address
self.rc = Roboclaw(port, baudrate)
self.rc.Open()
version = self.rc.ReadVersion(self.address)
if version[0] == False:
print("GETVERSION Failed - check power supply and conections")
return
else:
print(repr(version[1]))
def __init__(self):
# Initialize addresses = [0x80, 0x81]
self.addresses = [0x80, 0x81]
# Connect to roboclaw
serial_port = "/dev/ttyS0"
baudrate = 38400
self.roboclaw = Roboclaw(serial_port, baudrate)
self.roboclaw.Open()
self.robotspeed = 10
def __init__(self):
#setup variables
#Linux comport name
#self.rc = Roboclaw("/dev/ttyACM1",115200)
#Windows com-port name
self.rc = Roboclaw("COM8", 115200)
self.rc.Open()
#Declare variables
self.address = 0x80 #Controller 1, M1=Pitch, M2=Rotation
self.address_2 = 0x81 #Controller 2, M1=Lift, M2=Launch
self.pitch_pulses = 355000 #Encoder pulses from the linear actuator
self.pitch_length = 90.0 #Degrees
self.pitch_speed_pulses = 7000 #Pulses per second
self.pitch_speed_manual = 75 #From 0 to 127
self.pitch_ready = 70.0 #Pitch degrees for the launch (temporary)
self.rotation_pulses = 950000 #Encoder pulses from the rotation motor
self.rotation_length = 180.0 #Degrees
self.rotation_speed_pulses = 16000 #Pulses per second
self.rotation_speed_manual = 127 #From 0 to 127
self.rotation_ready = 10.0 #Rotation degress for the launch (temporary)
self.lift_pulses = 19000 #Encoder pulses from the lifting colum
self.lift_length = 130.0 #cm
self.lift_speed_pulses = 420 #Pulses per second
self.lift_speed_manual = 127 #From 0 to 127 - 7 bits
self.lift_ready = self.lift_length #Lift lenght for the launch (temporary)
self.launch_pulses = 14800 #Encoder pulses from the launch motor
self.launch_length = 111.0 #cm
self.launch_speed_pulses = 6 * 13400 #Pulses per second during launch (145000 max) (13400 pulses/m)
self.launch_speed_pulses_slow = 2500 #Pulses per second during preparation
self.launch_speed_manual = 12 #From 0 to 127
self.launch_acceleration = (
self.launch_speed_pulses**
2) / 13400 #Acceleration during launch (pulses/second2)
self.launch_max_speed = 10 #Maximum launch speed
self.launch_min_speed = 1 #Minimum launch speed
self.launch_max_acceleration = 48 #Maximum launch acceleration
self.launch_min_acceleration = 1 #Minimum launch acceleration
self.launch_standby = 8000 #Drone position during stand-by
self.launch_mount = 17000 #Drone position during mounting
self.launch_break = 21000 #Belt position during breaking
self.launch_bottom = 0 #Drone position at the back part of the capsule
self.launch_connect = 2190 #Belt position for touching the upper part
self.encoders_ready = 0 #At the beggining, the encoders are not ready
def __init__(self):
# GPIO.setmode(GPIO.BCM)
# GPIO.setwarnings(False)
self.address_front_wheels = 0x80
self.address_rear_wheels = 0x81
self.full_speed = 127
self.sleep_time = 0.005
self.roboclaw = Roboclaw("/dev/ttyS0", 38400)
self.roboclaw.Open()
print("Error")
print(self.roboclaw.ReadError(self.address_front_wheels))
self.roboclaw.SetMinVoltageMainBattery(self.address_front_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_front_wheels, 112)
self.roboclaw.SetMinVoltageMainBattery(self.address_rear_wheels, 62)
self.roboclaw.SetMaxVoltageMainBattery(self.address_rear_wheels, 112)
def main():
#
# error checking follows:
#
# must have one argument
#
usage_str = "usage:\npython3 motor_forward.py XXX"
arg_count = len(sys.argv)
if (arg_count != 2):
print(usage_str)
#
# get the move speed, must be between 0 and 127
#
else:
speed = int(sys.argv[1])
#
# address of the RoboClaw as set in Motion Studio
#
address = 0x80
#
# Creating the RoboClaw object, serial port and baudrate passed
#
robo = Roboclaw("/dev/ttymxc2", 38400)
#
# Starting communication with the RoboClaw hardware
#
opened = robo.Open()
if opened:
#
# Start motor 1 in the forward direction at half speed
#
robo.ForwardM2(address, speed)
#
# pause for two seconds
#
time.sleep(2.0)
#
# stop the motor
#
robo.ForwardM2(address, 0)
else:
print("port did not open")
return
def __init__(self):
self.rc = Roboclaw("/dev/ttyACM0", 115200) # Linux comport name
self.address = 0x80
self.rc.Open()
self.ready = True
version = self.rc.ReadVersion(self.address)
if not version[0]:
print("GETVERSION Failed")
exit()
else:
print(repr(version[1]))
print("Car main battery voltage at start of script: ", self.rc.ReadMainBatteryVoltage(self.address))
for i in range(1000):
try:
self.rc.ForwardM2(self.address, i)
time.sleep(0.1)
print(i)
except Exception as e:
print(e)
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...")
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)
#from pyfirmata import ArduinoMega, util
from time import sleep
from threading import Timer
import re
import RPi.GPIO as GPIO
import smbus
from smbus import SMBus
from PCA9685 import PWM
import serial
measWaitTime = 0
motorWaitTime = 0
ser = serial.Serial()
batteryVoltage = ""
roboclaw = Roboclaw("/dev/ttyS0", 38400)
def connectMotorSerial():
global roboclaw
motor_baudrate = 38400
print("connecting motors")
roboclaw.Open()
def connectSerial():
connectMotorSerial()
global ser
global batteryVoltage
try:
from roboclaw_3 import Roboclaw
#Windows comport name
rc = Roboclaw("COM3",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
print(rc.Open())
#
#formatter = logging.Formatter('%(asctime)s:%(name)s:%(levelname)s:%(message)s')
#
#file_handler = logging.FileHandler('launcher.log')
#file_handler.setLevel(logging.INFO)
#file_handler.setFormatter(formatter)
#
#logger.addHandler(file_handler)
logging.basicConfig(filename='launcher.log',level=logging.DEBUG)
#Open serial port
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
#Windows comport name
rc = Roboclaw("COM8",115200)
rc.Open()
encoders_ready = 1 # set to 1 so that the position method can be tested
origo = [90.0, 0, 0, 0]
actual_coordonates = origo[:]
case_open = 0
class motor():
def __init__(self, address, channel, pulses, length, speed_pulses, speed_manual, ready):
self.address = address
self.channel = channel
self.pulses = pulses
self.length = length
self.speed_pulses = speed_pulses
from roboclaw_3 import Roboclaw
import time
#***Before using this example the motor/controller combination must be
#***tuned and the settings saved to the Roboclaw using IonMotion.
#***The Min and Max Positions must be at least 0 and 50000
#Limits velocity: 0 to +/- maximum motor speed
#Limits position: Minimum an maximum encoder position
#Windows comport name
rc = Roboclaw("COM27",9600)
rc.Open()
address = 0x80
def displayspeed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
print("Encoder1:")
if(enc1[0]==1):
print(enc1[1])
print(format(enc1[2],'02x'))
else:
print("failed")
print("Encoder2:")
if(enc2[0]==1):
print(enc2[1])
print(format(enc2[2],'02x'))
else:
print("failed ")
# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
# Start streaming with requested config
pipe.start(cfg)
roboclaw_vid = 0x03EB # VID of Roboclaw motor driver in hex
found = False
for port in comports():
if port.vid == roboclaw_vid:
roboclawport = port.device
found = True
if found == True:
print("Roboclaw port:", roboclawport)
rc = Roboclaw(roboclawport, 0x80)
rc.Open() # Start motor controller
address = 0x80
version = rc.ReadVersion(address)
tickdistanceL = 10 # number of left encoder ticks per mm traveled
tickdistanceR = 10 # number of right encoder ticks per mm traveled
if version[0] == False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
# open waypoint file
if testmode:
waypoint_file = 'waypoints_test.csv'
class launcher(object):
"""
launcher object is created - here, one roboclaw instance is created
but one should be able to create as many roboclaw objects as one needs
"""
#Open serial port
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
#Windows comport name
rc = Roboclaw("COM8", 115200)
rc.Open()
encoders_ready = 0
def __init__(self):
pass
class motor():
"""
roboclaw configurations are added to the launcher object
"""
def __init__(self, address, channel, pulses, length, speed_pulses,
speed_manual, ready):
self.address = address
self.channel = channel
self.pulses = pulses
self.length = length
self.speed_pulses = speed_pulses
self.speed_manual = speed_manual
self.ready = ready
Master_M1 = motor(0x80, 1, 355000, 90.0, 7000, 127, 70.0) # pitch
Master_M2 = motor(0x80, 2, 950000, 180.0, 16000, 15, 10.0) # rotation
Slave_M1 = motor(0x81, 1, 19000, 130.0, 420, 127, 130.0) # lift
Slave_M2 = motor(0x81, 2, 14800, 111.0, 6 * 13400, 12,
0.0) # launch has more variables...
Slave_2_M1 = motor(0x82, 1, 5000, 0.0, 200, 127, 5.0) # case left
Slave_2_M2 = motor(0x82, 2, 5000, 0.0, 200, 127, 5.0) # case right
"""launch_acceleration=(launch_speed_pulses**2)/13400 #Acceleration during launch (pulses/second2)
launch_max_speed=10 #Maximum launch speed
launch_min_speed=1 #Minimum launch speed
launch_max_acceleration=48 #Maximum launch acceleration
launch_min_acceleration=1 #Minimum launch acceleration
launch_standby=8000 #Drone position during stand-by
launch_mount=17000 #Drone position during mounting
launch_break=21000 #Belt position during breaking
launch_bottom=0 #Drone position at the back part of the capsule
launch_connect=2190 #Belt position for touching the upper part """
def manual_up(self):
"""
This should drive any motor controller:
+ UP for PITCH, COLUMN, CASE LEFT and CASE RIGHT
+ RIGHT for rotation motor
+ FORWARD for the launcher belt
"""
try:
if self.channel == 1:
rc.ForwardM1(self.address, self.speed_manual)
# return (''), 204 #Returns an empty response - Flask
else:
rc.ForwardM2(self.address, self.speed_manual)
# return (''), 204 #Returns an empty response - Flask
except:
logger.info("I go up")
def manual_down(self):
"""
This should drive any motor controller:
+ DOWN for PITCH, COLUMN, CASE LEFT and CASE RIGHT
+ LEFT for rotation motor
+ BACKWARDS for the launcher belt
"""
try:
if self.channel == 1:
rc.BackwardM1(self.address, self.speed_manual)
# return (''), 204
else:
rc.BackwardM2(self.address, self.speed_manual)
except:
logger.info("I go down")
def manual_position(self):
"""
This should bring ANY motor, from their current position to a user defined position
"""
try:
""" if self.encoders_ready == 0: #Not execute if the encoders are not ready
return (''), 403 """
pitch_position = request.form.get('pitch_position', type=int)
if pitch_position > self.pitch_length or pitch_position < 0:
return (''), 400
elif pitch_position == 0:
pitch_objective = 0
else:
pitch_objective = int(self.pitch_pulses /
(self.pitch_length / pitch_position))
pitch_actual = rc.ReadEncM1(self.address)[1]
pitch_increment = pitch_objective - pitch_actual
if pitch_increment >= 0:
rc.SpeedDistanceM1(
self.address, self.pitch_speed_pulses, pitch_increment, 1
) #(address, +-speed, pulses, buffer(0=buffered, 1=Execute immediately))
rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
return (''), 204
else:
rc.SpeedDistanceM1(self.address, -self.pitch_speed_pulses,
-pitch_increment, 1)
rc.SpeedDistanceM1(self.address, 0, 0,
0) #To avoid deceleration
return (''), 204
except:
logger.info("I go to a certain position")
def prepare(self):
pass
def stop_all(self):
pass
def launch(self):
pass
def standby(self):
pass
def case_termometer(self):
pass
def CPU_temperature(self):
pass
def MPU92_65(self):
pass
def LED_strip(self):
pass
from roboclaw_3 import Roboclaw
import matlab.engine
import numpy as np
eng = matlab.engine.start_matlab()
eng.cd(r'C:\Users\BAB\Desktop\Fotballmaskin-master-python3\src\Fotballmaskin\Pcprogram\optimalisering')
eng.make_init(nargout=0)
roboclaw = Roboclaw('COM5', 115200)
roboclaw.Open()
## RUN ##
k_d=1.0
k_l=1.0
k_w=1.0
#While True/exit button clicked:
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() #Erstattes av brukergrensesnitt
desired_point_string_list=list(desired_point_string.split(','))
p_w=matlab.double(list(map(float,desired_point_string_list)))
[x_return] = eng.find_initvalues_speed(p_w,k_d,k_l,k_w,nargout=1)
print("x_return: ", x_return) #(9x1)[velocity,theta,psi,omega,lambda,gamma,kd,kl,kw]
if int(theta:=x_return[2])!= 0:
#Convert theta to position in encoder
roboclaw.SpeedAccelDeccelPositionM1(0x80,10000,2000,10000,15000,1) #SpeedAccelDeccelPositionM2(address,accel,speed,deccel,position,buffer)
import time
import paho.mqtt.client as mqtt
from flask import Flask, render_template, request
from roboclaw_3 import Roboclaw
geschwindigkeit = 60
roboclaw = Roboclaw('/dev/ttyACM0', 38400)
roboclaw.Open()
# Flask - Webseite
app = Flask(__name__)
@app.route("/", methods=['GET', 'POST'])
def index():
if request.method == 'POST':
#geschwindigkeit = request.form["geschwindigkeit"]
if request.form['steuerbefehl'] == "vor":
fahre_rover_vorwaerts(geschwindigkeit)
if request.form['steuerbefehl'] == "stop": stoppe_rover()
if request.form['steuerbefehl'] == "zurueck":
fahre_rover_rueckwaerts(geschwindigkeit)
if request.form['steuerbefehl'] == "rechts":
drehe_rover_nach_rechts(geschwindigkeit)
if request.form['steuerbefehl'] == "links":
drehe_rover_nach_links(geschwindigkeit)
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)
conn.send(data)
command = data.decode()
print(command)
if command.strip() == 'THROTTLE FORWARD':
rc.ForwardMixed(address, 32)
if command.strip() == 'THROTTLE REVERSE':
rc.BackwardMixed(address, 32)
if input() == 'c':
conn.close()
# Windows comport name
comport = "COM5"
rc = Roboclaw(comport, 115200)
rc.Open()
address = 0x80
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serv.bind(('10.24.178.220', 8080))
serv.listen(5)
while True:
conn, addr = serv.accept()
from_client = ''
while True: