def __init__(self,
camera_port_num=1,
left_fp=LFP,
left_bp=LBP,
left_en=LEP,
right_fp=RFP,
right_bp=RBP,
right_en=REP,
front_trigger=FTP,
front_echo=FEP,
front_distance_threshold=10):
self.power_train = Powertrain(left_fp, left_bp, left_en, right_fp,
right_bp, right_en)
def testConstructor(self):
"""Test constructor behaves as expected.
"""
p = Powertrain(1, 2, 3, 4, 5, 6)
# check left and right Motor's have expected values
self.assertEqual(p.left.frwd_p, 1)
self.assertEqual(p.left.bkwd_p, 2)
self.assertEqual(p.left.enbl_p, 3)
self.assertEqual(p.right.frwd_p, 4)
self.assertEqual(p.right.bkwd_p, 5)
self.assertEqual(p.right.enbl_p, 6)
import numpy as np
import cv2 # OpenCV 2.3.1
import sys
from powertrain import Powertrain
filename = sys.argv[1]
width = int(sys.argv[2])
height = int(sys.argv[3])
factor = int(sys.argv[4])
# setup transform kernel
loader = Powertrain(True)
loader.program = """
__kernel void ds(__global const uchar *img_g,
const int width,
const int height,
const int out_width,
const int out_height,
__global uchar *out_g) {
int gid = get_global_id(0);
int col = gid % width;
int row = gid / width;
if ((col >= width) || (row >= height)) {
return;
}
def __init__(self, manager, tile):
'''
'''
# setup transform kernel
loader = Powertrain(True)
loader.program = """
__kernel void ds(__global const uchar *img_g,
const int width,
const int height,
const int out_width,
const int out_height,
__global uchar *out_g) {
int gid = get_global_id(0);
int col = gid % width;
int row = gid / width;
if ((col >= width) || (row >= height)) {
return;
}
if (col < 0) {
return;
}
int new_row = row/2;
int new_col = col/2;
if ((new_col >= out_width) || (new_row >= out_height)) {
return;
}
if (new_col < 0) {
return;
}
int k = new_row*out_width + new_col;
if (row % 2 == 0 && col % 2 == 0) {
uchar c = img_g[gid];
uchar r = img_g[gid+1];
uchar b = img_g[gid+width];
uchar b_r = img_g[gid+width+1];
uchar val = (c + r + b + b_r) / 4;
//out_g[k] = img_g[gid];
out_g[k] = val;
}
}
__kernel void transform(__global const uchar *img_g,
const int width,
const int height,
const float angle,
const float Tx,
const float Ty,
const int out_width,
const int out_height,
__global uchar *out_g) {
int gid = get_global_id(0);
int col = gid % width;
int row = gid / width;
if ((col >= width) || (row >= height)) {
return;
}
if (col < 0) {
return;
}
//
float c = cos(angle);
float s = sin(angle);
// new position
int new_col = c * col - s * row + Tx;
int new_row = s * col + c * row + Ty;
if ((new_col >= out_width) || (new_row >= out_height)) {
return;
}
if (new_col < 0) {
return;
}
int k = new_row*out_width + new_col;
out_g[k] = img_g[gid];
}
"""
imagedata = Tile.load(tile)
width = imagedata.shape[0]
height = imagedata.shape[1]
# for i in range(len(imagedata)):
# memory[i] = imagedata[i]
transform = tile._transforms[1]
c = np.cos(transform.r)
s = np.sin(transform.r)
points = [[0, 0], [0, height - 1], [width - 1, 0], [width - 1, height - 1]]
min_x, min_y, max_x, max_y = [ sys.maxint, sys.maxint, -sys.maxint, -sys.maxint ]
for point in points:
new_x = c * point[0] - s * point[1] + transform.x
new_y = s * point[0] + c * point[1] + transform.y
min_x = min(min_x, new_x)
min_y = min(min_y, new_y)
max_x = max(max_x, new_x)
max_y = max(max_y, new_y)
tx = transform.x - min_x
ty = transform.y - min_y
output_width, output_height = (int(max_x - min_x)+1, int(max_y - min_y)+1)
mf = cl.mem_flags
in_img = cl.Buffer(loader.context, mf.READ_ONLY | mf.USE_HOST_PTR, hostbuf=imagedata)
for z,l in enumerate(tile._levels):
out_img = cl.Buffer(loader.context, mf.READ_WRITE, tile._levels[z]._imagedata.nbytes)
if z==0:
loader.program.transform(loader. queue,
(width*height,),
None,
in_img,
np.int32(width),
np.int32(height),
np.float32(transform.r),
np.float32(tx),
np.float32(ty),
np.int32(output_width),
np.int32(output_height),
out_img)
else:
loader.program.ds(loader. queue,
(width*height,),
None,
in_img,
np.int32(width),
np.int32(height),
np.int32(output_width),
np.int32(output_height),
out_img)
cl.enqueue_copy(loader.queue, tile._levels[z]._memory, out_img).wait()
in_img = out_img
width = output_width
height = output_height
output_width /= 2
output_height /= 2
if z==3:
cv2.imwrite('/tmp/test.jpg', tile._levels[z]._imagedata.reshape(height, width))
# print 'Worker', tile._mipmapLevels["0"]['imageUrl'], memory.address
# import cv2
# img = memory.reshape(tile._real_width, tile._real_height)
# cv2.imwrite('/tmp/'+os.path.basename(tile._mipmapLevels["0"]['imageUrl']), img)
tile._status.loaded()
# print Loader.shmem_as_ndarray(tile._memory)[20000:25000]
manager.onLoad(tile)
class myThread(threading.Thread):
def __init__(self, threadID, name, counter):
threading.Thread.__init__(self)
self.threadID = threadID
self.name = name
self.counter = counter
def run(self):
print("Ready")
while running:
#BrickPiUpdateValues() # Ask BrickPi to update values for sensors/motors
time.sleep(.2) # sleep for 200 ms
if __name__ == "__main__":
dexter = Powertrain(direction_pins, step_pins)
dexter.setup()
#BrickPiSetup() # setup the serial port for communication
#BrickPi.MotorEnable[PORT_A] = 1 #Enable the Motor A
#BrickPi.MotorEnable[PORT_D] = 1 #Enable the Motor D
#BrickPiSetupSensors() #Send the properties of sensors to BrickPi
running = True
thread1 = myThread(1, "Thread-1", 1)
thread1.setDaemon(True)
thread1.start()
application.listen(9093) #starts the websockets connection
tornado.ioloop.IOLoop.instance().start()
GPIO.cleanup()
#!python2
from ImageProcessor import ImageProcessor
from powertrain import Powertrain
from Ultrasonic import Ultrasonic
from car_config import *
import time
import RPi.GPIO as GPIO
imgpr = ImageProcessor(0, 0)
imgpr.init_camera()
pt = Powertrain(LFP, LBP, LEP, RFP, RBP, REP)
sensor = Ultrasonic()
def start():
# Speed at which to turn each motor
base_intensity = 0
left_duty_cycle = 45
right_duty_cycle = 45
try:
# Store the traveled direction between image processor calls
lastDir = 1
while (True):
# Get distance to nearest obstacle
distance = sensor.distance()
# If obstacle is too close, stop motors.
if distance < 20:
pt.stop()
continue
# Direction: -1 = left, 1 = right, 0 = no change
# Call image processor to determine which wheel to turn
#!python2
from powertrain import Powertrain
from car_config import *
import time
import RPi.GPIO as GPIO
try:
pt = Powertrain(LFP, LBP, LEP, RFP, RBP, REP)
time.sleep(3)
pt.turn_intensity(60, -10)
time.sleep(2)
pt.stop()
time.sleep(1)
pt.turn_intensity(60, 50)
time.sleep(1.5)
pt.turn_intensity(60, -50)
time.sleep(1.5)
pt.turn_intensity(50, 50)
time.sleep(1.5)
pt.turn_intensity(50, -50)
time.sleep(1.5)
pt.stop()
GPIO.cleanup()
except KeyboardInterrupt:
pt.stop()
GPIO.cleanup()
class SmartCar:
def __init__(self,
camera_port_num=1,
left_fp=LFP,
left_bp=LBP,
left_en=LEP,
right_fp=RFP,
right_bp=RBP,
right_en=REP,
front_trigger=FTP,
front_echo=FEP,
front_distance_threshold=10):
self.power_train = Powertrain(left_fp, left_bp, left_en, right_fp,
right_bp, right_en)
#self.obstacle_detector = ObstacleDetector(front_trigger, frot_echo, front_threshold);
#self.image_processor = ImageProcesor(camera_port_num);
# incr_factor = 1 for accelerate, and -1 for decelerate
def accelerate(self, incr_factor=1):
new_dc = 0
new_dc_left = 0
# without the adjustment
new_dc_right = 0
# without the adjustment
if (SharedData.pi_status['motors_stopped'] == True
and incr_factor == 1):
#self.power_train.accelerate(SharedData.pi_status['default_duty_cycle']);
new_dc = SharedData.pi_status['default_duty_cycle']
print "changing dc to: ", new_dc
new_dc_left = new_dc
# *SharedData.pi_status['left_motor_dc_adjustment'];
new_dc_right = new_dc
#*SharedData.pi_status['right_motor_dc_adjustment'];
SharedData.pi_status['motors_stopped'] = False
else:
#self.power_train.accelerate(SharedData.pi_status['default_duty_cycle']);
print "factor is ", incr_factor * SharedData.pi_status['dc_delta']
new_dc_left = SharedData.pi_status[
'left_motor_dc'] + incr_factor * SharedData.pi_status[
'dc_delta']
new_dc_right = SharedData.pi_status[
'left_motor_dc'] + incr_factor * SharedData.pi_status[
'dc_delta']
if (new_dc_left < 10 and new_dc_right < 10):
print "stopping left ....."
self.stop()
SharedData.pi_status['motors_stopped'] = True
new_dc_left = 0
# without the adjustment
new_dc_right = 0
# without the adjustment
else:
new_dc_left = 0 if new_dc_left < 10 else new_dc_left
new_dc_right = 0 if new_dc_right < 10 else new_dc_right
# update at last
# update global
# use un-adjusted original values!
SharedData.pi_status['left_motor_dc'] = new_dc_left
SharedData.pi_status['right_motor_dc'] = new_dc_right
# adjust now!
new_dc_left = new_dc_left * SharedData.pi_status[
'left_motor_dc_adjustment']
new_dc_right = new_dc_right * SharedData.pi_status[
'right_motor_dc_adjustment']
print "changing dc to: ", (new_dc_left, new_dc_right)
self.power_train.left.pwm.ChangeDutyCycle(new_dc_left)
self.power_train.right.pwm.ChangeDutyCycle(new_dc_right)
# change backward or forward : changes direction and stops
def set_forward_direction(self, forward=True):
self.power_train.left.change_dir(forward)
self.power_train.right.change_dir(forward)
SharedData.pi_status['direction'] = 0
if (SharedData.pi_status['headed_forward'] != forward):
SharedData.pi_status['headed_forward'] = forward
self.stop()
def change_left_right(self, dir):
SharedData.pi_status['direction'] -= direction_delta
# let it stop on its own
def stop(self):
print("stopping? ..")
SharedData.pi_status['left_motor_dc'] = 0
SharedData.pi_status['right_motor_dc'] = 0
self.power_train.stop()
def cleanup(self):
print("C => Cleaning up!")
self.power_train.cleanup()
def handle_cmd(self, cmd):
if (cmd == CMD_FORWARD):
self.set_forward_direction(True)
elif (cmd == CMD_BACKWARD):
self.set_forward_direction(False)
pass
elif (cmd == CMD_ACCELERATE):
print "Accelerating..."
self.accelerate()
elif (cmd == CMD_DECELERATE):
print "decelerating..."
self.accelerate(-1)
elif (cmd == CMD_LEFT):
SharedData.pi_status['direction'] -= SharedData.pi_status[
'direction_delta']
pass
elif (cmd == CMD_RIGHT):
SharedData.pi_status['direction'] += SharedData.pi_status[
'direction_delta']
pass
elif (cmd == CMD_STOP): # motors
self.stop()
elif (cmd == CMD_CLEANUP):
self.cleanup()
elif (cmd == CMD_CAL_LEFT):
# if car goes more towards right
# we need to pull it left means decrease speed of right motor.
if (SharedData.pi_status['right_motor_dc_adjustment'] > 0.1):
SharedData.pi_status['right_motor_dc_adjustment'] -= 0.1
elif (cmd == CMD_CAL_RIGHT):
if (SharedData.pi_status['left_motor_dc_adjustment'] > 0.1):
SharedData.pi_status['left_motor_dc_adjustment'] -= 0.1
elif (cmd == CMD_TERMINATE):
'''
self.wfile.write("closing...");
self.stopped = True;
self.server.shutdown();
self.socket.close()
print ("server: closing...");
'''
pass
return
"""Script to demonstrate the Powertrain class operating"""
import sys
import time
sys.path.append('..')
from powertrain import Powertrain
from car_config import *
import RPi.GPIO as GPIO
pwr = Powertrain(LFP, LBP, LEP, RFP, RBP, REP)
t = 2 # time to delay actions; in seconds
try:
# roll forward and backward
print("calling forward(80)")
pwr.forward(100)
time.sleep(t)
print("calling reverse(80)")
pwr.reverse(100)
time.sleep(t)
pwr.stop()
# pivot clockwise and counterclockwise
print("calling pivot(50)")
pwr.pivot(80)
time.sleep(t)
print("calling pivot(50, clockwise=True)")
pwr.pivot(80, True)
time.sleep(t)
pwr.stop()
def __init__(self, manager, view):
'''
'''
# setup stitch kernel
stitcher = Powertrain(True)
stitcher.program = """
__kernel void stitch(__global uchar *out_g,
const int out_width,
const int out_height,
__global const uchar *tile_g) {
int gid = get_global_id(0);
if (gid >= out_width*out_height)
return;
// col + row inside output
//int col = gid % out_width;
//int row = gid / out_width;
if (tile_g[gid] == 0) {
// check for boundary pixels (thin black lines)
//int k_top = (row+1)*out_width + col;
//int k_left = row*out_width + col+1;
//if (tile_g[k_top] != 0) {
// out_g[gid] = tile_g[k_top];
// return;
//}
//if (tile_g[k_left] != 0) {
// out_g[gid] = tile_g[k_left];
// return;
//}
return;
}
out_g[gid] = tile_g[gid];
}
"""
divisor = 2**view._zoomlevel
minX = view._bbox[0]
minY = view._bbox[2]
out_width = view._bbox[1]
out_height = view._bbox[3]
reshaped_imagedata = view._imagedata.reshape(out_height, out_width)
for t in view._tiles:
tile_width = t._real_width / divisor
tile_height = t._real_height / divisor
offset_x = 0
offset_y = 0
for transform in t._transforms:
offset_x += transform.x
offset_y += transform.y
offset_x /= divisor
offset_y /= divisor
offset_x = int(offset_x-minX) + 1
offset_y = int(offset_y-minY) + 1
# print 'placing tile', t, 'at', offset_x, offset_y
mf = cl.mem_flags
# output_subarray_start = offset_y*out_width + offset_x
# output_subarray_end = output_subarray_start + tile_width*tile_height
# output_subarray = view._imagedata[output_subarray_start:output_subarray_end]
output_subarray = reshaped_imagedata[offset_y:offset_y+tile_height,offset_x:offset_x+tile_width]
output_subarray = output_subarray.ravel()
# print output_subarray_start, output_subarray_end, output_subarray_end-output_subarray_start
out_img = cl.Buffer(stitcher.context, mf.WRITE_ONLY | mf.USE_HOST_PTR, hostbuf=output_subarray)
in_img = cl.Buffer(stitcher.context, mf.READ_ONLY | mf.USE_HOST_PTR, hostbuf=t._levels[view._zoomlevel]._memory)
# stitcher.program.stitch(stitcher.queue,
# (out_width*out_height,),
# None,
# out_img,
# np.int32(out_width),
# np.int32(out_height),
# np.int32(offset_x),
# np.int32(offset_y),
# np.int32(tile_width),
# np.int32(tile_height),
# in_img
# )
stitcher.program.stitch(stitcher.queue,
(tile_width*tile_height,),
None,
out_img,
np.int32(tile_width),
np.int32(tile_height),
in_img)
cl.enqueue_copy(stitcher.queue, output_subarray, out_img).wait()
reshaped_imagedata[offset_y:offset_y+tile_height,offset_x:offset_x+tile_width] = output_subarray.reshape(tile_height, tile_width)
# view._imagedata[output_subarray_start:output_subarray_end] = output_subarray
# print 'storing'
# img = view._imagedata.reshape(out_height, out_width)
# cv2.imwrite('/tmp/stitch.jpg', reshaped_imagedata)
view._status.loaded()
manager.onStitch(view)
"""Script to demonstrate the Powertrain class operating"""
import sys
import time
sys.path.append('..')
from powertrain import Powertrain
from car_config import *
pwr = Powertrain(LFP, LBP, LEP, RFP, RBP, REP)
t = 3 # time to delay actions; in seconds
# roll forward and backward
print("calling forward(80)")
pwr.forward(80)
time.sleep(t)
print("calling reverse(80)")
pwr.reverse(80)
time.sleep(t)
pwr.stop()
# pivot clockwise and counterclockwise
print("calling pivot(50)")
pwr.pivot(50)
time.sleep(t)
print("calling pivot(50, clockwise=True)")
pwr.pivot(50, True)
time.sleep(t)
pwr.stop()
import pyopencl as cl
import numpy as np
import cv2 # OpenCV 2.3.1
import sys
from powertrain import Powertrain
filename = sys.argv[1]
width = int(sys.argv[2])
height = int(sys.argv[3])
factor = int(sys.argv[4])
# setup transform kernel
loader = Powertrain(True)
loader.program = """
__kernel void ds(__global const uchar *img_g,
const int width,
const int height,
const int out_width,
const int out_height,
__global uchar *out_g) {
int gid = get_global_id(0);
int col = gid % width;
int row = gid / width;
if ((col >= width) || (row >= height)) {
return;
}
from powertrain import Powertrain;
## Note :
## Usage : python pt.py <test#> <args>
## Example
## To run test0 with argument 50
## python pt.py 0 50
##
## To run test1 with duty cycle of 40:
## python pt.py 1 40
##
## And similar for other tests!
# global
pt = Powertrain(LFP, LBP, LEP, RFP, RBP, REP);
# test0 is for motors
def test0(dc):
m1 = Motor(LFP, LBP, LEP);
m2 = Motor(RFP, RBP, REP);
m1.forward(dc[0]);
time.sleep(2);
m1.off();
m2.forward(dc[0]);
time.sleep(2);
# forward
def test1(dc):
pt.forward(dc[0]);
time.sleep(2);