Telepresence Vehicle

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

import RPi.GPIO as GPIO
import socket
import threading
import cv2
import numpy as np
import picamera
import subprocess
import re
import time
from bluedot.btcomm import BluetoothClient, BluetoothAdapter
import io
import struct
import socketserver
from http import server
from threading import Condition
import Robot #custom library

GPIO.setmode(GPIO.BCM)
GPIO.setup(6, GPIO.OUT)
GPIO.setup(13, GPIO.OUT)
p1 = GPIO.PWM(6, 100000/2150)
p2 = GPIO.PWM(13, 100000/2150)
p1.start(150/2150.0*100)
p2.start(150/2150.0*100)

## Get IP address
result = subprocess.run('hostname -I', stdout=subprocess.PIPE, 
shell=True)
result = result.stdout.decode('utf-8')
client_IP = re.split(' |\n', result)[0]
print("client_IP: "+client_IP)


PORT1 = 5000
BUFFER_SIZE = 1024
connected = True
run = True
class StreamingOutput(object):
    def __init__(self):
        self.frame = None
        self.buffer = io.BytesIO()
        self.condition = Condition()

    def write(self, buf):
        if buf.startswith(b'\xff\xd8'):
            self.buffer.truncate()
            with self.condition:
                self.frame = self.buffer.getvalue()
                self.condition.notify_all()
            self.buffer.seek(0)
        return self.buffer.write(buf)

class StreamingHandler(server.BaseHTTPRequestHandler):
    """
    Streaming video to HTTP server
    """
    def do_GET(self):
        if self.path == '/':
            self.send_response(200)
            self.send_header('Age', 0)
            self.send_header('Cache-Control', 'no-cache, private')
            self.send_header('Pragma', 'no-cache')
            self.send_header('Content-Type', 
            'multipart/x-mixed-replace; boundary=FRAME')
            self.end_headers()
            try:
                while True:
                    with output.condition:
                        output.condition.wait()
                        frame = output.frame
                    self.wfile.write(b'--FRAME\r\n')
                    self.send_header('Content-Type', 'image/jpeg')
                    self.send_header('Content-Length', len(frame))
                    self.end_headers()
                    self.wfile.write(frame)
                    self.wfile.write(b'\r\n')
            except Exception as e:
                pass


class StreamingServer(socketserver.ThreadingMixIn, server.HTTPServer):
    allow_reuse_address = True
    daemon_threads = True



def data_received(data):
    """
    Callback function for bluetooth data received 
    """
    global host_IP
    global flag
    print('host_IP: ' + data)
    host_IP = data
    flag = False
    
def sendIP():
    """
    Repeatedly sends the string containing IP address to target. 
    Requires the MAC address of the target to be hardcoded.
    Receives back IP address from target and saves it for UDP 
    connection
    Blocks until we connect with base station 
    """
    global flag
    flag = True
    c = BluetoothClient('B8:27:EB:2A:46:91', data_received, 
    power_up_device=True)
    while flag:
        c.send(client_IP)
        time.sleep(1)
    c.disconnect()

sendIP()
### Connection good!! Start everything else
s1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s1.bind((client_IP, PORT1))
s1.setblocking(0)

camera = picamera.PiCamera(resolution=(320, 240), framerate=24)
output = StreamingOutput()
camera.start_recording(output, format='mjpeg')
address = ('', 8000)
server = StreamingServer(address, StreamingHandler)

def func(server):
    """
    Threading function for hosting camera stream
    """
    server.serve_forever()


thread = threading.Thread(target=func, args=(server,))
thread.setDaemon = True
thread.start()

robot = Robot.Robot()
t = 0
while run:
    try:
        data = s1.recv(BUFFER_SIZE)
        data = data.decode('utf-8')
        robot.command(data)
        print(data)
        t = time.time() + 1
        
    except BlockingIOError:
        data = "0:0:0:0:0" #sends invalid data to stop robot
        if time.time()>t:
            robot.command(data) 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246

