diff --git a/PCA9685.py b/PCA9685.py new file mode 100644 index 0000000..a26f5f6 --- /dev/null +++ b/PCA9685.py @@ -0,0 +1,274 @@ +#!/usr/bin/python +''' +********************************************************************** +* Filename : PCA9685.py +* Description : A driver module for PCA9685 +* Author : Cavon +* Brand : SunFounder +* E-mail : service@sunfounder.com +* Website : www.sunfounder.com +* Version : v1.2.0 +********************************************************************** +''' + +import smbus +import time +import math + +class PWM(object): + """A PWM control class for PCA9685.""" + _MODE1 = 0x00 + _MODE2 = 0x01 + _SUBADR1 = 0x02 + _SUBADR2 = 0x03 + _SUBADR3 = 0x04 + _PRESCALE = 0xFE + _LED0_ON_L = 0x06 + _LED0_ON_H = 0x07 + _LED0_OFF_L = 0x08 + _LED0_OFF_H = 0x09 + _ALL_LED_ON_L = 0xFA + _ALL_LED_ON_H = 0xFB + _ALL_LED_OFF_L = 0xFC + _ALL_LED_OFF_H = 0xFD + + _RESTART = 0x80 + _SLEEP = 0x10 + _ALLCALL = 0x01 + _INVRT = 0x10 + _OUTDRV = 0x04 + + RPI_REVISION_0 = ["900092"] + RPI_REVISION_1_MODULE_B = ["Beta", "0002", "0003", "0004", "0005", "0006", "000d", "000e", "000f"] + RPI_REVISION_1_MODULE_A = ["0007", "0008", "0009",] + RPI_REVISION_1_MODULE_BP = ["0010", "0013"] + RPI_REVISION_1_MODULE_AP = ["0012"] + RPI_REVISION_2_MODULE_B = ["a01041", "a21041"] + RPI_REVISION_3_MODULE_B = ["a02082", "a22082"] + RPI_REVISION_3_MODULE_BP = ["a020d3"] + + _DEBUG = False + _DEBUG_INFO = 'DEBUG "PCA9685.py":' + + def _get_bus_number(self): + pi_revision = self._get_pi_revision() + if pi_revision == '0': + return 0 + elif pi_revision == '1 Module B': + return 0 + elif pi_revision == '1 Module A': + return 0 + elif pi_revision == '1 Module B+': + return 1 + elif pi_revision == '1 Module A+': + return 0 + elif pi_revision == '2 Module B': + return 1 + elif pi_revision == '3 Module B': + return 1 + elif pi_revision == '3 Module B+': + return 1 + + def _get_pi_revision(self): + "Gets the version number of the Raspberry Pi board" + # Courtesy quick2wire-python-api + # https://github.com/quick2wire/quick2wire-python-api + # Updated revision info from: http://elinux.org/RPi_HardwareHistory#Board_Revision_History + try: + f = open('/proc/cpuinfo','r') + for line in f: + if line.startswith('Revision'): + if line[11:-1] in self.RPI_REVISION_0: + return '0' + elif line[11:-1] in self.RPI_REVISION_1_MODULE_B: + return '1 Module B' + elif line[11:-1] in self.RPI_REVISION_1_MODULE_A: + return '1 Module A' + elif line[11:-1] in self.RPI_REVISION_1_MODULE_BP: + return '1 Module B+' + elif line[11:-1] in self.RPI_REVISION_1_MODULE_AP: + return '1 Module A+' + elif line[11:-1] in self.RPI_REVISION_2_MODULE_B: + return '2 Module B' + elif line[11:-1] in self.RPI_REVISION_3_MODULE_B: + return '3 Module B' + elif line[11:-1] in self.RPI_REVISION_3_MODULE_BP: + return '3 Module B+' + else: + print "Error. Pi revision didn't recognize, module number: %s" % line[11:-1] + print 'Exiting...' + quit() + except Exception, e: + f.close() + print e + print 'Exiting...' + quit() + finally: + f.close() + + def __init__(self, bus_number=None, address=0x40): + '''Init the class with bus_number and address''' + if self._DEBUG: + print self._DEBUG_INFO, "Debug on" + self.address = address + if bus_number == None: + self.bus_number = self._get_bus_number() + else: + self.bus_number = bus_number + self.bus = smbus.SMBus(self.bus_number) + if self._DEBUG: + print self._DEBUG_INFO, 'Reseting PCA9685 MODE1 (without SLEEP) and MODE2' + self.write_all_value(0, 0) + self._write_byte_data(self._MODE2, self._OUTDRV) + self._write_byte_data(self._MODE1, self._ALLCALL) + time.sleep(0.005) + + mode1 = self._read_byte_data(self._MODE1) + mode1 = mode1 & ~self._SLEEP + self._write_byte_data(self._MODE1, mode1) + time.sleep(0.005) + self.frequency = 60 + + def _write_byte_data(self, reg, value): + '''Write data to I2C with self.address''' + if self._DEBUG: + print self._DEBUG_INFO, 'Writing value %2X to %2X' % (value, reg) + try: + self.bus.write_byte_data(self.address, reg, value) + except Exception, i: + print i + self._check_i2c() + + def _read_byte_data(self, reg): + '''Read data from I2C with self.address''' + if self._DEBUG: + print self._DEBUG_INFO, 'Reading value from %2X' % reg + try: + results = self.bus.read_byte_data(self.address, reg) + return results + except Exception, i: + print i + self._check_i2c() + + def _check_i2c(self): + import commands + bus_number = self._get_bus_number() + print "\nYour Pi Rivision is: %s" % self._get_pi_revision() + print "I2C bus number is: %s" % bus_number + print "Checking I2C device:" + cmd = "ls /dev/i2c-%d" % bus_number + output = commands.getoutput(cmd) + print 'Commands "%s" output:' % cmd + print output + if '/dev/i2c-%d' % bus_number in output.split(' '): + print "I2C device setup OK" + else: + print "Seems like I2C has not been set. Use 'sudo raspi-config' to set I2C" + cmd = "i2cdetect -y %s" % self.bus_number + output = commands.getoutput(cmd) + print "Your PCA9685 address is set to 0x%02X" % self.address + print "i2cdetect output:" + print output + outputs = output.split('\n')[1:] + addresses = [] + for tmp_addresses in outputs: + tmp_addresses = tmp_addresses.split(':')[1] + tmp_addresses = tmp_addresses.strip().split(' ') + for address in tmp_addresses: + if address != '--': + addresses.append(address) + print "Conneceted i2c device:" + if addresses == []: + print "None" + else: + for address in addresses: + print " 0x%s" % address + if "%02X" % self.address in addresses: + print "Wierd, I2C device is connected. Try to run the program again. If the problem's still, email the error message to service@sunfounder.com" + else: + print "Device is missing." + print "Check the address or wiring of PCA9685 servo driver, or email the error message to service@sunfounder.com" + print 'Exiting...' + quit() + + @property + def frequency(self): + return _frequency + + @frequency.setter + def frequency(self, freq): + '''Set PWM frequency''' + if self._DEBUG: + print self._DEBUG_INFO, 'Set frequency to %d' % freq + self._frequency = freq + prescale_value = 25000000.0 + prescale_value /= 4096.0 + prescale_value /= float(freq) + prescale_value -= 1.0 + if self._DEBUG: + print self._DEBUG_INFO, 'Setting PWM frequency to %d Hz' % freq + print self._DEBUG_INFO, 'Estimated pre-scale: %d' % prescale_value + prescale = math.floor(prescale_value + 0.5) + if self._DEBUG: + print self._DEBUG_INFO, 'Final pre-scale: %d' % prescale + + old_mode = self._read_byte_data(self._MODE1); + new_mode = (old_mode & 0x7F) | 0x10 + self._write_byte_data(self._MODE1, new_mode) + self._write_byte_data(self._PRESCALE, int(math.floor(prescale))) + self._write_byte_data(self._MODE1, old_mode) + time.sleep(0.005) + self._write_byte_data(self._MODE1, old_mode | 0x80) + + def write(self, channel, on, off): + '''Set on and off value on specific channel''' + if self._DEBUG: + print self._DEBUG_INFO, 'Set channel "%d" to value "%d"' % (channel, off) + self._write_byte_data(self._LED0_ON_L+4*channel, on & 0xFF) + self._write_byte_data(self._LED0_ON_H+4*channel, on >> 8) + self._write_byte_data(self._LED0_OFF_L+4*channel, off & 0xFF) + self._write_byte_data(self._LED0_OFF_H+4*channel, off >> 8) + + def write_all_value(self, on, off): + '''Set on and off value on all channel''' + if self._DEBUG: + print self._DEBUG_INFO, 'Set all channel to value "%d"' % (off) + self._write_byte_data(self._ALL_LED_ON_L, on & 0xFF) + self._write_byte_data(self._ALL_LED_ON_H, on >> 8) + self._write_byte_data(self._ALL_LED_OFF_L, off & 0xFF) + self._write_byte_data(self._ALL_LED_OFF_H, off >> 8) + + def map(self, x, in_min, in_max, out_min, out_max): + '''To map the value from arange to another''' + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + + @property + def debug(self): + return self._DEBUG + + @debug.setter + def debug(self, debug): + '''Set if debug information shows''' + if debug in (True, False): + self._DEBUG = debug + else: + raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug)) + + if self._DEBUG: + print self._DEBUG_INFO, "Set debug on" + else: + print self._DEBUG_INFO, "Set debug off" + +if __name__ == '__main__': + import time + + pwm = PWM() + pwm.frequency = 60 + for i in range(16): + time.sleep(0.5) + print '\nChannel %d\n' % i + time.sleep(0.5) + for j in range(4096): + pwm.write(i, 0, j) + print 'PWM value: %d' % j + time.sleep(0.0003) diff --git a/actuator-pca9685_l298n.py b/actuator-pca9685_l298n.py new file mode 100644 index 0000000..52b2c5a --- /dev/null +++ b/actuator-pca9685_l298n.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python +import RPi.GPIO as GPIO +import PCA9685 as p +import time # Import necessary modules + +def Map(x, in_min, in_max, out_min, out_max): + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + +# =========================================================================== +# Raspberry Pi pin11, 12, 13 and 15 to realize the clockwise/counterclockwise +# rotation and forward and backward movements +# =========================================================================== +Motor0_A = 11 # pin11 +Motor0_B = 12 # pin12 +Motor1_A = 13 # pin13 +Motor1_B = 15 # pin15 + +# =========================================================================== +# Set channel 4 and 5 of the servo driver IC to generate PWM, thus +# controlling the speed of the car +# =========================================================================== +EN_M0 = 4 # servo driver IC CH4 +EN_M1 = 5 # servo driver IC CH5 + +pins = [Motor0_A, Motor0_B, Motor1_A, Motor1_B] + +def setup(busnum=None): + global leftPWM, rightPWM, homePWM + leftPWM = 450 + homePWM = 750 + rightPWM = 1050 + offset =0 + leftPWM += offset + homePWM += offset + rightPWM += offset + + global forward0, forward1, backward1, backward0, pwm + if busnum == None: + pwm = p.PWM() # Initialize the servo controller. + else: + pwm = p.PWM(bus_number=busnum) # Initialize the servo controller. + + pwm.frequency = 60 + forward0 = 'True' + forward1 = 'True' + GPIO.setwarnings(False) + GPIO.setmode(GPIO.BOARD) # Number GPIOs by its physical location + + if forward0 == 'True': + backward0 = 'False' + elif forward0 == 'False': + backward0 = 'True' + if forward1 == 'True': + backward1 = 'False' + elif forward1 == 'False': + backward1 = 'True' + for pin in pins: + GPIO.setup(pin, GPIO.OUT) # Set all pins' mode as output + + +# =========================================================================== +# Control the DC motor to make it rotate clockwise, so the car will +# move forward. +# =========================================================================== + +def motor0(x): + if x == 'True': + GPIO.output(Motor0_A, GPIO.LOW) + GPIO.output(Motor0_B, GPIO.HIGH) + elif x == 'False': + GPIO.output(Motor0_A, GPIO.HIGH) + GPIO.output(Motor0_B, GPIO.LOW) + else: + print 'Config Error' + +def motor1(x): + if x == 'True': + GPIO.output(Motor1_A, GPIO.LOW) + GPIO.output(Motor1_B, GPIO.HIGH) + elif x == 'False': + GPIO.output(Motor1_A, GPIO.HIGH) + GPIO.output(Motor1_B, GPIO.LOW) + +# init +def init(default_speed=50): + setup() + set_speed(default_speed) + +# throttle +cur_speed = 0 +MAX_SPEED = 100 + +def set_speed(speed): + global cur_speed + speed = int(MAX_SPEED * speed / 100) + cur_speed = min(MAX_SPEED, speed) + speed *= 40 + print 'speed is: ', cur_speed + pwm.write(EN_M0, 0, speed) + pwm.write(EN_M1, 0, speed) + +def get_speed(): + return int(cur_speed * 100 / MAX_SPEED) + +def stop(): + for pin in pins: + GPIO.output(pin, GPIO.LOW) + +def ffw(spd = 100): + if spd > 0: + motor0(forward0) + motor1(forward1) + if spd < 0: + motor0(backward0) + motor1(backward1) + spd = -spd + set_speed(spd) + +def rew(spd = 100): + if spd > 0: + motor0(backward0) + motor1(backward1) + if spd < 0: + motor0(forward0) + motor1(forward1) + spd = -spd + set_speed(spd) + +#def ffw(spd = 50): +# set_speed(spd) +# motor0(forward0) +# motor1(forward1) + +#def rew(spd = 50): +# set_speed(spd) +# motor0(backward0) +# motor1(backward1) + +# steering +def center(): + turn(0) + +def left(): + turn(30) # CH0 + +def right(): + turn(-30) + +def turn(angle): + angle = Map(angle, 0, 255, leftPWM, rightPWM) + pwm.write(0, 0, angle) + +# exit +def turn_off(): + stop() + center() + +def calibrate(x): + pwm.write(0, 0, 450+x) + +def test(): + i=0 + while i<2: + i += 1 + ffw(-50) + time.sleep(3) + ffw(50) + time.sleep(3) + ffw(100) + turn(45) + rew(50) + time.sleep(3) + turn(0) + time.sleep(3) + ffw(100) + turn(-45) + time.sleep(3) + turn(0) + time.sleep(3) + + +if __name__ == '__main__': + setup() + test() + stop() diff --git a/params.py b/params.py index 23eaf75..a7c554b 100644 --- a/params.py +++ b/params.py @@ -12,10 +12,10 @@ ########################################################## # actuator selection -# "actuator-drv8835", "actuator-adafruit_hat" +# "actuator-drv8835", "actuator-adafruit_hat", "actuator-pca9685_l298n" # "actuator-null" ########################################################## -actuator="actuator-drv8835" +actuator="actuator-pca9685_l298n" ########################################################## # model selection @@ -58,3 +58,4 @@ epochs = OrderedDict() epochs['train'] = [1,3,5,7,9,11] epochs['val'] = [2,4,6,8,10] + diff --git a/picar-ps4-joy-common.py b/picar-ps4-joy-common.py new file mode 100644 index 0000000..ca58313 --- /dev/null +++ b/picar-ps4-joy-common.py @@ -0,0 +1,280 @@ +#!/usr/bin/python +import os +import time +import atexit +import cv2 +import math +import numpy as np +import sys +import params +import argparse + +import input_kbd + +import pygame +from pygame.locals import * + +endProg = False + +########################################################## +# Initialize the joystick components +########################################################## +pygame.init() +pygame.joystick.init() + +joystick_count = pygame.joystick.get_count() + +if joystick_count < 1: + print("No joysticks found.") + sys.exit() + +print("Found " + str(joystick_count) + " joysticks.") + +joy = pygame.joystick.Joystick(0) +joy.init() + +########################################################## +# import deeppicar's sensor/actuator modules +########################################################## +camera = __import__(params.camera) +actuator = __import__(params.actuator) + +########################################################## +# global variable initialization +########################################################## +use_dnn = False +use_thread = True +view_video = False + +cfg_cam_res = (320, 240) +cfg_cam_fps = 30 +cfg_throttle = 100 # 50% power. + +NCPU = 3 + +frame_id = 0 +angle = 0.0 +btn = ord('k') # center +period = 0.05 # sec (=50ms) + +########################################################## +# local functions +########################################################## +def deg2rad(deg): + return deg * math.pi / 180.0 +def rad2deg(rad): + return 180.0 * rad / math.pi + +def g_tick(): + t = time.time() + count = 0 + while True: + count += 1 + yield max(t + count*period - time.time(),0) + +def turn_off(): + actuator.stop() + camera.stop() + + keyfile.close() + keyfile_btn.close() + vidfile.release() + +########################################################## +# program begins +########################################################## +parser = argparse.ArgumentParser(description='DeepPicar main') +parser.add_argument("-d", "--dnn", help="Enable DNN", action="store_true") +parser.add_argument("-t", "--throttle", help="throttle percent. [0-100]%", type=int) +parser.add_argument("-n", "--ncpu", help="number of cores to use.", type=int) +args = parser.parse_args() + +if args.dnn: + print ("DNN is on") + use_dnn = True +if args.throttle: + cfg_throttle = args.throttle + print ("throttle = %d pct" % (args.throttle)) +if args.ncpu > 0: + NCPU = args.ncpu + +# create files for data recording +keyfile = open('out-key.csv', 'w+') +keyfile_btn = open('out-key-btn.csv', 'w+') +keyfile.write("ts_micro,frame,wheel\n") +keyfile_btn.write("ts_micro,frame,btn,speed\n") +rec_start_time = 0 +# fourcc = cv2.VideoWriter_fourcc(*'XVID') +fourcc = cv2.cv.CV_FOURCC(*'XVID') +vidfile = cv2.VideoWriter('out-video.avi', fourcc, + cfg_cam_fps, cfg_cam_res) + +# initlaize deeppicar modules +actuator.init(cfg_throttle) +camera.init(res=cfg_cam_res, fps=cfg_cam_fps, threading=use_thread) +atexit.register(turn_off) + +# initilize dnn model +if use_dnn == True: + print ("Load TF") + import tensorflow as tf + model = __import__(params.model) + import local_common as cm + import preprocess + + print ("Load Model") + config = tf.ConfigProto(intra_op_parallelism_threads=NCPU, + inter_op_parallelism_threads=NCPU, \ + allow_soft_placement=True, + device_count = {'CPU': 1}) + + sess = tf.InteractiveSession(config=config) + saver = tf.train.Saver() + model_load_path = cm.jn(params.save_dir, params.model_load_file) + saver.restore(sess, model_load_path) + print ("Done..") + +# null_frame = np.zeros((cfg_cam_res[0],cfg_cam_res[1],3), np.uint8) +# cv2.imshow('frame', null_frame) + +g = g_tick() +start_ts = time.time() + +# enter main loop +while not endProg: + try: + #timeStart = pygame.time.get_ticks() + + for event in pygame.event.get(): + if event.type == pygame.QUIT: + endProg = True + + keys = pygame.key.get_pressed() + + if keys[pygame.K_ESCAPE]: + endProg = True + + #throttle = round(joy.get_axis(1)*100, 0) + #degree = int(joy.get_axis(0)*50) + #angle = deg2rad(degree) + endProg = joy.get_button(0) + +# actuator.ffw(throttle) + + if use_thread: + time.sleep(next(g)) + frame = camera.read_frame() + ts = time.time() + + # read a frame + # ret, frame = cap.read() + + if view_video == True: + cv2.imshow('frame', frame) + ch = cv2.waitKey(1) & 0xFF + else: + ch = ord(input_kbd.read_single_keypress()) + + if ch == ord('j') or joy.get_button(4): + actuator.left() + print ("left") + angle = deg2rad(-30) + btn = ord('j') + elif ch == ord('k'): + actuator.center() + print ("center") + angle = deg2rad(0) + btn = ord('k') + elif ch == ord('l') or joy.get_button(5): + actuator.right() + print ("right") + angle = deg2rad(30) + btn = ord('l') + elif ch == ord('a') or joy.get_button(2): + actuator.ffw(cfg_throttle) + print ("accel") + elif ch == ord('s') or joy.get_button(3): + actuator.stop() + print ("stop") + btn = ch + elif ch == ord('z') or joy.get_button(9): + actuator.rew(cfg_throttle) + print ("reverse") + elif ch == ord('q') or joy.get_button(0): + endProg = True + elif ch == ord('r') or joy.get_button(1): + print ("toggle record mode") + if rec_start_time == 0: + rec_start_time = ts + else: + rec_start_time = 0 + elif ch == ord('t') or joy.get_button(8): + print ("toggle video mode") + if view_video == False: + view_video = True + else: + view_video = False + cv2.destroyAllWindows() + elif ch == ord('d') or joy.get_button(10): + print ("toggle DNN mode") + if use_dnn == False: + use_dnn = True + else: + use_dnn = False + + if use_dnn == True: + # 1. machine input + img = preprocess.preprocess(frame) + angle = model.y.eval(feed_dict={model.x: [img]})[0][0] + + degree = int(rad2deg(angle)) + if degree < 1 and degree > -1: + degree = 0 + elif degree >= 50: + degree = 50 + elif degree <= -50: + degree = -50 + + actuator.turn(degree) + #print degree + + dur = time.time() - ts + if dur > period: + print("%.3f: took %d ms - deadline miss." + % (ts - start_ts, int(dur * 1000))) + else: + print("%.3f: took %d ms" % (ts - start_ts, int(dur * 1000))) + + if rec_start_time > 0: + # increase frame_id + frame_id += 1 + + # write input (angle) + str = "{},{},{}\n".format(int(ts*1000), frame_id, angle) + keyfile.write(str) + + # write input (button: left, center, stop, speed) + str = "{},{},{},{}\n".format(int(ts*1000), frame_id, btn, cfg_throttle) + keyfile_btn.write(str) + + # write video stream + vidfile.write(frame) + + if frame_id >= 1000: + print ("recorded 1000 frames") + break + + print ("%.3f %d %.3f %d %d(ms)" % + (ts, frame_id, angle, btn, int((time.time() - ts)*1000))) + + #timeEnd = pygame.time.get_ticks() + #deltaTime = timeEnd - timeStart + #pygame.time.delay(max(0, 50 - deltaTime)) + + except KeyboardInterrupt: + endProg = True + break + +pygame.quit() +turn_off() +print ("Finish..")