def setup():
global dtarget,dval,dcycles,breakstate
running = True
dval =0
dtarget=5
dcycles=10000
breakstate = False
global realitypub,targetsub,breaksub
realitypub = rospy.Publisher("/N2/reality",IntArr)
targetsub = rospy.Subscriber("/N2/target",IntArr,target_callback)
breaksub = rospy.Subscriber("/breakServo",Bool,break_callback)
rospy.init_node("dummy_rightteensy",anonymous=False)
rospy.loginfo("> succesfully initialised")
while running:
if(not breakstate):
if dcycles>0:
dcycles-=1
noise = np.random.randint(-2,2)
dval = (8*dval+2*dtarget+noise)/10
msg = IntArr()
msg.ticks = dval
msg.cycles = dcycles
realitypub.publish(msg)
rospy.loginfo("ticks: "+str(msg.ticks)+ " || cycles: "+str(msg.cycles))
else:
rospy.loginfo("cycles empty")
else:
rospy.loginfo("____breaking____")
time.sleep(0.01)
def simplemainloop(self):
while True:
self.scalc()
msg = IntArr()
msg.ticks = self.actualticks * self.sens
msg.cycles = self.targetcycle
self.realityPub.publish(msg)
time.sleep(self.st)
def coordcallback(msg):
global rightpub, leftpub , V , K, targetAngle, buffer
if(buffer%10==0):
alpha = w(targetAngle) - w(msg.theta)
alpha = w(alpha)
Vr = V*(np.cos(alpha)+K*np.sin(alpha)) #rad/s
Vl = V*(np.cos(alpha)-K*np.sin(alpha)) #rad/s
msg = IntArr()
msg.cycles = 1000
msg.ticks = Vr
rightpub.publish(msg)
msg.ticks = Vl
leftpub.publish(msg)
rospy.loginfo("|| "+str(Vr)+" | "+str(Vl)+" |")
buffer+=1
def OnSendClicked(self, e):
try:
ticks = self.ctrl_target_ticks.GetValue()
cycles = self.ctrl_target_cycles.GetValue()
angle = self.ctrl_target_angle.GetValue()
vitesse = self.ctrl_target_vitesse.GetValue()
except ValueError:
print("OnSendClicked => can't get values from inputs")
print(ValueError)
try:
msg = IntArr()
msg.ticks = int(ticks)
msg.cycles = int(cycles)
ctrlMsg = FloatArr()
ctrlMsg.theta = float(angle)
ctrlMsg.v = float(vitesse)
except ValueError:
print("OnSendClicked => can't create ros msgs")
print(ValueError)
try:
rospy.loginfo("control => angle : " + str(angle) +
" | vitesse : " + str(vitesse))
controlpub.publish(ctrlMsg)
except ValueError:
print("OnSendClicked => can't publish msg")
print(ValueError)
if pid_cote == 0:
try:
rospy.loginfo('gauche | ticks : ' + str(ticks) +
" | cycles : " + str(cycles))
#N1targetpub.publish(msg)
except ValueError:
print("OnSendClicked => can't publish msg")
print(ValueError)
else:
try:
rospy.loginfo('droit | ticks : ' + str(ticks) +
" | cycles : " + str(cycles))
#N2targetpub.publish(msg)
except ValueError:
print("OnSendClicked => can't publish msg")
print(ValueError)
e.Skip()
def setup():
running = True
ultrapub = rospy.Publisher("/ultrasound", IntArr)
rospy.init_node("dummy_ultra", anonymous=False)
rospy.loginfo("> succesfully initialised")
maxdist = 2000
distlist = []
for i in range(5):
distlist.append(maxdist / 2)
while running:
for i in range(len(distlist)):
distlist[i] += np.random.randint(maxdist * -1 / 5, maxdist / 5)
distlist[i] = max(0, min(maxdist, distlist[i]))
msg = IntArr()
msg.ticks = int(distlist[i])
msg.cycles = i
ultrapub.publish(msg)
time.sleep(0.1)
def signal_handler(signal, frame):
sys.exit(0)
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
global rightpub, leftpub
rospy.init_node("tickviewer", anonymous=False)
breakpub = rospy.Publisher("/breakServo", Bool, queue_size=1)
rightpub = rospy.Publisher("/N1/target", IntArr, queue_size=2)
leftpub = rospy.Publisher("/N2/target", IntArr, queue_size=2)
print("waiting for full init")
time.sleep(5)
msg = IntArr()
msg.cycles = 30000
msg.ticks = 0
rightpub.publish(msg)
msg.ticks = 0
leftpub.publish(msg)
bmsg = Bool()
bmsg.data = True
breakpub.publish(bmsg)
print("ON TEST BENCH")
print("test for motor connectivity")
print("when ready press enter:")
raw_input("...")
msg.ticks = 30
rightpub.publish(msg)
time.sleep(0.2)
def rightcallback(msg):
global rcount, rightpub, rtarget
rcount += msg.ticks
tmsg = IntArr()
tmsg.ticks = rtarget
tmsg.cycles = 32000
def leftcallback(msg):
global lcount, leftpub, ltarget
lcount += msg.ticks
tmsg = IntArr()
tmsg.ticks = ltarget
tmsg.cycles = 32000