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)