import socket
import sys
import subprocess
import re
import time
import threading
import cv2
import numpy as np
import requests
from PIL import Image
from bluedot.btcomm import BluetoothServer, BluetoothAdapter

import RPi.GPIO as GPIO
import pygame
import os
import bno #custom module

os.putenv('SDL_VIDEODRIVER', 'fbcon') # Display on piTFT
os.putenv('SDL_FBDEV', '/dev/fb1') 
os.putenv('SDL_MOUSEDRV', 'TSLIB') #setup mouse in pygame
os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen') #touchscreen as mouse



result = subprocess.run('hostname -I', stdout=subprocess.PIPE, 
shell=True)
result = result.stdout.decode('utf-8')
host_IP = re.split(' |\n', result)[0]
print("host_IP: "+host_IP)

result = subprocess.run('hcitool dev|grep hci0', stdout=subprocess.PIPE, 
shell=True)
result = result.stdout.decode('utf-8')
host_mac = re.split(' |\n|\t', result)[2]
print("host_mac: "+host_mac)

client_IP = '10.148.4.162' #default Client IP

code_running = True
turn = 0

#######GPIO CALLBACK FUNCTIONS (Interrupt Handlers)############
def GPIO22_callback(channel):
    """
    interrupt handler for GPIO17; button on piTFT
    Sets the control enabled bit for whether we are turning on 
    motion controls on the robot
    """
    print ("in interrupt 22")
    global turn 
    turn = 1 if turn==0 else 0
    

def GPIO23_callback(channel):
    """
    Unused
    """
    print ("in interrupt 23")
    
    
    
def GPIO27_callback(channel):
    """
    Exits the program. Breakout button
    """
    print ("in interrupt 27")
    global code_running
    code_running = False
    GPIO.cleanup()
    sys.exit(0)
    
