def LaneHandling(virtual_lane_available, unavailable_thres, n):
center = int(len(virtual_lane_available)/2)
speed = 5
#if center lane is getting on threshold
if virtual_lane_available[center] > unavailable_thres:
if virtual_lane_available[center-n] > unavailable_thres and virtual_lane_available[center+n] > unavailable_thres:
n+=1
if n > center:
print("GO BACK")
eg.mvStraight(-speed, -1)
else:
LaneHandling(virtual_lane_available, unavailable_thres, n)
elif virtual_lane_available[center-n] > unavailable_thres:
print("TURN RIGHT")
eg.mvRotate(speed*2, -1, True)
elif virtual_lane_available[center+n] > unavailable_thres:
print("TURN LEFT")
eg.mvRotate(speed*2, -1, False)
elif virtual_lane_available[center-n] > virtual_lane_available[center+n]:
print("TURN RIGHT")
eg.mvRotate(speed*2, -1, True)
else:
print("TURN LEFT")
eg.mvRotate(speed*2, -1, False)
else:
print("GO STRAIGHT")
eg.mvStraight(speed, -1)
def GoEasy(direc):
if direc == 4: # Backward
easyGo.mvStraight(-SPEED, -1)
elif direc == 1: # Go straight
easyGo.mvStraight(SPEED, -1)
elif direc == 2: # turn left
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3: # turn right
easyGo.mvRotate(ROTATE_SPEED, -1, True)
def main():
speed = 5
count = 0
f = keyCap.KeyPoller()
with f as poller:
start_time, end_time = 0, 0
while True:
getch = poller.poll()
if getch != None:
if getch == 'w':
easyGo.mvStraight(abs(speed), -1)
print("[Move Forward] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == "a":
easyGo.mvRotate(abs(speed) * 4, -1, False)
print("[Trun CounterColckwise] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == "s":
easyGo.mvStraight(-abs(speed), -1)
print("[Move Backward] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == 'd':
easyGo.mvRotate(abs(speed) * 4, -1, True)
print("[Trun Colckwise] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == 'e':
speed += 0.1
print('set speed :', speed)
keyCap._cls()
elif getch == "c":
speed -= 0.2
if speed < 0:
speed = 0
print('set speed :', speed)
keyCap._cls()
elif getch == "x":
speed = 5
print('set speed :', speed)
elif getch == "q":
print('Program terminated')
easyGo.stop()
return
else:
easyGo.stop()
count += 1
start_time = time.time()
else:
end_time = time.time()
if end_time - start_time < 0.1:
continue
easyGo.stop()
count = 0
def GoEasy(direc):
if direc == 4:
easyGo.mvStraight(-SPEED, -1)
elif direc == 0 or direc == 1:
#print("COME HERE")
easyGo.mvStraight(SPEED, -1)
elif direc == 2:
#print("COME HERE2")
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3:
easyGo.mvRotate(ROTATE_SPEED, -1, True)
def GoEasy(direc, speed_ratio):
if direc == 4: # Backward
easyGo.mvStraight(-SPEED * speed_ratio, -1)
elif direc == 0 or direc == 1: # Go straight
easyGo.mvStraight(SPEED * speed_ratio, -1)
elif direc == 2: # turn left
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3: # turn right
easyGo.mvRotate(ROTATE_SPEED, -1, True)
elif direc == 5: # stop
easyGo.stop()
def callback(data):
global totalTime, count, csv_flag, DRIVE_INDEX
'''
print()
print("Car speed: " + str(car_speed.data * 3.6) + " km/h")
print("Car Yaw: " + str(car_yaw.data))
print("Steering Angle: " + str(steering_angle.data))
print("Simulation status: " + ("Running" if sim_status.data >=
0 else cm.status_dic.get(sim_status.data)))
'''
'''
if longitudinal_speed.data == 0:
print(slip_angle.data, 0)
else:
print(slip_angle.data, math.atan2(lateral_speed.data, longitudinal_speed.data))
floatmsg = Float32()
floatmsg.data = car_speed.data * 3.6
speedpub.publish(floatmsg)
floatmsg.data = car_yaw.data
yawpub.publish(floatmsg)
floatmsg.data = steering_angle.data
steerpub.publish(floatmsg)
'''
# csv_save = data.buttons[0] # A button
## CM vehicle control
control_speed = (data.axes[5] + 1) / 2 # Right trigger
control_speed_back = (data.axes[2] + 1) / 2 # Left trigger
#control_steer = - data.axes[3] # Right stick
control_steer = data.axes[0] # Left stick
control_brake = data.buttons[5] # Right Button RB
if abs(control_steer) < 0.15: # give a threshold due to the poor joystick
control_steer = 0
if control_speed_back:
control_speed = -control_speed_back
control_speed *= MAX_SPEED
# control_brake *= MAX_BRAKE
control_steer *= MAX_STEER
start = data.buttons[4]
control_steer = (int)(round(control_steer))
control_speed = (int)(round(control_speed))
if control_brake:
easyGo.stop()
elif control_speed != 0:
easyGo.mvStraight(control_speed, -1)
elif control_steer > 0:
easyGo.mvRotate(control_steer, -1, True)
elif control_steer < 0:
easyGo.mvRotate(-control_steer, -1, False)
else:
easyGo.stop()
print("speed: ", control_speed, " , brake: ", control_brake, ", steer: ",
control_steer)
#rate.sleep()
'''
