def _twist_callback(self, msg):
self._cmd_lin_vel = msg.linear.x
self._cmd_ang_vel = msg.angular.z
print "x: " + str(self._cmd_lin_vel)
print "yaw: " + str(self._cmd_ang_vel)
# Disable both motors
if self._cmd_lin_vel == 0 and self._cmd_ang_vel == 0:
move.motorStop()
else:
# Forward driving
if self._cmd_lin_vel > 0:
self._left_motor_dir = 1
self._right_motor_dir = 0
# Reverse driving
elif self._cmd_lin_vel < 0:
self._left_motor_dir = 0
self._right_motor_dir = 1
# CCW Rotation
elif self._cmd_ang_vel < 0:
self._left_motor_dir = 0
self._right_motor_dir = 0
# CW Rotation
elif self._cmd_ang_vel > 0:
self._left_motor_dir = 1
self._right_motor_dir = 1
move.motor_left(1, self._left_motor_dir, 100)
move.motor_right(1, self._right_motor_dir, 100)
def keepDisProcessing(self):
print("keepDistanceProcessing")
distanceGet = ultra.checkdist()
if distanceGet > (self.rangeKeep / 2 + 0.1):
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
elif distanceGet < (self.rangeKeep / 2 - 0.1):
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
else:
move.motorStop()
def automaticProcessing(self):
print("automaticProcessing")
scGear.moveAngle(2, 0)
if self.scanPos == 1:
pwm.set_pwm(self.scanServo, 0, pwm1_init + self.scanRange)
time.sleep(0.3)
self.scanList[0] = ultra.checkdist()
elif self.scanPos == 2:
pwm.set_pwm(self.scanServo, 0, pwm1_init)
time.sleep(0.3)
self.scanList[1] = ultra.checkdist()
elif self.scanPos == 3:
pwm.set_pwm(self.scanServo, 0, pwm1_init - self.scanRange)
time.sleep(0.3)
self.scanList[2] = ultra.checkdist()
self.scanPos = self.scanPos + self.scanDir
if self.scanPos > self.scanNum or self.scanPos < 1:
if self.scanDir == 1:
self.scanDir = -1
elif self.scanDir == -1:
self.scanDir = 1
self.scanPos = self.scanPos + self.scanDir * 2
print(self.scanList)
if min(self.scanList) < self.rangeKeep:
if self.scanList.index(min(self.scanList)) == 0:
scGear.moveAngle(2, -30)
elif self.scanList.index(min(self.scanList)) == 1:
if self.scanList[0] < self.scanList[2]:
scGear.moveAngle(2, -45)
else:
scGear.moveAngle(2, 45)
elif self.scanList.index(min(self.scanList)) == 2:
scGear.moveAngle(2, 30)
if (
max(self.scanList) < self.rangeKeep
or min(self.scanList) < self.rangeKeep / 3
):
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
else:
# move along
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
pass
def automaticProcessing(self):
# print('automaticProcessing')
scGear.moveAngle(1, 0)
dist = self.distRedress()
print(dist, "cm")
time.sleep(0.2)
if dist >= 40: # More than 40CM, go straight.
scGear.moveAngle(2, 0)
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
print("Forward")
# More than 20cm and less than 40cm, detect the distance between the left and right sides.
elif dist > 20 and dist < 50:
move.motor_left(1, 0, 0)
move.motor_right(1, 0, 0)
scGear.moveAngle(1, 30)
time.sleep(0.3)
distLeft = self.distRedress()
self.scanList[0] = distLeft
# Go in the direction where the detection distance is greater.
scGear.moveAngle(1, -30)
time.sleep(0.3)
distRight = self.distRedress()
self.scanList[1] = distRight
print(self.scanList)
scGear.moveAngle(1, 0)
if self.scanList[0] >= self.scanList[1]:
scGear.moveAngle(2, 30)
time.sleep(0.3)
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
print("Left")
time.sleep(1)
else:
scGear.moveAngle(2, -30)
time.sleep(0.3)
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
print("Right")
time.sleep(1)
else: # The distance is less than 20cm, back.
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
print("Back")
time.sleep(1)
def main():
print("Setup")
move.setup()
print("Running main")
move.move(
40, 'forward', 'no', 0
) # speed, {'forward','backward','no'}, {'left','right','no'}, 0 < radius <= 1
time.sleep(2)
move.motorStop()
time.sleep(2)
move.motor_left(1, move.Dir_forward,
40) # Non-zero to go, 0=forward 1=backwards, speed
time.sleep(2)
# move.motorStop()
move.motor_left(0, move.Dir_forward, 0)
time.sleep(2)
move.motor_right(1, 0, 40) # Non-zero to go, 0=forward 1=backwards, speed
time.sleep(2)
move.motorStop()
time.sleep(2)
print("Done")
def trackLineProcessing(self):
status_right = GPIO.input(line_pin_right)
status_middle = GPIO.input(line_pin_middle)
status_left = GPIO.input(line_pin_left)
# print('R%d M%d L%d'%(status_right,status_middle,status_left))
if status_middle == 0:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
elif status_left == 0:
scGear.moveAngle(2, 45)
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
elif status_right == 0:
scGear.moveAngle(2, -45)
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
print(status_left, status_middle, status_right)
time.sleep(0.1)
def findLineCtrl(self, posInput, setCenter): #2
if posInput:
if posInput > (setCenter + findLineError):
# move.motorStop()
#turnRight
error = (posInput - 320) / 3
outv = int(round((pid.GenOut(error)), 0))
CVThread.scGear.moveAngle(2, -outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
elif posInput < (setCenter - findLineError):
# move.motorStop()
#turnLeft
error = (320 - posInput) / 3
outv = int(round((pid.GenOut(error)), 0))
CVThread.scGear.moveAngle(2, outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
else:
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
#forward
pass
else:
pass
def robotCtrl(command_input, response): global direction_command, turn_command if 'forward' == command_input: direction_command = 'forward' move.motor_left(1, 0, speed_set) move.motor_right(1, 0, speed_set) RL.both_on() elif 'backward' == command_input: direction_command = 'backward' move.motor_left(1, 1, speed_set) move.motor_right(1, 1, speed_set) RL.red() elif 'DS' in command_input: direction_command = 'no' move.motorStop() if turn_command == 'left': RL.both_off() RL.turnLeft() elif turn_command == 'right': RL.both_off() RL.turnRight() elif turn_command == 'no': RL.both_off() elif 'left' == command_input: turn_command = 'left' scGear.moveAngle(2, 45) RL.both_off() RL.turnLeft() elif 'right' == command_input: turn_command = 'right' scGear.moveAngle(2,-45) RL.both_off() RL.turnRight() elif 'TS' in command_input: turn_command = 'no' scGear.moveAngle(2, 0) if direction_command == 'forward': RL.both_on() elif direction_command == 'backward': RL.both_off() RL.red() elif direction_command == 'no': RL.both_off() elif 'lookleft' == command_input: P_sc.singleServo(1, 1, 7) elif 'lookright' == command_input: P_sc.singleServo(1,-1, 7) elif 'LRstop' in command_input: P_sc.stopWiggle() elif 'up' == command_input: T_sc.singleServo(0, 1, 7) elif 'down' == command_input: T_sc.singleServo(0,-1, 7) elif 'UDstop' in command_input: T_sc.stopWiggle() elif 'home' == command_input: P_sc.moveServoInit([init_pwm1]) T_sc.moveServoInit([init_pwm0]) G_sc.moveServoInit([init_pwm2])
def run():
global v_command
# obtain audio from the microphone
r = sr.Recognizer()
with sr.Microphone(device_index =2,sample_rate=48000) as source:
r.record(source,duration=2)
#r.adjust_for_ambient_noise(source)
RL.both_off()
RL.yellow()
print("Command?")
audio = r.listen(source)
RL.both_off()
RL.blue()
try:
v_command = r.recognize_sphinx(audio,
keyword_entries=[('forward',1.0),('backward',1.0),
('left',1.0),('right',1.0),('stop',1.0)]) #You can add your own command here
print(v_command)
RL.both_off()
RL.cyan()
except sr.UnknownValueError:
print("say again")
RL.both_off()
RL.red()
except sr.RequestError as e:
RL.both_off()
RL.red()
pass
#print('pre')
if 'forward' in v_command:
scGear.moveAngle(2, 0)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
elif 'backward' in v_command:
scGear.moveAngle(2, 0)
move.motor_left(1, 1, speed_set)
move.motor_right(1, 1, speed_set)
time.sleep(2)
move.motorStop()
elif 'left' in v_command:
scGear.moveAngle(2, 45)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
scGear.moveAngle(2, 0)
elif "right" in v_command:
scGear.moveAngle(2,-45)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
scGear.moveAngle(2, 0)
elif 'stop' in v_command:
move.motorStop()
else:
pass
def appCommand(data_input):
global direction_command, turn_command, servo_command
if data_input == 'forwardStart\n':
direction_command = 'forward'
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
# RL.both_on()
elif data_input == 'backwardStart\n':
direction_command = 'backward'
move.motor_left(1, 1, speed_set)
move.motor_right(1, 1, speed_set)
# RL.red()
elif data_input == 'leftStart\n':
turn_command = 'left'
scGear.moveAngle(2,30)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
elif data_input == 'rightStart\n':
turn_command = 'right'
scGear.moveAngle(2,-30)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
# RL.both_off()
# RL.turnRight()
elif 'forwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'backwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'leftStop' in data_input:
turn_command = 'no'
# servo.turnMiddle()
# move.motorStop()
scGear.moveAngle(2, 0)
move.motorStop()
elif 'rightStop' in data_input:
turn_command = 'no'
# servo.turnMiddle()
# move.motorStop()
scGear.moveAngle(2, 0)
move.motorStop()
if data_input == 'lookLeftStart\n':
P_sc.singleServo(1, 1, 7)
elif data_input == 'lookRightStart\n':
P_sc.singleServo(1,-1, 7)
elif data_input == 'downStart\n':
T_sc.singleServo(0,-1, 7)
elif data_input == 'upStart\n':
T_sc.singleServo(0, 1, 7)
elif 'lookLeftStop' in data_input:
P_sc.stopWiggle()
elif 'lookRightStop' in data_input:
P_sc.stopWiggle()
elif 'downStop' in data_input:
T_sc.stopWiggle()
elif 'upStop' in data_input:
T_sc.stopWiggle()
if data_input == 'aStart\n':
if LED.ledfunc != 'police':
LED.ledfunc = 'police'
ledthread.resume()
elif LED.ledfunc == 'police':
LED.ledfunc = ''
ledthread.pause()
elif data_input == 'bStart\n':
if LED.ledfunc != 'rainbow':
LED.ledfunc = 'rainbow'
ledthread.resume()
elif LED.ledfunc == 'rainbow':
LED.ledfunc = ''
ledthread.pause()
elif data_input == 'cStart\n':
switch.switch(1,1)
switch.switch(2,1)
switch.switch(3,1)
elif data_input == 'dStart\n':
switch.switch(1,0)
switch.switch(2,0)
switch.switch(3,0)
elif 'aStop' in data_input:
pass
elif 'bStop' in data_input:
pass
elif 'cStop' in data_input:
pass
elif 'dStop' in data_input:
pass
print(data_input)
def turn_left():
# move.move(speed_set, DIRECTION_FORWARD, TURN_LEFT, rad)
move.motor_left(1, 0, rot_speed)
move.motor_right(1, 0, rot_speed)
def trackLineProcessing(self):
status_right = GPIO.input(line_pin_right)
status_middle = GPIO.input(line_pin_middle)
status_left = GPIO.input(line_pin_left)
global mark
if status_left == 0 and status_middle == 1 and status_right == 0: # (0 1 0)
move.motor_left(
1, left_forward, 80
) # move.motor_left(status, left_forward, speed) status:1 means action, 0 means end. left_forward:Left motor forward. speed: motor speed.
move.motor_right(1, right_forward,
80) # right_forward: Right motor forward
mark = 1
elif status_left == 1 and status_middle == 1 and status_right == 0: # (1 1 0 )
if mark != 2:
move.motor_left(1, left_backward,
80) # left_backward: Left motor backward
move.motor_right(1, right_backward,
80) # right_backward: Right motor backward
time.sleep(0.03)
move.motor_left(1, left_forward, 70)
move.motor_right(1, right_forward, 100)
mark = 2
elif status_left == 1 and status_middle == 0 and status_right == 0: #(1 0 0)
if mark != 3:
move.motor_left(1, left_backward, 80)
move.motor_right(1, right_backward, 80)
time.sleep(0.03)
move.motor_left(1, left_forward, 0)
move.motor_right(1, right_forward, 100)
time.sleep(0.02)
mark = 3
elif status_left == 0 and status_middle == 1 and status_right == 1: # (0 1 1)
if mark != 4:
move.motor_left(1, left_backward, 80)
move.motor_right(1, right_backward, 80)
time.sleep(0.03)
move.motor_left(1, left_forward, 100)
move.motor_right(1, right_forward, 70)
mark = 4
elif status_left == 0 and status_middle == 0 and status_right == 1: # (0 0 1)
if mark != 5:
move.motor_left(1, left_backward, 80)
move.motor_right(1, right_backward, 80)
time.sleep(0.03)
move.motor_left(1, left_forward, 100)
move.motor_right(1, right_forward, 0)
time.sleep(0.02)
mark = 5
else:
if mark == 0:
move.motor_left(1, left_forward, 80)
move.motor_right(1, right_forward, 80)
elif mark == 1:
move.motor_left(1, left_forward, 80)
move.motor_right(1, right_forward, 80)
elif mark == 2 or mark == 3: # (1 0 0)
move.motor_left(1, left_forward, 0)
move.motor_right(1, right_forward, 100)
time.sleep(0.03)
elif mark == 4 or mark == 5:
move.motor_left(1, left_forward, 100)
move.motor_right(1, right_forward, 0)
time.sleep(0.03)
time.sleep(0.1)
def trackLineProcessing(self):
status_right = GPIO.input(line_pin_right)
status_middle = GPIO.input(line_pin_middle)
status_left = GPIO.input(line_pin_left)
global mark
# print('R%d M%d L%d'%(status_right,status_middle,status_left))
if status_left == 0 and status_middle == 1 and status_right == 0: # (0 1 0)
move.motor_left(
1, left_forward, 60
) # move.motor_left(status, left_forward, speed) status:1 means action, 0 means end. left_forward:Left motor forward. speed: motor speed.
move.motor_right(1, right_forward,
60) # right_backward: Right motor backward
scGear.moveAngle(
0, 0
) #scGear.moveAngle(0,Angle) Angle:Angle of front wheel rotation(-60 ~ 60)
mark = 1
elif status_left == 1 and status_middle == 1 and status_right == 0: # (1 1 0 )
if mark != 2:
move.motor_left(1, left_backward,
60) # left_backward: Left motor backward
move.motor_right(1, right_backward,
60) # right_backward: Right motor backward
time.sleep(0.03)
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, 30)
mark = 2
elif status_left == 1 and status_middle == 0 and status_right == 0: #(1 0 0)
if mark != 3:
move.motor_left(1, left_backward, 60)
move.motor_right(1, right_backward, 60)
time.sleep(0.03)
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, 60)
mark = 3
elif status_left == 0 and status_middle == 1 and status_right == 1: # (0 1 1)
if mark != 4:
move.motor_left(1, left_backward, 60)
move.motor_right(1, right_backward, 60)
time.sleep(0.03)
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, -30)
mark = 4
elif status_left == 0 and status_middle == 0 and status_right == 1: # (0 0 1)
if mark != 5:
move.motor_left(1, left_backward, 60)
move.motor_right(1, right_backward, 60)
time.sleep(0.03)
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, -60)
mark = 5
else:
if mark == 0:
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
elif mark == 1:
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
elif mark == 2 or mark == 3: # (1 0 0)
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, 60)
elif mark == 4 or mark == 5:
move.motor_left(1, left_forward, 60)
move.motor_right(1, right_forward, 60)
scGear.moveAngle(0, -60)
time.sleep(0.1)