GPIO.setmode(GPIO.BCM)
GPIO.setup(22, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(23, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(27, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(22,GPIO.FALLING,callback=GPIO22_callback,
bouncetime=300)
GPIO.add_event_detect(23,GPIO.FALLING,callback=GPIO23_callback,
bouncetime=300)
GPIO.add_event_detect(27,GPIO.FALLING,callback=GPIO27_callback,
bouncetime=300)

size = 320,240
screen = pygame.display.set_mode(size)
pygame.init()

pygame.mouse.set_visible(False)
my_font = pygame.font.Font(None,30)

COMMAND_PORT=5000
CAM_PORT = 8000
BUFFER_SIZE = 1024
get_ip()
time.sleep(2)

def data_received(data):
    """
    Callback fuction for receiving bluetooth messages
    """
    global client_IP
    global flag
    print("Got IP from Client! IP: "+ str(data))
    client_IP = data
    flag = False

def get_ip():
    """
    Receives sender IP through bluetooth
    Relays its IP back to the sender using send's IP that was 
    received
    Also generates pygame GUI to inform user that we are looking
    for bluetooth.
    Blocks until one robot connects 
    """
    global flag
    blue =  BluetoothServer(data_received, power_up_device=True)
    flag = True
    i = 0
    while flag:
        screen.fill((0,0,0))
        mytext = "Connecting to Bluetooth"
        
        if(i==0):
            mytext = mytext+'.'
        elif (i==1):
            mytext = mytext+'..'
        elif (i==2):
            mytext = mytext+'...'
        else:
            i=-1
        i+=1
        text_surface = my_font.render(mytext,True,(255,255,255))
        text_surface = pygame.transform.rotate(text_surface,180)
        rect = text_surface.get_rect(center=(160, 120))	
        screen.blit(text_surface,rect)
        
        time.sleep(1)
        pygame.display.flip()
    blue.send(host_IP)
    blue.stop()


def camera_receive(client_IP):
    """
    Requests video feed from a server hosted by the Robot.
    Saves the images in an encoded array
    """
    while code_running:
        try: 
            r= requests.get('http://' + client_IP + ':8000', stream=True,
            timeout=5)
            if(r.status_code == 200):
                bytes1 = bytes()
                global image
                for chunk in r.iter_content(chunk_size=1024):
                    bytes1 += chunk
                    a = bytes1.find(b'\xff\xd8')
                    b = bytes1.find(b'\xff\xd9')
                    if a != -1 and b != -1:
                        jpg = bytes1[a:b+2]
                        bytes1 = bytes1[b+2:]
                        image = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8), 
                        cv2.IMREAD_COLOR)
                        image = np.flip(image, axis=1)
        except Exception as e:
            print("error :{}".format(e))
            time.sleep(1)

h = 0 #global variables for BNO055 data
r = 0
p = 0

def send_command():
    """
    Sends the BNO055 data to the robot. This function is ran in a thread that
    regularly checks the sensor for data to send. 
    The message is send using UDP socket. The message format is:
    valid:control:heading:roll:pitch

    valid  : if message sent is valid
    control: control enable
    heading: not used 
    roll   : control robot direction
    pitch  : control robot speed and forward/backward movement
    """
    global h,r,p
    while code_running:
        command_server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        bno.bno_init()
        try:
            while code_running:
                h,r,p = bno.bno_poll()
                message = str(1)+":"+str(turn)+":"+str(h)+":"+str(r)+":"+str(p)
                command_server.sendto(message.encode(), (client_IP, COMMAND_PORT))
                time.sleep(0.3) #defines the period of when to check and send data
        except Exception as e:
            print("error :{}".format(e))
        command_server.close()

th = threading.Thread(target=camera_receive, args=(client_IP,))
th.setDaemon = True
th.start()

command_thread = threading.Thread(target=send_command)
command_thread.setDaemon = True
command_thread.start()

image = np.zeros((320, 240), dtype=np.uint8)
t = 0
turn_p = turn

while code_running:
    try:
        surf = pygame.Surface((image.shape[0], image.shape[1]))	
        pygame.surfarray.blit_array(surf, image.astype(np.int))
        rect1 = surf.get_rect()
        surf = pygame.transform.rotate(surf, 90)
        rect1.x = 0
        rect1.y = 0
        screen.blit(surf, rect1)
        my_font_hud = pygame.font.Font(None,8)
        my_text_hud = 't:'+str(turn)+' h:'+str(h)+' r:'+str(r)+' p:'+str(p)
        text_surface_hud = my_font.render(my_text_hud,True,(50,255,50))
        text_surface_hud = pygame.transform.rotate(text_surface_hud,180)
        rect = text_surface_hud.get_rect(center=(160, 220))	
        screen.blit(text_surface_hud,rect)	
        if(turn_p != turn):
            
            t = time.time() + 2
            turn_p = turn
            
        if time.time() < t:
            if turn == True:
                my_text = 'Control Enabled!'
            else:
                my_text = 'Control Disabled!'
            text_surface = my_font.render(my_text,True,(255,0,255))
            text_surface = pygame.transform.rotate(text_surface,180)
            rect = text_surface.get_rect(center=(160, 120))		 
            screen.blit(text_surface,rect)
        pygame.display.flip()
    except KeyboardInterrupt:
        code_running = False
        
GPIO.cleanup()
sys.exit(0)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166

import RPi.GPIO as GPIO 
import time

class Robot:
    """
    Initializes the two servos using RPi.GPIO library to control GPIO. 
    Handles the timing of the servos and sets the speed and rotation 
    directions of the robot
    """
    
    LEFT_SERVO_PIN = 6
    RIGHT_SERVO_PIN = 13
    STOP_T  = 0.0015 
    CLKW_T  = 0.0013
    CCLKW_T = 0.0017
    DOWN_T  = 0.02
    STOP_DC = STOP_T/(STOP_T+DOWN_T)*100 #cycle in percentage
    STOP_FREQ = 1/(STOP_T+DOWN_T)

    def __init__(self):
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.LEFT_SERVO_PIN, GPIO.OUT)
        GPIO.setup(self.RIGHT_SERVO_PIN, GPIO.OUT)
        self.right_speed = self.STOP_T
        self.left_speed  = self.STOP_T
        self.left_servo = GPIO.PWM(self.LEFT_SERVO_PIN, self.STOP_FREQ)
        self.right_servo = GPIO.PWM(self.RIGHT_SERVO_PIN, self.STOP_FREQ)
        self.left_servo.start(self.STOP_DC)
        self.right_servo.start(self.STOP_DC)

    def stop(self):
        self.left_servo.ChangeDutyCycle(self.STOP_DC)
        self.right_servo.ChangeDutyCycle(self.STOP_DC)
        self.left_servo.ChangeFrequency(self.STOP_FREQ)
        self.right_servo.ChangeFrequency(self.STOP_FREQ)
    
    def forward(self):
        print("Moving forward")
        self.left_servo.ChangeDutyCycle(self.CCLKW_T/(self.CCLKW_T+self.DOWN_T)*100)
        self.left_servo.ChangeFrequency(1/(self.CCLKW_T+self.DOWN_T))
        self.right_servo.ChangeDutyCycle(self.CLKW_T/(self.CLKW_T+self.DOWN_T)*100)
        self.right_servo.ChangeFrequency(1/(self.CLKW_T+self.DOWN_T))

    def left(self):
        print("Moving left")
        self.left_servo.ChangeDutyCycle(self.CLKW_T/(self.CLKW_T+self.DOWN_T)*100)
        self.left_servo.ChangeFrequency(1/(self.CLKW_T+self.DOWN_T))
        self.right_servo.ChangeDutyCycle(self.CLKW_T/(self.CLKW_T+self.DOWN_T)*100)
        self.right_servo.ChangeFrequency(1/(self.CLKW_T+self.DOWN_T))
    
    def right(self):
        print("Moving right")
        self.left_servo.ChangeDutyCycle(self.CCLKW_T/(self.CCLKW_T+self.DOWN_T)*100)
        self.left_servo.ChangeFrequency(1/(self.CCLKW_T+self.DOWN_T))
        self.right_servo.ChangeDutyCycle(self.CCLKW_T/(self.CCLKW_T+self.DOWN_T)*100)
        self.right_servo.ChangeFrequency(1/(self.CCLKW_T+self.DOWN_T))

    def set_speed(self, interval1, interval2):
        print(str(interval1) + ' ' + str(interval2))
        if interval1 !=-1:
            self.left_servo.ChangeDutyCycle(interval1/(self.DOWN_T+interval1)*100)
            self.left_servo.ChangeFrequency(1/(self.DOWN_T+interval1))
        if interval1 != -1:
            self.right_servo.ChangeDutyCycle(interval2/(self.DOWN_T+interval2)*100)
            self.right_servo.ChangeFrequency(1/(self.DOWN_T+interval2))
    
    def command(self, input_str):
        '''
        Decodes message from the base station
        Values_List = [Valid,turn,heading,roll,pitch] 
        Parameter input_str: string of commands sent through UDP
        Moves robot if turn is 1
        Stops robot if turn or valid is 0
        Decodes the robot speed and direction based on heading and
        pitch
        '''
        data = input_str.split(':')
        valid = int(data[0])
        if valid: 
            turn = int(data[1])
            if turn:
                h = float(data[2])
                r = float(data[3])
                p = float(data[4])
                
                if r>180:
                    r-=360
                elif r<-180:
                    r+=360
                if p>180:
                    p-=360
                elif p<-180:
                    p+=360
                
                threshold = 5
                
                if r<threshold and r>-threshold:
                    r = 0
                if p<threshold and p>-threshold:
                    p = 0
                
                roll_max = 50
                roll_min = -50
                pitch_max = 50
                pitch_min = -50
                if r<roll_min:
                    roll = roll_min
                elif r>roll_max:
                    roll = roll_max
                else:
                    roll = r
                if p<pitch_min:
                    pitch = pitch_min
                elif p>pitch_max:
                    pitch = pitch_max
                else:
                    pitch = p
                
                rslope = 0.0001/roll_max
                pslope = 2*rslope
                
                left = self.left_speed + rslope*roll - pslope * pitch
                right = self.right_speed + rslope*roll + pslope * pitch 
                
                if left>self.CCLKW_T:
                    left = self.CCLKW_T
                if right<self.CLKW_T:
                    right = self.CLKW_T
                
                self.set_speed(left,right)
                print ("left: "+str(left)+" right:"+str(right))
            else:
                self.stop()
            
        else:
            self.stop()


    def calibrate(self):
        """
        Calibrates the servo by setting the robot to move in all
        four directions
        """
        self.left_servo.start(self.STOP_DC)
        self.right_servo.start(self.STOP_DC)
        self.stop()
        time.sleep(5)
        self.forward()
        time.sleep(5)
        self.right()
        time.sleep(5)
        self.left()
        time.sleep(5)
    
    
    def shutdown(self):
        GPIO.cleanup()


