1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
| #Import Libraries:
import vrep #V-rep library
import sys
import time #used to keep track of time
import numpy as np #array library
import math
import matplotlib as mpl #used for image plotting
#Pre-Allocation
PI=math.pi #pi=3.14..., constan
sensor_h=[] #empty list for handles
sensor_val=np.array([]) #empty array for sensor measurements
#orientation of all the sensors:
sensor_loc=np.array([-PI/2, -50/180.0*PI,-30/180.0*PI,-10/180.0*PI,10/180.0*PI,30/180.0*PI,50/180.0*PI,PI/2,PI/2,130/180.0*PI,150/180.0*PI,170/180.0*PI,-170/180.0*PI,-150/180.0*PI,-130/180.0*PI,-PI/2])
#for loop to retrieve sensor arrays and initiate sensors
for x in range(1,16+1):
x=str(x);
sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #
t = time.time()
while (time.time()-t)<60:
#Loop Execution
sensor_val=np.array([])
for x in range(1,16+1):
sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values
#controller specific
sensor_sq=sensor_val[0:8]*sensor_val[0:8] #square the values of front-facing sensors 1-8
min_ind=np.where(sensor_sq==np.min(sensor_sq))
min_ind=min_ind[0][0]
if sensor_sq[min_ind]<0.2:
steer=-1/sensor_loc[min_ind]
else:
steer=0
v=1 #forward velocity
kp=0.5 #steering gain
vl=v+kp*steer
vr=v-kp*steer
print "V_l =",vl
print "V_r =",vr
time.sleep(0.2) #loop executes once every 0.2 seconds (= 5 Hz) |
Partager