コード例 #1
1
import time

from api import Api
from motors import Servo, Motor
from remote import Remote

servo = Servo(port=1, min_pulse=750, max_pulse=2250, min_position=0.30, max_position=0.7)
motor = Motor(port=1, duty_cycle=0)

def wave(min=0.0, max=1.0, n=1, pause=0.4):
    for x in range(n):
        servo.set(min)
        time.sleep(pause)
        servo.set(max)
        time.sleep(pause)
        servo.set(0.5)

api = Api()
api.demonize()
remote = Remote()

while True:
    # api events
    for event, kwargs in api.events:
        if event == "wave":
            wave(**kwargs)
        if event == "set":
            servo.set(0.75-float(kwargs["position"])*0.5)
        if event == "set_speed":
            motor.duty_cycle = kwargs['position']
    api.events = []
コード例 #2
0
ファイル: hal.py プロジェクト: zappyfish/MakeHarvard2019
    def __init__(self):
        z_pin = 18
        angle_one_pin = 13
        angle_two_pin = 12

        self.z_motor = Motor(z_pin)
        self.shoulder_motor = Motor(angle_one_pin, 20 * pi / 180)
        self.elbow_motor = Motor(angle_two_pin, 70 * pi / 180)

        self.shoulder_motor.change_angle(-1)
        self.elbow_motor.change_angle(-1)
コード例 #3
0
ファイル: beerbot.py プロジェクト: kirberich/beerbot
from bot_serial import SerialWrapper
from motors import Motor
from recorder import CVCaptureRecorder

if __name__ == '__main__':
    parser = argparse.ArgumentParser(
        description='Control the potato beer delivery system via an arduino.')
    parser.add_argument('dev',
                        type=str,
                        help='serial device the arduino is connected to')
    args = parser.parse_args()

    recorder = CVCaptureRecorder()

    serial = SerialWrapper(dev=args.dev, speed=57600)
    speed_motor = Motor(serial, arduino_command="S")
    direction_motor = Motor(serial, arduino_command="D")
    api = Api(recorder=recorder)
    api.demonize(port=8000)

    while True:
        # api events
        recorder.update_frame_rate()
        recorder.handle_frame()

        with api.lock:
            for event, kwargs in api.events:
                if event == "set":
                    direction_motor.set(kwargs["position"])
                if event == "set_speed":
                    speed_motor.set(kwargs["position"])
コード例 #4
0
ファイル: test.py プロジェクト: ilyaLibin/FlightController
from motors import Motor
import getch


# FRONT_RIGHT min 305
# FRONT_LEFT min 308
# BACK_LEFT min  332
# BACK_RIGHT min 322
m1 = Motor(Motor.FRONT_LEFT)
m2 = Motor(Motor.FRONT_RIGHT)
m3 = Motor(Motor.BACK_LEFT)
m4 = Motor(Motor.BACK_RIGHT)
speed = 270
while True:
  c = getch.getch()

  if c == 'w':
    speed = speed + 1
  if c == 's':
    speed = speed - 1

  m1.setSpeed(speed)
  m2.setSpeed(speed)
  m3.setSpeed(speed)
  m4.setSpeed(speed)
コード例 #5
0
ファイル: stabilize.py プロジェクト: N-Turner/beaglepilot
def limitThrust(thrust):
    if thrust > 100:
        thrush = 100
    elif thrust < -100:
        thrust = -100
    return thrust


# instantiate IMU
# TODO see how to import C interface to python
imu = IMU()
# MyKalman=KalmanFilter(....)

# instantiate motors and put them together in a list
motor1 = Motor(1)
motor2 = Motor(2)
motor3 = Motor(3)
motor4 - Motor(4)
motors = [motor1, motor2, motor3, motor4]

# TODO instantiate PID controllers
rollPID = PID()
pitchPID = PID()
yawPID = PID()
# zPID=PID(.....)
# xposPID=PID(.....)
# yposPID=PID(.....)

############################
# loop
コード例 #6
0
from __future__ import division

from time import sleep, time
from threading import Thread

import Adafruit_PCA9685
import RPi.GPIO as GPIO

pwm = Adafruit_PCA9685.PCA9685()

import configuration
from motors import Motor
from servo import Servo

servo = Servo(GPIO, configuration.servo_pin, 180)
motor_f_r = Motor(configuration.motor_f_r, pwm, GPIO)
motor_f_l = Motor(configuration.motor_f_l, pwm, GPIO)
motor_b_r = Motor(configuration.motor_b_r, pwm, GPIO)
motor_b_l = Motor(configuration.motor_b_l, pwm, GPIO)

current_milli_time = lambda: int(round(time() * 1000))

import matplotlib.pyplot as plt
import random


def sign(num):
    return -1 if num < 0 else 1


class PID(Thread):
コード例 #7
0
GPIO.setup("P8_4", GPIO.OUT)
GPIO.output("P8_4", GPIO.HIGH)

GPIO.setup("P8_11", GPIO.OUT)
GPIO.output("P8_11", GPIO.HIGH)

GPIO.setup("P9_15", GPIO.OUT)
GPIO.output("P9_15", GPIO.HIGH)

GPIO.setup("P9_23", GPIO.OUT)
GPIO.output("P9_23", GPIO.HIGH)
##############################

speed = 0

m1 = Motor(1)
m1.setSpeed(speed)
m1.go()

m2 = Motor(2)
m2.setSpeed(speed * (-1))
m2.go()

m3 = Motor(3)
m3.setSpeed(speed)
m3.go()

