Another Ambassador Moment: Rescue Robot: Scouting Owl

#Include all the libraries
import RPi.GPIO as GPIO #The GPIO library
import time #The time library
import sys #The sys library
import os #The os library
import pygame #The pygame library
from math import * #The math library
import numpy as np #The numpy library
import busio #The bus input output library
import board #The board library
import socket #The TCP IP socket library
from scipy.interpolate import griddata #The scipy library
from colour import Color #The colour library
import adafruit_amg88xx #The adafruit library for IR camera
import matplotlib.pyplot as plt #The matplotlib library

pygame.mixer.init() #Pygame mixer initialization for sounds
GPIO.setmode(GPIO.BCM) #Set BCM for GPIO

motor1PWMPin = 26 #PWM Pin for motor 1
motor1Pin1 = 5 #Pin 1 for motor 1
motor1Pin2 = 6 #Pin 2 for motor 1
motor2PWMPin = 16 #PWM Pin for motor 2
motor2Pin1 = 20 #Pin 1 for motor 2
motor2Pin2 = 21 #Pin 2 for motor 2

freq = 50 #Set PWM frequency to 50 Hz
dutyCycle = 50 #Set default duty cycle to 50%

CLK = 11 #Clock pin for SPI
MISO = 9 #MISO pin for SPI
MOSI = 10 #MOSI pin for SPI
CS = 17 #Chip Select pin for SPI

GPIO_TRIGGER = 18 #Trigger pin for ultrasonic
GPIO_ECHO = 24 #Echo pin for ultrasonic
GPIO_WATERLEVEL = 22 #Pin for water level sensor
GPIO_FLAME = 13 #Pin for flame level sensor
GPIO_ANTENNA = 27 #Pin for sensing radiation

GPIO.setup(motor1PWMPin, GPIO.OUT) #Setup motor 1 PWM pin as output
GPIO.setup(motor1Pin1, GPIO.OUT) #Setup motor 1 pin 1 as output
GPIO.setup(motor1Pin2, GPIO.OUT) #Setup motor 1 pin 2 as output
GPIO.setup(motor2PWMPin, GPIO.OUT) #Setup motor 2 PWM pin as output
GPIO.setup(motor2Pin1, GPIO.OUT) #Setup motor 2 pin 1 as output
GPIO.setup(motor2Pin2, GPIO.OUT) #Setup motor 2 pin 2 as output
GPIO.output(motor1Pin1, GPIO.LOW) #Set motor 1 pin 1 to be low
GPIO.output(motor1Pin2, GPIO.LOW) #Set motor 1 pin 2 to be low
GPIO.output(motor2Pin1, GPIO.LOW) #Set motor 2 pin 1 to be low
GPIO.output(motor2Pin2, GPIO.LOW) #Set motor 2 pin 2 to be low
motor1PWM = GPIO.PWM(motor1PWMPin, freq) #Initialize motor 1 PWM pin
motor2PWM = GPIO.PWM(motor2PWMPin, freq) #Initialize motor 2 PWM pin
motor1PWM.start(dutyCycle) #Start motor 1 PWM with the given duty cycle
motor2PWM.start(dutyCycle) #Start motor 2 PWM with the given duty cycle

GPIO.setup(GPIO_TRIGGER, GPIO.OUT) #Setup trigger pin as output
GPIO.setup(GPIO_ECHO, GPIO.IN) #Setup echo pin as input
GPIO.setup(GPIO_WATERLEVEL, GPIO.IN, pull_up_down = GPIO.PUD_DOWN) #Setup water level sensor pin as pulled down input
GPIO.setup(GPIO_FLAME, GPIO.IN, pull_up_down = GPIO.PUD_DOWN) #Setup flame sensor pin as pulled down input
GPIO.setup(GPIO_ANTENNA, GPIO.IN, pull_up_down = GPIO.PUD_DOWN) #Setup radiation sensor pin as pulled down input

fireVar = "No Fire" #Value for fire data to be sent
waterVar = "No Water" #Value for water data to be sent
radiationVar = "No Radiation" #Value for radiation data to be sent

MINTEMP = 26.0 #Minimum temperature to be detected
MAXTEMP = 32.0 #Maximum temperature to be detected
COLORDEPTH = 1024 #Colour depth to be 1024

distVar = 50 #Value for distance sensor
tempVar = 25 #Temperature data to be sent
distToStop = 15 #Distance threshold for the robot to stop

command = "STOP" #The command to control robot
prevCommand = "STOP" #The last received command

#Constraint function to limit the value
def constrain(val, min_val, max_val):
    return min(max_val, max(min_val, val))

