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 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])