m4 = Motor(4)
m4.setSpeed(speed * (-1))
m4.go()
コード例 #8
0
class Tortoise:
    """
        This is the class which implements the high-level behaviour of the tortoises. In order to make a proper use of the tortoises, an instance of this class should be created.
    """

    def __init__(self):
        """
        This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.

        The reasons of termination of a user process could be because of normal termination 
        or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
        and may be still on. In this case, the motors and LEDs should be turned off
        for the battery not to drain. 

        The solution implemented is to have a background process (a watchdog) running 
        continously. This process checks if the user process doesn't exist anymore (termination).
        If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
        In order to identy that the user script has finished, a file with the name [PID].pid is
        created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.

        Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created. 
        However, this hasn't been implemented yet. The idea could be to save the light 
        values in a file and read that file when creating the tortoise object. 
        Light conditions could have changed, so this should be done carefully. 

        At the moment, the tortoise object is created without calibration. If the users
        want to use the light sensors, they need will need to execute the calibrateLight
        function before using those sensors.
        """

        # Variables that control the calibration of the light sensors
        global isLightCalibrated
        global lowerBoundLight
        global upperBoundLight

        isLightCalibrated = False
        lowerBoundLight = 0
        upperBoundLight = 0
        # --- Variables that control the calibration of the light sensors ---


        # No warnings from the GPIO library
        GPIO.setwarnings(False)


        # Variables that control the random motion
        self.lastRandomCommand = None
        self.timesSameRandomCommandExecuted = 0
        self.numberRepeatsRandomCommand = -1
        self.lastRandomStepsWheelA = None
        self.lastRandomStepsWheelB = None
        self.lastRandomDirection = None
        # --- Variables that control the random motion ---


        # Setting the motors, sensors and actuators    

        # Pin numbers of the motors
        motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
        self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
        self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])

        self.sensors = Sensors()
        self.actuators = Actuators()

        # Position 1 of the light sensors area in the PCB assigned to pin 17
        self.sensors.setSensor(enums.SensorType.light, 1, 17) 

        # Position 2 of the light sensors area in the PCB assigned to pin 4
        self.sensors.setSensor(enums.SensorType.light, 2, 4)

        # Position 1 of the touch sensors area in the PCB assigned to pin 3
        self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) 

        # Position 2 of the touch sensors area in the PCB assigned to pin 27
        self.sensors.setSensor(enums.SensorType.touch, 2, 27) 

        # Position 3 of the touch sensors area in the PCB assigned to pin 2
        self.sensors.setSensor(enums.SensorType.touch, 3, 2)

        # Position 4 of the touch sensors area in the PCB assigned to pin 18
        self.sensors.setSensor(enums.SensorType.touch, 4, 18)

        # Position 1 of the proximity sensors area in the PCB assigned to pin 19
        self.sensors.setSensor(enums.SensorType.proximity, 1, 19)

        # Position 2 of the proximity sensors area in the PCB assigned to pin 21
        self.sensors.setSensor(enums.SensorType.proximity, 2, 21) 

        # Position 3 of the proximity sensors area in the PCB assigned to pin 22
        self.sensors.setSensor(enums.SensorType.proximity, 3, 22) 

        # Position 4 of the proximity sensors area in the PCB assigned to pin 26
        self.sensors.setSensor(enums.SensorType.proximity, 4, 26) 

         # Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
        ledPins = [8, 16, 25, 12]
        self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) 
        self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) 
        self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) 
        self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
        # --- Setting the motors, sensors and actuators ---


        # Times pressed the touch sensor for the latching behavour
        self.lastTouch = [-1,-1,-1]

        # Minimum milliseconds to send to the motors as delay
        self.minDelayMotors = 2


        # Creation of a file with the PID of the process

        # PID of process
        pid = os.getpid()

        # ~/.tortoise_pids/
        directory = os.path.expanduser("~") + "/.tortoise_pids/"

        # Filename: [PID].pid
        f = open(directory + str(pid) + ".pid", "w")

        # First line: motor pins
        f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")

        # Second line: LED pins
        f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")

        f.close()
        # --- Creation of a file with the PID of the process ---



        # Waiting for the user to press the e-stop button
        self.state = enums.State.paused

        messages.printMessage('greetings')
        while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
            time.sleep(0.1)

        messages.printMessage('running')

        self.state = enums.State.running
        # --- Waiting for the user to press the e-stop button ---



    def getStateTortoise(self):
        """ 
        Returns the state of the tortoise, either paused or running.

        :rtype: enums.State
        """

        return self.state


    def setStateTortoise(self, toState):

        self.state = toState


    def calibrateLight(self):
        """
        Calibrates the light sensor, defining the upper and lower bounds of the light, i.e. the values returned by the light sensor of the ambience and when a light source is placed in front of it.

        Currently, only the light sensor in the position 1 of the PCB is used for calibration.
        """

        global lowerBoundLight, upperBoundLight, isLightCalibrated

        messages.printMessage('calibration_ambient')
        raw_input()
        lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)

        messages.printMessage('calibration_light_source')
        raw_input()
        upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)

        isLightCalibrated = True

        messages.printMessage('calibration_complete')



    def getSensorData(self, sensor_type, position):
        """
        Returns a different value depending on the type of sensor queried:

        - For the light sensor: an int in the range [0, 9]
        - For the touch sensor: 1 if the sensor has been pressed since the last time it was queried, 0 if not
        - For the e-stop: 1 if it's ON, 0 if it's OFF
        - For the proximity sensor: 1 if there's an obstacle (it's ON), 0 if not (it's OFF)

        :param sensor_type: type of the sensor queried
        :param position: position in the PCB of the sensor queried
        :type sensor_type: enums.SensorType
        :type position: int
        :rtype: int
        """

        if (sensor_type == enums.SensorType.touch):

            if (position < 1 or position > 3):

                messages.printMessage('bad_touch_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.emergencyStop):

            if (position != 4):

                messages.printMessage('bad_emergency_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.light):

            if (position != 1 and position!=2):

                messages.printMessage('bad_light_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.proximity):

            if (position < 1 or position > 4):

                messages.printMessage('bad_proximity_sensor')
                self.blinkLEDs_error()
                return -1

        else:
                messages.printMessage('bad_sensor')
                self.blinkLEDs_error()
                return -1


        # The value of the sensor is read at lowlevel
        value = self.sensors.readSensor(sensor_type, position)


        # For the light sensor, 'value' is the count of the light
        if sensor_type == enums.SensorType.light:
            return value
            if (upperBoundLight - lowerBoundLight) == 0:
                messages.printMessage('no_calibration')
                self.blinkLEDs_error()
                return -1

            # A scale to the range [0, 9] is done
            lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))

            if lightVal < 0:
                messages.printMessage('no_calibration')
                self.blinkLEDs_error()
                return -1

            return lightVal


        # For the touch sensor, 'value' is the number of times the sensor has been pressed
        elif sensor_type == enums.SensorType.touch:

            # Returns if the sensor has been pressed since the last time it was queried
            return self.getSwitchTriggered(position,value)

        # For the e-stop, 'value' is the number of times the sensor has been pressed
        elif sensor_type == enums.SensorType.emergencyStop:

            # Returns if it's either 1 (ON) or 0 (OFF)
            return value % 2

        # For the proximity sensor, 'value' is either 1 (ON) or 0 (OFF)
        elif sensor_type == enums.SensorType.proximity:

            # Returns 1 (ON) or 0 (OFF)
            return value



    def getSwitchTriggered(self, position, value):

        if self.lastTouch[position-1] < 0:

            self.lastTouch[position-1] = value
            return 0

        elif self.lastTouch[position-1] == value:

            return 0

        else:

            self.lastTouch[position-1] = value
            return 1


    def getLEDValue(self, position):
        """
        Returns 1 if the LED is ON, 0 if it's OFF.

        :param position: position in the PCB of the sensor queried
        :type position: int
        :rtype: int
        """

        if (position < 1 or position > 4):
            messages.printMessage('bad_LED')
            self.blinkLEDs_error()
            return -1

        return self.actuators.getActuatorValue(enums.ActuatorType.led, position)



    def setLEDValue(self, position, value):
        """
        Turns on/off an LED.

        :param position: position in the PCB of the sensor queried
        :param value: 1 (ON) or 0 (OFF)
        :type position: int
        :type value: int
        """

        if(position < 1 or position > 4):
            messages.printMessage('bad_LED')
            self.blinkLEDs_error()
            return -1

        if(value != 0 and value != 1):
            messages.printMessage('bad_LED_value')
            self.blinkLEDs_error()
            return -1

        self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
        return 0



    def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):
        """
        Blinks the LEDs wanted the number of times specified.

        :param positions: position or positions in the PCB of the LEDs wanted
        :param numberOfBlinks: number of blinks
        :param delay: milliseconds to wait between blinking
        :param blocking: whether the function should block forever or not. 
        :type positions: int, [int]
        :type numberOfBlinks: int
        :type delay: int
        :type blocking: boolean

        .. warning:: If blocking == True, the thread will block forever because of infinite loop. It's only used when there are errors.
        """

        if numberOfBlinks < 0:
            messages.printMessage('blinks_negative')
            self.blinkLEDs_error()
            return -1

        if numberOfBlinks == 0:
            messages.printMessage('blinks_zero')
            self.blinkLEDs_error()
            return -1

        if delay < 0:
            messages.printMessage('blinking_fast')
            self.blinkLEDs_error()
            return -1


        # If no TypeError exception raised, 'positions' is an array
        try:

            # All positions are checked
            for y in range(0, len(positions)):

                if positions[y] < 0 or positions[y] > 4:
                    messages.printMessage('bad_LED')
                    self.blinkLEDs_error()
                    return -1

        except TypeError: # It's not an array but an integer

            if positions < 0 or positions > 4:
                messages.printMessage('bad_LED')
                self.blinkLEDs_error()
                return -1


        # The current state of the LEDs is saved to restore it later
        previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]

        cont = True

        # If blocking == True, it's an infinite loop to "stop" the execution of the program and keep blinkind the LEDs
        while cont:

            for x in range(0, numberOfBlinks):

                # If no TypeError exception raised, 'positions' is an array
                try:

                    for y in range(0, len(positions)):

                        self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)

                    time.sleep(delay)

                    for y in range(0, len(positions)):
                        self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)

                    time.sleep(delay)

                except TypeError: # It's not an array but an integer

                    self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
                    time.sleep(delay)
                    self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
                    time.sleep(delay)


            # Depending on the parameter, it blocks or not
            cont = blocking



        # If it doesn't block, the previous state of the LEDs is restored
        for x in range(1, 5):
            self.setLEDValue(x, previousStateLEDs[x - 1])

        return 0



    def blinkLEDs_error(self):

        self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True)



    def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):
        """
        Move the motors of the wheels.

        Running the motors is done in different threads so that both wheels can move at the same time. The thread that executes this functions waits until the threads are finished, i.e. the motion is done. However, it doesn't block, but checks every half a second if the e-stop button has been pressed. If so, it stops the motors and exits.

        :param stepsWheelA: number of rotations of the motor attached to wheel A
        :param stepsWheelB: number of rotations of the motor attached to wheel B
        :param delayWheelA: controls the speed of rotation of the motor attached to wheel A (minimum is 2 milliseconds)
        :param delayWheelB: controls the speed of rotation of the motor attached to wheel B (minimum is 2 milliseconds)
        :param direction: the direction in which the tortoise should move
        :type stepsWheelA: int
        :type stepsWheelB: int
        :type delayWheelA: int
        :type delayWheelB: int
        :type direction: enums.Direction
        """

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards  and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise  ) :

            messages.printMessage('bad_direction')
            self.blinkLEDs_error()
            return -1

        if(stepsWheelA < 0 or stepsWheelB < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
            messages.printMessage('bad_delay')
            self.blinkLEDs_error()
            return -1



        # If a stop command has been sent, the turtle will stop its movement
        if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:

            if self.getStateTortoise() == enums.State.running:

                self.setStateTortoise(enums.State.paused)
                messages.printMessage('paused')

        else:

            if self.getStateTortoise() == enums.State.paused:
                    self.setStateTortoise(enums.State.running)
                    messages.printMessage('resumed')


            # The threads are created. They aren't started yet though.
            motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
            motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
            motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
            motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))


            # The specific wheels are started in order to accomplish the desired movement in the direction commanded

            if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:

                if stepsWheelA > 0:
                    motorAprocess_backwards.start()

                if stepsWheelB > 0:
                    motorBprocess_backwards.start()

            elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:

                if stepsWheelA > 0:
                    motorAprocess_forwards.start()

                if stepsWheelB > 0:
                    motorBprocess_forwards.start()

            elif direction == enums.Direction.clockwise:

                if stepsWheelA > 0:
                    motorAprocess_backwards.start()

                if stepsWheelB > 0:
                    motorBprocess_forwards.start()

            elif direction == enums.Direction.counterClockwise:

                if stepsWheelA > 0:
                    motorAprocess_forwards.start()

                if stepsWheelB > 0:
                    motorBprocess_backwards.start()




            # The main loop pools the emergencyStop while the motors are running
            while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():

                # If a stop command has been sent, the turtle will stop its movement and exit this function
                if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:

                    if self.getStateTortoise() == enums.State.running:

                        self.setStateTortoise(enums.State.paused)
                        messages.printMessage('paused')

                        if motorAprocess_backwards.is_alive():
                            motorAprocess_backwards.terminate()
                            motorAprocess_backwards.join()

                        if motorBprocess_backwards.is_alive():
                            motorBprocess_backwards.terminate()
                            motorBprocess_backwards.join()

                        if motorAprocess_forwards.is_alive():
                            motorAprocess_forwards.terminate()
                            motorAprocess_forwards.join()

                        if motorBprocess_forwards.is_alive():
                            motorBprocess_forwards.terminate()
                            motorBprocess_forwards.join()

                elif self.getStateTortoise() == enums.State.paused:
                    self.setStateTortoise(enums.State.running)
                    messages.printMessage('resumed')


                time.sleep(0.5)


        # When the movement finishes, the motors are turned off
        self.A.stopMotors()
        self.B.stopMotors()

        return 0



    def moveForwards(self, steps):
        """
        The tortoise moves forwards.

        :param steps: number of rotations of the motors
        :type steps: int
        """

        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards)



    def moveBackwards(self, steps):
        """
        The tortoise moves backwards.

        :param steps: number of rotations of the motors
        :type steps: int
        """

        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards)



    def turnOnTheSpot(self, steps, direction):
        """
        This function makes the tortoise turn with the centre of rotation in one of the wheels.

        :param steps: number of rotations of the motors
        :param direction: one of these four combinations: [forwards/backwards]_[left/right]
        :type steps: int
        :type direction: enums.Direction
        """

        if(steps < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
            direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
            messages.printMessage('bad_direction_turn')
            self.blinkLEDs_error()
            return -1

    
        # Only wheel A moves
        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
            return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)

        # Only wheel B moves
        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
            return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)



    def shuffleOnTheSpot(self, steps, direction):
        """
        This function makes the tortoise turn with the centre of rotation between both wheels.

        :param steps: number of rotations of the motors
        :param direction: either clockwise or counter-clockwise
        :type steps: int
        :type direction: enums.Direction
        """

        if(steps < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
            messages.printMessage('bad_shuffle')
            self.blinkLEDs_error()
            return -1

        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)



    def shuffle45degrees(self, direction):
        """
        This function tries to make the tortoise shuffle 45 degrees.

        :param direction: one of these four combinations: [forwards/backwards]_[left/right]
        :type direction: enums.Direction
        """

        return self.shuffleOnTheSpot(180, direction)


    def turn(self, stepsWheelA, stepsWheelB, direction):
        """
        This function makes the tortoise turn by specifying different steps for the wheels.

        The function computes the delay that the wheel with less rotations should have in order to finish at the same time than the other wheel.

        :param stepsWheelA: number of rotations of the motor attached to wheel A
        :param stepsWheelB: number of rotations of the motor attached to wheel B
        :param direction: one of these four combinations: [forwards/backwards]_[left/right]
        :type stepsWheelA: int
        :type stepsWheelB: int
        :type direction: enums.Direction
        """

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
            direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
            messages.printMessage('bad_direction_turn')
            self.blinkLEDs_error()
            return -1

        if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
            messages.printMessage('bad_turn')
            self.blinkLEDs_error()
            return -1

        if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
            messages.printMessage('bad_turn')
            self.blinkLEDs_error()
            return -1

        if(stepsWheelA < 0 or stepsWheelB < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1



        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:

            # The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
            delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB

            return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)

        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:

            # The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
            delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA

            return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)




    def turn45degrees_sharp(self, direction):
        """
        This function tries to make the tortoise turn 45 degrees sharply.

        :param direction: one of these four combinations: [forwards/backwards]_[left/right]
        :type direction: enums.Direction
        """

        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:

            return self.turn(400, 75, direction)

        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:

            return self.turn(75, 400, direction)



    def turn30degrees_wide(self, direction):
        """
        This function tries to make the tortoise turn 45 degrees wide.

        :param direction: one of these four combinations: [forwards/backwards]_[left/right]
        :type direction: enums.Direction
        """

        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:

            return self.turn(450, 250, direction)

        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:

            return self.turn(250, 450, direction)
        


    def doRandomMovement(self):
        """
        Performs a natural random movement.

        The function chooses a movement, and it can be repeated up to three times. The probabilities of repeating the movement are as follows:

        - No repetition: 60%
        - Once: 25%
        - Twice: 10%
        - Three times: 5%

        The probabilities of choosing a random movement are as follows:

        - Move forwards: 30%
        - Move backwards: 10%
        - Shuffling: 10%
        - Turning 30 or 45 degrees: 10%
        - Turning with random steps: 40%

        The direction of movement for the turns and shuffles is chosen randomly.
        """

        # New random command
        if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand:

            # The number of times the command is repeated is chosen randomly with decreasing probabilities up to 3 times.
            self.numberRepeatsRandomCommand = np.random.choice([0, 1, 2, 3], 1, p = [0.6, 0.25, 0.1, 0.05])

            # As this is a new command, no repetitions done
            self.timesSameRandomCommandExecuted = 0

            # Random steps for wheel A
            self.lastRandomStepsWheelA = np.random.randint(30, 300)

            # Random number between 0 and 1 for decision on the random movement
            randomNumber = np.random.random_sample()

            # 40% probability of moving forwards/backwards
            if(randomNumber < 0.4):

                # 75% of probability of moving forwards
                if(randomNumber < 0.30):

                    self.moveForwards(self.lastRandomStepsWheelA)
                    self.lastRandomCommand = self.moveForwards

                # 25% probability of moving backwards
                else:

                    self.moveBackwards(self.lastRandomStepsWheelA)
                    self.lastRandomCommand = self.moveBackwards


            # 10% probability of shuffling
            elif (randomNumber < 0.5):

                # Equal probability of going clockwise or counter clockwise
                self.lastRandomDirection = np.random.choice([enums.Direction.clockwise, enums.Direction.counterClockwise], 1)

                self.shuffle45degrees(self.lastRandomDirection)

                self.lastRandomCommand = self.shuffle45degrees


            # 10% probability of turning 30 or 45 degrees
            elif (randomNumber < 0.6):

                # Equal probability of moving forwards/backwards lef/right
                self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1)

                # Equal probability of turning 30 degrees wide or 45 degrees sharp
                if(randomNumber < 0.55):

                    self.turn45degrees_sharp(self.lastRandomDirection)
                    self.lastRandomCommand = self.turn45degrees_sharp

                else:

                    self.turn30degrees_wide(self.lastRandomDirection)
                    self.lastRandomCommand = self.turn30degrees_wide


            # 40% of turning randomly
            else:

                # Random steps for wheel B
                self.lastRandomStepsWheelB = np.random.randint(30, 300)

                # Equal probability of moving forwards/backwards lef/right
                self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1)

                if(self.lastRandomDirection == enums.Direction.forwards_left or self.lastRandomDirection == enums.Direction.backwards_left):

                    if(self.lastRandomStepsWheelA >= self.lastRandomStepsWheelB):
                
                        aux = self.lastRandomStepsWheelA
                        self.lastRandomStepsWheelA = self.lastRandomStepsWheelB - 1 # To avoid the case of equals
                        self.lastRandomStepsWheelB = aux

                else:

                    if(self.lastRandomStepsWheelB >= self.lastRandomStepsWheelA):
                
                        aux = self.lastRandomStepsWheelA
                        self.lastRandomStepsWheelA = self.lastRandomStepsWheelB
                        self.lastRandomStepsWheelB = aux - 1 # To avoid the case of equals


                self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection)
    
                self.lastRandomCommand = self.turn



        # Repeat last command
        else:
    
            self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1


            if self.lastRandomCommand == self.moveForwards:

                self.moveForwards(self.lastRandomStepsWheelA)

            elif self.lastRandomCommand == self.moveBackwards:

                self.moveBackwards(self.lastRandomStepsWheelA)

            elif self.lastRandomCommand == self.shuffle45degrees:

                self.shuffle45degrees(self.lastRandomDirection)

            elif self.lastRandomCommand == self.turn45degrees_sharp:

                self.turn45degrees_sharp(self.lastRandomDirection)

            elif self.lastRandomCommand == self.turn30degrees_wide:

                self.turn30degrees_wide(self.lastRandomDirection)

            elif self.lastRandomCommand == self.turn:

                self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection)  