#Function to fetch the distance from the ultrasonic sensor
def distance():
    GPIO.output(GPIO_TRIGGER, True)
    time.sleep(0.00001)
    GPIO.output(GPIO_TRIGGER, False)
    
    StartTime = time.time()
    StopTime = time.time()
    
    while GPIO.input(GPIO_ECHO) == 0:
        StartTime = time.time()
 
    while GPIO.input(GPIO_ECHO) == 1:
        StopTime = time.time()
 

    TimeElapsed = StopTime - StartTime
    distance = (TimeElapsed * 34300) / 2
    return distance

#Function to convert robot coordinates to global coordinates
def robot2global(pose, xyR):
    theta = pose[2]
    Trg = [[cos(theta), -sin(theta), pose[0]],
           [sin(theta), cos(theta), pose[1]],
           [0, 0, 1]]
    Trg = np.array(Trg)
    xyR.append(1)
    xyR = np.array(xyR)
    xyG = np.matmul(Trg, xyR.transpose())
    xyG = xyG[0:2]
    return xyG

#The mapping function
def startMap():
    global dutyCycle, motor1Pin1, motor1Pin2, motor2Pin1, motor2Pin2, motor1PWM, motor2PWM
    
    #Change the duty cycle to 100%
    motor1PWM.ChangeDutyCycle(100)
    motor2PWM.ChangeDutyCycle(100)
    
    distArray = []
    sensorOrigin = [10.8, 0]
    numPt = 200 #Number of points to be plotted
    theta = np.linspace(0, 2 * pi, numPt)
    
    #Fetch the distance for all the points
    for i in range(0, numPt):
        GPIO.output(motor1Pin1, GPIO.HIGH)
        GPIO.output(motor1Pin2, GPIO.LOW)
        GPIO.output(motor2Pin1, GPIO.HIGH)
        GPIO.output(motor2Pin2, GPIO.LOW)
        time.sleep(0.01)
        GPIO.output(motor1Pin1,GPIO.LOW)
        GPIO.output(motor1Pin2,GPIO.LOW)
        GPIO.output(motor2Pin1,GPIO.LOW)
        GPIO.output(motor2Pin2,GPIO.LOW)
        time.sleep(0.1)
        distArray.append(distance())
    xVal = []
    yVal = []
    for i in range(0, numPt):
        dpy = 0
        dpx = distArray[i] + sensorOrigin[0]
        depthPtG = robot2global([0,0,theta[i]], [dpx, dpy])
        xVal.append(depthPtG[0])
        yVal.append(depthPtG[1])
    plt.plot(0, 0, 'r+') #Plot the bot as a red +
    plt.plot(xVal, yVal, 'bo') #Plot the distances as blue circles
    plt.show() #Show the plot
    #Change the duty cycle back
    motor1PWM.ChangeDutyCycle(dutyCycle)
    motor2PWM.ChangeDutyCycle(dutyCycle)