if __name__=='__main__':
    
    robot = Robot()
    
    
    robot.calibrate()
    robot.shutdown()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

import logging
import sys
import time

from Adafruit_BNO055 import BNO055
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)

is_calibrating = False
zero_heading = 0
zero_roll = 0
zero_pitch = 0
bno = 0

def GPIO17_callback(channel):
    """
    interrupt handler for GPIO17; button on piTFT
    Changes to whether we are calibrating or not
    """
    print ("in interrupt 17")
    global is_calibrating
    is_calibrating = not is_calibrating

GPIO.add_event_detect(17, GPIO.FALLING, callback=GPIO17_callback, bouncetime=300)

def calibrate(bno):
    """
    Set up the natural position the sensor values by setting the global zero_heading, 
    zero_roll and zero_pitch values. These are the natural position values.
    """
    print("CALIBRATING")
    global zero_heading, zero_roll, zero_pitch
    while (is_calibrating):
        print("CALIBRATING")
        #bno.set_calibration(bno.get_calibration())

        zero_heading, zero_roll, zero_pitch = bno.read_euler()
        sys, gyro, accel, mag = bno.get_calibration_status()
        # Print everything out.
        print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} \
        Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format(
                zero_heading, zero_roll, zero_pitch, sys, gyro, accel, mag))
        time.sleep(1)
    
    
