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)