class Roomba: def __init__(self, MotorE, MotorA, MotorB, servo_pin, trigger, echo,green,yellow,red,distance_timer): GPIO.setwarnings(False) self.distance_timer = distance_timer self.dcmotor = DCMotor(MotorE, MotorA, MotorB) self.servomotor = ServoMotor(servo_pin) self.distancesensor = DistanceSensor(trigger,echo) self.green = green self.yellow = yellow self.red = red GPIO.setup(self.green,GPIO.OUT) GPIO.setup(self.yellow,GPIO.OUT) GPIO.setup(self.red,GPIO.OUT) def red_ON(self): GPIO.output(self.red,GPIO.HIGH) def red_OFF(self): GPIO.output(self.red,GPIO.LOW) def yellow_ON(self): GPIO.output(self.yellow,GPIO.HIGH) def yellow_OFF(self): GPIO.output(self.yellow,GPIO.LOW) def green_ON(self): GPIO.output(self.green,GPIO.HIGH) def green_OFF(self): GPIO.output(self.green,GPIO.LOW) def cleanup(self): GPIO.setup(self.green,GPIO.LOW) GPIO.setup(self.yellow,GPIO.LOW) GPIO.setup(self.red,GPIO.LOW) self.servomotor.cleanup() self.dcmotor.cleanup() self.distancesensor.cleanup() GPIO.cleanup() def reverse_turn(self,direction = 1): self.dcmotor.stop() self.servomotor.turn(direction*50) time.sleep(0.5) while True: print("reverse") self.dcmotor.change_speed(100) self.dcmotor.backward() time.sleep(0.8) self.dcmotor.stop() dist = self.distancesensor.get_distance() if dist > 60: break self.servomotor.turn(0) time.sleep(0.5) self.dcmotor.change_speed(90) self.dcmotor.forward() def random_turn(self): LAG = 0.4 if (random.random() < 0.5): direction = +1 else: direction = -1 print("turning") self.dcmotor.change_speed(90) while True: dist = self.distancesensor.get_distance() if dist > 100: time.sleep(LAG) print("turn complete") self.servomotor.turn(0) self.dcmotor.change_speed(80) print(self.servomotor.angle) break elif 80 < dist <= 100: self.servomotor.turn(direction*10) elif 60 < dist <= 80: self.servomotor.turn(direction*30) elif 40 < dist <= 60: self.servomotor.turn(direction*50) elif dist <= 20: print("stopped while turning") self.dcmotor.stop() self.reverse_turn(direction) break time.sleep(self.distance_timer)
def printCmdTips(): print('\nPlease enter the following cmd to continue ↓') print('f: Run forward, b: Run backward, h: High speed, s: Stop\n') def go(): printCmdTips() while True: x = input() try: if x == 'f': dcMotorA.runForward(G_MEDIUM_SPEED) elif x == 'b': dcMotorA.runBackward(G_LOW_SPEED) elif x == 'h': dcMotorA.runForward(G_HIGH_SPEED) elif x == 's': dcMotorA.stop() else: printCmdTips() except Exception as e: print('Error:', e) if __name__ == "__main__": try: go() finally: dcMotorA.cleanup()