def bno_init():
    """
    Initializes the BNO055 sensor. Handles when BNO connection fails.
    """
    global bno
    bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)

    #if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
    #   logging.basicConfig(level=logging.DEBUG)
    while True:
        try:
            if not bno.begin():
                raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')    
            status, self_test, error = bno.get_system_status()
            break
        except Exception as e:
            print("error :{}".format(e))
            time.sleep(0.5)
        
    print('System status: {0}'.format(status))
    print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
    # Print out an error if system status is in error mode.
    if status == 0x01:
        print('System error: {0}'.format(error))
        print('See datasheet section 4.3.59 for the meaning.')

    # Print BNO055 software revision and other diagnostic data.
    sw, bl, accel, mag, gyro = bno.get_revision()
    print('Software version:   {0}'.format(sw))
    print('Bootloader version: {0}'.format(bl))
    print('Accelerometer ID:   0x{0:02X}'.format(accel))
    print('Magnetometer ID:    0x{0:02X}'.format(mag))
    print('Gyroscope ID:       0x{0:02X}\n'.format(gyro))
        

def bno_poll():
    """
    Polls from BNO and also checks calibration values
    """
    if is_calibrating:
        calibrate(bno)
    
    # Read the Euler angles for heading, roll, pitch (all in degrees).
    heading, roll, pitch = bno.read_euler()
    # Read the calibration status, 0=uncalibrated and 3=fully calibrated.
    sys, gyro, accel, mag = bno.get_calibration_status()
    # Print everything out.
    #print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} \
    # Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format(
        # heading, roll, pitch, sys, gyro, accel, mag))
    D_H = heading-zero_heading
    D_R = roll-zero_roll
    D_P = pitch-zero_pitch
    #print ("D_H = {0:0.2F} D_R = {1:0.2F} D_P={2:0.2F}".format(
    #D_H, D_R, D_P))
    return (D_H, D_R, D_P)