Exemplo n.º 1
0
    def test_get_pwm_frequency(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act
        dev.set_pwm_frequency(197)

        # Assert
        self.assertEqual(dev.get_pwm_frequency(), 197)
Exemplo n.º 2
0
    def test_set_pwm_frequency_wrong_input(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act & Assert
        with self.assertRaises(DeviceException):
            dev.set_pwm_frequency(2000)
        with self.assertRaises(DeviceException):
            dev.set_pwm_frequency(10)
Exemplo n.º 3
0
    def test_set_pwm_frequency_wrong_input(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act & Assert
        with self.assertRaises(DeviceException):
            dev.set_pwm_frequency(2000)
        with self.assertRaises(DeviceException):
            dev.set_pwm_frequency(10)
Exemplo n.º 4
0
    def test_get_pwm_frequency(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act
        dev.set_pwm_frequency(197)

        # Assert
        self.assertEqual(dev.get_pwm_frequency(), 197)
Exemplo n.º 5
0
    def test_set_pwm_frequency(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act
        dev.set_pwm_frequency(200)

        # Assert
        self.assertEqual(dev.bus.wrote_values[0], (0, 17))
        self.assertEqual(dev.bus.wrote_values[1], (254, 30))
        self.assertEqual(dev.bus.wrote_values[2], (0, 1))
Exemplo n.º 6
0
    def test_set_pwm_frequency(self):
        # Arrange
        dev = Device(0, 0, FakeSMBus)

        # Act
        dev.set_pwm_frequency(200)

        # Assert
        self.assertEqual(dev.bus.wrote_values[0], (0, 17))
        self.assertEqual(dev.bus.wrote_values[1], (254, 30))
        self.assertEqual(dev.bus.wrote_values[2], (0, 1))
    def initPCA9685(self):
        """
        @description configure Jetson Nano GPIO for communication with PCA9685
        @return a PCA9685 Device object to communicate with PCA9685 chip
        """
        GPIO.setmode(GPIO.BOARD)
        mode = GPIO.getmode()
        print("Jetson Nano GPIO Mode: {}".format(mode))

        # discover I2C devices
        i2c_devs = Device.get_i2c_bus_numbers()
        print("The following /dev/i2c-* devices were found:\n{}".format(
            i2c_devs))

        # Create I2C device
        """
        working_devs = list()
        print("Looking out which /dev/i2c-* devices is connected to PCA9685")
        for dev in i2c_devs:
            try:
                pca9685 = Device(0x40,dev)
                # Set duty cycle
                pca9685.set_pwm(5, 2047)

                # set pwm freq
                pca9685.set_pwm_frequency(1000)
                print("Device {} works!".format(dev))
                working_devs.append(dev)
            except:
                print("Device {} does not work.".format(dev))

        # Select any working device, for example, the first one
        print("Configuring PCA9685 connected to /dev/i2c-{} device.".format(working_devs[0]))
        pca9685 = Device(0x40, working_devs[0]) 
        """
        pca9685 = Device(
            0x40, 1
        )  # Immediately set a working device, this assumes PCA9685 is connected to I2C channel 1

        # ESC work at 50Hz
        pca9685.set_pwm_frequency(50)

        # Set slow speed duty cycles
        self.set_channel_duty_cycle(pca9685, 0, 5.0)
        self.set_channel_duty_cycle(pca9685, 1, 5.0)
        self.set_channel_duty_cycle(pca9685, 2, 5.0)
        self.set_channel_duty_cycle(pca9685, 3, 5.0)

        # configure rest of channels to 0 duty cycle
        rest = np.arange(4, 16, 1)
        for channel in rest:
            self.set_channel_duty_cycle(pca9685, channel, 0.0)

        return pca9685
Exemplo n.º 8
0
class Drive:
    """Control motors of RC car"""
    def __init__(self, thread_id, thread_name):
        """Initialize PCA9685 servo driver and set angles for servo motors

        Args:
            thread_id (int): id of thread
            thread_name (str): name of thread
        """
        self.thread_id = thread_id
        self.thread_name = thread_name
        self.pwm = Device(0x40)  # setup PCA9685 servo driver device
        self.pwm.set_pwm_frequency(60)  # setup PCA9685 servo driver frequency
        self.steering_angle = 90  # set initial angle of servo for steering
        self.motor_angle = 133  # set initial angle of servo for motor
        self.set_angle(14, self.steering_angle)  # set angle for motors
        self.set_angle(15, self.motor_angle)  # set angle for motors

    def set_angle(self, channel, angle):
        """Calculate pulse width and set angle of servo motor

        Args:
            channel (int): channel of servo motor which is to be changed
            angle (int): angle to set servo motor to
        """
        pulse = (int(angle) * 2.5) + 150
        self.pwm.set_pwm(channel, int(pulse))

    def car_stopped(self):
        self.steering_angle = 90
        self.motor_angle = 133
        self.set_angle(14, self.steering_angle)  # set angle for motors
        self.set_angle(15, self.motor_angle)  # set angle for motors

    def drive_forward(self):
        self.steering_angle = 90
        self.motor_angle = 128
        self.set_angle(14, self.steering_angle)  # set angle for motors
        self.set_angle(15, self.motor_angle)  # set angle for motors

    def drive_left(self):
        self.steering_angle = 120
        self.motor_angle = 128
        self.set_angle(14, self.steering_angle)  # set angle for motors
        self.set_angle(15, self.motor_angle)  # set angle for motors

    def drive_right(self):
        self.steering_angle = 60
        self.motor_angle = 128
        self.set_angle(14, self.steering_angle)  # set angle for motors
        self.set_angle(15, self.motor_angle)  # set angle for motors
Exemplo n.º 9
0
class Robot(SingletonConfigurable):

    left_motor = traitlets.Instance(Motor)
    right_motor = traitlets.Instance(Motor)

    # config
    i2c_bus = traitlets.Integer(default_value=1).tag(config=True)
    left_motor_channel = traitlets.Integer(default_value=0).tag(config=True)
    left_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    right_motor_channel = traitlets.Integer(default_value=1).tag(config=True)
    right_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)

    def __init__(self, *args, **kwargs):
        super(Robot, self).__init__(*args, **kwargs)
        self.motor_driver = Device(0x40, self.i2c_bus)
        self.left_motor = Motor(self.motor_driver,
                                channel=self.left_motor_channel,
                                alpha=self.left_motor_alpha)
        self.right_motor = Motor(self.motor_driver,
                                 channel=self.right_motor_channel,
                                 alpha=self.right_motor_alpha)
        self.motor_driver.set_pwm_frequency(50)

    def set_motors(self, left_speed, right_speed):
        self.left_motor.value = left_speed
        self.right_motor.value = right_speed

    def forward(self, speed=1.0, duration=None):
        self.left_motor.value = -speed
        self.right_motor.value = speed

    def backward(self, speed=1.0):
        self.left_motor.value = speed
        self.right_motor.value = -speed

    def left(self, speed=1.0):
        self.left_motor.value = -speed
        self.right_motor.value = -speed

    def right(self, speed=1.0):
        self.left_motor.value = speed
        self.right_motor.value = speed

    def stop(self):
        self.left_motor.value = 0
        self.right_motor.value = 0
from pca9685_driver import Device


def set_duty_cycle(pwmdev, channel, dt):
    """    
    @channel Channel or PIN number in PCA9685 to configure 0-15
    @dt desired duty cycle
    """
    val = (dt * 4095) // 100
    pwmdev.set_pwm(channel, val)


pca9685 = Device(0x40, 1)

# Set duty cycle
#pca9685.set_pwm(0, 2047)

# set pwm freq
pca9685.set_pwm_frequency(60)

set_duty_cycle(pca9685, 0, 7)
Exemplo n.º 11
0
class Motors:
    def __init__(self):

        #importing needed data
        self.CWL = clawWidthList.clawWidthList()
        self.UsSensor = UsSensor.UsSensor()

        # setting channel names
        self.StepY = 22
        self.DirectionY = 19
        self.ResetY = 21
        self.clockZ = 5
        self.dataZ = 3
        self.clawChannel = 0  #unchanged
        self.emChannel = 1  #unchanged
        self.StepX = 38
        self.DirectionX = 40
        self.ResetX = 36
        self.PWMZ = 14  #,dutycycle is speed
        self.DirectionZ = 15
        self.ResetZ = 11

        self.dutycyclevalue = 0
        self.deltaerror = 1
        self.error0 = 1
        self.error1 = 1
        self.kp = .34
        self.kd = .01
        self.ki = 20
        self.readings = 50

        # Assuming X is direction in which trays open/close

        # These are the ports for the motors which move things in the x,y,or z axis.
        self.PORTX = "X"
        self.PORTY = "Y"
        self.PORTZ = "Z"

        # These are locations for the robot when they trays are closed
        self.CLOSETRAYY = 0

        # these are locations for when the part is being dropped from serializer
        self.DEFAULTX = 100
        self.DEFAULTY = 1000

        # Variable becomes false if it is reset

        # Code to set up GPIO stuff
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(self.DirectionX, GPIO.OUT, initial=GPIO.LOW)  # directionX
        GPIO.setup(self.StepX, GPIO.OUT, initial=GPIO.LOW)  # stepX
        GPIO.setup(self.ResetX, GPIO.IN)

        GPIO.setup(self.DirectionY, GPIO.OUT, initial=GPIO.LOW)  # directionX
        GPIO.setup(self.StepY, GPIO.OUT, initial=GPIO.LOW)  # stepX
        GPIO.setup(self.ResetY, GPIO.IN)

        GPIO.setup(self.ResetZ, GPIO.IN)

        self.xpos = 0.0
        self.ypos = 0.0
        self.zpos = 0.0

        # Set state: GPIO.output(channel,state)
        # set state: GPIO.output(channel,state, intital = GPIO.HIGH  OR GPIO.LOW)
        # Read value (high or low): GPIO.input(channel)

        self.move = True

        # working_devs = [1,2,3,4,5]
        self.pca9685 = Device(0x40, 1)
        # Set duty cycle
        self.pca9685.set_pwm(0, 2047)

        # set pwm freq
        self.pca9685.set_pwm_frequency(50)

        self.stepFrac = 1

        self.set_duty_cycle(self.pca9685, self.PWMZ, 0)

        self.zerror = 1

    def set_duty_cycle(self, pwmdev, channel, dt):
        """
        @pwmdev a Device class object already configured
        @channel Channel or PIN number in PCA9685 to configure 0-15
        @dt desired duty cycle
        """
        val = (int((dt * 4095) // 100))
        pwmdev.set_pwm(channel, val)

    def goTo(self, locationList, pocketNumber):
        goalx = int(locationList[0])
        goaly = int(locationList[1])
        goalz = float(locationList[2])

        #First turn everything on
        #make sure the claw is closed
        #Move to pos to get part
        #first y plus to get out of the way of the trays
        #next x since it can now move properly
        #z does not need to move
        #next move to the position to place the part
        #first move z since it is out of the way
        #Next move x since it is away from the trays
        #Next move y to 0 to grab the tray
        #Now move y to the part position
        #Place part (open claw and then sleep to let it be open for a little while)
        #close tray
        #Close claw
        #Move y to 0
        #turn off magnet
        #Move y 100

        #First turn everything on
        #make sure the claw is closed
        self.closeClaw()
        time.sleep(.25)
        self.MagnetOff()
        #Move to pos to get part
        #first y plus to get out of the way of the trays
        self.MotorGoTo("Y", self.DEFAULTY)
        #next x since it can now move properly
        self.MotorGoTo("X", self.DEFAULTX)
        #z does not need to move
        #next move to the position to place the part
        #first move z since it is out of the way
        self.MotorGoTo("Z", goalz)
        #Next move x since it is away from the trays
        self.MotorGoTo("X", goalx)
        #Next move y to 0 to grab the tray
        self.MotorGoTo("Y", self.CLOSETRAYY)
        time.sleep(.1)
        self.MagnetOn()
        time.sleep(.1)
        #Now move y to the part position
        self.MotorGoTo("Y", goaly)
        #Place part (open claw and then sleep to let it be open for a little while)
        self.MagnetOff()
        time.sleep(.1)
        self.openClaw()
        time.sleep(.25)
        #close tray
        #Close claw
        self.closeClaw()
        #Move y to 0 (close tray)
        self.MotorGoTo("Y", -1000)
        self.MotorGoTo("X", -1000)
        #turn off magnet
        self.MagnetOff()
        #Move y 100
        self.MotorGoTo("Y", 100)

    def MotorGoTo(self, name, goal):
        # TODO Add low level motor stuff
        self.move = True

        print("Motor " + str(name) + " is moving to " + str(goal))
        if name == "X":
            # assuming at 0
            # step angle = 1.2 deg
            # OD = 15 mm
            # Circ = 47.23 mm
            # 300 steps per circumference
            # .157 mm / step
            if self.xpos < goal and goal < 201:
                GPIO.output(self.DirectionX, GPIO.HIGH)
                while self.xpos < goal and self.move == True:
                    GPIO.output(self.StepX, GPIO.HIGH)
                    time.sleep(0.002 / self.stepFrac)
                    GPIO.output(self.StepX, GPIO.LOW)
                    time.sleep(0.002 / self.stepFrac)
                    self.xpos = self.xpos + .192 / self.stepFrac
                    #if GPIO.input(self.ResetX) == GPIO.LOW:
                    #self.xpos = 0
                    #self.move = False
                    print(self.xpos)
                self.move = True

            elif self.xpos > goal:
                GPIO.output(self.DirectionX, GPIO.LOW)
                while self.xpos > goal and self.move == True:
                    GPIO.output(self.StepX, GPIO.HIGH)
                    time.sleep(0.002 / self.stepFrac)
                    GPIO.output(self.StepX, GPIO.LOW)
                    time.sleep(0.002 / self.stepFrac)
                    self.xpos = self.xpos - .192 / self.stepFrac
                    if GPIO.input(self.ResetX) == GPIO.LOW:
                        self.xpos = 0
                        self.move = False
                    print(self.xpos)
                self.move = True

            else:
                pass

        elif name == "Y":
            # assuming at 0
            # step angle = 1.2 deg
            # OD = 15 mm
            # Circ = 47.23 mm
            # 300 steps per circumference
            # .157 mm / step
            if self.ypos < goal and goal < 400:
                GPIO.output(self.DirectionY, GPIO.HIGH)
                while self.ypos < goal and self.move == True:
                    GPIO.output(self.StepY, GPIO.HIGH)
                    time.sleep(0.001 / self.stepFrac)
                    GPIO.output(self.StepY, GPIO.LOW)
                    time.sleep(0.001 / self.stepFrac)
                    self.ypos = self.ypos + .195 / self.stepFrac
                    #if GPIO.input(self.ResetY) == GPIO.LOW:
                    #self.ypos = 0
                    #self.move = False
                    print(self.ypos)
                self.move = True

            elif self.ypos > goal:
                GPIO.output(self.DirectionY, GPIO.LOW)
                while self.ypos > goal and self.move == True:
                    GPIO.output(self.StepY, GPIO.HIGH)
                    time.sleep(0.001 / self.stepFrac)
                    GPIO.output(self.StepY, GPIO.LOW)
                    time.sleep(0.001 / self.stepFrac)
                    self.ypos = self.ypos - .195 / self.stepFrac
                    if GPIO.input(self.ResetY) == GPIO.LOW:
                        self.ypos = 0
                        self.move = False
                    print(self.ypos)
                self.move = True

            else:
                pass
        elif name == "Z":
            self.error0 = 0
            # iterator = 0
            # self.zpos = self.UsSensor.USMeasure()
            # if self.zpos < goal and goal < 100:
            #     # ztime = 0.0
            #     # while iterator<100 and self.move == True:
            #     #     self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
            #     #     self.set_duty_cycle(self.pca9685, self.PWMZ, 20)
            #     #     time.sleep(abs(goal)/100)
            #     #     #if GPIO.input(self.ResetZ) == GPIO.LOW:
            #     #         #self.ypos = 0
            #     #         #self.move = False
            #     #     print(self.zpos)
            #     #     iterator += 1
            #     # self.mov+e = True
            #     # self.set_duty_cycle(self.pca9685, self.PWMZ, 0)
            #     self.zerror = goal - self.zpos
            #     self.error0 = 0
            #     self.error1 = 0
            self.zerror = 20
            while not (self.zerror < 5 and self.zerror > -5):
                reading = 0
                for n in range(self.readings):
                    reading += self.UsSensor.USMeasure()
                    time.sleep(.0001)

                self.zpos = reading / self.readings
                self.zerror = goal - self.zpos
                self.error0 = self.error1 + self.error0
                self.error1 = self.zerror
                self.deltaerror = self.error0 - self.error1
                self.dutycyclevalue = int(self.kp * self.zerror +
                                          self.kd * self.deltaerror)
                if self.dutycyclevalue > 0:
                    if self.dutycyclevalue > 100:
                        self.dutycyclevalue = 100
                        self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
                        self.set_duty_cycle(self.pca9685, self.PWMZ,
                                            self.dutycyclevalue)
                    else:
                        self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
                        self.set_duty_cycle(self.pca9685, self.PWMZ,
                                            self.dutycyclevalue)

                elif self.dutycyclevalue < 0:
                    if self.dutycyclevalue < -100:
                        self.dutycyclevalue = -100
                        self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
                        self.set_duty_cycle(self.pca9685, self.PWMZ,
                                            -1 * self.dutycyclevalue)
                    else:
                        self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
                        self.set_duty_cycle(self.pca9685, self.PWMZ,
                                            -1 * self.dutycyclevalue)
                time.sleep(.1)
                print(str(self.zpos) + " z pos")
                print(str(goal) + " goal")
                print(str(self.zerror) + " z error")
                print(str(self.dutycyclevalue) + "DCV")
            # elif self.zpos > goal and abs(float(goal)) < 100:
            #     ztime = 0.0
            #     while iterator < 100 and self.move == True:
            #         self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
            #         self.set_duty_cycle(self.pca9685, self.PWMZ, 40)
            #         time.sleep(abs(goal)/100)
            #         if GPIO.input(self.ResetZ) == GPIO.LOW:
            #             self.ypos = 0
            #             self.move = False
            #         print(self.zpos)
            #         iterator += 1
            #     self.move = True
            #     self.set_duty_cycle(self.pca9685, self.PWMZ, 0)

            # else:
            #     pass
            self.set_duty_cycle(self.pca9685, self.PWMZ, 0)

    def MagnetOn(self):
        print("Magnet turning on")
        self.set_duty_cycle(self.pca9685, self.emChannel, 100)

    def MagnetOff(self):
        print("Magnet turning off")
        self.set_duty_cycle(self.pca9685, self.emChannel, 0)

    def dropPart(self):
        print("Dropping part")

    # Flaps are 2 inches wide
    def openClaw(self):
        print("Opening claw")
        self.set_duty_cycle(self.pca9685, self.clawChannel, 4)
        time.sleep(1)

    def neutralClaw(self):
        print("Opening claw neutral")
        self.set_duty_cycle(self.pca9685, self.clawChannel, 6.85)

    def closeClaw(self):
        print("Closing claw")
        self.set_duty_cycle(self.pca9685, self.clawChannel, 8.5)

    def openClawPercent(self, percent):
        print("opening claw " + percent + " %")
        DCValue = percent * 5 + 5
        self.set_duty_cycle(self.pca9685, self.clawChannel, DCValue)

    def openClawWidth(self, width):
        print("opening claw to a width of " + str(width) + " mm")
        widthPow = (((8.6 / math.pi) * (math.acos((-1 * width / 89) +
                                                  (45 / 44.5)) + 3.7))) - 7
        print("duty cycle is " + str(widthPow))
        self.set_duty_cycle(self.pca9685, self.clawChannel, widthPow)

    def MotorGoToXZ(self, goalx, goalz):
        done = False
        xdir = 2
        zdir = 2
        error0 = 0
        error1 = 0
        if self.xpos > goalx:
            xdir = -1
            GPIO.output(self.DirectionX, GPIO.LOW)
        elif self.xpos < goalx:
            xdir = 1
            GPIO.output(self.DirectionX, GPIO.HIGH)
        else:
            xdir = 0

        if self.zpos > goalz:
            zdir = -1
        elif self.zpos < goalz:
            zdir = 1
        else:
            zdir = 0

        while done == False:
            if (self.xpos + .1 < goalx
                    or self.xpos - .1 > goalx) and (self.xpos + 1 < goalx
                                                    or self.xpos - 1 > goalx):
                done == True
            if not (self.xpos + .1 < goalx or self.xpos - .1 > goalx):
                self.xpos = self.xpos + xdir * .157 / self.stepFrac
                GPIO.output(self.StepX, GPIO.HIGH)
                time.sleep(0.002 / self.stepFrac)
                GPIO.output(self.StepX, GPIO.LOW)
                time.sleep(0.002 / self.stepFrac)
            if (not (self.zpos + .1 < goalz or self.zpos - .1 > goalz)):
                #zpos = USReading
                #set direction pin to the correct direction
                if self.zpos - 1 > goalz:
                    self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
                else:
                    self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
                self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
                #Set dutycycle to value deterined by controller
                error0 = error1
                error1 = goalx - self.zpos
                deltaerror = error1 - error0
                DCContVal = kp * error1 + kd * (deltaerror)
                self.set_duty_cycle(self.pca9685, self.PWMZ, DCContVal)
Exemplo n.º 12
0
# discover I2C devices
i2c_devs = Device.get_i2c_bus_numbers()
print("The following /dev/i2c-* devices were found:\n{}".format(i2c_devs))

# Create I2C device
working_devs = list()
print("Looking out which /dev/i2c-* devices is connected to PCA9685")
for dev in i2c_devs:
    try:
        pca9685 = Device(0x40, dev)
        # Set duty cycle
        pca9685.set_pwm(5, 2047)

        # set pwm freq
        pca9685.set_pwm_frequency(1000)
        print("Device {} works!".format(dev))
        working_devs.append(dev)
    except:
        print("Device {} does not work.".format(dev))

# Select any working device, for example, the first one
print("Configuring PCA9685 connected to /dev/i2c-{} device.".format(
    working_devs[0]))
pca9685 = Device(0x40, working_devs[0])

# Set duty cycle
pca9685.set_pwm(0, 2047)

# set pwm freq
pca9685.set_pwm_frequency(1000)
Exemplo n.º 13
0
import sys
import time
import datetime
import random

import state

sys.path.append('./PCA9685Driver')
# Import the PCA9685 module.
from pca9685_driver import Device

# launch servo
pwm = Device(0x41)
# Set frequency to 60hz, good for pwm.
pwm.set_pwm_frequency(60)

#PC9685 - TB6612FNG
# PWM8  - PWMA
# PWM9  - AIN1
# PWM10 - AIN2
#
# PWM11 - BIN1
# PWM12 - BIN2
# PWM13 - PWMB

pwmA = 8
ain1 = 9
ain2 = 10

pwmB = 11
Exemplo n.º 14
0
# drive rc car with xbox one controller, only used for testing

from picamera.array import PiRGBArray  # for pi camera
from picamera import PiCamera  # for pi camera
from approxeng.input.selectbinder import ControllerResource  # for xbox controller
from pca9685_driver import Device  # for PCA9685 servo driver
from time import sleep

pwm = Device(0x40)  # setup PCA9685 servo driver
pwm.set_pwm_frequency(60)  # setup PCA9685 servo driver

steering_angle = 90  # initial angle of servo for steering
motor_angle = 133  # initial angle of servo for motor


def set_angle(channel, angle):
    pulse = (int(angle) * 2.5) + 150
    pwm.set_pwm(channel, int(pulse))


print('Ready, drive only')
while 1:
    try:
        with ControllerResource(print_events=False, controller_class=None, hot_zone=0.1,
                                dead_zone=0.1) as controller:
            print('Found a controller')
            while controller.connected:
                set_angle(14, steering_angle)
                set_angle(15, motor_angle)
            
                # right trigger moved (motor forward)
class DataCapture(object):
    """Collect image and controller input data for neural network to .npy file
       collected data consists of:
       processed image with edge detection
       input data from controller, array of [left_val, motor_val, right_val] (0 or 1) e.g. [0,1,0] is forward
    """
    def __init__(self, thread_id, name):
        self.thread_id = thread_id
        self.name = name

        self.pwm = Device(0x40)  # setup PCA9685 servo driver device
        self.pwm.set_pwm_frequency(60)  # setup PCA9685 servo driver frequency

        self.steering_angle = 90  # set initial angle of servo for steering
        self.motor_angle = 133  # set initial angle of servo for motor

        # numpy data setup
        self.npy_file = 'datasets/dataset.npy'  # numpy file for storing training data
        self.left_val = 0  # [0,*,*]
        self.forward_val = 0  # [*,0,*]
        self.right_val = 0  # [*,*,0]
        self.training_data = [
        ]  # array for controller input data [left_val, motor_val, right_val]
        self.printed_length = False  # used to print length of training data

        self.stream = frame.Frame(1, 'SaveFrame')  # setup camera stream
        self.stream.start()  # start camera stream
        self.start()  # start data collection

    def start(self):
        Thread(target=self.gather_data(), args=()).start()
        return self

    def set_angle(self, channel, angle):
        """Calculate pulse width and set angle of servo motor
        Args:
            channel: channel of servo motor which is to be changed
            angle: angle to set servo motor to
        """
        pulse = (int(angle) * 2.5) + 150
        self.pwm.set_pwm(channel, int(pulse))

    def gather_data(self):
        """Loop to gather data using image data and controller input data"""
        if os.path.isfile(self.npy_file):
            print('DataSet file exists.. loading previous data')
            self.training_data = list(np.load(self.npy_file))
        else:
            print('DataSet file does not exist.. starting new file')
            self.training_data = []
        print('Training data samples: {}'.format(len(self.training_data)))

        print(
            'Ready, connect controller and drive around to collect data ((X) controller button to stop collection)'
        )
        while 1:
            try:
                with ControllerResource(print_events=False,
                                        controller_class=None,
                                        hot_zone=0.1,
                                        dead_zone=0.1) as controller:
                    print('Found a controller')
                    while controller.connected:
                        self.set_angle(
                            14, self.steering_angle)  # set angle for motors
                        self.set_angle(
                            15, self.motor_angle)  # set angle for motors

                        # right trigger moved (motor forward)
                        if controller['rt'] == 1:
                            self.forward_val = int(
                                controller['rt']
                            )  # update forward val for input data array [*,0,*]
                            self.motor_angle = 125  # update servo angle for motor
                            self.printed_length = False
                            img = self.stream.read(
                            )  # read latest frame from camera stream
                            self.save_sample(img)  # save sample to dataset

                        # left stick moved (steering)
                        if controller['lx'] is not 0:
                            stick_val = int(controller['lx'])
                            if stick_val > 0:  # stick moved right
                                self.right_val = stick_val  # update value for input data array [*,*,1]
                                self.steering_angle = 60  # update servo angle for steering
                            if stick_val < 0:  # stick moved left
                                self.left_val = stick_val * -1  # update value for input data array [1,*,*]
                                self.steering_angle = 120  # update servo angle for steering

                        # left stick released (steering centered)
                        if (controller['lx'] is 0) and (self.steering_angle
                                                        is not 90):
                            self.steering_angle = 90  # update servo angle for steering
                            self.left_val = 0  # update value for input data array [0,*,*]
                            self.right_val = 0  # update value for input data array [*,*,0]

                        # right trigger released (motor stopped)
                        if controller['rt'] is 0:
                            self.motor_angle = 133  # update servo angle for motor
                            self.forward_val = 0  # update value for input data array [*,0,*]

                        # save dataset to npy file
                        if controller[
                                'square']:  # (X) button on Xbox One controller
                            self.stop_collection()

                        if controller[
                                'triangle']:  # (Y) button on Xbox One controller
                            if not self.printed_length:  # to print length of data only once in loop
                                self.printed_length = True
                                print("Length of data: {}".format(
                                    len(self.training_data))
                                      )  # print current data length

                        if (len(self.training_data) % 100 == 0) and (len(
                                self.training_data) is not 0):
                            print("100 more samples")

            except IOError:
                print('Waiting for controller...')
                sleep(1)

    def save_sample(self, img):
        """Save individual data sample
        sample consists of (processed image, input array)
        print input data array and show new processed image frame
        Args:
            img: image which is to be processed with edge detection
        """
        edges = self.stream.process(img)  # edge detection
        output = [
            self.left_val, self.forward_val, self.right_val
        ]  # [left, forward, right] update array of controller input data
        self.training_data.append([edges, output])
        print(output)
        cv2.imshow('Data Collection Frame Preview', edges)
        cv2.waitKey(1) & 0xFF

    def stop_collection(self):
        """Stop data collection, save dataset, close camera preview, close stream"""
        print('-----Stopping Data Capture-----')
        self.save_dataset()
        cv2.destroyAllWindows()
        self.stream.stop_stream()
        print("-----END-----")
        exit()

    def save_dataset(self):
        """Save complete dataset to npy file"""
        print("Saving training data to file: ", self.npy_file)
        np.save(self.npy_file, self.training_data)
        print('Training data samples: {}'.format(len(self.training_data)))
Exemplo n.º 16
0
class Plugin(plugin.PluginProto):
    PLUGIN_ID = 22
    PLUGIN_NAME = "Extra IO - PCA9685"
    PLUGIN_VALUENAME1 = "PWM"
    MAX_PINS = 15
    MAX_PWM = 4095
    MIN_FREQUENCY = 23.0  # Min possible PWM cycle frequency
    MAX_FREQUENCY = 1500.0  # Max possible PWM cycle frequency

    def __init__(self, taskindex):  # general init
        plugin.PluginProto.__init__(self, taskindex)
        self.dtype = rpieGlobals.DEVICE_TYPE_I2C
        self.vtype = rpieGlobals.SENSOR_TYPE_NONE
        self.ports = 0
        self.valuecount = 0
        self.senddataoption = False
        self.timeroption = False
        self.pca = None

    def plugin_init(self, enableplugin=None):
        plugin.PluginProto.plugin_init(self, enableplugin)
        self.initialized = False
        self.pca = None
        if self.enabled:
            i2cport = -1
            try:
                for i in range(0, 2):
                    if gpios.HWPorts.is_i2c_usable(
                            i) and gpios.HWPorts.is_i2c_enabled(i):
                        i2cport = i
                        break
            except:
                i2cport = -1
            if i2cport > -1 and int(self.taskdevicepluginconfig[0]) > 0:
                try:
                    freq = int(self.taskdevicepluginconfig[1])
                    if freq < self.MIN_FREQUENCY or freq > self.MAX_FREQUENCY:
                        freq = self.MAX_FREQUENCY
                except:
                    freq = self.MAX_FREQUENCY
                try:
                    self.pca = Device(address=int(
                        self.taskdevicepluginconfig[0]),
                                      bus_number=int(i2cport))
                    if self.pca is not None:
                        self.initialized = True
                        self.pca.set_pwm_frequency(freq)
                except Exception as e:
                    misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
                                "PCA9685 device init failed: " + str(e))
                    self.pca = None
            if self.pca is not None:
                pass

    def webform_load(self):  # create html page for settings
        choice1 = int(float(
            self.taskdevicepluginconfig[0]))  # store i2c address
        optionvalues = []
        for i in range(0x40, 0x78):
            optionvalues.append(i)
        options = []
        for i in range(len(optionvalues)):
            options.append(str(hex(optionvalues[i])))
        webserver.addFormSelector("Address", "p022_adr", len(options), options,
                                  optionvalues, None, choice1)
        webserver.addFormNote(
            "Enable <a href='pinout'>I2C bus</a> first, than <a href='i2cscanner'>search for the used address</a>!"
        )
        webserver.addFormNumericBox("Frequency", "p022_freq",
                                    self.taskdevicepluginconfig[2],
                                    self.MIN_FREQUENCY, self.MAX_FREQUENCY)
        webserver.addUnit("Hz")
        return True

    def webform_save(self, params):  # process settings post reply
        cha = False
        par = webserver.arg("p022_adr", params)
        if par == "":
            par = 0x40
        if self.taskdevicepluginconfig[0] != int(par):
            cha = True
        self.taskdevicepluginconfig[0] = int(par)
        par = webserver.arg("p022_freq", params)
        if par == "":
            par = self.MAX_FREQUENCY
        if self.taskdevicepluginconfig[2] != int(par):
            cha = True
        self.taskdevicepluginconfig[2] = int(par)
        if cha:
            self.plugin_init()
        return True

    def plugin_write(self, cmd):  # handle incoming commands
        res = False
        cmdarr = cmd.split(",")
        cmdarr[0] = cmdarr[0].strip().lower()
        if self.pca is not None:
            if cmdarr[0] == "pcapwm":
                pin = -1
                val = -1
                try:
                    pin = int(cmdarr[1].strip())
                    val = int(cmdarr[2].strip())
                except:
                    pin = -1
                if pin > -1 and val >= 0 and val <= self.MAX_PWM:
                    misc.addLog(rpieGlobals.LOG_LEVEL_DEBUG,
                                "PCAPWM" + str(pin) + " set to " + str(val))
                    try:
                        self.pca.set_pwm(pin, val)
                    except Exception as e:
                        misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
                                    "PCAPWM" + str(pin) + ": " + str(e))
                return True
            if cmdarr[0] == "pcafrq":
                freq = -1
                try:
                    freq = int(cmdarr[1].strip())
                except:
                    freq = -1
                if (freq > -1) and (freq >= self.MIN_FREQUENCY) and (
                        freq <= self.MAX_FREQUENCY):
                    misc.addLog(rpieGlobals.LOG_LEVEL_DEBUG,
                                "PCAFRQ" + str(freq))
                    try:
                        self.pca.set_pwm_frequency(freq)
                    except Exception as e:
                        misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
                                    "PCAFRQ" + str(e))
                return True

        return res
Exemplo n.º 17
0
    else:
        print('\nBoard addr {} NOT in center file, using default.'.format(
            board_addr))
else:
    print('\nFile {} does not exist, using default pwm list.'.format(
        center_pwm_file))
    servo_center_dict = {}

# 0x40 from i2cdetect -y 1 (1 if Raspberry pi 2)
print('i2c hex board_addr:', args.board_addr, type(args.board_addr))
if args.board_addr == 40:
    dev = Device(0x40)
if args.board_addr == 41:
    dev = Device(0x41)

dev.set_pwm_frequency(50)

min_pwm = 110
max_pwm = 515
mid_pwm = int((min_pwm + max_pwm) / 2)
delta_pwm = 10

cur_servo = 0
#[dev.set_pwm(k, v) for k,v in pwm_dict.items()]


def moveCursorRefresh(stdscr):
    stdscr.move(curses.LINES - 1, curses.COLS - 1)
    stdscr.refresh()  #Do this after addstr

Exemplo n.º 18
0
#!/usr/bin/env python3

import time
from pyPS4Controller.controller import Controller
import board
import busio
import Jetson.GPIO as GPIO
import sys

sys.path.append('/opt/nvidia/jetson-gpio/lib/python')
sys.path.append('/opt/nvidia/jetson-gpio/lib/python/Jetson/GPIO')

from pca9685_driver import Device

servo = Device(0x40,1)
servo.set_pwm_frequency(50)

time.sleep(0.5)

servo.set_pwm(0,330)#range 250x350

def connect():
    print("connected")
    pass

class MyController(Controller):

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

    def on_x_press(self):