コード例 #9
0
GPIO.setup("P8_4", GPIO.OUT)
GPIO.output("P8_4", GPIO.HIGH)

GPIO.setup("P8_11", GPIO.OUT)
GPIO.output("P8_11", GPIO.HIGH)

GPIO.setup("P9_15", GPIO.OUT)
GPIO.output("P9_15", GPIO.HIGH)

GPIO.setup("P9_23", GPIO.OUT)
GPIO.output("P9_23", GPIO.HIGH)
##############################

speed = 0

m1 = Motor(1)
m1.setSpeed(speed)
m1.go()

m2 = Motor(2)
m2.setSpeed(speed * (-1))
m2.go()

m3 = Motor(3)
m3.setSpeed(speed)
m3.go()

m4 = Motor(4)
m4.setSpeed(speed * (-1))
m4.go()
コード例 #10
0
#!/usr/bin/env micropython
"""Development/debug test program."""

from motors import Motor, Mechanism
from sensors import Touch
from time import sleep

# Set up devices
gripper_motor = Motor('outA', gear_ratio=12)
gripper_switch = Touch('in1')

# Define and reset mechanism
targets = {'open': 0, 'closed': 110, 'up': 400, 'reset': 430}
default_speed = 50
gripper = Mechanism(gripper_motor, targets, default_speed, gripper_switch)

