robotObjectSim.py

from sr.robot3 import *
import time
import math

class robot:
    
    def __init__(self):
        
        #Creates an instance of the robot object from sr.robot3 and saves it as in attribute to the instance of this class
        self.R = Robot() 
        
        markers = self.R.camera.see_ids()

        corner0 = [25,26,27,0,1,2]
        corner1 = [4,5,6,7,8,9]
        corner2 = [11,12,13,14,15,16]
        corner3 = [18,19,20,21,22,23]
        self.cornerNum = 0
        self.wallMarkers = [corner0, corner1, corner2, corner3]
       
        #Deploys arms to the correct position upon initialising the robot 
        #self.R.servo_board.servos[1].position = 0.8
        #self.R.servo_board.servos[2].position = -0.8
    def move(self):
        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = 0.5
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
        
    def spin(self):
        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = -0.5
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
    
    def checkCollisons(self):
        collided = False
        if (self.R.ruggeduino.pins[A0].analogue_read() < 0.5 or self.R.ruggeduino.pins[A0].analogue_read() < 0.5) and ((self.R.ruggeduino.pins[A4].analogue_read() > 0) and (self.R.ruggeduino.pins[A4].analogue_read() < 0.17)):
            collided = True
        return collided 
    
    def findStartingCorner(self):
        self.spin()
        markers = self.R.camera.see()
        mini = markers[0]
        if mini.id in self.wallMarkers[0]:
            self.starting_corner = 0
        elif mini.id in self.wallMarkers[1]:
            self.starting_corner = 1
        elif mini.id in self.wallMarkers[2]:
            self.starting_corner = 2
        elif mini.id in self.wallMarkers[3]:
            self.starting_corner = 3
        
    
    def moveDist(self, dist, speed=0.5):
        
        #speed defaults to 0.5 so it doesn't need to be passed
        rotDist = 100 * math.pi
        self.R.ruggeduino.command("s")
        self.R.motor_board.motors[0].power = speed
        self.R.motor_board.motors[1].power = speed
        encLeft = self.R.ruggeduino.command("x")
        encRight = self.R.ruggeduino.command("y")
        while (encLeft + encRight)/2 < (dist/rotDist)*360:
            encLeft = self.R.ruggeduino.command("x")
            encRight = self.R.ruggeduino.command("y")
            time.sleep(0.005)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0 

    def faceDirection(self, direc, speed=0.5):
        
        #this method is used for turning the robot to face one of the 4 walls 
        #uses a 2D array to determine wall markers in each direction relative to a birds eye perspective 
        wallMarkers = [[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]]
        #input sanitation 
        direc = direc.lower()
        #chooses the used markers based on input given
        if direc == "north":
            usedMarkers = self.wallMarkers[0]
        elif direc == "south":
            usedMarkers = self.wallMarkers[2]
        elif direc == "east":
            usedMarkers = self.wallMarkers[1]
        elif direc == "west":
            usedMarkers = self.wallMarkers[3]
        print(usedMarkers)
        markers = self.R.camera.see_ids()
        print(markers)
        #creates an array of markers that the camera both sees and are on the correct wall relative to the robot 
        intersection = [marker for marker in markers if marker in usedMarkers]
        print(intersection)
        #rotates the robot until the above task is complete 
        while len(intersection) < 1:
            self.R.motor_board.motors[0].power = 0.5
            self.R.motor_board.motors[1].power = -0.5
            self.R.sleep(0.1)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            markers = self.R.camera.see_ids()
            intersection = [marker for marker in markers if marker in usedMarkers]
            
        markersFull = self.R.camera.see()
       
        #creates a second list of full markers where only the markers that have been seen and are located on the correct wall are used 
        intersection2 = [marker for marker in markersFull if marker.id in usedMarkers]
        
        #checks for the angle to the wall within a 5 degree margin of error and turns to face that wall accordingly 
        Facing = False
        while not Facing:
            #loops through each value the set of markers seen and checks for perpendicularity 
            for i in intersection2:
                if i.orientation.rot_y * (180/math.pi) > 0 and i.orientation.rot_y * (180/math.pi) < 0:
                    Facing = True
            self.R.motor_board.motors[0].power = 0.2
            self.R.motor_board.motors[1].power = -0.2
            time.sleep(0.1)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            
            markersFull = self.R.camera.see()
            intersection2 = [marker for marker in markersFull if marker.id in usedMarkers]

    
    #functions for interacting with brainboard LEDs 
    def turnOnLED (self, name="A", colour="cyan"):
        if name == "A" and colour == "cyan":
            self.R.kch.leds[UserLED.A] = Colour.CYAN
        if name == "B" and colour == "cyan":
            self.R.kch.leds[UserLED.B] = Colour.CYAN
        if name == "C" and colour == "cyan":
            self.R.kch.leds[UserLED.C] = Colour.CYAN
        if name == "A" and colour == "cyan":
            self.R.kch.leds[UserLED.A] = Colour.RED
        if name == "B" and colour == "cyan":
            self.R.kch.leds[UserLED.B] = Colour.RED
        if name == "C" and colour == "cyan":
            self.R.kch.leds[UserLED.C] = Colour.RED
        
    def turnOffLED (self, name="A"):
        if name == "A" and colour == "cyan":
            self.R.kch.leds[UserLED.A] = Colour.OFF
        if name == "B" and colour == "cyan":
            self.R.kch.leds[UserLED.B] = Colour.OFF
        if name == "C" and colour == "cyan":
            self.R.kch.leds[UserLED.C] = Colour.OFF
    
    #to be added for a rotation function
    def rotate(self, degrees):
        pass
    
    def faceMarker(self, marker_id, speed=0.5):
        markers = self.R.camera.see_ids()
        print(markers) 
        #the robot will stop roating as soon it sees the correct marker in its peripheral 
        while marker_id not in markers:
            self.R.motor_board.motors[0].power = 0.5
            self.R.motor_board.motors[1].power = -0.5
            self.R.sleep(0.1)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            markers = self.R.camera.see_ids()

        #sets up the initial distance as it compares the distance to the marker to previous distances  
        markers = self.R.camera.see()
        for marker in markers:
            if marker.id == marker_id:
                dist = marker.distance
        dist_diff = 1
        dist2=0
        while dist_diff > 0:
            
            self.R.motor_board.motors[0].power = 0.2
            self.R.motor_board.motors[1].power = -0.2
            time.sleep(0.1)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            markers = self.R.camera.see()
            for marker in markers:
                if marker.id == marker_id:
                    dist2 = marker.distance
            dist_diff = dist - dist2 
            dist = dist2

        self.R.motor_board.motors[0].power = -0.2
        self.R.motor_board.motors[1].power = 0.2
        time.sleep(0.1)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
        
    

    def goToMarker(self, marker_id, speed=0.5):
        self.faceMarker(marker_id)
        markers = self.R.camera.see()
        for marker in markers:
            if marker.id == marker_id:
                usedMarker = marker
        #adds 5cm buffer between robot and wall
        while usedMarker.distance > 50 and not self.checkCollisons() :
            print(self.R.ruggeduino.pins[A0].analogue_read())
            self.R.motor_board.motors[0].power = 0.5
            self.R.motor_board.motors[1].power = 0.5
            self.R.sleep(0.5)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
    
    def goHome(self):
        markers = self.R.camera.see_ids()
        intersection = [marker for marker in markers if marker in self.wallMarkers[self.starting_corner][:len(self.wallMarkers[self.starting_corner])//2]]
        while len(intersection) < 1:
            self.spin()
            markers = self.R.camera.see_ids()
            intersection = [marker for marker in markers if marker in self.wallMarkers[self.starting_corner][:len(self.wallMarkers[self.starting_corner])//2]]
        self.goToMarker(intersection[0])
    
    def grabBoxes(self):
        self.R.motor_board.motors[0].power = -0.25
        self.R.motor_board.motors[1].power = 0.25
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
        markers = self.R.camera.see()
        for marker in markers:
            if marker.id > 27:
                print(marker.distance)
                print(marker.id)
                self.goToMarker(marker.id)
                if self.R.ruggeduino.pins[A4].analogue_read() > 1:
                    print(self.R.ruggeduino.pins[A4].analogue_read())
                    self.R.servo_board.servos[1].position = 0
                    while not self.R.ruggeduino.pins[2].digital_read():
                        self.R.motor_board.motors[0].power = -0.5
                        self.R.motor_board.motors[1].power = -0.5
                        self.R.sleep(0.5)
                        self.R.motor_board.motors[0].power = 0
                        self.R.motor_board.motors[1].power = 0
                    self.R.servo_board.servos[0].position = 1
    
    #drives to marker until close enough
    def driveToMarker(self, marker):

        #hard coded testing for the method, can be removed once goToMarker is accurate
        
        self.R.faceMarker(marker)
        self.R.motor_board.motors[1].power = 0.5
        self.R.sleep(0.28)       
        self.R.motor_board.motors[0].power = 0.5

        #to see if the robot is close enough to the marker or any obstacles, breaks out of loop once that is the case
        going = True
        while going:
            going = self.checkMarker()
            self.R.sleep(0.1)
        
        #stops to robot
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0

    #method that checks distance of obstacle to robot
    def checkMarker(self, toStop=0.5):

        going = True

        #checks whether closest thing is a marker or an obstacle and sets the distance to stop accordingly
        markers = self.R.camera.see()
        if markers[0].id > 27:
            toStop = 0.2
        

        #checks the distance of the nearest object to the robot
        frontDistance = self.R.ruggeduino.pins[A4].analogue_read()
        frontLeft = self.R.ruggeduino.pins[A0].analogue_read()
        frontRight = self.R.ruggeduino.pins[A1].analogue_read()


        #if distance to object is less than a certain distance, the method tells robot to stop through the return
        if frontDistance < toStop or frontLeft < toStop - 0.2 or frontRight < toStop - 0.2:
            going = False
        
        return going
    
    def faceClosestToken(self, counter):
        markers = self.R.camera.see()
        idx = 0 
        minDist = 9000000000 
        for i in range(counter):
            self.R.motor_board.motors[0].power = 0.08
            self.R.motor_board.motors[1].power = -0.08
            self.R.sleep(0.5)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            if markers[0].distance < minDist and markers[0].id > 27:
                idx = i 
            #for marker in markers:
                #if marker.id > 27:
                    #if marker.distance < minDist:
                        #print(marker.distance, marker.id)
                        #idx = i 
            markers = self.R.camera.see()
        for i in range(counter-idx):
            self.R.motor_board.motors[0].power = -0.08
            self.R.motor_board.motors[1].power = 0.08
            self.R.sleep(0.5)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
        
        if idx == 0:
            self.faceClosestToken(counter+1)
        markers = self.R.camera.see()
        markers = [marker for marker in markers if marker.id > 27]
        usedAngle = markers[0].spherical.rot_y
        if usedAngle < 0:
            speed = -0.01
        else:
            speed = 0.01 
        minAngle = 999090909
        while abs(markers[0].spherical.rot_y) <= minAngle:
            self.R.motor_board.motors[0].power = speed
            self.R.motor_board.motors[1].power = -speed
            self.R.sleep(0.5)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0
            if abs(markers[0].spherical.rot_y) < minAngle:
                minAngle = abs(markers[0].spherical.rot_y)
            markers = self.R.camera.see()
            markers = [marker for marker in markers if marker.id > 27]
        self.R.motor_board.motors[0].power = -speed
        self.R.motor_board.motors[1].power = speed
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0


    def getAngles(self):
        markers = self.R.camera.see()
        for marker in markers:
            print(marker.spherical.rot_y)
    
    def getBox(self):

        while True:

            while not self.facingCorner("adjacent"):
                self.R.motor_board.motors[0].power = -0.15
                self.R.motor_board.motors[1].power = 0.25
                self.R.sleep(0.3)
                self.R.motor_board.motors[0].power = 0
                self.R.motor_board.motors[1].power = 0
            self.faceClosestToken(2)
            going = self.checkMarker()
            idx = 0 
            while going:
                self.R.motor_board.motors[0].power = 0.5
                self.R.motor_board.motors[1].power = 0.5
                self.R.sleep(0.5)
                self.R.motor_board.motors[0].power = 0
                self.R.motor_board.motors[1].power = 0
                idx+=1
                going = self.checkMarker()

            self.R.servo_board.servos[0].position = 0.5
            self.R.servo_board.servos[1].position = 0.5
            self.R.sleep(0.3)
            
            cout = 0
            while not self.facingCorner("home"):
                self.R.motor_board.motors[0].power = 0.15
                self.R.motor_board.motors[1].power = -0.15
                self.R.sleep(0.3)
                self.R.motor_board.motors[0].power = 0
                self.R.motor_board.motors[1].power = 0
                cout +=1
                if cout == 4:
                    for  i in range(2):
                        self.R.motor_board.motors[0].power = 0.5
                        self.R.motor_board.motors[1].power = 0.5
                        self.R.sleep(0.5)
                        self.R.motor_board.motors[0].power = 0
                        self.R.motor_board.motors[1].power = 0
                    cout = 0 

            for i in range(idx+1):
                self.R.motor_board.motors[0].power = 0.5
                self.R.motor_board.motors[1].power = 0.5
                self.R.sleep(0.5)
                self.R.motor_board.motors[0].power = 0
                self.R.motor_board.motors[1].power = 0
            self.R.servo_board.servos[0].position = -1
            self.R.servo_board.servos[1].position = -1
            self.R.motor_board.motors[0].power = -0.5
            self.R.motor_board.motors[1].power = -0.5
            self.R.sleep(0.5)
            self.R.motor_board.motors[0].power = 0
            self.R.motor_board.motors[1].power = 0

            self.spin()

            for i in range(3):
                self.R.motor_board.motors[0].power = 0.5
                self.R.motor_board.motors[1].power = 0.5
                self.R.sleep(0.5)
                self.R.motor_board.motors[0].power = 0
                self.R.motor_board.motors[1].power = 0
    
    
    #in case we are stuck or cant see anything: drives back and turns roughly 180 degrees
    def escape(self):
        #drive back
        self.R.motor_board.motors[0].power = -1
        self.R.motor_board.motors[1].power = -1
        self.R.sleep(0.1)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0

        #turn 180 degrees
        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = -0.5
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0

    #method to run at start of code to find out what corner the robot is in
    #ends with robot turning roughly 90 degrees left into enermy corner (unless escape method is somehow done)
    def knowHome(self):

        count = 0
        breakFlag = False
        whileFlag = True
        
        #turns around the robot by roughly 180 degrees to see the wall markers behind it
        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = -0.5
        self.R.sleep(0.5)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
        

        #while loop until a target marker has been found
        while whileFlag:
            markers = self.R.camera.see_ids()
            if self.seeWallMarkers(markers):
                
                #sees what corner the wall markers it is seeing is in
                for corner in self.wallMarkers:
                    for marker in corner:
                        if marker in markers:
                            self.cornerNum = count
                            breakFlag = True
                            break

                    if breakFlag:
                        break

                    count += 1
                
                self.home = self.cornerNum
                whileFlag = False

            else:
                self.escape()


        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = -0.5
        self.R.sleep(0.25)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0
    
    
        

    #input of target refers to which corner's marker you are trying to find: "home", "adjacent", "opposite", "end"
    #output of True if robot is facing home corner and False if the robot is not
    def facingCorner(self, target):

        self.markerToFace = None
        whileFlag = True
        n = None

        if target == "home":
            n = 0
        elif target == "adjacent":
            n = 1
        elif target == "opposite":
            n = 2
        elif target == "end":
            n = 3
        else:
            print("INVALID INPUT for facingHome method")
            return None
        
        #modulus wrap around to deal with the home corenr being varied
        if self.home + n >= (len(self.wallMarkers)):
            corner = (self.home + n) % (len(self.wallMarkers))
        else:
            corner = self.home + n
        
        #list of the wall markers we would want to face towards
        wallMarkersWeWant = self.wallMarkers[corner]
        
        #while loop until a target marker has been found
        while whileFlag:
            markers = self.R.camera.see_ids()
            if self.seeWallMarkers(markers):
                
                #checks whether the markers we are looking for are within the vision of the robot
                for marker in markers:
                    if marker in wallMarkersWeWant:
                    
                    #attribute is updated to the marker that the robot is seeing and within the wall makrers we are targetting for
                    #this attribute can be used to identify which marker the robot needs to turn to
                        self.markerToFace = marker
                        return True
                
                return False

            else:
                self.escape()
    
    
    
    #Will be used to see if robot is seeing wall Markers (only the ones we care about)
    #returns True if robot is seeing wall markers
    def seeWallMarkers(self, listy):
        count = 0
        for i in range(len(listy)):
            if listy[i] == 99 or listy[i] == 3 or listy[i] == 10 or listy[i] == 17 or listy[i] == 24:
                count += 1
        
        if count < len(listy):
            return True
        
        return False
    
    def testMethod(self):
        self.knowHome()
        print(self.home)

        self.R.motor_board.motors[0].power = 1
        self.R.motor_board.motors[1].power = 1
        self.R.sleep(1)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0

        self.R.motor_board.motors[0].power = 0.5
        self.R.motor_board.motors[1].power = -0.5
        self.R.sleep(0.25)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0

        self.R.motor_board.motors[0].power = 1
        self.R.motor_board.motors[1].power = 1
        self.R.sleep(1)
        self.R.motor_board.motors[0].power = 0
        self.R.motor_board.motors[1].power = 0


        print(self.facingCorner("home"))
        print(self.facingCorner("adjacent"))
        print(self.facingCorner("opposite"))
        print(self.facingCorner("end"))