diff --git a/Adafruit_I2C.py b/Adafruit_I2C.py new file mode 100755 index 0000000..2b24b67 --- /dev/null +++ b/Adafruit_I2C.py @@ -0,0 +1,161 @@ +#!/usr/bin/python +import re +import smbus + +# =========================================================================== +# Adafruit_I2C Class +# =========================================================================== + +class Adafruit_I2C(object): + + @staticmethod + def getPiRevision(): + "Gets the version number of the Raspberry Pi board" + # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History + try: + with open('/proc/cpuinfo', 'r') as infile: + for line in infile: + # Match a line of the form "Revision : 0002" while ignoring extra + # info in front of the revsion (like 1000 when the Pi was over-volted). + match = re.match('Revision\s+:\s+.*(\w{4})$', line) + if match and match.group(1) in ['0000', '0002', '0003']: + # Return revision 1 if revision ends with 0000, 0002 or 0003. + return 1 + elif match: + # Assume revision 2 if revision ends with any other 4 chars. + return 2 + # Couldn't find the revision, assume revision 0 like older code for compatibility. + return 0 + except: + return 0 + + @staticmethod + def getPiI2CBusNumber(): + # Gets the I2C bus number /dev/i2c# + return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 + + def __init__(self, address, busnum=-1, debug=False): + self.address = address + # By default, the correct I2C bus is auto-detected using /proc/cpuinfo + # Alternatively, you can hard-code the bus version below: + # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) + # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) + self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber()) + self.debug = debug + + def reverseByteOrder(self, data): + "Reverses the byte order of an int (16-bit) or long (32-bit) value" + # Courtesy Vishal Sapre + byteCount = len(hex(data)[2:].replace('L','')[::2]) + val = 0 + for i in range(byteCount): + val = (val << 8) | (data & 0xff) + data >>= 8 + return val + + def errMsg(self): + print "Error accessing 0x%02X: Check your I2C address" % self.address + return -1 + + def write8(self, reg, value): + "Writes an 8-bit value to the specified register/address" + try: + self.bus.write_byte_data(self.address, reg, value) + if self.debug: + print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) + except IOError, err: + return self.errMsg() + + def write16(self, reg, value): + "Writes a 16-bit value to the specified register/address pair" + try: + self.bus.write_word_data(self.address, reg, value) + if self.debug: + print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % + (value, reg, reg+1)) + except IOError, err: + return self.errMsg() + + def writeRaw8(self, value): + "Writes an 8-bit value on the bus" + try: + self.bus.write_byte(self.address, value) + if self.debug: + print "I2C: Wrote 0x%02X" % value + except IOError, err: + return self.errMsg() + + def writeList(self, reg, list): + "Writes an array of bytes using I2C format" + try: + if self.debug: + print "I2C: Writing list to register 0x%02X:" % reg + print list + self.bus.write_i2c_block_data(self.address, reg, list) + except IOError, err: + return self.errMsg() + + def readList(self, reg, length): + "Read a list of bytes from the I2C device" + try: + results = self.bus.read_i2c_block_data(self.address, reg, length) + if self.debug: + print ("I2C: Device 0x%02X returned the following from reg 0x%02X" % + (self.address, reg)) + print results + return results + except IOError, err: + return self.errMsg() + + def readU8(self, reg): + "Read an unsigned byte from the I2C device" + try: + result = self.bus.read_byte_data(self.address, reg) + if self.debug: + print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % + (self.address, result & 0xFF, reg)) + return result + except IOError, err: + return self.errMsg() + + def readS8(self, reg): + "Reads a signed byte from the I2C device" + try: + result = self.bus.read_byte_data(self.address, reg) + if result > 127: result -= 256 + if self.debug: + print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % + (self.address, result & 0xFF, reg)) + return result + except IOError, err: + return self.errMsg() + + def readU16(self, reg, little_endian=True): + "Reads an unsigned 16-bit value from the I2C device" + try: + result = self.bus.read_word_data(self.address,reg) + # Swap bytes if using big endian because read_word_data assumes little + # endian on ARM (little endian) systems. + if not little_endian: + result = ((result << 8) & 0xFF00) + (result >> 8) + if (self.debug): + print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) + return result + except IOError, err: + return self.errMsg() + + def readS16(self, reg, little_endian=True): + "Reads a signed 16-bit value from the I2C device" + try: + result = self.readU16(reg,little_endian) + if result > 32767: result -= 65536 + return result + except IOError, err: + return self.errMsg() + +if __name__ == '__main__': + try: + bus = Adafruit_I2C(address=0) + print "Default I2C bus is accessible" + except: + print "Error accessing default I2C bus" diff --git a/Adafruit_PWM_Servo_Driver.py b/Adafruit_PWM_Servo_Driver.py new file mode 100644 index 0000000..35c993c --- /dev/null +++ b/Adafruit_PWM_Servo_Driver.py @@ -0,0 +1,92 @@ +#!/usr/bin/python + +import time +import math +from Adafruit_I2C import Adafruit_I2C + +# ============================================================================ +# Adafruit PCA9685 16-Channel PWM Servo Driver +# ============================================================================ + +class PWM : + # Registers/etc. + __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 + + # Bits + __RESTART = 0x80 + __SLEEP = 0x10 + __ALLCALL = 0x01 + __INVRT = 0x10 + __OUTDRV = 0x04 + + general_call_i2c = Adafruit_I2C(0x00) + + @classmethod + def softwareReset(cls): + "Sends a software reset (SWRST) command to all the servo drivers on the bus" + cls.general_call_i2c.writeRaw8(0x06) # SWRST + + def __init__(self, address=0x40, debug=False): + self.i2c = Adafruit_I2C(address) + self.i2c.debug = debug + self.address = address + self.debug = debug + if (self.debug): + print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2" + self.setAllPWM(0, 0) + self.i2c.write8(self.__MODE2, self.__OUTDRV) + self.i2c.write8(self.__MODE1, self.__ALLCALL) + time.sleep(0.005) # wait for oscillator + + mode1 = self.i2c.readU8(self.__MODE1) + mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep) + self.i2c.write8(self.__MODE1, mode1) + time.sleep(0.005) # wait for oscillator + + def setPWMFreq(self, freq): + "Sets the PWM frequency" + prescaleval = 25000000.0 # 25MHz + prescaleval /= 4096.0 # 12-bit + prescaleval /= float(freq) + prescaleval -= 1.0 + if (self.debug): + print "Setting PWM frequency to %d Hz" % freq + print "Estimated pre-scale: %d" % prescaleval + prescale = math.floor(prescaleval + 0.5) + if (self.debug): + print "Final pre-scale: %d" % prescale + + oldmode = self.i2c.readU8(self.__MODE1); + newmode = (oldmode & 0x7F) | 0x10 # sleep + self.i2c.write8(self.__MODE1, newmode) # go to sleep + self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) + self.i2c.write8(self.__MODE1, oldmode) + time.sleep(0.005) + self.i2c.write8(self.__MODE1, oldmode | 0x80) + + def setPWM(self, channel, on, off): + "Sets a single PWM channel" + self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) + self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) + self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) + self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) + + def setAllPWM(self, on, off): + "Sets a all PWM channels" + self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF) + self.i2c.write8(self.__ALL_LED_ON_H, on >> 8) + self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF) + self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8) diff --git a/DCTest.py b/DCTest.py deleted file mode 100644 index bd3b3d7..0000000 --- a/DCTest.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/python -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor -import sys -import time -import atexit - -# create a default object, no changes to I2C address or frequency -mh = Adafruit_MotorHAT(addr=0x60) - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - -atexit.register(turnOffMotors) - -################################# DC motor test! -print int(sys.argv[1]) -myMotor = mh.getMotor(int(sys.argv[1])) - -# set the speed to start, from 0 (off) to 255 (max speed) -myMotor.setSpeed(150) -myMotor.run(Adafruit_MotorHAT.FORWARD); -# turn on motor -myMotor.run(Adafruit_MotorHAT.RELEASE); - - -while (True): - print "Forward! " - myMotor.run(Adafruit_MotorHAT.FORWARD) - - print "\tSpeed up..." - for i in range(255): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "\tSlow down..." - for i in reversed(range(255)): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "Backward! " - myMotor.run(Adafruit_MotorHAT.BACKWARD) - - print "\tSpeed up..." - for i in range(255): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "\tSlow down..." - for i in reversed(range(255)): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "Release" - myMotor.run(Adafruit_MotorHAT.RELEASE) - time.sleep(1.0) diff --git a/README.md b/README.md index 5a863e4..76712bf 100644 --- a/README.md +++ b/README.md @@ -1 +1,176 @@ -# robot \ No newline at end of file +# robot + +This will run a simple robot with a webserver on a raspberry PI with the Adafruit Motor Hat. I wrote this up for myself for fun and to help me remember how I set things up. + +High level overview can be found in this article: https://www.oreilly.com/learning/how-to-build-a-robot-that-sees-with-100-and-tensorflow + +## Hardware + +- Raspberry PI 3 +- 16GB (or larger) SIM Card +- Adafruit Motor Hat (for wheels) +- Any chassis with DC motors - for example: https://www.amazon.com/Emgreat-Chassis-Encoder-wheels-Battery/dp/B00GLO5SMY/ref=sr_1_2?ie=UTF8&qid=1486959207&sr=8-2&keywords=robot+chassis +- Adafruit Servo Hat (for arms) +- HC-SR04 sonars +- Any stepper motor arm - for example: SainSmart DIY Control Palletizing Robot Arm for the arm (https://www.amazon.com/dp/B0179BTLZ2/ref=twister_B00YTW763Y?_encoding=UTF8&psc=1) +- Raspberry PI compatible camera - for example: https://www.amazon.com/Raspberry-Pi-Camera-Module-Megapixel/dp/B01ER2SKFS/ref=sr_1_1?s=electronics&ie=UTF8&qid=1486960149&sr=1-1&keywords=raspberry+pi+camera + +To get started, you should be able to make the robot work without the arm, sonar and servo hat. + +## Programs + +- robot.py program will run commands from the commandline +- sonar.py tests sonar wired into GPIO ports +- wheels.py tests simple DC motor wheels +- arm.py tests a servo controlled robot arm +- autonomous.py implements a simple driving algorithm using the wheels and sonal +- inception_server.py runs an image classifying microservice + +## Example Robots + +Here are two robots I made that use this software + +![Robots](https://joyfulgrit.files.wordpress.com/2013/10/img_0183.jpg?w=700) + +## Wiring The Robot +### Sonar + +If you want to use the default sonar configuation, wire like this: + +- Left sonar trigger GPIO pin 23 echo 24 +- Center sonar trigger GPIO pin 17 echo 18 +- Right sonar trigger GPIO pin 22 echo 27 + +You can modify the pins by making a robot.conf file. + +### Wheels + +You can easily change this but this is what wheels.py expects + +- M1 - Front Left +- M2 - Back Left (optional - leave unwired for 2wd chassis) +- M3 - Back Right (optional - leave unwired for 2wd chassis) +- M4 - Front Right + + +## Installation + +### basic setup + +There are a ton of articles on how to do basic setup of a Raspberry PI - one good one is here https://www.howtoforge.com/tutorial/howto-install-raspbian-on-raspberry-pi/ + +You will need to turn on i2c and optionally the camera + +``` +raspi-config +``` + +Next you will need to download i2c tools and smbus + +``` +sudo apt-get install i2c-tools python-smbus python3-smbus +``` + +Test that your hat is attached and visible with + +``` +i2cdetect -y 1 +``` + +Install this code + +``` +sudo apt-get install git +git clone https://github.com/lukas/robot.git +cd robot +``` + +Install dependencies + +``` +pip install -r requirements.txt +``` + +At this point you should be able to drive your robot locally, try: + +``` +./robot.py forward +``` + +### server + +To run a webserver in the background with a camera you need to setup gunicorn and nginx + +#### nginx + +Nginx is a lightway fast reverse proxy - we store the camera image in RAM and serve it up directly. This was the only way I was able to get any kind of decent fps from the raspberry pi camera. We also need to proxy to gunicorn so that the user can control the robot from a webpage. + +copy the configuration file from nginx/nginx.conf to /etc/nginx/nginx.conf + +``` +sudo apt-get install nginx +sudo cp nginx/nginx.conf /etc/nginx/nginx.conf +``` + +restart nginx + +``` +sudo nginx -s reload +``` + +#### gunicorn + +install gunicorn + + +copy configuration file from services/web.service /etc/systemd/system/web.service + +``` +sudo cp services/web.service /etc/systemd/system/web.service +``` + +start gunicorn web app service + +``` +sudo systemctl daemon-reload +sudo systemctl enable web +sudo systemctl start web +``` + +Your webservice should be started now. You can try driving your robot with buttons or arrow keys + +#### camera + +In order to stream from the camera you can use RPi-cam. It's documented at http://elinux.org/RPi-Cam-Web-Interface but you can also just run the following + +``` +git clone https://github.com/silvanmelchior/RPi_Cam_Web_Interface.git +cd RPi_Cam_Web_Interface +chmod u+x *.sh +./install.sh +``` + +Now a stream of images from the camera should be constantly updating the file at /dev/shm/mjpeg. Nginx will serve up the image directly if you request localhost/cam.jpg. + +#### tensorflow + +There is a great project at https://github.com/samjabrahams/tensorflow-on-raspberry-pi that gives instructions on installing tensorflow on the Raspberry PI. Recently it's gotten much easier, just do + +``` +wget https://github.com/samjabrahams/tensorflow-on-raspberry-pi/releases/download/v0.11.0/tensorflow-0.11.0-cp27-none-linux_armv7l.whl +sudo pip install tensorflow-0.11.0-cp27-none-linux_armv7l.whl +``` + +Next start a tensorflow service that loads up an inception model and does object recognition the the inception model + +``` +sudo cp services/inception.service /etc/systemd/system/inception.service +sudo systemctl daemon-reload +sudo systemctl enable inception +sudo systemctl start inception +``` + + + + + diff --git a/arm.py b/arm.py new file mode 100644 index 0000000..39be508 --- /dev/null +++ b/arm.py @@ -0,0 +1,48 @@ +#!/usr/bin/python + + +from flask import Flask, render_template, request, Response, send_file + +import os +import atexit + +from Adafruit_PWM_Servo_Driver import PWM +import time + +pwm = PWM(0x40, debug=True) + +servoMin = 150 # Min pulse length out of 4096 +servoMax = 600 # Max pulse length out of 4096 + +pwm.setPWMFreq(60) # Set frequency to 60 Hz + +app = Flask(__name__) + +def setServoPulse(channel, pulse): + pulseLength = 1000000 # 1,000,000 us per second + pulseLength /= 60 # 60 Hz + print "%d us per period" % pulseLength + pulseLength /= 4096 # 12 bits of resolution + print "%d us per bit" % pulseLength + pulse *= 1000 + pulse /= pulseLength + pwm.setPWM(channel, 0, pulse) + + +@app.route("/") +def main(): + return render_template('arm.html') + +@app.route('/slider') +def slider(): + num = int(request.args.get('num')) + value = int(request.args.get('value')) + print("%i %i" % (num, value)) + fraction = int(servoMin + (value/100.0)*(servoMax - servoMin)) + print("Setting servo to %i" % fraction) + pwm.setPWM(num, 0, fraction) + return "" + + +if __name__ == "__main__": + app.run() diff --git a/autonomous.py b/autonomous.py new file mode 100644 index 0000000..9d90f18 --- /dev/null +++ b/autonomous.py @@ -0,0 +1,61 @@ + +import wheels +import sonar +import time + + + + + +FORWARD=1 +LEFT=2 +RIGHT=3 +BACKWARD=4 + +def autodrive(dur): + start_time = time.time() + end_time = time.time() + dur + + mode = FORWARD + + wheels.forward(-150) + + + while(time.time() < end_time): + time.sleep(0.1) + cdist = sonar.cdist() + ldist = sonar.ldist() + rdist= sonar.rdist() + + print ("%d %d %d" % (ldist, cdist, rdist)) + + if (mode == FORWARD): + if (cdist < 35 or ldist <6 or rdist < 6): + print ("turning") + wheels.stop() + if (ldist < rdist): + mode=RIGHT + wheels.backward(-100) + time.sleep(1) + wheels.right(-250) + time.sleep(1) + else: + mode=LEFT + wheels.backward(-100) + time.sleep(1) + wheels.left(-250) + time.sleep(1) + + if (mode==LEFT or mode==RIGHT): + if (cdist > 50): + mode=FORWARD + wheels.forward(-150) + + + + wheels.stop() + + + +if (__name__ == '__main__'): + autodrive(10) diff --git a/configure.py b/configure.py new file mode 100644 index 0000000..32a4986 --- /dev/null +++ b/configure.py @@ -0,0 +1,28 @@ +import json +import os.path + +data={} + +# Default configuration +# If you want to change make a json file that looks like +# { +# "LTRIG":"17", +# "CTRIG":"23", +# "RTRIG":"22", +# "LECHO":"18", +# "CECHO":"24", +# "RECHO":"27" +# } + +data["LTRIG"] = 23 # default pin for left sonar trigger +data["CTRIG"] = 17 # default pin for center sonar trigger +data["RTRIG"] = 22 # default pin for right sonar trigger +data["LECHO"] = 24 # default pin for left sonar echo +data["CECHO"] = 18 # default pin for center sonar echo +data["RECHO"] = 27 # default pin for right sonar echo + +if os.path.isfile('robot.conf'): + with open('robot.conf') as data_file: + data = json.load(data_file) +else: + print("Couldn't find robot.conf file, using default configuration") diff --git a/drive.py b/drive.py deleted file mode 100644 index d047972..0000000 --- a/drive.py +++ /dev/null @@ -1,250 +0,0 @@ -#!/usr/bin/python -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor - -from flask import Flask, render_template, request, Response, send_file - -from camera_pi import Camera - -app = Flask(__name__) - - -import os -import time -import atexit - -# create a default object, no changes to I2C address or frequency -mh = Adafruit_MotorHAT(addr=0x60) - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - -atexit.register(turnOffMotors) - -################################# DC motor test! -mFL = mh.getMotor(1) -mBL = mh.getMotor(2) -mBR = mh.getMotor(3) -mFR = mh.getMotor(4) - -def wakeup(m): - # set the speed to start, from 0 (off) to 255 (max speed) - m.setSpeed(150) - m.run(Adafruit_MotorHAT.FORWARD); - # turn on motor - m.run(Adafruit_MotorHAT.RELEASE); - - -wakeup(mFL) -wakeup(mBL) -wakeup(mFR) -wakeup(mBL) - - - - -def gof(): - mBR.run(Adafruit_MotorHAT.FORWARD) - mBL.run(Adafruit_MotorHAT.FORWARD) - mFL.run(Adafruit_MotorHAT.FORWARD) - mFR.run(Adafruit_MotorHAT.FORWARD) - mBR.setSpeed(200) - mBL.setSpeed(200) - mFR.setSpeed(200) - mFL.setSpeed(200) - -def gob(): - mBR.run(Adafruit_MotorHAT.BACKWARD) - mBL.run(Adafruit_MotorHAT.BACKWARD) - mFR.run(Adafruit_MotorHAT.BACKWARD) - mFL.run(Adafruit_MotorHAT.BACKWARD) - mBR.setSpeed(200) - mBL.setSpeed(200) - mFR.setSpeed(200) - mFL.setSpeed(200) - -def gol(): - mBR.run(Adafruit_MotorHAT.BACKWARD) - mBL.run(Adafruit_MotorHAT.FORWARD) - mFR.run(Adafruit_MotorHAT.BACKWARD) - mFL.run(Adafruit_MotorHAT.FORWARD) - mBR.setSpeed(200) - mBL.setSpeed(200) - mFR.setSpeed(200) - mFL.setSpeed(200) - -def gor(): - mBR.run(Adafruit_MotorHAT.FORWARD) - mBL.run(Adafruit_MotorHAT.BACKWARD) - mFR.run(Adafruit_MotorHAT.FORWARD) - mFL.run(Adafruit_MotorHAT.BACKWARD) - mBR.setSpeed(200) - mBL.setSpeed(200) - mFR.setSpeed(200) - mFL.setSpeed(200) - -def stop(): - mFL.run(Adafruit_MotorHAT.RELEASE) - mFR.run(Adafruit_MotorHAT.RELEASE) - mBL.run(Adafruit_MotorHAT.RELEASE) - mBR.run(Adafruit_MotorHAT.RELEASE) - - - -def forward(speed, dur): - print "Forward! " - mR.run(Adafruit_MotorHAT.FORWARD) - mL.run(Adafruit_MotorHAT.FORWARD) - mR.setSpeed(speed) - mL.setSpeed(speed) - time.sleep(dur) - - mL.run(Adafruit_MotorHAT.RELEASE) - mR.run(Adafruit_MotorHAT.RELEASE) - return '' - -def backward(speed, dur): - print "Backward! " - mR.run(Adafruit_MotorHAT.BACKWARD) - mL.run(Adafruit_MotorHAT.BACKWARD) - mR.setSpeed(speed) - mL.setSpeed(speed) - time.sleep(dur) - - mL.run(Adafruit_MotorHAT.RELEASE) - mR.run(Adafruit_MotorHAT.RELEASE) - return '' - -def left(speed, dur): - print "Left " - mR.run(Adafruit_MotorHAT.BACKWARD) - mR.setSpeed(speed) - - time.sleep(dur) - mR.run(Adafruit_MotorHAT.RELEASE) - return '' - -def right(speed, dur): - print "Right " - mL.run(Adafruit_MotorHAT.BACKWARD) - mL.setSpeed(speed) - - time.sleep(dur) - mL.run(Adafruit_MotorHAT.RELEASE) - return '' - - - - -#forward(200, 3) -#backward(200, 3) - - -@app.route("/") -def main(): - return render_template('index.html') - -@app.route('/forward') -def fward(): - forward(200, 3) - return '' - -@app.route('/backward') -def bward(): - backward(200, 3) - return '' - -@app.route('/left') -def l(): - left(200, 3) - return '' - -@app.route('/forward') -def r(): - right(200, 3) - return '' - -@app.route('/f') -def gof_r(): - gof() - return '' - -@app.route('/b') -def gob_r(): - gob() - return '' - -@app.route('/r') -def gor_r(): - gor() - return '' - -@app.route('/l') -def gol_r(): - gol() - return '' - -@app.route('/stop') -def stop_r(): - stop() - return '' - -@app.route('/setSpeed') -def setSpeed(): - print 'gotParams'; - print request.query_string - print request.args - print request.args.getlist('name[]') - # print request.form.get('left',1,type=int) - - #print request.form.get('right',1,type=int) - print request.args['right'] - print request.args['left'] - - return '' - -@app.route('/latest.jpg') -def latest(): -# img_num = request.args.get('i') -# if img_num is None: - filename = 'images/latest_img.jpg' -# else: -# filename = 'images/img'+img_num+'.jpg' - - return send_file(filename, mimetype='image/jpg') - -@app.route('/data') -def data(): - img_num = request.args.get('i') - if img_num is None: - filename = 'images/latest_data' - else: - filename = 'images/data'+img_num - - f = open(filename, 'r') - data = f.read() - return data - -@app.route('/img_rec') -def img_rec(): - os.system('python image.py') - -def gen(camera): - """Video streaming generator function.""" - while True: - frame = camera.get_frame() - yield (b'--frame\r\n' - b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n') - time.sleep(0.5) - -@app.route('/video_feed') -def video_feed(): - """Video streaming route. Put this in the src attribute of an img tag.""" - return Response(gen(Camera()), - mimetype='multipart/x-mixed-replace; boundary=frame') - -if __name__ == "__main__": - app.run() diff --git a/drive_safe.py b/drive_safe.py deleted file mode 100644 index 4286fca..0000000 --- a/drive_safe.py +++ /dev/null @@ -1,238 +0,0 @@ -import subprocess - -def do(cmd): - print(cmd) - p =subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) - -from random import random - -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor - -from flask import Flask, render_template, request, Response, send_file - -from camera_pi import Camera - -import RPi.GPIO as GPIO - -import time - -GPIO.setmode(GPIO.BCM) - -TRIG = [23, 17, 22] -ECHO = [24, 18, 27] - - -def setup(): - for i in range(3): - GPIO.setup(TRIG[i],GPIO.OUT) - GPIO.setup(ECHO[i],GPIO.IN) - GPIO.output(TRIG[i], False) - - print "Waiting For Sensor To Settle" - - -def distance(i): -# print "Distance Measurement In Progress" - - GPIO.output(TRIG[i], True) - - time.sleep(0.00001) - - GPIO.output(TRIG[i], False) - - pulse_end = 0; - pulse_start = 0; - - while GPIO.input(ECHO[i])==0: - pulse_start = time.time() - - while GPIO.input(ECHO[i])==1: - pulse_end = time.time() - - if (pulse_end == 0 or pulse_start==0): - return 1000 - - pulse_duration = pulse_end - pulse_start - - distance = pulse_duration * 17150 - - distance = round(distance, 2) - - # print "Distance:",distance,"cm" - - return distance - - - - -import os -import time -import atexit - -# create a default object, no changes to I2C address or frequency -mh = Adafruit_MotorHAT(addr=0x60) - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - GPIO.cleanup() - -atexit.register(turnOffMotors) - -################################# DC motor test! -mFL = mh.getMotor(1) -mBL = mh.getMotor(2) -mBR = mh.getMotor(3) -mFR = mh.getMotor(4) - -def wakeup(m): - # set the speed to start, from 0 (off) to 255 (max speed) - m.setSpeed(150) - m.run(Adafruit_MotorHAT.FORWARD); - # turn on motor - m.run(Adafruit_MotorHAT.RELEASE); - - -wakeup(mFL) -wakeup(mBL) -wakeup(mFR) -wakeup(mBL) -setup() - -def gof(): - mBR.run(Adafruit_MotorHAT.BACKWARD) - mBL.run(Adafruit_MotorHAT.BACKWARD) - mFL.run(Adafruit_MotorHAT.BACKWARD) - mFR.run(Adafruit_MotorHAT.BACKWARD) - mBR.setSpeed(200) - mBL.setSpeed(200) - mFR.setSpeed(200) - mFL.setSpeed(200) - -def setSpeed(speed): - mBR.setSpeed(speed) - mBL.setSpeed(speed) - mFR.setSpeed(speed) - mFL.setSpeed(speed) - -def backward(speed, dur): - print "Backward! " - mFR.run(Adafruit_MotorHAT.FORWARD) - mFL.run(Adafruit_MotorHAT.FORWARD) - mFR.setSpeed(speed) - mFL.setSpeed(speed) - time.sleep(dur) - - mFL.run(Adafruit_MotorHAT.RELEASE) - mFR.run(Adafruit_MotorHAT.RELEASE) - return '' - -def stop(): - mFL.run(Adafruit_MotorHAT.RELEASE) - mFR.run(Adafruit_MotorHAT.RELEASE) - mBL.run(Adafruit_MotorHAT.RELEASE) - mBR.run(Adafruit_MotorHAT.RELEASE) - -def left(speed, dur): - print "Left " - mFR.run(Adafruit_MotorHAT.BACKWARD) - mFR.setSpeed(speed) - - time.sleep(dur) - mFR.run(Adafruit_MotorHAT.RELEASE) - return '' - -def right(speed, dur): - print "Right " - mFL.run(Adafruit_MotorHAT.BACKWARD) - mFL.setSpeed(speed) - - time.sleep(dur) - mFL.run(Adafruit_MotorHAT.RELEASE) - return '' - - -def getAttention(): - with open('../python-mindave-mobile/ATTENTION', 'r') as f: - read_data = f.read() - print("Read Speed: " + read_data) - - spd=0 - if read_data == '': - spd = 0 - else: - spd = int(read_data) - - if (spd < 50): - newSpd = 0 - else: - newSpd = (spd-50)*4 - - return newSpd - -stopped = True; -turning = False; -THRESH = 25; -turnCount = 0; -maxTurnCount = 3; - -while(1==1): - print("driving!") - -# while True: -# s = getAttention(); -# print "Speed "+str(s) -# if s > 0: -# break -# setSpeed(0) -# time.sleep(0.2) - - mind = 1000 - d=[] - - for i in range(3): - d.append( distance(i)); - if d[i] < mind: - mind = d[i] - print(d[i]); - - print(d) - print("Min d " + str(mind)) - - if (mind<6 and stopped==True and turning==True): - do('echo "backing up." | flite ') - backward(255, 1) - elif (turnCount > maxTurnCount): - do('echo "backing up." | flite ') - backward(255, 1); - turnCount = 0; - elif (mind>=THRESH and stopped==True): - stopped=False; - turning = False; - gof(); - turnCount = 0 - elif (mind d[0]: - do('echo "turning left." | flite ') - left(255, 0.5); - else: - do('echo "turning right." | flite ') - right(255, 0.5); - - time.sleep(0.3); - stopped=True -# elif (turning==False): # robot is moving happily - #newSpd = getAttention(); - #setSpeed(newSpd); diff --git a/drive_server.py b/drive_server.py new file mode 100644 index 0000000..0bdb094 --- /dev/null +++ b/drive_server.py @@ -0,0 +1,124 @@ +#!/usr/bin/python + +from flask import Flask, render_template, request, Response, send_file +from camera_pi import Camera +import wheels +import speaker +import autonomous +import os + +app = Flask(__name__) + +@app.route("/") +def main(): + return render_template('index.html') + +@app.route('/forward') +def forward(): + wheels.forward(200, 3) + return '' + +@app.route('/backward') +def backward(): + wheels.backward(200, 3) + return '' + + +@app.route('/shake') +def shake(): + wheels.left(200, 0.2) + wheels.right(200, 0.2) + return '' + +@app.route('/left') +def left(): + wheels.left(200, 3) + return '' + +@app.route('/right') +def right(): + wheels.right(200, 3) + return '' + +@app.route('/f') +def f(): + wheels.forward(200) + return '' + +@app.route('/b') +def b(): + wheels.backward(200) + return '' + +@app.route('/r') +def r(): + wheels.right(200) + return '' + +@app.route('/l') +def l(): + wheels.left(200) + return '' + +@app.route('/stop') +def stop(): + wheels.stop() + return '' + +@app.route('/latest.jpg') +def latest(): + filename = 'images/latest_img.jpg' + return send_file(filename, mimetype='image/jpg') + + +@app.route('/drive') +def drive(): + time = 10 + if 'time' in request.args: + time = int(request.args.get('time')) + + + autonomous.autodrive(time) + + return '' + +@app.route('/say') +def say(): + text = request.args.get('text') + speaker.say(text) + return '' + +@app.route('/data') +def data(): + img_num = request.args.get('i') + if img_num is None: + filename = 'images/latest_data' + else: + filename = 'images/data'+img_num + + f = open(filename, 'r') + data = f.read() + return data + +@app.route('/img_rec') +def img_rec(): + wheels.stop() +# os.system('python image.py') + return '' + +def gen(camera): + """Video streaming generator function.""" + while True: + frame = camera.get_frame() + yield (b'--frame\r\n' + b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n') + time.sleep(0.5) + +@app.route('/video_feed') +def video_feed(): + """Video streaming route. Put this in the src attribute of an img tag.""" + return Response(gen(Camera()), + mimetype='multipart/x-mixed-replace; boundary=frame') + +if __name__ == "__main__": + app.run() diff --git a/gunicorn/gunicorn.service b/gunicorn/gunicorn.service deleted file mode 100644 index 897b5c5..0000000 --- a/gunicorn/gunicorn.service +++ /dev/null @@ -1,14 +0,0 @@ -#goes in /etc/systemd/system/gunicorn.service - -[Unit] -Description=gunicorn daemon -After=network.target - -[Service] -User=pi -Group=www-data -WorkingDirectory=/home/pi/robot -ExecStart=/usr/local/bin/gunicorn --workers 3 --bind unix:/home/pi/drive.sock drive:app - -[Install] -WantedBy=multi-user.target diff --git a/image.py b/image.py index 90b8b5e..bef0cce 100644 --- a/image.py +++ b/image.py @@ -17,14 +17,17 @@ def classify(n): dataFile = 'images/data'+suffix latestImage = 'images/latest_img.jpg' latestData = 'images/latest_data' - do('echo "I\'m thinking." | flite') + do('echo "I\'m thinking." | flite -voice slt') do('cp /dev/shm/mjpeg/cam.jpg '+imageFile); do('ln -f '+imageFile+' '+latestImage); + do('echo "thinking" > ' + dataFile); + do('ln -f '+dataFile+' '+latestData); do('bash run_and_parse_inception.sh '+imageFile+ " " +dataFile) - do('ln -f '+dataFile+' '+latestData); - do('{ echo "I think I see a "; cat '+dataFile+' | sed -e \'$ ! s/$/. or maybe a/\'; } | flite') + + + do('{ echo "I think I see ah "; head -1 '+dataFile+' | sed -e \'$ ! s/$/. or maybe a/\'; } | flite -voice slt') do('echo '+suffix+' > images/INDEX') diff --git a/inception_server.py b/inception_server.py new file mode 100644 index 0000000..50916fb --- /dev/null +++ b/inception_server.py @@ -0,0 +1,226 @@ +# Copyright 2015 Google Inc. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== + +"""Simple image classification server that runs the inception model Inception. + +Run image classification with Inception trained on ImageNet 2012 Challenge data +set. + +This is modified from the classify_image_timed.py script in the tensorflow-on-rasberry-pi +github repository +""" + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import os.path +import re +import sys +import tarfile + +import numpy as np +from six.moves import urllib +import tensorflow as tf + +FLAGS = tf.app.flags.FLAGS + +tf.app.flags.DEFINE_string( + 'model_dir', '/tmp/imagenet', + """Path to classify_image_graph_def.pb, """ + """imagenet_synset_to_human_label_map.txt, and """ + """imagenet_2012_challenge_label_map_proto.pbtxt.""") +tf.app.flags.DEFINE_string('image_file', '', + """Absolute path to image file.""") +tf.app.flags.DEFINE_integer('num_top_predictions', 5, + """Display this many predictions.""") + +DATA_URL = 'http://download.tensorflow.org/models/image/imagenet/inception-2015-12-05.tgz' +IMAGE_PATH = '/dev/shm/mjpeg/cam.jpg' + + +class NodeLookup(object): + """Converts integer node ID's to human readable labels.""" + + def __init__(self, + label_lookup_path=None, + uid_lookup_path=None): + if not label_lookup_path: + label_lookup_path = os.path.join( + FLAGS.model_dir, 'imagenet_2012_challenge_label_map_proto.pbtxt') + if not uid_lookup_path: + uid_lookup_path = os.path.join( + FLAGS.model_dir, 'imagenet_synset_to_human_label_map.txt') + self.node_lookup = self.load(label_lookup_path, uid_lookup_path) + + def load(self, label_lookup_path, uid_lookup_path): + """Loads a human readable English name for each softmax node. + + Args: + label_lookup_path: string UID to integer node ID. + uid_lookup_path: string UID to human-readable string. + + Returns: + dict from integer node ID to human-readable string. + """ + if not tf.gfile.Exists(uid_lookup_path): + tf.logging.fatal('File does not exist %s', uid_lookup_path) + if not tf.gfile.Exists(label_lookup_path): + tf.logging.fatal('File does not exist %s', label_lookup_path) + + # Loads mapping from string UID to human-readable string + proto_as_ascii_lines = tf.gfile.GFile(uid_lookup_path).readlines() + uid_to_human = {} + p = re.compile(r'[n\d]*[ \S,]*') + for line in proto_as_ascii_lines: + parsed_items = p.findall(line) + uid = parsed_items[0] + human_string = parsed_items[2] + uid_to_human[uid] = human_string + + # Loads mapping from string UID to integer node ID. + node_id_to_uid = {} + proto_as_ascii = tf.gfile.GFile(label_lookup_path).readlines() + for line in proto_as_ascii: + if line.startswith(' target_class:'): + target_class = int(line.split(': ')[1]) + if line.startswith(' target_class_string:'): + target_class_string = line.split(': ')[1] + node_id_to_uid[target_class] = target_class_string[1:-2] + + # Loads the final mapping of integer node ID to human-readable string + node_id_to_name = {} + for key, val in node_id_to_uid.items(): + if val not in uid_to_human: + tf.logging.fatal('Failed to locate: %s', val) + name = uid_to_human[val] + node_id_to_name[key] = name + + return node_id_to_name + + def id_to_string(self, node_id): + if node_id not in self.node_lookup: + return '' + return self.node_lookup[node_id] + + +def create_graph(): + """Creates a graph from saved GraphDef file and returns a saver.""" + # Creates graph from saved graph_def.pb. + with tf.gfile.FastGFile(os.path.join( + FLAGS.model_dir, 'classify_image_graph_def.pb'), 'rb') as f: + graph_def = tf.GraphDef() + graph_def.ParseFromString(f.read()) + _ = tf.import_graph_def(graph_def, name='') + + +def run_inference_on_image(image): + """Runs inference on an image. + + Args: + image: Image file name. + + Returns: + results of inference + """ + print("Running inference") + if not tf.gfile.Exists(image): + tf.logging.fatal('File does not exist %s', image) + image_data = tf.gfile.FastGFile(image, 'rb').read() + + # Creates graph from saved GraphDef. + create_graph() + + with tf.Session() as sess: + # Some useful tensors: + # 'softmax:0': A tensor containing the normalized prediction across + # 1000 labels. + # 'pool_3:0': A tensor containing the next-to-last layer containing 2048 + # float description of the image. + # 'DecodeJpeg/contents:0': A tensor containing a string providing JPEG + # encoding of the image. + # Runs the softmax tensor by feeding the image_data as input to the graph. + softmax_tensor = sess.graph.get_tensor_by_name('softmax:0') + + predictions = sess.run(softmax_tensor, + {'DecodeJpeg/contents:0': image_data}) + + predictions = np.squeeze(predictions) + + node_lookup = NodeLookup() + + top_k = predictions.argsort()[-1:][::-1] + results = [] + for node_id in top_k: + print(node_id) + human_string = node_lookup.id_to_string(node_id) + score = predictions[node_id] + print('%s (score = %.5f)' % (human_string, score)) + results.append(human_string) + + return results + + + + +def maybe_download_and_extract(): + """Download and extract model tar file.""" + dest_directory = FLAGS.model_dir + if not os.path.exists(dest_directory): + os.makedirs(dest_directory) + filename = DATA_URL.split('/')[-1] + filepath = os.path.join(dest_directory, filename) + if not os.path.exists(filepath): + def _progress(count, block_size, total_size): + sys.stdout.write('\r>> Downloading %s %.1f%%' % ( + filename, float(count * block_size) / float(total_size) * 100.0)) + sys.stdout.flush() + filepath, _ = urllib.request.urlretrieve(DATA_URL, filepath, + reporthook=_progress) + print() + statinfo = os.stat(filepath) + print('Succesfully downloaded', filename, statinfo.st_size, 'bytes.') + tarfile.open(filepath, 'r:gz').extractall(dest_directory) + + +def main(_): + maybe_download_and_extract() + image = (FLAGS.image_file if FLAGS.image_file else + os.path.join(FLAGS.model_dir, 'cropped_panda.jpg')) + run_inference_on_image(image) + + + + + +from flask import Flask +app = Flask(__name__) + +@app.route('/') +def hello_world(): + result = run_inference_on_image(IMAGE_PATH) + print("found") + print(result) + return(result[0]) + + +if __name__ == '__main__': + print("Starting up") + maybe_download_and_extract() + print("Warming up by running an inference") + # "warm up" by running the classifer on an image + run_inference_on_image(IMAGE_PATH) + print("Ready to label!") + app.run(host='0.0.0.0', port=9999) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..1edb861 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,7 @@ +ez_setup +Adafruit-MotorHAT +picamera +flask +RPi.GPIO +smbus + diff --git a/robot.py b/robot.py new file mode 100755 index 0000000..e084197 --- /dev/null +++ b/robot.py @@ -0,0 +1,86 @@ +#! /usr/bin/python + +import wheels +import sys +import sonar +import configure + +usage = ''' +robot + +Tell the robot what to do. + +Commands + +Actions +------- +forward +backward +left +right +stop +shake + +Test Wheels +----------- +wheel1 +wheel2 +wheel3 +wheel4 + +Test Sonar +---------- +leftdist +rightdist +centerdist +''' + +print sys.argv + +if (len(sys.argv) == 1): + print usage + exit(1) + +cmd = sys.argv[1] + +if (cmd == 'forward'): + wheels.forward(200, 1) + +elif (cmd == 'backward'): + wheels.backward(200, 1) + +elif (cmd == 'left'): + wheels.left(200, 1) + +elif (cmd == 'right'): + wheels.right(200, 1) + +elif (cmd == 'wheel1'): + wheels.spinMotor(1, 200, 1) + +elif (cmd == 'wheel2'): + wheels.spinMotor(2, 200, 1) + +elif (cmd == 'wheel3'): + wheels.spinMotor(3, 200, 1) + +elif (cmd == 'wheel4'): + wheels.spinMotor(4, 200, 1) + +elif (cmd == 'shake'): + wheels.left(200, 0.2) + wheels.right(200, 0.2) + +elif (cmd == 'leftdist'): + print sonar.ldist() +elif (cmd == 'rightdist'): + print sonar.rdist() +elif (cmd == 'centerdist'): + print sonar.cdist() + +else: + print usage + + + + diff --git a/server.sh b/server.sh index 076a24c..ce3c1ba 100644 --- a/server.sh +++ b/server.sh @@ -1 +1 @@ -gunicorn -w 4 -b0.0.0.0:8000 --timeout 1000 drive:app +gunicorn -w 4 -b0.0.0.0:8000 --timeout 1000 drive_server:app diff --git a/services/inception.service b/services/inception.service new file mode 100644 index 0000000..d4f0893 --- /dev/null +++ b/services/inception.service @@ -0,0 +1,14 @@ +#goes in /etc/systemd/system/inception.service + +[Unit] +Description=inception daemon +After=network.target + +[Service] +User=pi +Group=www-data +WorkingDirectory=/home/pi/robot +ExecStart=/usr/local/bin/gunicorn --workers 1 -b0.0.0.0:9999 --timeout 10000 inception_server:app + +[Install] +WantedBy=multi-user.target diff --git a/services/web.service b/services/web.service new file mode 100644 index 0000000..6d16460 --- /dev/null +++ b/services/web.service @@ -0,0 +1,14 @@ +#goes in /etc/systemd/system/web.service + +[Unit] +Description=gunicorn web daemon +After=network.target + +[Service] +User=pi +Group=www-data +WorkingDirectory=/home/pi/robot +ExecStart=/usr/bin/gunicorn --workers 3 --bind unix:/home/pi/drive.sock drive_server:app + +[Install] +WantedBy=multi-user.target diff --git a/sonar.py b/sonar.py index e8e4ac5..01be3c5 100644 --- a/sonar.py +++ b/sonar.py @@ -1,51 +1,131 @@ import RPi.GPIO as GPIO -import sys +import os import time import atexit +import sys +from subprocess import call +import configure -# recommended for auto-disabling motors on shutdown! -def turnOffGPIO(): - GPIO.cleanup() -atexit.register(turnOffGPIO) +def setup(): + for i in range(3): + GPIO.setup(TRIG[i],GPIO.OUT) + GPIO.setup(ECHO[i],GPIO.IN) + GPIO.output(TRIG[i], False) + + print "Waiting For Sensor To Settle" + +#if ('LTRIG' in os.environ): +# TRIG = [int(os.environ['LTRIG']), +# int(os.environ['CTRIG']), +# int(os.environ['RTRIG'])] +# ECHO = [int(os.environ['LECHO']), +# int(os.environ['CECHO']), +# int(os.environ['RECHO'])] -GPIO.setmode(GPIO.BCM) -TRIG = int(sys.argv[1]) +if ('LTRIG' in configure.data): + TRIG = [int(configure.data['LTRIG']), + int(configure.data['CTRIG']), + int(configure.data['RTRIG'])] + ECHO = [int(configure.data['LECHO']), + int(configure.data['CECHO']), + int(configure.data['RECHO'])] -ECHO = int(sys.argv[2]) + GPIO.setmode(GPIO.BCM) + setup() -print "Distance Measurement In Progress" +def turnOffGPIO(): + GPIO.cleanup() -GPIO.setup(TRIG,GPIO.OUT) +atexit.register(turnOffGPIO) + + +def raw_distance(TRIG, ECHO): + #check a sonar with trigger argv1 and echo argv2 + #example usage + #python sonar.py 22 27 + + # recommended for auto-disabling motors on shutdown! + GPIO.setmode(GPIO.BCM) + + + print "Distance Measurement In Progress" + + GPIO.setup(TRIG,GPIO.OUT) + + GPIO.setup(ECHO,GPIO.IN) + + GPIO.output(TRIG, False) + + print "Waiting For Sensor To Settle" + + time.sleep(2) + + GPIO.output(TRIG, True) + + time.sleep(0.00001) + + GPIO.output(TRIG, False) + + while GPIO.input(ECHO)==0: + pulse_start = time.time() + + while GPIO.input(ECHO)==1: + pulse_end = time.time() + + pulse_duration = pulse_end - pulse_start + + distance = pulse_duration * 17150 + + distance = round(distance, 2) + + print "Distance:",distance,"cm" + -GPIO.setup(ECHO,GPIO.IN) +def distance(i): +# print "Distance Measurement In Progress" -GPIO.output(TRIG, False) + GPIO.output(TRIG[i], True) -print "Waiting For Sensor To Settle" + time.sleep(0.00001) -time.sleep(2) + GPIO.output(TRIG[i], False) -GPIO.output(TRIG, True) + pulse_end = 0; + pulse_start = 0; + + while GPIO.input(ECHO[i])==0: + pulse_start = time.time() -time.sleep(0.00001) + while GPIO.input(ECHO[i])==1: + pulse_end = time.time() + + if (pulse_end == 0 or pulse_start==0): + return 1000 + + pulse_duration = pulse_end - pulse_start -GPIO.output(TRIG, False) + distance = pulse_duration * 17150 -while GPIO.input(ECHO)==0: - pulse_start = time.time() + distance = round(distance, 2) -while GPIO.input(ECHO)==1: - pulse_end = time.time() + # print "Distance:",distance,"cm" -pulse_duration = pulse_end - pulse_start + return distance -distance = pulse_duration * 17150 +def ldist(): + return distance(0) -distance = round(distance, 2) +def rdist(): + return distance(2) -print "Distance:",distance,"cm" +def cdist(): + return distance(1) - +if __name__ == "__main__": + TRIG = int(sys.argv[1]) + + ECHO = int(sys.argv[2]) + raw_distance(TRIG, ECHO) diff --git a/speaker.py b/speaker.py new file mode 100644 index 0000000..4981b52 --- /dev/null +++ b/speaker.py @@ -0,0 +1,11 @@ +import subprocess +import os + +def say(text): + if 'VOICE' in os.environ: + voice = os.environ['VOICE'] + subprocess.Popen(['flite', '-voice', voice,'-t', text]) + else: + subprocess.Popen(['flite', '-t', text]) + + diff --git a/static/arm.js b/static/arm.js new file mode 100644 index 0000000..5970d97 --- /dev/null +++ b/static/arm.js @@ -0,0 +1,164 @@ +function call(name) { + console.log(name); + var xhr = new XMLHttpRequest(); + xhr.open('GET', name, true); + xhr.send(); +} + +function saveFields() { + var data = {}; + data.max = []; + data.min = []; + for (i=0; i<5; i++) { + data.min[i] = document.getElementById("tmin" + i).value; + data.max[i] = document.getElementById("tmax" + i).value; + } + localStorage.setItem("myData", JSON.stringify(data)); +} + +function loadFields() { + var data = localStorage.getItem("myData"); + var dataObject; + if (data != null) { + dataObject = JSON.parse(data); + for (i=0; i<5; i++) { + document.getElementById("tmin" + i).value = dataObject.min[i] + document.getElementById("tmax" + i).value = dataObject.max[i] + } + + } else { + for (i=0; i<5; i++) { + document.getElementById("tmin" + i).value = 0 + document.getElementById("tmax" + i).value = 100 + } + } +} + +function slider(num, value) { + console.log(num); + tid = "t" + num; + document.getElementById(tid).value = value; + var xhr = new XMLHttpRequest(); + xhr.open('GET', "http://github.com/slider?num="+num+"&value="+value); + xhr.send(); +} + +function sliderInc(num) { + sid = "s" + num; + v = parseInt(document.getElementById(sid).value); + document.getElementById(sid).value = v+1; + slider(num,document.getElementById(sid).value); +} + +function sliderDec(num) { + sid = "s" + num; + v = parseInt(document.getElementById(sid).value); + document.getElementById(sid).value = v-1; + slider(num,document.getElementById(sid).value); +} + +function sliderReset(num) { + sid = "s" + num; + v = parseInt(document.getElementById(sid).value); + document.getElementById(sid).value = 50; + slider(num,document.getElementById(sid).value); +} + +function randomizeTargets() { + for (i=0; i<5; i++) { + min = parseInt(document.getElementById("tmin" + i).value); + max = parseInt(document.getElementById("tmax" + i).value); + r = Math.floor(Math.random() * (max-min) + min) + document.getElementById("target"+i).value = r + } +} + + + +document.onkeyup = function(e) { + +} + +document.onkeydown = function(e) { + var key = e.keyCode ? e.keyCode : e.which; + console.log(key); + + if (key == 81) { //q + sliderDec(0); + } else if (key == 69) { + sliderInc(0); + } else if (key == 87) { + sliderReset(0); + } + else if (key == 65) { + sliderDec(1); + } else if (key == 68) { + sliderInc(1); + } else if (key == 83) { + sliderReset(1); + } + else if (key == 90) { + sliderDec(2); + } else if (key == 67) { + sliderInc(2); + } else if (key == 88) { + sliderReset(2); + } + else if (key == 84) { + sliderDec(3); + } else if (key == 85) { + sliderInc(3); + } else if (key == 89) { + sliderReset(3); + } + else if (key == 71) { + sliderDec(4); + } else if (key == 74) { + sliderInc(4); + } else if (key == 72) { + sliderReset(4); + } + else if (key == 80) { + randomizeTargets(); + } else if (key == 77) { + moveToTarget(); + } +} + +function stepToTarget() { + stepSize = 1; + for (i=0; i<5; i++) { + stopFlag = true; + pos = parseInt(document.getElementById("s" + i).value); + target = parseInt(document.getElementById("target" + i).value); + if ( pos == target ) { + newPos = target; + } else if ( Math.abs(pos-target) < stepSize) { + newPos = target; + } else if ( pos < target ) { + stopFlag = false; + newPos = pos + stepSize; + } else if ( pos > target ) { + stopFlag = false; + newPos = pos - stepSize; + } + document.getElementById("s" + i).value = newPos; + slider(i,newPos); + if (stopFlag) { + clearFlag(timer); + } + } +} + + + +var timer; +function moveToTarget() { + timer=setInterval(function() { stepToTarget(); }, 100); + +} + + +window.onload = function() { + loadFields(); +} diff --git a/static/robot.js b/static/robot.js new file mode 100644 index 0000000..6a8180f --- /dev/null +++ b/static/robot.js @@ -0,0 +1,72 @@ +function refreshCam() { + var camImg = document.getElementById("camera"); + camImg.src="http://github.com/cam.jpg?t="+new Date().getTime(); +} +function refreshLatest() { + var latestImg = document.getElementById("latest"); + latestImg.src="http://github.com/latest.jpg?t="+new Date().getTime(); + + var xhr = new XMLHttpRequest(); + xhr.open('GET', 'http://github.com/data', true); + xhr.onload = function() { + console.log('HERE'); + var status = xhr.status; + if (status == 200) { + var guesses = document.getElementById("guesses"); + guesses.innerHTML = xhr.response + } + }; + xhr.send(); + +} + +setInterval(refreshCam, 200); +setInterval(refreshLatest, 1001); +function call(name) { + console.log(name); + var xhr = new XMLHttpRequest(); + xhr.open('GET', name, true); + xhr.send(); +} + +function setSpeed(left, right) { + var xhr = new XMLHttpRequest(); + xhr.open('GET', 'http://github.com/setSpeed?left='+left+'&right='+right, true); + xhr.send(); +} + +document.onkeyup = function(e) { + call('stop'); +} + +document.onkeydown = function(e) { + var key = e.keyCode ? e.keyCode : e.which; + console.log(key); + + if (key == 37) { //Left + call('l'); + } else if (key == 38) { //up + call('b'); + } else if (key == 39) {//right + call('r'); + } else if (key == 40) {// down + call('f'); + } else if (key == 90) {//z + call('stop'); + } else if (key == 32) { //space + call('img_rec'); + } +} + +if (window.DeviceMotionEvent != undefined) { + window.ondevicemotion = function(e) { + ax = event.accelerationIncludingGravity.x * 5; + ay = event.accelerationIncludingGravity.y * 5; + + left = ax + ay; + right = -ax + ay; + + + setSpeed(left, right); + } +} diff --git a/static/style.css b/static/style.css new file mode 100644 index 0000000..28710c1 --- /dev/null +++ b/static/style.css @@ -0,0 +1,18 @@ +body { + font-family: sans-serif; background: #eee; +} +input[type="number"] { + width: 50px; +} + +.min { + readonly: "readonly" +} + +.max { + readonly: "readonly" +} + +.target { + readonly: "readonly" +} \ No newline at end of file diff --git a/templates/arm.html b/templates/arm.html new file mode 100644 index 0000000..c1936c4 --- /dev/null +++ b/templates/arm.html @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Claw
Wrist
Elbow
Sholder
Rotate
+ + + + + + diff --git a/templates/index.html b/templates/index.html index 6476a9f..a11c18f 100644 --- a/templates/index.html +++ b/templates/index.html @@ -1,87 +1,19 @@ - - - + - } - - setInterval(refreshCam, 200); - setInterval(refreshLatest, 1001); - function call(name) { - console.log(name); - var xhr = new XMLHttpRequest(); - xhr.open('GET', name, true); - xhr.send(); - } + - function setSpeed(left, right) { - var xhr = new XMLHttpRequest(); - xhr.open('GET', 'http://github.com/setSpeed?left='+left+'&right='+right, true); - xhr.send(); - } - document.onkeyup = function(e) { - call('stop'); - } - - document.onkeydown = function(e) { - var key = e.keyCode ? e.keyCode : e.which; - console.log(key); + - if (key == 37) { //Left - call('l'); - } else if (key == 38) { //up - call('b'); - } else if (key == 39) {//right - call('r'); - } else if (key == 40) {// down - call('f'); - } else if (key == 90) {//z - call('stop'); - } else if (key == 32) { //space - call('img_rec'); - } - } - if (window.DeviceMotionEvent != undefined) { - window.ondevicemotion = function(e) { - ax = event.accelerationIncludingGravity.x * 5; - ay = event.accelerationIncludingGravity.y * 5; - - left = ax + ay; - right = -ax + ay; - setSpeed(left, right); - } - } - - - - -

+ diff --git a/wheels.py b/wheels.py new file mode 100644 index 0000000..b49f5db --- /dev/null +++ b/wheels.py @@ -0,0 +1,109 @@ + +from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor + +import os +import time +import atexit + +# create a default object, no changes to I2C address or frequency +mh = Adafruit_MotorHAT(addr=0x60) + +# recommended for auto-disabling motors on shutdown! +def turnOffMotors(): + mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) + mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) + mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) + mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) + +atexit.register(turnOffMotors) + +################################# DC motor test! +mFL = mh.getMotor(1) +mBL = mh.getMotor(2) +mBR = mh.getMotor(3) +mFR = mh.getMotor(4) + + + +def wakeup(m): + # set the speed to start, from 0 (off) to 255 (max speed) + m.setSpeed(150) + m.run(Adafruit_MotorHAT.FORWARD); + # turn on motor + m.run(Adafruit_MotorHAT.RELEASE); + + +wakeup(mFL) +wakeup(mBL) +wakeup(mFR) +wakeup(mBL) + +def spin(wheel, speed): + if (speed > 0): + wheel.run(Adafruit_MotorHAT.FORWARD) + wheel.setSpeed(speed) + elif (speed < 0): + wheel.run(Adafruit_MotorHAT.BACKWARD) + wheel.setSpeed(-speed) + else: + wheel.run(Adafruit_MotorHAT.RELEASE) + +def spinMotor(motorId=1, speed=200, dur=-1): + m = mh.getMotor(motorId) + spin(m, speed) + if (dur >= 0): + time.sleep(dur) + stop() + + +def stop(): + mFL.run(Adafruit_MotorHAT.RELEASE) + mFR.run(Adafruit_MotorHAT.RELEASE) + mBL.run(Adafruit_MotorHAT.RELEASE) + mBR.run(Adafruit_MotorHAT.RELEASE) + + +def forward(speed=200, dur=-1): + spin(mFR, speed) + spin(mFL, speed) + spin(mBR, speed) + spin(mBL, speed) + + if (dur >= 0): + time.sleep(dur) + stop() + + +def backward(speed, dur=-1): + spin(mFR, -speed) + spin(mFL, -speed) + spin(mBR, -speed) + spin(mBL, -speed) + + if (dur >- 0): + time.sleep(dur) + stop() + + +def left(speed, dur=-1): + spin(mFR, -speed) + spin(mFL, speed) + spin(mBR, -speed) + spin(mBL, speed) + + if (dur >- 0): + time.sleep(dur) + stop() + +def right(speed, dur=-1): + spin(mFR, speed) + spin(mFL, -speed) + spin(mBR, speed) + spin(mBL, -speed) + + if (dur >- 0): + time.sleep(dur) + stop() + + +