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)
def set_speed_ball(self,velocity):
spin=-velocity/(0.1*2*np.pi)
spinM1=self.calibM1*spin
spinM2=self.calibM2*spin
if spinM1<-184879 or spinM2<-178401:
print("speed is to high!")
return False
flagM1=self.set_speedM1(spinM1)
flagM2=self.set_speedM2(spinM2)
if not flagM1 and flagM2:
return False
return True
from roboclaw_3 import Roboclaw
#Windows comport name
#rc = Roboclaw("COM3",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyS0", 38400)
rc.Open()
address = 0x80
#rc.ResetEncoders(address)
while (1):
print("Pos 50000")
rc.SpeedAccelDeccelPositionM1(address, 500, 2000, 500, -5000, 1)
for i in range(0, 80):
print("Position: %d, Setpoint: %d", rc.ReadEncM1(address), 0)
#displayspeed()
time.sleep(0.1)
time.sleep(2)
#rc.SetEncM1(address, -10000)
print("Pos 0")
rc.SpeedAccelDeccelPositionM1(address, 4000, 4000, 4000, 0, 1)
for i in range(0, 80):
print(rc.ReadEncM1(address))
#displayspeed()
time.sleep(0.1)
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)