#Function to map a value in a given range
def map_value(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

#Setup the SPI pins
def setupSpiPins(clkPin, misoPin, mosiPin, csPin):
    GPIO.setup(clkPin, GPIO.OUT)
    GPIO.setup(misoPin, GPIO.IN)
    GPIO.setup(mosiPin, GPIO.OUT)
    GPIO.setup(csPin, GPIO.OUT)
     
#Read the ADC values using SPI
def readAdc(channel, clkPin, misoPin, mosiPin, csPin):
    if (channel < 0) or (channel > 7):
        print("Invalid ADC Channel number, must be between [0,7]")
        return -1
        
    GPIO.output(csPin, GPIO.HIGH)
    GPIO.output(csPin, GPIO.LOW)
    GPIO.output(clkPin, GPIO.HIGH)
    read_command = 0x18
    read_command |= channel
    
    sendBits(read_command, 5, clkPin, mosiPin)
    
    adcValue = recvBits(12, clkPin, misoPin)
    GPIO.output(csPin, GPIO.HIGH)
    return adcValue

#Send the bits    
def sendBits(data, numBits, clkPin, mosiPin):
    data <<= (8 - numBits)
    
    for bit in range(numBits):
        if data & 0x80:
            GPIO.output(mosiPin, GPIO.HIGH)
        else:
            GPIO.output(mosiPin, GPIO.LOW)
        data <<= 1
        
        GPIO.output(clkPin, GPIO.HIGH)
        GPIO.output(clkPin, GPIO.LOW)

#Receive the data bits
def recvBits(numBits, clkPin, misoPin):
    retVal = 0
    
    for bit in range(numBits):
        GPIO.output(clkPin, GPIO.HIGH)
        GPIO.output(clkPin, GPIO.LOW)
        if GPIO.input(misoPin):
            retVal |= 0x1
        
        retVal <<= 1
    
    return (retVal/2)

#Callback function for water level sensor
def trigWater(channel):
    global waterVar
    if(waterVar == "Water"):
        waterVar = "No Water"
    else:
        waterVar = "Water"

#Callback function for fire sensor    
def trigFire(channel):
    global fireVar
    if(fireVar == "Fire"):
        fireVar = "No Fire"
    else:
        fireVar = "Fire"

#Callback function for radiation sensor
def trigRadiation(channel):
    global radiationVar, fireVar, waterVar
    radiationVar = "Radiation"
    if(fireVar == "Fire"):
        fireVar = "No Fire"
    else:
        fireVar = "Fire"
    if(waterVar == "Water"):
        waterVar = "No Water"
    else:
        waterVar = "Water"

i2c_bus = busio.I2C(board.SCL, board.SDA) #Setup the I2C bus
sensor = adafruit_amg88xx.AMG88XX(i2c_bus) #Setup the IR sensor using I2C

points = [(floor(ix / 8), (ix % 8)) for ix in range(0, 64)] #Points for the IR sensor
grid_x, grid_y = np.mgrid[0:7:32j, 0:7:32j] #Create the grid

blue = Color("indigo") #The colour blue
colors = list(blue.range_to(Color("red"), COLORDEPTH)) #The colours list
colors = [(int(c.red * 255), int(c.green * 255), int(c.blue * 255)) for c in colors]

time.sleep(0.1) #Do not remove. Lets sensor initialize

GPIO.add_event_detect(GPIO_WATERLEVEL, GPIO.BOTH, callback = trigWater, bouncetime = 300) #The event detection for water level sensor
GPIO.add_event_detect(GPIO_FLAME, GPIO.BOTH, callback = trigFire, bouncetime = 300) #The event detection for fire sensor
GPIO.add_event_detect(GPIO_ANTENNA, GPIO.BOTH, callback = trigRadiation, bouncetime = 300) #The event detection for radiation detection sensor

timeStamp = time.time() #The start time flag

if __name__ == '__main__':
    try:
        setupSpiPins(CLK, MISO, MOSI, CS) #Call the SPI setup function
        HOST = '10.49.32.252' #The server IP address (RaspberryPi in this case)
        PORT = 8000 #The port for the socket
        print(socket.gethostname()) #Print the hostname
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #Start the socket
        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) #Configure the socket
        s.bind((HOST, PORT)) #Bind the socket
        s.listen(5) #Listen for clients
        conn, addr = s.accept() #Accept the connection
        print(addr) #Print the address of the connected clients

        connected = False #The connected flag
    
        while True: #While infinity
            connected = False #Default the connected flag to false
            pixels = [] #The pixels array
            for row in sensor.pixels:
                pixels = pixels + row
            pixels = [map_value(p, MINTEMP, MAXTEMP, 0, COLORDEPTH - 1) for p in pixels]

            bicubic = griddata(points, pixels, (grid_x, grid_y), method="cubic")

            data = "" #The data to be transmitted
            for pixel in pixels: #Append the pixels to the data to be transmitted with a ',' delimiter
                data = data + str(pixel) + ","
            data = data + ";"  #Append the ';' delimiter
            for bi in bicubic: #Append the bicubic to the data to be transmitted with a ',' delimiter
                for b in bi:
                    data = data + str(b) + ","
            data = data + ";" #Append the ';' delimiter
            for color in colors: #Append the color to the data to be transmitted with a ':' delimiter
                data = data + str(color) + ":"
            data = data + ";" #Append the ';' delimiter
            data = data + waterVar + "," + fireVar + "," + radiationVar + "," + str(tempVar) + "," + str(distVar) + ";" #Append the sensor data to the data to be transmitted with a ',' delimiter
            try:
                conn.send((data + "\n").encode()) #Encode the data and transmit over TCP/IP connection
                connected = True #If no error is thrown, mark the connected flag as true
            except: #If an error is thrown
                #Try reconnecting
                s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
                s.bind((HOST, PORT))
                s.listen(5)
                conn, addr = s.accept()
                print(addr)
                connected = True #Mark the connected flag as true
            if connected: #If connected
                try:
                    command = conn.recv(1024) #Receive the data from clients
                    command = command.decode().strip() #If the data is received, decode the command, then strip all the whitespaces
                except: #If the data reception fails
                    connected = False #Mark the connected flag as false
            if(command != prevCommand): #If the command has changed
                print(command) #Print the new command
                
            if(command == "STOP" and prevCommand != "STOP"): #If the new command is STOP
                GPIO.output(motor1Pin1, GPIO.LOW)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.LOW)
            elif(command == "UP" and prevCommand != "UP"): #If the new command is UP
                if distVar > distToStop: #If the distance is greater than the threshold value
                    GPIO.output(motor1Pin1, GPIO.LOW)
                    GPIO.output(motor1Pin2, GPIO.HIGH)
                    GPIO.output(motor2Pin1, GPIO.HIGH)
                    GPIO.output(motor2Pin2, GPIO.LOW)
                else:
                    GPIO.output(motor1Pin1, GPIO.LOW)
                    GPIO.output(motor1Pin2, GPIO.LOW)
                    GPIO.output(motor2Pin1, GPIO.LOW)
                    GPIO.output(motor2Pin2, GPIO.LOW)
            elif(command == "DOWN" and prevCommand != "DOWN"): #If the new command is DOWN
                GPIO.output(motor1Pin1, GPIO.HIGH)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.HIGH)
            elif(command == "LEFT" and prevCommand != "LEFT"): #If the new command is LEFT
                GPIO.output(motor1Pin1, GPIO.HIGH)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.HIGH)
                GPIO.output(motor2Pin2, GPIO.LOW)
            elif(command == "RIGHT" and prevCommand != "RIGHT"): #If the new command is RIGHT
                GPIO.output(motor1Pin1, GPIO.LOW)
                GPIO.output(motor1Pin2, GPIO.HIGH)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.HIGH)
            elif(command == "HELP" and prevCommand != "HELP"): #If the new command is HELP
                GPIO.output(motor1Pin1, GPIO.LOW)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.LOW)
                pygame.mixer.music.load("help.wav") #Load the audio file
                pygame.mixer.music.play() #Play the audio file
            elif(command == "RECORD" and prevCommand != "RECORD"): #If the new command is RECORD
                GPIO.output(motor1Pin1, GPIO.LOW)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.LOW)
                pygame.mixer.music.load("record.wav") #Load the audio file
                pygame.mixer.music.play() #Play the audio file
                while(pygame.mixer.music.get_busy() == True): #While the audio files is being played
                    pass #Pass
                os.system('arecord --device=hw:2,0 -d 5 --format S16_LE --rate 44100 -c1 victimRecord.wav -V mono') #System command to record from microphone
            elif(command == "PLAY" and prevCommand != "PLAY"): #If the new command is PLAY
                GPIO.output(motor1Pin1, GPIO.LOW)
                GPIO.output(motor1Pin2, GPIO.LOW)
                GPIO.output(motor2Pin1, GPIO.LOW)
                GPIO.output(motor2Pin2, GPIO.LOW)
                pygame.mixer.music.load("victimRecord.wav") #Load the audio file
                pygame.mixer.music.play() #Play the audio file
            elif(command == "MAP" and prevCommand != "MAP"): #If the new command is MAP
                startMap() #Start mapping
            elif(command == "INC"): #If the new command is INC
                if dutyCycle < 100: #If duty cycle is less than 100
                    dutyCycle += 1 #Increment duty cycle
                    #Change duty cycle
                    motor1PWM.ChangeDutyCycle(dutyCycle)
                    motor2PWM.ChangeDutyCycle(dutyCycle)
            elif(command == "DEC"): #If the new command is DEC
                if dutyCycle > 40: #If duty cycle is greater than 40
                    dutyCycle -= 1 #Decrement duty cycle
                    #Change duty cycle
                    motor1PWM.ChangeDutyCycle(dutyCycle)
                    motor2PWM.ChangeDutyCycle(dutyCycle)
            
            prevCommand = command #Change previous command
            
            if(time.time() - timeStamp > 0.01): #If the time elapsed is 1 millisecond
                adc = readAdc(0, CLK, MISO, MOSI, CS) #Read ADC
                try:
                    tempVar = 10 * log(10000 * (1024 / (adc / 3.3) - 1), 10) - 10 #Scale the temperature
                    tempVar = round(tempVar, 3) #Round the temperature to 3 digits
                except: #If it throws an error
                    pass
                distVar = round(distance(), 3) #Fetch the distance and round it to 3 digits
                if(distVar < distToStop): #If the distance is less tha  threshold distance
                    print("Dist Stopping..") #Print "Dist Stopping.."
                timeStamp = time.time() #Update the timestamp
    except KeyboardInterrupt: #If interrupted from keyboard
        motor1PWM.stop() #Stop the motor 1 PWM
        motor2PWM.stop() #Stop the motor 2 PWM
        GPIO.cleanup() #Cleanup GPIO
        print("Exiting...") #Print "Exiting..."
        sys.exit(0) #Exit the program