ord Posted May 29, 2021 Author Posted May 29, 2021 @melon thank you :). I haven't made a Studio file - hopefully HRU_Bricks' will suffice. @HRU_Bricks @Berthil I did some small tests and the grooves in GBC balls did indeed cause the vacuum not to work for me. It would be great to see the robot used in a GBC in one way or another! Quote
aeh5040 Posted June 9, 2021 Posted June 9, 2021 This is an amazing model. I did not even know about this kind of mechanism. Quote
ord Posted June 11, 2021 Author Posted June 11, 2021 @aeh5040 thanks! Neither did I until recently - thank you YouTube recommendations! Quote
ord Posted January 12 Author Posted January 12 Here's the code. Note that it's raw and a bit messy - I never intended to publish it. from pybricks.hubs import TechnicHub from pybricks.pupdevices import Motor from pybricks.parameters import Port, Stop, Color, Direction from pybricks.tools import wait from math import sin, pi, fabs, sqrt print("START") # Initialize the hub hub = TechnicHub() # Initialize the motors motor_x = Motor(Port.A, Direction.COUNTERCLOCKWISE, [20, 75]) motor_y = Motor(Port.B, Direction.COUNTERCLOCKWISE, [20, 75]) motor_z = Motor(Port.C, Direction.COUNTERCLOCKWISE, [20, 75]) #motor_suction = Motor(Port.D, Direction.CLOCKWISE, [12, 36]) # Initialize variables old_x = 0.0 old_y = 0.0 old_z = 0.0 # Flash light def flash(): hub.light.on(Color.GREEN) wait(100) hub.light.off() # Find zero def reset_motors(): print("Finding origin...") motor_x.run_until_stalled(-100, duty_limit=30) print("x zero set.") #flash() motor_y.run_until_stalled(-100, duty_limit=30) print("y zero set.") #flash() motor_z.run_until_stalled(-100, duty_limit=30) print("z zero set.") #flash() motor_x.reset_angle(0) motor_y.reset_angle(0) motor_z.reset_angle(0) #motor_suction.reset_angle(0) # Move end effector def move(x, y, z, speed): global old_x global old_y global old_z rel_x = x - old_x rel_y = y - old_y rel_z = z - old_z segment_length = sqrt(pow(rel_x,2) + pow(rel_y,2) + pow(rel_z,2)) time = segment_length / speed if time == 0: time = 0.01 x_speed = fabs(rel_x) / time y_speed = fabs(rel_y) / time z_speed = fabs(rel_z) / time #print("Time = " + str(time) + "; Segment length = " + str(segment_length) + "; Speed = " + str(x_speed) + ", " + str(y_speed) + ", " + str(z_speed) + "; Move to " + str(x) + ", " + str(y) + ", " + str(z)) if x_speed > 1: motor_x.run_target(x_speed, x*10, then=Stop.COAST, wait=False) if y_speed > 1: motor_y.run_target(y_speed, y*10, then=Stop.COAST, wait=False) if z_speed > 1: motor_z.run_target(z_speed, z*10, then=Stop.COAST, wait=False) wait(time * 12000) old_x = x old_y = y old_z = z # Move each axis +/- def axes_demo(speed): move(7,7,0,speed) move(0,7,0,speed) move(0,0,0,speed) move(0,7,0,speed) move(0,7,5,speed) move(0,7,0,speed) # Circle (diagonal) def circle(speed): move(6.0,5.3,4.3,speed) move(4.9,6.5,3.5,speed) move(3.7,7.0,2.5,speed) move(2.1,6.5,1.5,speed) move(1.0,5.3,0.7,speed) move(0.7,3.5,0.5,speed) move(1.0,1.7,0.7,speed) move(2.1,0.5,1.5,speed) move(3.5,0.0,2.5,speed) move(4.9,0.5,3.5,speed) move(6.0,1.8,4.3,speed) move(6.3,3.5,4.5,speed) # Circle (diagonal reverse) def circle2(speed): move(6.0,1.8,4.3,speed) move(4.9,0.5,3.5,speed) move(3.7,0.0,2.5,speed) move(2.1,0.5,1.5,speed) move(1.0,1.7,0.7,speed) move(0.7,3.5,0.5,speed) move(1.0,5.3,0.7,speed) move(2.1,6.5,1.5,speed) move(3.5,7.0,2.5,speed) move(4.9,6.5,3.5,speed) move(6.0,5.3,4.3,speed) move(6.3,3.5,4.5,speed) # Circle (diagonal reverse raised) def circle3(speed): move(6.7,1.8,4.8,speed) move(5.6,0.5,4.0,speed) move(4.2,0.0,3.0,speed) move(2.8,0.5,2.0,speed) move(1.7,1.7,1.2,speed) move(1.4,3.5,1.0,speed) move(1.7,5.3,1.2,speed) move(2.8,6.5,2.0,speed) move(4.2,7.0,3.0,speed) move(5.6,6.5,4.0,speed) move(6.7,5.3,4.8,speed) move(7.0,3.5,5.0,speed) # Circle (top plane) def circle4(speed): move(7,3.5,5,speed) move(6.96,4,5,speed) move(6.85,4.5,5,speed) move(6.66,5,5,speed) move(6.37,5.5,5,speed) move(5.95,6,5,speed) move(5.46,6.4,5,speed) move(4.92,6.7,5,speed) move(4.33,6.9,5,speed) move(3.5,7,5,speed) move(2.67,6.9,5,speed) move(2.08,6.7,5,speed) move(1.54,6.4,5,speed) move(1.05,6,5,speed) move(0.63,5.5,5,speed) move(0.34,5,5,speed) move(0.15,4.5,5,speed) move(0.04,4,5,speed) move(0,3.5,5,speed) move(0.04,3,5,speed) move(0.15,2.5,5,speed) move(0.34,2,5,speed) move(0.63,1.5,5,speed) move(1.05,1,5,speed) move(1.54,0.6,5,speed) move(2.08,0.3,5,speed) move(2.67,0.1,5,speed) move(3.5,0,5,speed) move(4.33,0.1,5,speed) move(4.92,0.3,5,speed) move(5.46,0.6,5,speed) move(5.95,1,5,speed) move(6.37,1.5,5,speed) move(6.66,2,5,speed) move(6.85,2.5,5,speed) move(6.96,3,5,speed) move(7,3.5,5,speed) # Suction movement def suction_move(x,y,z): move(6.8,1.2,5,100) #wait(500) move(6.8,1.2,z,100) wait(200) # Suction on motor_suction.run_target(500, 180, then=Stop.BRAKE, wait=False) wait(1000) move(6.8,1.2,5.3,100) #wait(500) move(x,y,5.3,150) #wait(500) move(x,y,1.5,150) #wait(500) # Suction off motor_suction.run_target(500, 0, then=Stop.BRAKE, wait=True) wait(100) # Video part 1a def part_1a(): # Bottom square move(0,7,0,60) wait(500) move(7,7,0,60) wait(500) move(7,0,0,60) wait(500) move(0,0,0,60) wait(1000) # Move z move(0,0,5,300) wait(500) #move(0,0,5,300) #wait(500) #move(7,0,0,300) #wait(500) #move(0,7,5,300) #wait(500) #move(0,7,0,300) #wait(500) move(7,0,5,300) wait(1000) # Two circles then home move(6.3,3.5,4.5,250) circle(140) circle(140) wait(500) move(0,0,0,100) # Video part 1b def part_1b(): move(0,7,0,100) wait(3000) axes_demo(70) wait(1000) # Top circle move(7,0,5,100) wait(1000) circle4(100) wait(2000) move(6.3,3.5,4.5,100) circle(130) circle(130) wait(600) move(0,0,0,140) # Video part 2 def part_2(): move(0,0,5,300) wait(500) suction_move(0,2.8,3.0) move(0,2.8,5,150) suction_move(3,2.8,2.0) move(3,2.8,5,150) suction_move(0,4.8,1.0) move(0,4.8,5,150) suction_move(3,4.8,0.0) # Push tray wait(1000) move(1.5,7,1.5,80) move(1.5,7,0,80) move(1.5,0,0,50) wait(200) # Back right top corner move(7,7,5,400) wait(500) # PROGRAM HERE reset_motors() hub.light.on(Color.WHITE * 0.8) wait(5000) part_1b() #part_1b or part_2 here print("END") Quote
Recommended Posts
Join the conversation
You can post now and register later. If you have an account, sign in now to post with your account.