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)
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)
allData = [False, False, False, False] wR = 0 wL = 0 wdotR = 0 wdotL = 0 encRPrev = 0 encLPrev = 0 wRprev = 0 wLprev = 0 wdotRprev = 0 wdotLprev = 0 codeStartTime = 0 BotNumber = 0 PWM = PwmInput() q = np.array([[0.0], [0.0], [0.0]]) q_prev = np.array([[0.0], [0.0], [0.0]]) previousCallbackTime = 0 F_bar = np.matrix([[0.002], [0.002]]) tau = np.array([[0.0], [0.0]]) m = 1.72 #mass in kg d = 0.065 #distance from CG to axis R = 0.085 #Semi-Distance between wheels r = 0.025 #Radius of wheels K_wdot = 0.088508 #(b+RJ/Kt). This is the coefficient of w_dot K_w = 0.0002705 K_tau_R = 18000 K_tau_L = 18000
#Torque to PWM
#We will neglect the w_ddot term as well as the tau_dot terms. The former is difficult to obtain with our current encoder.
#The latter is eliminated as its computations are quite tedious.
K_wdot=0.088 #(b+RJ/Kt). This is the coefficient of w_dot
K_w=0.003
K_tau_R=18000
K_tau_L=18000
#Controller requisite gains.
alpha=1
Gamma=1
gamma=1
k1=1
k2=1
pwmInput=PwmInput()
codeStartTime=0
#The experimental data is added to the data.csv file
head=['time','interval','wR','wL','wdotR','wdotL','pwmR','pwmL','x','y','theta','vel_v','vel_w','e_x','e_y','e_theta','u_R','u_L','tau_R','tau_L']
with open('data.csv','w') as myfile:
writer=csv.writer(myfile)
writer.writerow(head)
#Since tf package can't be directly installed on RasPi the function below has be written.
#Note that this function cannot handle singularities. It gives ZYX rotation Euler angles in radian.
def quat2eul(qu):
global phi
global theta
global psi
#!/usr/bin/env python
import rospy
#import csv
import time
import struct
from fb5_torque_ctrl.msg import encoderData
from fb5_torque_ctrl.msg import PwmInput
import math
from geometry_msgs.msg import TransformStamped
#Defining as global variable
pwmInput=PwmInput()
codeStartTime=0
encRPrev=0
encLPrev=0
wRprev=0
wLprev=0
wdotRprev=0
wdotLprev=0
N_bots=4
bot_loc=np.empty((2,N_bots));
BotNumber=0 #<----This has to be modified in each bot. [0,1,2,3]
#The experimental data is added to the data.csv file
#head=['time','interval','encR','encL','wR','wL','wdotR','wdotL','wddotR','wddotL','pwmR','pwmL']
#with open('data.csv','w') as myfile:
# writer=csv.writer(myfile)
# writer.writerow(head)
#The callbackEnc function takes the encoder readings at each point and
#updates global variabis number is les storing the angular velocity and the angular acceleration of each wheel.