# Go to predefined absolute target
gripper.go_to_target('closed')
sleep(1)
gripper.go_to_target('open')
コード例 #11
0
ファイル: test_motor.py プロジェクト: sjtony/erle_control
The call to this method is:
     python text_motor.py [speed = 10] [time = 5]
'''

from motors import Motor
import Adafruit_BBIO.GPIO as GPIO
import time
import os
import sys

speed = 10
if len(sys.argv) > 1:
	speed = int(sys.argv[1])


m1 = Motor(1);
m1.setSpeedBrushless(speed)
m1.go()

m2 = Motor(2);
m2.setSpeedBrushless(speed);
m2.go()

m3 = Motor(3);
m3.setSpeedBrushless(speed);
m3.go()

m4 = Motor(4);
m4.setSpeedBrushless(speed);
m4.go()
コード例 #12
0
ファイル: init_motors.py プロジェクト: rossd15/erle_control
def initMotors():

    speed = 10

    m1 = Motor(1)
    m1.setSpeedBrushless(speed)
    m1.go()

    m2 = Motor(2)
    m2.setSpeedBrushless(speed)
    m2.go()

    m3 = Motor(3)
    m3.setSpeedBrushless(speed)
    m3.go()

    m4 = Motor(4)
    m4.setSpeedBrushless(speed)
    m4.go()

    time.sleep(0.5)

    #####
    # shutdown the motors
    #####

    speed = 0

    m1.setSpeedBrushless(speed)
    m1.go()

    m2.setSpeedBrushless(speed)
    m2.go()

    m3.setSpeedBrushless(speed)
    m3.go()

    m4.setSpeedBrushless(speed)
    m4.go()
コード例 #13
0
GPIO.setup("P8_4", GPIO.OUT) 
GPIO.output("P8_4", GPIO.HIGH)

GPIO.setup("P8_11", GPIO.OUT)
GPIO.output("P8_11", GPIO.HIGH)

GPIO.setup("P9_15", GPIO.OUT)
GPIO.output("P9_15", GPIO.HIGH)

GPIO.setup("P9_23", GPIO.OUT)
GPIO.output("P9_23", GPIO.HIGH)
##############################

speed = 0

m1 = Motor(1);
m1.setSpeed(speed);
m1.go()

m2 = Motor(2);
m2.setSpeed(speed*(-1));
m2.go()

m3 = Motor(3);
m3.setSpeed(speed);
m3.go()

m4 = Motor(4);
m4.setSpeed(speed*(-1));
m4.go()
コード例 #14
0
def limitThrust(thrust):
    if thrust > 100:
        thrush = 100
    elif thrust < -100:
        thrust = -100
    return thrust


#instantiate IMU
#TODO see how to import C interface to python
imu = IMU()
#MyKalman=KalmanFilter(....)

#instantiate motors and put them together in a list
motor1 = Motor(1)
motor2 = Motor(2)
motor3 = Motor(3)
motor4 - Motor(4)
motors = [motor1, motor2, motor3, motor4]

#TODO instantiate PID controllers
rollPID = PID()
pitchPID = PID()
yawPID = PID()
#zPID=PID(.....)
#xposPID=PID(.....)
#yposPID=PID(.....)

############################
#loop
コード例 #15
0
 def __init__(self):
     self.lmotor = Motor(4, 0x09, 1)
     self.rmotor = Motor(4, 0x09, 2)
     self.camera = Camera()
     self.sensor = Sensor()
コード例 #16
0
ファイル: stabilize.py プロジェクト: sjtony/erle_control
############################

#instantiate the BT controller
bt = BT_Controller(z_d)
bt.run()

#instantiate IMU
#TODO see how to import C interface to python
imu = IMU()
#MyKalman=KalmanFilter(....)

# dynamical model instance
dyn_model = Dynamical_Model()

#instantiate motors and put them together in a list
motor1 = Motor(1)
motor2 = Motor(2)
motor3 = Motor(3)
motor4 = Motor(4)
motors = [motor1, motor2, motor3, motor4]

# instantiate PID controllers
rollPID = PID(Kp, Kd, Ki)  # Kp, Kd, Ki
pitchPID = PID(Kp, Kd, Ki)
yawPID = PID(0, 0, 0)
zPID = PID(1, 0, 0)
#xposPID=PID(-0.09, -0.1, 0)
#yposPID=PID(-0.09, -0.1, 0)

# test variables
frequencies = []
コード例 #17
0
ファイル: init_motors.py プロジェクト: erlerobot/erle_control
def initMotors():

	speed = 10

	m1 = Motor(1);
	m1.setSpeedBrushless(speed)
	m1.go()

	m2 = Motor(2);
	m2.setSpeedBrushless(speed);
	m2.go()

	m3 = Motor(3);
	m3.setSpeedBrushless(speed);
	m3.go()

	m4 = Motor(4);
	m4.setSpeedBrushless(speed);
	m4.go()

	time.sleep(0.5)

	#####
	# shutdown the motors
	#####

	speed = 0

	m1.setSpeedBrushless(speed);
	m1.go()

	m2.setSpeedBrushless(speed);
	m2.go()

	m3.setSpeedBrushless(speed);
	m3.go()

	m4.setSpeedBrushless(speed);
	m4.go()
コード例 #18
0
    def __init__(self):
        """
        This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.

        The reasons of termination of a user process could be because of normal termination 
        or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
        and may be still on. In this case, the motors and LEDs should be turned off
        for the battery not to drain. 

        The solution implemented is to have a background process (a watchdog) running 
        continously. This process checks if the user process doesn't exist anymore (termination).
        If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
        In order to identy that the user script has finished, a file with the name [PID].pid is
        created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.

        Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created. 
        However, this hasn't been implemented yet. The idea could be to save the light 
        values in a file and read that file when creating the tortoise object. 
        Light conditions could have changed, so this should be done carefully. 

        At the moment, the tortoise object is created without calibration. If the users
        want to use the light sensors, they need will need to execute the calibrateLight
        function before using those sensors.
        """

        # Variables that control the calibration of the light sensors
        global isLightCalibrated
        global lowerBoundLight
        global upperBoundLight

        isLightCalibrated = False
        lowerBoundLight = 0
        upperBoundLight = 0
        # --- Variables that control the calibration of the light sensors ---


        # No warnings from the GPIO library
        GPIO.setwarnings(False)


        # Variables that control the random motion
        self.lastRandomCommand = None
        self.timesSameRandomCommandExecuted = 0
        self.numberRepeatsRandomCommand = -1
        self.lastRandomStepsWheelA = None
        self.lastRandomStepsWheelB = None
        self.lastRandomDirection = None
        # --- Variables that control the random motion ---


        # Setting the motors, sensors and actuators    

        # Pin numbers of the motors
        motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
        self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
        self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])

        self.sensors = Sensors()
        self.actuators = Actuators()

        # Position 1 of the light sensors area in the PCB assigned to pin 17
        self.sensors.setSensor(enums.SensorType.light, 1, 17) 

        # Position 2 of the light sensors area in the PCB assigned to pin 4
        self.sensors.setSensor(enums.SensorType.light, 2, 4)

        # Position 1 of the touch sensors area in the PCB assigned to pin 3
        self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) 

        # Position 2 of the touch sensors area in the PCB assigned to pin 27
        self.sensors.setSensor(enums.SensorType.touch, 2, 27) 

        # Position 3 of the touch sensors area in the PCB assigned to pin 2
        self.sensors.setSensor(enums.SensorType.touch, 3, 2)

        # Position 4 of the touch sensors area in the PCB assigned to pin 18
        self.sensors.setSensor(enums.SensorType.touch, 4, 18)

        # Position 1 of the proximity sensors area in the PCB assigned to pin 19
        self.sensors.setSensor(enums.SensorType.proximity, 1, 19)

        # Position 2 of the proximity sensors area in the PCB assigned to pin 21
        self.sensors.setSensor(enums.SensorType.proximity, 2, 21) 

        # Position 3 of the proximity sensors area in the PCB assigned to pin 22
        self.sensors.setSensor(enums.SensorType.proximity, 3, 22) 

        # Position 4 of the proximity sensors area in the PCB assigned to pin 26
        self.sensors.setSensor(enums.SensorType.proximity, 4, 26) 

         # Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
        ledPins = [8, 16, 25, 12]
        self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) 
        self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) 
        self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) 
        self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
        # --- Setting the motors, sensors and actuators ---


        # Times pressed the touch sensor for the latching behavour
        self.lastTouch = [-1,-1,-1]

        # Minimum milliseconds to send to the motors as delay
        self.minDelayMotors = 2


        # Creation of a file with the PID of the process

        # PID of process
        pid = os.getpid()

        # ~/.tortoise_pids/
        directory = os.path.expanduser("~") + "/.tortoise_pids/"

        # Filename: [PID].pid
        f = open(directory + str(pid) + ".pid", "w")

        # First line: motor pins
        f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")

        # Second line: LED pins
        f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")

        f.close()
        # --- Creation of a file with the PID of the process ---



        # Waiting for the user to press the e-stop button
        self.state = enums.State.paused

        messages.printMessage('greetings')
        while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
            time.sleep(0.1)

        messages.printMessage('running')

        self.state = enums.State.running
コード例 #19
0
ファイル: sensors.py プロジェクト: ilyaLibin/FlightController
import socket
import select
import sys
from motors import Motor
from PID import PID
import getch

global x, y, z, w, throttle
# Initial rotation
x = 0.7325378163287418
y = 0.4619397662556433
z = -0.19134171618254486
w = 0.4619397662556433

# ================================ Main loop ==================================
m_front_left = Motor(Motor.FRONT_LEFT)
m_front_right = Motor(Motor.FRONT_RIGHT)
m_back_right = Motor(Motor.BACK_RIGHT)
m_back_left = Motor(Motor.BACK_LEFT)

PitchPID = PID()
RollPID = PID()
YawPID = PID()


throttle = 300


def getInput():
    global throttle
    while True:
コード例 #20
0
ファイル: Controller.py プロジェクト: yassinechaouch/S4G1
from pyPS4Controller.controller import Controller
from motors import Motor

Motor_1 = Motor(3, 5)
Motor_2 = Motor(7, 8)

#from ROADENT.App import Motor_1, Motor_2


class MyController(Controller):
    def __init__(self, **kwargs):
        Controller.__init__(self, **kwargs)

    def on_up_arrow_press(self):
        print("Forward")
        Motor_1.clockwise()
        Motor_2.clockwise()

    def on_up_down_arrow_release(self):
        print("Stop")
        Motor_1.stop()
        Motor_2.stop()

    def on_down_arrow_press(self):
        Motor_1.c_clockwise()
        Motor_2.c_clockwise()

    def on_right_arrow_press(self):
        print("turn right")
        Motor_2.clockwise()
        Motor_1.c_clockwise()
コード例 #21
0
ファイル: beerbot.py プロジェクト: kirberich/beerbot
import argparse

from api import Api
from bot_serial import SerialWrapper
from motors import Motor
from recorder import CVCaptureRecorder

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Control the potato beer delivery system via an arduino.')
    parser.add_argument('dev', type=str, help='serial device the arduino is connected to')
    args = parser.parse_args()

    recorder = CVCaptureRecorder()

    serial = SerialWrapper(dev=args.dev, speed=57600)
    speed_motor = Motor(serial, arduino_command="S")
    direction_motor = Motor(serial, arduino_command="D")
    api = Api(recorder=recorder)
    api.demonize(port=8000)


    while True:
        # api events
        recorder.update_frame_rate()
        recorder.handle_frame()

        with api.lock:
            for event, kwargs in api.events:
                if event == "set":
                    direction_motor.set(kwargs["position"])
                if event == "set_speed":
コード例 #22
0
ファイル: main.py プロジェクト: octopusengine/octopuslab
from machine import Pin

import blesync_server
import blesync_uart.server
from motors import Motor
import pinout
from steering import Steering

motor_r = Motor(pinout.MOTOR_1A, pinout.MOTOR_2A, pinout.MOTOR_12EN)
motor_l = Motor(pinout.MOTOR_3A, pinout.MOTOR_4A, pinout.MOTOR_34EN)

steering = Steering(motor_l, motor_r)
built_in_led = Pin(2, Pin.OUT)


@blesync_uart.server.UARTService.on_message
def on_message(service, conn_handle, message):
    if message == b'!B516':
        steering.center(1000)
    elif message == b'!B507':
        steering.center(0)
    elif message == b'!B615':
        steering.center(-1000)
    elif message == b'!B606':
        steering.center(0)
    elif message == b'!B813':
        steering.right(1000)
    elif message == b'!B804':
        steering.right(0)
    elif message == b'!B714':
        steering.left(1000)
コード例 #23
0
ファイル: tortoise.py プロジェクト: kwinkle/greyWalter
class Tortoise:

    def __init__(self):

        global isLightCalibrated
        global lowerBoundLight
        global upperBoundLight

        GPIO.setwarnings(False)

#        self.lock = threading.RLock()

        self.lastRandomCommand = None
        self.timesSameRandomCommandExecuted = 0
        self.numberRepeatsRandomCommand = -1
        self.lastRandomStepsWheelA = None
        self.lastRandomStepsWheelB = None
        self.lastRandomDirection = None

        isLightCalibrated = False
        lowerBoundLight = 0
        upperBoundLight = 0

        # Previous: [4, 17, 23, 24, 27, 22, 18, 5]
        motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
        ledPins = [8, 16, 25, 12]

        # CREATING FILE WITH PID

        # PID of process
        pid = os.getpid()

        # ~/.tortoise_pids/
        directory = os.path.expanduser("~") + "/.tortoise_pids/"

        # Filename: [PID].pid
        f = open(directory + str(pid) + ".pid", "w")

        # First line: motor pins
        f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")

        # Second line: LED pins
        f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")

        f.close()
        # ----------------------


        # TODO: change to self.Motor.Left
        self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
        self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
        self.sensors = Sensors()
        self.actuators = Actuators()
        self.minDelayMotors = 2
        self.state = enums.State.paused


        self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16
        self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2
        self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6
        self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8
        self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13
        self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7
        self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10
        self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11
        self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x
        self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x

        self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19
        self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26
        self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x
        self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x

        self.lastTouch = [-1,-1,-1]

        #print "light sensor value:"
        #print self.sensors.readSensor(enums.SensorType.light, 1)
        #if not isLightCalibrated:
                #self.calibrateLight()

#        try:
#             thread.start_new_thread(self.pauseAndResume, ())
#        except:
#            print "Error: unable to start thread"

        messages.printMessage('greetings')
        while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
            time.sleep(0.1)

        messages.printMessage('running')

        self.state = enums.State.running


    def getStateTortoise(self):
        return self.state


    def setStateTortoise(self, toState):
        self.state = toState


    def calibrateLight(self):
        global lowerBoundLight, upperBoundLight, isLightCalibrated

        messages.printMessage('calibration_ambient')
        raw_input()
        #lowerBoundLight = max(self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2))
        lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
        #print "Light in normal conditions is: ", lowerBoundLight

        messages.printMessage('calibration_light_source')
        raw_input()
        #upperBoundLight = min((self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2)))
        upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
#        print "Light when there is a light source is:", upperBoundLight

        isLightCalibrated = True

        messages.printMessage('calibration_complete')



    def getSensorData(self, sensor_type, position):

        if (sensor_type == enums.SensorType.touch):

            if (position < 1 or position > 3):

                messages.printMessage('bad_touch_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.light):

            if (position != 1 and position!=2):

                messages.printMessage('bad_light_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.proximity):

            if (position < 1 or position > 4):

                messages.printMessage('bad_proximity_sensor')
                self.blinkLEDs_error()
                return -1

        elif (sensor_type == enums.SensorType.emergencyStop):

            if (position != 1):

                messages.printMessage('bad_emergency_sensor')
                self.blinkLEDs_error()
                return -1

        else:
                messages.printMessage('bad_sensor')
                self.blinkLEDs_error()
                return -1


        value = self.sensors.readSensor(sensor_type, position)

        if sensor_type == enums.SensorType.light:
            return value
            if (upperBoundLight - lowerBoundLight) == 0:
                messages.printMessage('no_calibration')
                self.blinkLEDs_error()
                return -1

            # Scale
            lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))

            if lightVal < 0:
                messages.printMessage('no_calibration')
                self.blinkLEDs_error()
                return -1

            return lightVal

        elif sensor_type == enums.SensorType.touch:

            return self.getSwitchTriggered(position,value)

        elif sensor_type == enums.SensorType.emergencyStop:

            return value % 2

        else:
            return value

    def getSwitchTriggered(self, position, value):
        if self.lastTouch[position-1]<0:
            self.lastTouch[position-1]=value
            return 0
        elif self.lastTouch[position-1]==value:
            return 0
        else:
            self.lastTouch[position-1]=value
            return 1


    def getLEDValue(self, position):

        if (position < 1 or position > 4):
            messages.printMessage('bad_LED')
            self.blinkLEDs_error()
            return -1

        return self.actuators.getActuatorValue(enums.ActuatorType.led, position)



    def setLEDValue(self, position, value):

        if(position < 1 or position > 4):
            messages.printMessage('bad_LED')
            self.blinkLEDs_error()
            return -1

        if(value != 0 and value != 1):
            messages.printMessage('bad_LED_value')
            self.blinkLEDs_error()
            return -1

        self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
        return 0



    def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):

        if numberOfBlinks < 0:
            messages.printMessage('blinks_negative')
            self.blinkLEDs_error()
            return -1

        if numberOfBlinks == 0:
            messages.printMessage('blinks_zero')
            self.blinkLEDs_error()
            return -1

        if delay < 0:
            messages.printMessage('blinking_fast')
            self.blinkLEDs_error()
            return -1


        try:
            for y in range(0, len(positions)):

                if positions[y] < 0 or positions[y] > 4:
                    messages.printMessage('bad_LED')
                    self.blinkLEDs_error()
                    return -1

        except TypeError: # It's not an array but an integer

            if positions < 0 or positions > 4:
                messages.printMessage('bad_LED')
                self.blinkLEDs_error()
                return -1



        previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]

        cont = True

        # Infinite loop to "stop" the execution of the program and keep blinkind the LEDs
        while cont:

            for x in range(0, numberOfBlinks):

                try:
                    for y in range(0, len(positions)):

                        self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)

                    time.sleep(delay)

                    for y in range(0, len(positions)):
                        self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)

                    time.sleep(delay)

                except TypeError: # It's not an array but an integer

                    self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
                    time.sleep(delay)
                    self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
                    time.sleep(delay)


            cont = blocking



        # If it doesn't block, the previous state of the LEDs is restored
        for x in range(1, 5):
            self.setLEDValue(x, previousStateLEDs[x - 1])

        return 0


    def blinkLEDs_error(self):
        self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True)



    def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards  and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise  ) :

            messages.printMessage('bad_direction')
            self.blinkLEDs_error()
            return -1

        if(stepsWheelA < 0 or stepsWheelB < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
            messages.printMessage('bad_delay')
            self.blinkLEDs_error()
            return -1

        # If a stop command has been sent, the turtle will stop its movement
        if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:

            if self.getStateTortoise() == enums.State.running:

                self.setStateTortoise(enums.State.paused)
                messages.printMessage('paused')

        else:

            if self.getStateTortoise() == enums.State.paused:
                    self.setStateTortoise(enums.State.running)
                    messages.printMessage('resumed')

            motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
            motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
            motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
            motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))


            if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:

                if stepsWheelA > 0:
                    motorAprocess_backwards.start()

                if stepsWheelB > 0:
                    motorBprocess_backwards.start()

            elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:

                if stepsWheelA > 0:
                    motorAprocess_forwards.start()

                if stepsWheelB > 0:
                    motorBprocess_forwards.start()

            elif direction == enums.Direction.clockwise:

                if stepsWheelA > 0:
                    motorAprocess_backwards.start()

                if stepsWheelB > 0:
                    motorBprocess_forwards.start()

            elif direction == enums.Direction.counterClockwise:

                if stepsWheelA > 0:
                    motorAprocess_forwards.start()

                if stepsWheelB > 0:
                    motorBprocess_backwards.start()




            # The main loop pools the emergencyStop
            while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():

                # If a stop command has been sent, the turtle will stop its movement
                if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:

                    if self.getStateTortoise() == enums.State.running:

                        self.setStateTortoise(enums.State.paused)
                        messages.printMessage('paused')

                        if motorAprocess_backwards.is_alive():
                            motorAprocess_backwards.terminate()
                            motorAprocess_backwards.join()

                        if motorBprocess_backwards.is_alive():
                            motorBprocess_backwards.terminate()
                            motorBprocess_backwards.join()

                        if motorAprocess_forwards.is_alive():
                            motorAprocess_forwards.terminate()
                            motorAprocess_forwards.join()

                        if motorBprocess_forwards.is_alive():
                            motorBprocess_forwards.terminate()
                            motorBprocess_forwards.join()

                elif self.getStateTortoise() == enums.State.paused:
                    self.setStateTortoise(enums.State.running)
                    messages.printMessage('resumed')


                time.sleep(0.5)


        self.A.stopMotors()
        self.B.stopMotors()

        return 0



    def moveForwards(self, steps):

        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards)



    def moveBackwards(self, steps):

        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards)



    def turnOnTheSpot(self, steps, direction):

        if(steps < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
            direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
            messages.printMessage('bad_direction_turn')
            self.blinkLEDs_error()
            return -1



        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
            return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)

        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
            return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)



    def shuffleOnTheSpot(self, steps, direction):

        if(steps < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1

        if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
            messages.printMessage('bad_shuffle')
            self.blinkLEDs_error()
            return -1



        return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)





    def turn(self, stepsWheelA, stepsWheelB, direction):

        if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
            direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
            messages.printMessage('bad_direction_turn')
            self.blinkLEDs_error()
            return -1

        if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
            messages.printMessage('bad_turn')
            self.blinkLEDs_error()
            return -1

        if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
            messages.printMessage('bad_turn')
            self.blinkLEDs_error()
            return -1

        if(stepsWheelA < 0 or stepsWheelB < 0):
            messages.printMessage('bad_steps')
            self.blinkLEDs_error()
            return -1



        if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:

            delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB

            return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)

        elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:

            delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA

            return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)



    def doRandomMovement2(self):

        maxTimesCommandRepeated = 3

        # New random command
        if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand:

            self.numberRepeatsRandomCommand = np.random.randint(maxTimesCommandRepeated + 1)
            self.timesSameRandomCommandExecuted = 0
    

            # Random number between 30 and 180
            numberOfSteps = np.random.randint(30, 180)

            self.lastRandomStepsWheelA = numberOfSteps
            self.lastRandomStepsWheelB = numberOfSteps
            

            # Random number between 0 and 1
            randomNumber = np.random.random_sample()

            if(randomNumber < 0.4):

                if(randomNumber < 0.2):

                    self.moveForwards(numberOfSteps)
                    self.lastRandomCommand = self.moveForwards

                else:

                    self.moveBackwards(numberOfSteps)
                    self.lastRandomCommand = self.moveBackwards

            else:

                # Random enums.Direction: left of right
                if(np.random.random_sample() < 0.5):
                    direction = enums.Direction.forwards_left
                else:
                    direction = enums.Direction.forwards_right

                self.lastRandomDirection = direction


                if(randomNumber < 0.7):
                    self.turnOnTheSpot(numberOfSteps, direction)
                else:
                    self.turnOnTheSpot(numberOfSteps, direction)   

    
                self.lastRandomCommand = self.turnOnTheSpot



        # Repeat last command
        else:
    
            self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1

            # TODO: change wheel A/B!

            if self.lastRandomCommand == self.moveForwards:

                self.moveForwards(self.lastRandomStepsWheelA)

            elif self.lastRandomCommand == self.moveBackwards:

                self.moveBackwards(self.lastRandomStepsWheelA)

            elif self.lastRandomCommand == self.turnOnTheSpot:

                self.turnOnTheSpot(self.lastRandomStepsWheelA, self.lastRandomDirection)  



    def doRandomMovement(self):

        # Random number between 30 and (509/4 + 30)
        numberOfSteps = int(509/4*np.random.random_sample() + 30)

        # Random number between 0 and 1
        randomNumber = np.random.random_sample()

        if(randomNumber < 0.4):

            if(randomNumber < 0.2):

                self.moveForwards(numberOfSteps)

            else:

                self.moveBackwards(numberOfSteps)

        else:

            # Random enums.Direction: left of right
            if(np.random.random_sample() < 0.5):
                direction = enums.Direction.forwards_left
            else:
                direction = enums.Direction.forwards_right


            if(randomNumber < 0.7):
                self.turnOnTheSpot(numberOfSteps, direction)
            else:
                self.turnOnTheSpot(numberOfSteps, direction)
コード例 #24
0
ファイル: hal.py プロジェクト: zappyfish/MakeHarvard2019
class HardwareAbstractionLayer:

    ARM_ONE_LENGTH = 1
    ARM_TWO_LENGTH = 1
    Z_MOVEMENT_FACTOR = 5
    ANGLE_DISPLACEMENT_MAGNITUDE = 0.5

    def __init__(self):
        z_pin = 18
        angle_one_pin = 13
        angle_two_pin = 12

        self.z_motor = Motor(z_pin)
        self.shoulder_motor = Motor(angle_one_pin, 20 * pi / 180)
        self.elbow_motor = Motor(angle_two_pin, 70 * pi / 180)

        self.shoulder_motor.change_angle(-1)
        self.elbow_motor.change_angle(-1)

    def translate_instrument(self, delta_x, delta_y):
        desired = np.array([delta_x, delta_y])
        theta_one = self.shoulder_motor.get_angle()
        theta_two = self.elbow_motor.get_angle()
        jacobian = self.compute_jacobian(theta_one, theta_two)

        control_inputs = np.linalg.lstsq(jacobian, desired)[0]
        if control_inputs[0] != 0 and control_inputs[1] != 0:
            control_inputs[1] = -control_inputs[1]
            # control_inputs[0] = -control_inputs[0] # TODO: check this
            control_inputs = self.remap_controls(control_inputs)
            return self.shoulder_motor.change_angle(
                control_inputs[0]) and self.elbow_motor.change_angle(
                    control_inputs[1])
        else:
            return False

    def remap_controls(self, control_inputs):
        return control_inputs * (self.ANGLE_DISPLACEMENT_MAGNITUDE /
                                 np.linalg.norm(control_inputs))

    def move_instrument_up(self):
        return self.z_motor.change_angle(self.Z_MOVEMENT_FACTOR)

    def move_instrument_down(self):
        return self.z_motor.change_angle(-self.Z_MOVEMENT_FACTOR)

    def compute_jacobian(self, theta_one, theta_two):
        l1 = self.ARM_ONE_LENGTH
        l2 = self.ARM_TWO_LENGTH
        t1 = theta_one
        t2 = theta_two
        dx_dt1 = l1 * sin(t1) - l2 * sin(t1 + t2)
        dx_dt2 = -l2 * sin(t1 + t2)

        dy_dt1 = l1 * cos(t1) + l2 * cos(t1 + t2)
        dy_dt2 = l2 * cos(t1 + t2)

        return np.array([[dx_dt1, dx_dt2], [dy_dt1, dy_dt2]])

    def test_move(self, x, y, r=20):
        for i in range(r):
            self.translate_instrument(x, y)
コード例 #25
0
ファイル: tortoise.py プロジェクト: kwinkle/greyWalter
    def __init__(self):

        global isLightCalibrated
        global lowerBoundLight
        global upperBoundLight

        GPIO.setwarnings(False)

#        self.lock = threading.RLock()

        self.lastRandomCommand = None
        self.timesSameRandomCommandExecuted = 0
        self.numberRepeatsRandomCommand = -1
        self.lastRandomStepsWheelA = None
        self.lastRandomStepsWheelB = None
        self.lastRandomDirection = None

        isLightCalibrated = False
        lowerBoundLight = 0
        upperBoundLight = 0

        # Previous: [4, 17, 23, 24, 27, 22, 18, 5]
        motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
        ledPins = [8, 16, 25, 12]

        # CREATING FILE WITH PID

        # PID of process
        pid = os.getpid()

        # ~/.tortoise_pids/
        directory = os.path.expanduser("~") + "/.tortoise_pids/"

        # Filename: [PID].pid
        f = open(directory + str(pid) + ".pid", "w")

        # First line: motor pins
        f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")

        # Second line: LED pins
        f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")

        f.close()
        # ----------------------


        # TODO: change to self.Motor.Left
        self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
        self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
        self.sensors = Sensors()
        self.actuators = Actuators()
        self.minDelayMotors = 2
        self.state = enums.State.paused


        self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16
        self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2
        self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6
        self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8
        self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13
        self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7
        self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10
        self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11
        self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x
        self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x

        self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19
        self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26
        self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x
        self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x

        self.lastTouch = [-1,-1,-1]

        #print "light sensor value:"
        #print self.sensors.readSensor(enums.SensorType.light, 1)
        #if not isLightCalibrated:
                #self.calibrateLight()

#        try:
#             thread.start_new_thread(self.pauseAndResume, ())
#        except:
#            print "Error: unable to start thread"

        messages.printMessage('greetings')
        while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
            time.sleep(0.1)

        messages.printMessage('running')

        self.state = enums.State.running
コード例 #26
0
    in the range (-100,100)
"""
def limitThrust(thrust, upperLimit = 100, lowerLimit = 0):
    if thrust > upperLimit:
        thrust = upperLimit
    elif thrust < lowerLimit:
        thrust = lowerLimit
    return thrust

#instantiate IMU
#TODO see how to import C interface to python
imu=IMU() 
#MyKalman=KalmanFilter(....)

#instantiate motors and put them together in a list
motor1=Motor(1)
motor2=Motor(2)
motor3=Motor(3)
motor4=Motor(4)
motors=[motor1,motor2,motor3,motor4]

# instantiate PID controllers and init them
rollPID=PID(0.9, 0.2, 0.3) # Kp, Kd, Ki
pitchPID=PID(0.9, 0.2, 0.3)
yawPID=PID(0.06, 0.02, 0.01)
#zPID=PID(.....)
#xposPID=PID(.....)
#yposPID=PID(.....)

#init pitch, roll and yaw:
roll = 0
コード例 #27
0
ファイル: test_motor.py プロジェクト: Blacklotis/beaglepilot
#GPIO.setup("P8_11", GPIO.OUT)
#GPIO.output("P8_11", GPIO.HIGH)
#
#GPIO.setup("P9_15", GPIO.OUT)
#GPIO.output("P9_15", GPIO.HIGH)
#
#GPIO.setup("P9_23", GPIO.OUT)
#GPIO.output("P9_23", GPIO.HIGH)
###############################

# initial timer
time.sleep(10)

speed = 10

m1 = Motor(1);
m1.setSpeed(speed);
m1.go()

m2 = Motor(2);
m2.setSpeed(speed);
m2.go()

m3 = Motor(3);
m3.setSpeed(speed);
m3.go()

m4 = Motor(4);
m4.setSpeed(speed));
m4.go()