示例#1
0
def main():

    count = 40
    print(
        "Motor with stepper sensor example. Moves {0} steps forwards, then {0} steps backwards and stops."
        .format(count))

    #20210218 DPM: The code of this example is configured for the NUCLEO-L746RG board.
    #              Please, adapt according to the actual configuration.

    PID_KP = 250.0
    PID_KI = 0.0
    PID_KD = 0.0

    mpu = Mpu6050(1)
    mpu.start()

    motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
    motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
    motorDriver = Driver(motorLeft, motorRight)

    mc = MotionController(mpu, motorDriver, PID_KP, PID_KI,
                          PID_KD).setStepper(Stepper(Pin.board.D7,
                                                     Pin.PULL_UP))

    try:
        uasyncio_run(mainTask(mc, count))
    finally:
        mpu.cleanup()
        motorDriver.cleanup()
示例#2
0
def main():

    print("Turn_to test")

    #20210218 DPM: The code of this example is configured for the NUCLEO-L746RG board.
    #              Please, adapt according to the actual configuration.

    PID_KP = 250.0
    PID_KI = 0.0
    PID_KD = 0.0

    mpu = Mpu6050(1)
    mpu.start()

    motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
    motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
    motorDriver = Driver(motorLeft, motorRight)

    mc = MotionController(mpu, motorDriver, PID_KP, PID_KI,
                          PID_KD).setStepper(Stepper(Pin.board.D7,
                                                     Pin.PULL_UP))

    try:
        uasyncio_run(mainTask(mc))
    finally:
        mpu.cleanup()
        motorDriver.cleanup()
示例#3
0
def main():
    
    print("Turn test")
    
    #20210618 DPM: The code of this example is configured for the NUCLEO-L746RG board.
    #              Please, adapt according to the actual configuration.
    
    PID_KP = 250.0
    PID_KI = 0.0
    PID_KD = 0.0
    
    print("initializing MPU")
    mpu=Mpu6050(1)
    mpu.start()
    print("MPU initialized")
    
    motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
    motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
    motorDriver = Driver(motorLeft, motorRight)
    
    motion = MotionController(mpu, motorDriver, PID_KP, PID_KI, PID_KD)    
    
    try:
        uasyncio_run(mainTask(motion))
    finally:
        motion.stop()
        mpu.cleanup()
        motorDriver.cleanup()
示例#4
0
    def __init__(self, leftMotor, leftIcTimerId, leftIcPin, rightMotor,
                 rightIcTimerId, rightIcPin):
        '''
        Constructor
        
        @param leftMotor: The left motor
        @param leftIcTimerId: The id-number of the timer for input-capturing on the left motor
        @param leftIcPin: The pin where the capture of pulses on the left motor will be performed
        @param rightIcTimerId: The id-number of the timer for input-capturing on the right motor
        @param rightIcPin: The pin where the capture of pulses on the right motor will be performed
        '''

        Driver.__init__(self, leftMotor, rightMotor)

        self._lpfAlpha = SmartDriver.DEFAULT_LPF_ALPHA

        self._leftThrottle = 0.0
        self._rightThrottle = 0.0

        self._leftTimer = InputCapture(leftIcTimerId, leftIcPin)
        self._rightTimer = InputCapture(rightIcTimerId, rightIcPin)

        self._pidInputValues = [0.0] * SmartDriver.PID_RANGE

        self._pid = PidCoroutine(SmartDriver.PID_RANGE, self._readPidInput,
                                 self._setPidOutput, "SmartDriver-PID")
        self._pid.setProportionalConstants([0.0] * SmartDriver.PID_RANGE)
        self._pid.setIntegralConstants([0.0] * SmartDriver.PID_RANGE)
        self._pid.setDerivativeConstants([0.0] * SmartDriver.PID_RANGE)
        self._pid.setModulus([0.0, 0.0, 2 * pi])

        self._mpu = None
示例#5
0
    def start(self):

        if self._mpu != None:
            self._mpu.start()

        Driver.start(self)

        self._pid.init(SmartDriver.PID_FREQ)
        self._pid.setTargets([0.0] * SmartDriver.PID_RANGE)
示例#6
0
    def setMode(self, mode):

        Driver.setMode(self, mode)

        if self._mpu != None:
            if mode == Driver.MODE_DRIVE:
                self._pid.unlockIntegral(2)
                self._pid.setTarget(2, self._mpu.readAngles()[2])

            else:
                self._pid.lockIntegral(2)
示例#7
0
    def cleanup(self):
        '''
        Releases and finishes all used resources
        '''

        self._pid.stop()
        self._leftTimer.cleanup()
        self._rightTimer.cleanup()
        if self._mpu != None:
            self._mpu.cleanup()
        Driver.cleanup(self)
示例#8
0
def main():
    '''
    Main application function
    Initializes the resources, launches the activity and performs a heart-beat led running 
    '''
    
    #TODO: 20200917 DPM Move the ultra-sound initialization to the robot-class
    distanceSensor = Ultrasound(Pin.board.D2, Pin.board.D4)
    
    motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
    motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
    motorDriver = Driver(motorLeft, motorRight)
    
    #TODO: 20200918 DPM Move the MPU initialization to the robot-class
    mpu=Mpu6050(1)
    mpu.start()
    
    #TODO: 20200918 DPM Move the motion-controller initialization to the robot-class
    motion = MotionController(mpu, motorDriver, PID_KP, PID_KI, PID_KD).setStepper(Stepper(Pin.board.D7, Pin.PULL_UP))
    
    #TODO: 20200918 DPM Move the wifi-module initialization to the robot-class
    esp = Esp8266(3, Pin.board.D3, 115200, debug=True)
    esp.start()
    
    robot = Robot()
    
    # Add activities here:
    robot.addActivity(RandomMotionActivity(motion, distanceSensor)) #.setObstacleLed(pyb.LED(3))
    robot.addActivity(RemoteControlledActivity(motion, esp, distanceSensor))
    robot.addActivity(MusicalInstrumentActivity(distanceSensor))
    
    try:
    
        robot.run()   
    
    finally:
        
        robot.cleanup()
        #TODO: 20200918 DPM move devices' cleanup into robot's 
        esp.cleanup()
        mpu.cleanup()
        motorDriver.cleanup()
        distanceSensor.cleanup()
示例#9
0
def main():

    print("Initializing")
    ml = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
    mr = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
    d = Driver(ml, mr)

    mpu = Mpu6050(1)
    mpu.start()

    mc = MotionController(mpu, d, PID_KP, PID_KI, PID_KD)
    mc.setThrottle(80.0)
    mc.setRotation(60.0)

    loop = get_event_loop()
    loop.run_until_complete(_comain(mc))
    loop.close()

    mpu.cleanup()
    d.cleanup()
    print("Finished")
示例#10
0
def main():
    ml = Motor(pyb.Pin.board.D10, 4, 1, pyb.Pin.board.D11)
    mr = Motor(pyb.Pin.board.D9, 8, 2, pyb.Pin.board.D8)
    d = Driver(ml, mr)
    d.setMode(Driver.MODE_DRIVE)
    try:
        d.setMotionVector(80, 0)
        utime.sleep(3)
        d.stop()
        utime.sleep(1)
        d.setMotionVector(-80, 0)
        utime.sleep(3)
        d.stop()
    finally:
        d.cleanup()