def headCorrection(currhead,reqhead):
pub_PWM=rospy.Publisher('pwmCmd_square',PwmInput,queue_size=10)
pwmInput = PwmInput()
while(((currhead-reqhead) > (5*3.14/180)) or ((currhead-reqhead) < (-5*3.14/180))):
if(currhead>reqhead):
pwmInput.rightInput = -120
pwmInput.leftInput = 120
if(currhead<reqhead):
pwmInput.rightInput = 120
pwmInput.leftInput = -120
pub_PWM.publish(pwmInput)
def drawSquare():
while not rospy.is_shutdown():
global pos, accW,i,publish
if (publish == False):
initial_pos = [pos.N, pos.E]
pwmInput = PwmInput()
if ((pos.N-initial_pos[0])*(pos.N-initial_pos[0]) + (pos.E-initial_pos[1])*(pos.E-initial_pos[1]) < 10):
pwmInput.rightInput = 150
pwmInput.leftInput = 150
pub_PWM.publish(pwmInput)
publish = True
else:
pwmInput.rightInput = 0
pwmInput.leftInput = 0
pub_PWM.publish(pwmInput)
def drawSquare():
rospy.init_node('draw_square', anonymous=True)
pub_PWM = rospy.Publisher('pwmCmd_square', PwmInput, queue_size=10)
rospy.Subscriber("GPSmessage", GPSmsg, CallbackGPS, 0)
rospy.Subscriber("IMU message", IMUmsg, CallbackGPS, 0)
pwmInput = PwmInput()
global pos, accW, i
if ((xt[i] - pos.X) * (xt[i] - pos.X) + (yt[i] - pos.Y) *
(yt[i] - pos.Y) < 1):
i = i + 1
break
if (i == 1):
reqhead = math.atan((yt[i] - pos.Y) / (xt[i] - pos.X))
else:
reqhead = math.atan2((yt[i] - pos.Y) / (xt[i] - pos.X))
headCorrection(currhead, reqhead)
pwmInput.rightInput = 150
pwmInput.leftInput = 150
pub_PWM.publish(pwmInput)
pub_PWM=rospy.Publisher('pwmCmd',PwmInput,queue_size=10)
#The torque controller outputs commands at only 10Hz.
#VICON data subscriber. Change the name to the required name here. The bot this is running on is to be placed last.
rospy.Subscriber("/vicon/vijeth_1/vijeth_1", TransformStamped, callbackVICON, 1)
rospy.Subscriber("/vicon/vijeth_2/vijeth_2", TransformStamped, callbackVICON, 2)
rospy.Subscriber("/vicon/vijeth_3/vijeth_3", TransformStamped, callbackVICON, 3)
rospy.Subscriber("/vicon/vijeth_0/vijeth_0", TransformStamped, callbackVICON, 0)
#The encoder data is still at 25 Hz as determined by the publisher in the other file
codeStartTime=rospy.get_time()
rate = rospy.Rate(10)
timeSinceInput=rospy.get_time()
timeBetInputs=5
while not rospy.is_shutdown():
i=0
while i<2:
pwmInput.rightInput=200*i
pwmInput.leftInput=200*i
#rospy.loginfo(pwmInput)
pub_PWM.publish(pwmInput)
if((rospy.get_time()-timeSinceInput)>=timeBetInputs):
timeSinceInput=rospy.get_time()
i=i+1
rate.sleep()
if __name__ == '__main__':
try:
torqueController()
except rospy.ROSInterruptException:
pass