def test_set_pwm_value(self): # Arrange dev = Device(0, 0, FakeSMBus) # Act dev.set_pwm(4, 1042) # Assert self.assertEqual(dev.bus.values[24], 18) self.assertEqual(dev.bus.values[25], 4)
def test_set_pwm_value(self): # Arrange dev = Device(0, 0, FakeSMBus) # Act dev.set_pwm(4, 1042) # Assert self.assertEqual(dev.bus.values[24], 18) self.assertEqual(dev.bus.values[25], 4)
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
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
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)
leg_servos_dict = {i: [2 * i, 2 * i + 1] for i in range(N_legs)} # Servo indices for each leg pairs_legs_dict = {i: [2 * i, 2 * i + 1] for i in range(N_pairs)} # Leg indices for each pair min_pwm = 400 max_pwm = 500 '''mid_pwm = int(0.5*(min_pwm + max_pwm)) diff_pwm = max_pwm - mid_pwm''' mid_pwm = 312 diff_pwm = 50 for servo_pair in leg_servos_dict.values(): for servo in servo_pair: dev.set_pwm(servo, mid_pwm) # I guess I'll do the phase per leg for now, since I assume the 2 # servos for a leg should be in phase legs_phase_dict = {i: pi * (i // 2) for i in range(N_legs)} legs_direction_dict = {i: (-1)**i for i in range(N_legs)} i = 0 delta = float(args.delta) delta_i = 0.05 * 2 * pi space_delay_sleep = 0.00 pos_sleep = 0.01 try: while True:
dev = Device(0x40, 1) #endereco 0x40 , device(bus) 0,1,2,3,4,5,6 import time time.sleep(1) t0 = 0.01 t1 = 0.1 #inicio #frequencia dev.set_pwm_frequency(50) #seguranca for i in range(15): dev.set_pwm(i, 330) time.sleep(1) for i in range(330, 400): dev.set_pwm(0, i) time.sleep(t0) for i in reversed(range(260, 400)): dev.set_pwm(0, i) time.sleep(t0) for i in range(260, 330): dev.set_pwm(0, i) time.sleep(t0) #motor
from pca9685_driver import Device from time import sleep # 0x40 from i2cdetect -y 1 (1 if Raspberry pi 2) dev = Device(0x40) # set the duty cycle for LED05 to 50% dev.set_pwm(0, 204) # set the pwm frequency (Hz) dev.set_pwm_frequency(50) lb = int(0.01 * 4097) ub = int(0.12 * 4097) for i in range(lb, ub): dev.set_pwm(0, i) print(i) sleep(0.01)
#!/usr/bin/env python # sudo apt install libffi-dev # sudo pip install -U pip setuptools # sudo pip install PCA9685-driver from pca9685_driver import Device from time import sleep import signal import sys # 0x40 from i2cdetect -y 1 (1 if Raspberry pi 2) dev = Device(0x40) # set the pwm frequency (Hz) dev.set_pwm_frequency(50) dev.set_pwm(0, 0) def turnoff(signal, frame): print("turnoff") dev.set_pwm(0, 0) sys.exit(0) signal.signal(signal.SIGINT, turnoff) dev.set_pwm(0, int(sys.argv[1])) sleep(0.2) dev.set_pwm(0, 0)
max_pwm_val = 4097 min_pwm = 110 max_pwm = 515 mid_pwm = int((min_pwm + max_pwm) / 2) delta_pwm = 10 cur_servo = args.index pwm_mid_list = [ 190, 312, 442, 292, 292, 305, 282, 192, 312, 282, 292, 312, 312, 272, 332, 342 ] pwm_dict = {i: pwm_mid_list[i] for i in range(len(pwm_mid_list))} #init_pwm = mid_pwm #pwm_dict = {i:init_pwm for i in range(args.N_servos)} #[dev.set_pwm(k, v) for k,v in pwm_dict.items()] dev.set_pwm(args.index, pwm_mid_list[args.index]) def moveCursorRefresh(stdscr): stdscr.move(curses.LINES - 1, curses.COLS - 1) stdscr.refresh() #Do this after addstr def DCloop(stdscr): #https://docs.python.org/3/howto/curses.html #https://docs.python.org/3/library/curses.html#curses.window.clrtobot global cur_servo, pwm_dict, delta_pwm, mid_pwm move_str_pos = [2, 6] cur_servo_str_pos = [4, 6] pwm_str_pos = [6, 6]
GPIO.setmode(GPIO.BOARD) mode = GPIO.getmode() print(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]) # Set duty cycle pca9685.set_pwm(0, 2047)
time.sleep(1) print("Release Buttons") time.sleep(1) while joystick.connected: time.sleep(0.05) os.system('clear') print("Ctrt+c : test Out") a = joystick['lx'] b = ((a*(-1))+1)/2*1 print("Drive",b) servo = int((120*b)+(250)) dev.set_pwm(0,servo) print("servo",servo) brake = joystick['rx'] brake = ((brake*(-1))+1)/2*0.5 print("brake",brake) throttle = joystick['ry'] throttle = ((throttle*(-1))+1)/2*0.5 print("throttle",throttle) print("Press Squere ",joystick['cross']) signal_power = brake+throttle print("signal_power",signal_power) power = 330
time.sleep(1) print("Liberar Controles") time.sleep(2) while joystick.connected: time.sleep(0.1) os.system('clear') print("Ctrt+c : Sair do Teste") a = joystick['lx'] b = ((a * (-1)) + 1) / 2 * 1 print("Direcao", b) servo = int((120 * b) + (250)) dev.set_pwm(0, servo) print("servo", servo) freio = joystick['rx'] freio = ((freio * (-1)) + 1) / 2 * 0.5 print("freio", freio) acelerador = joystick['ry'] acelerador = ((acelerador * (-1)) + 1) / 2 * 0.5 print("acelerador", acelerador) sinal_motor = freio + acelerador print("sinal_motor", sinal_motor) motor = int((220 * sinal_motor) + (220))
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)))
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): print("squere pressed") def on_square_press(self): print("triangle pressed")
pulse_length //= 4096 # 12 bits of resolution print('{0}us per bit'.format(pulse_length)) pulse *= 1000 pulse //= pulse_length pwm.set_pwm(channel, pulse) # Set frequency to 60hz, good for servos. pwm.set_pwm_frequency(60) servo_number = 0 print('Moving servo on channel ' + str(servo_number) + ' 0, press Ctrl-C to quit...') try: # do two launches Motors.launchServoStart() time.sleep(1.0) Motors.launchServoRetract() time.sleep(2.0) Motors.launchServoStart() time.sleep(1.0) Motors.launchServoRetract() except: print('shutting down servos') pwm.set_pwm(servo_number, servo_max) time.sleep(0.8) pwm.set_pwm(servo_number, 0) pwm = Device(0x41)