def goToPoint(r, pos): g = P.goal vx = P.control_k_vx*(r[0]-pos[0]) vy = P.control_k_vy*(r[1]-pos[1]) theta_d = m.atan2(g[1]-r[1], g[0]-r[0]) omega = P.control_k_phi*(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def defendBall(r, b): g = P.goal vx = P.control_k_vx * (r[0]) vy = P.control_k_vy * (r[1] - b[1]) theta_d = m.atan2(g[1] - r[1], g[0] - r[1]) omega = P.control_k_phi * (r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def defendBall(r,b): g = P.goal vx = P.control_k_vx*(r[0]) vy = P.control_k_vy*(r[1]-b[1]) theta_d = m.atan2(g[1]-r[1], g[0]-r[1]) omega = P.control_k_phi*(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def goToGoal(r): g = P.goal vx = -(r[0]-g[0]) vy = -(r[1]-g[1]) theta_d = m.atan2(g[1]-r[1], g[0]-r[1]) omega = -(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def goToPoint(r, pos): g = P.goal vx = P.control_k_vx * (r[0] - pos[0]) vy = P.control_k_vy * (r[1] - pos[1]) theta_d = m.atan2(g[1] - r[1], g[0] - r[0]) omega = P.control_k_phi * (r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def launch(self): self.start = round(time.time(),2) self.stop = round(self.start+self.runTime,2) velchangers.goXYOmegaTheta(self.vel_x, self.vel_y, self.omega, self.theta) while(time.time() < self.stop): pass velchangers.goXYOmega(0,0,0)
def goToGoal(r): g = P.goal vx = -(r[0] - g[0]) vy = -(r[1] - g[1]) theta_d = m.atan2(g[1] - r[1], g[0] - r[1]) omega = -(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def vect2motors(data): # speed = 1 xg = 180 yg = 0 # info from vision, need to format correctly xb = data.ball_x yb = data.ball_y xr = data.home1_x yr = data.home1_y tr = data.home1_theta rospy.loginfo("robot x,y,theta : %f, %f" %(xr,yr,tr)) print "ballx,bally,homex,homey, hometheta",xb,yb,xr,yr,tr vel.goXYOmega(xr,yr,tr) xball = xb-xr if xball == 0: xball = .01 xgoal = xg-xr if xgoal == 0: xgoal = .01 try: toBall = math.acos(float(xball)/math.sqrt(float(xball)**2+float(yb-yr)**2))+tr toGoal = math.acos(float(xgoal)/math.sqrt(float(xgoal)**2+float(yg-yr)**2))+tr rospy.loginfo("toBall and toGoal : %f, %f" %(toBall,toGoal)) except ValueError: print "Please enter 3 valid sides" #print toBall #print toGoal run() xCommand = math.cos(toBall)#*speed yCommand = math.sin(toBall)#*speed fixAngle = float(toGoal/500) rospy.loginfo("xcommand,ycommand and fixangle : %f, %f,%f" %(xCommand,yCommand,fixAngle)) vel.testrun(xCommand,yCommand,fixAngle)
def goToStartDefender(r): pos = P.startDefender g = P.goal vx = P.control_k_vx*(r[0]-pos[0]) vy = P.control_k_vy*(r[1]-pos[1]) theta_d = m.atan2(g[1]-r[1], g[0]-r[1]) omega = P.control_k_phi*(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def goToGoal(r): g = P.goal #vx = vx = -P.control_k_vx*(r[0]-g[0]) vy = -P.control_k_vy*(r[1]-g[1]) theta_d = m.atan2(g[1]-r[1], g[0]-r[1]) omega = -P.control_k_phi*(r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def launch(self): self.start = round(time.time(), 2) self.stop = round(self.start + self.runTime, 2) velchangers.goXYOmegaTheta(self.vel_x, self.vel_y, self.omega, self.theta) while (time.time() < self.stop): pass velchangers.goXYOmega(0, 0, 0)
def goToStartDefender(r): pos = P.startDefender g = P.goal vx = P.control_k_vx * (r[0] - pos[0]) vy = P.control_k_vy * (r[1] - pos[1]) theta_d = m.atan2(g[1] - r[1], g[0] - r[1]) omega = P.control_k_phi * (r[2] - theta_d) vel.goXYOmega(vx, vy, omega)
def goHomeGoal(bret): distGoalX = 1.56 #distGoaly = data.home1_y goX = bret[0] - distGoalX goY = bret[1] tg = bret[2] #goTheta = tg - data.home1_theta vel.goXYOmega(goX, goY, tg)
def goHomeGoal(bret): distGoalX = 1.56 #distGoaly = data.home1_y goX = bret[0]-distGoalX goY = bret[1] tg = bret[2] #goTheta = tg - data.home1_theta vel.goXYOmega(goX,goY,tg)
def goToGoal(r): distGoalX = 1.75 #distGoaly = data.home1_y goX = r[0]+distGoalX#data.home1_x+distGoalX goY = r[1]#data.home1_y tg = r[2]#data.home1_theta #goTheta = tg - data.home1_theta vel.goXYOmega(goX,goY,tg)
def goToGoal(r): distGoalX = 1.75 #distGoaly = data.home1_y goX = r[0] + distGoalX #data.home1_x+distGoalX goY = r[1] #data.home1_y tg = r[2] #data.home1_theta #goTheta = tg - data.home1_theta vel.goXYOmega(goX, goY, tg)
def goCenter(data): xb = data.ball_x yb = data.ball_y xr = data.home1_x yr = data.home1_y tr = data.home1_theta print "info for debugg" print "ballx,bally,homex,homey, hometheta", xb, yb, xr, yr, tr vel.goXYOmega(xr, yr, tr)
def goCenter(data): xb = data.ball_x yb = data.ball_y xr = data.home1_x yr = data.home1_y tr = data.home1_theta print "info for debugg" print "ballx,bally,homex,homey, hometheta",xb,yb,xr,yr,tr vel.goXYOmega(xr,yr,tr)
def getBall(bret, ball, field): robotX = ball[0] - bret[0] #has to be balllocation - robotLocation robotY = ball[1] - bret[1] fieldX = field[0] / 2 fieldY = field[1] / 2 oldPosX = robotX oldPosY = robotY if robotX == 0: robotX = .01 xgoal = fieldX - bret[0] if xgoal == 0: xgoal = .01 toGoal = float( math.acos( float(xgoal) / math.sqrt(float(xgoal)**2 + float(fieldY - bret[1])**2)) + bret[2]) #rospy.loginfo("toBall and toGoal : %f, %f" %(toBall,toGoal)) goalX = fieldX - 0.08 if ball[0] > goalX or ball[0] < -goalX: goStart(bret) else: vel.goXYOmega(-robotX, -robotY, toGoal) kickX = abs(ball[0] - bret[0]) kickY = abs(ball[1] - bret[1]) if ((kickX < 0.09 and kickX > 0) and (kickY < 0.045 and kickY > 0)): print "Is kicking positive :", kickX, kickY kick.kick() kickX = 0.0 kickY = 0.0 print "Reseting kicker :", kickX, kickY else: count = 0
def getBall(bret, ball, field): robotX = ball[0]-bret[0] #has to be balllocation - robotLocation robotY = ball[1]-bret[1] fieldX = field[0]/2 fieldY = field[1]/2 oldPosX = robotX oldPosY = robotY if robotX == 0: robotX = .01 xgoal = fieldX-bret[0] if xgoal == 0: xgoal = .01 toGoal = float(math.acos(float(xgoal)/math.sqrt(float(xgoal)**2+float(fieldY-bret[1])**2))+bret[2]) #rospy.loginfo("toBall and toGoal : %f, %f" %(toBall,toGoal)) goalX = fieldX - 0.08 if ball[0] > goalX or ball[0] < -goalX: goStart(bret) else: vel.goXYOmega(-robotX,-robotY,toGoal) kickX = abs(ball[0]-bret[0]) kickY = abs(ball[1]-bret[1]) if ((kickX < 0.09 and kickX > 0) and (kickY < 0.045 and kickY > 0)): print "Is kicking positive :",kickX,kickY kick.kick() kickX =0.0 kickY=0.0 print "Reseting kicker :",kickX,kickY else: count =0
def goStart(bret): start = 0.45 print "info for debugg" xposition = float(bret[0] - start) #print "ballx,bally,homex,homey, hometheta",-ball[0],ball[1],bret[0],bret[1],bret[2] vel.goXYOmega(xposition, bret[1], bret[2])
def goTopoint(x,y,t): vel.goXYOmega(x,y,t)
def goStart(bret): start = 0.45 print "info for debugg" xposition = float(bret[0]-start) #print "ballx,bally,homex,homey, hometheta",-ball[0],ball[1],bret[0],bret[1],bret[2] vel.goXYOmega(xposition,bret[1],bret[2])
def goTopoint(x, y, t): vel.goXYOmega(x, y, t)
def execute(self): velchangers.goXYOmega(self.vel_x, self.vel_y, self.omega) self.timestamp = 0 #getTime()