Khepera

Hey everyone, I have a project in which I need to program a Khepera robot too navigate through a maze and find a beacon on the first run.
*On the second run it must go to the bacon via the shortest path.
*The grid is a 4*3 (so i have to use an array).
*Furthermore it must realign it self (according to the surrounding walls). I know this sounds very confusing, so if you dont understand please let me know.
*At the moment I am having great difficulty with it realign itself (it keeps hitting the wall).
*I have to write the program on computer the send through a cable. im not sure what more i can tell you other the I need help :S.
*Also I have tried to use the simulator but with no success.

Thanks so much in advance.
(P.S. I may take some time to reply as I dont always have access to the net, thank)

python

Contributors

Replies

Views

9 Years

Discussion Span

9 Years Ago

Last Post by Freaky_Chris

So... where's your code? Where are your errors? Please read the posting guidelines, and get to know them. If you follow the posting guidelines we can actually help you.

Sorry about that, im still learning the rules, but thanks for the advice. Heres my code and i forgot to mention that each block in the array is 10cm (*Please Note : i have not uploaded my array code yet as i am still working on it!!!), Thanks

import serial,math,string,time,msvcrt
wheelSpeeds=[5,5]

class Myrobot:
def __init__(self,speed = 9600,port=0,stopbits=2):
self.speed = speed
self.port = port
self.stopbits = 2

def connect(self):
" connects the serial port "
self.serial = serial.Serial(baudrate=self.speed,port=self.port,
stopbits=self.stopbits)
self.serial.open()
print self.serial.isOpen()
print self.serial
print self.serial.parity
self.serial.write("G,0,0\n")
print self.serial.readline()

def disconnect(self):
self.serial.close()
#new function
def reset(self):
self.serial.write("G,0,0\n")
print self.serial.readline()
#new function
def readPos(self):
self.serial.write("H\n")
print self.serial.readline()

#Purpose: Sets the speed of wheel in mm/sec, range is 0<speed<=1000 mm/sec.
def move(self,leftSpeed,rightSpeed):
leftSpeed=(leftSpeed*127)/1000
rightSpeed=(rightSpeed*127)/1000
command = "D,"+str(leftSpeed)+","+str(rightSpeed)+"\n"
self.serial.write(command)
self.serial.readline()
self.serial.write("E \n")
self.serial.readline()

#Purpose: Maks robot move forward indefinitely.
def moveForward(self):
command="D,"+str(wheelSpeeds[0])+","+str(wheelSpeeds[1])+"\n"
self.serial.write(command)
self.serial.readline()

#Purpose: Make robot move backword indefinitely.
def moveBackward(self):
commmand="D,-"+str(wheelSpeeds[0])+",-"+str(wheelSpeeds[1])+"\n"
self.serial.write(command)
self.serial.readline()

#Purpose: Makes robot move forward n millimeters, range is 0<distance<=670000mm.
def moveForwardDistance(self,distance):
self.reset()
rotations=(distance*1000)/80
leftCounter=rotations
rightCounter=rotations
command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
self.serial.write(command)
self.serial.readline()
print str(leftCounter) + " " + str(rightCounter);

#Purpose: Makes robot move backward n millimeters.
def moveBackwardDistance(self,distance):
self.reset()
rotations=(distance*1000)/80
leftCounter=-rotations
rightCounter=-rotations
command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
self.serial.write(command)
self.serial.readline()
print str(leftCounter) + " " + str(rightCounter);

#Purpose: Makes robot stop.
def stop(self):
command="D,0,0\n"
self.serial.write(command)
self.serial.readline()

#Purpose: Makes robot turn left degrees passed as parameter.
def turnLeft(self,degree):
self.reset()
counter=(1035*degree)/180
if((float(1035*degree)/180)>=90):
counter=counter+1
leftCounter=-counter
rightCounter=counter
self.serial.write("D,0,0\n")
self.serial.readline()
command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
self.serial.write(command)
self.serial.readline()
print str(leftCounter) + " " + str(rightCounter);

#Purpose: Makes robt turn right degrees passed as parameter.
def turnRight(self,degree):
self.reset()
counter=(1035*degree)/180
if((float(1035*degree)/180)>=90):
counter=counter+1
leftCounter=counter
rightCounter=-counter
self.serial.write("D,0,0\n")
self.serial.readline()
command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
self.serial.write(command)
self.serial.readline()
print str(leftCounter) + " " + str(rightCounter);

#Purpose: Reads the speeds of wheels.
def wheelSpeeds(self):
self.serial.write("E\n")
reply=self.serial.readline()
rawdata=string.split(reply,",")
for i in range(2):
speeds=string.atoi(rawdata[i+1])
for i in range(2):
print speeds

def getLeftWheelSpeed(self):
print speeds[0]

def getRightWheelSpeed(self):
print speeds[1]

#Purpose: Reads the values of each of eight light sensors.
def readLightSensors(self):
self.serial.write("O\n")
reply=self.serial.readline()
self.LightSensors=string.split(reply,",")
print self.LightSensors
for i in range(8):
self.LightSensors=string.atoi(self.LightSensors[i+1])
return self.LightSensors

#Purpose: Display the values of each of eight light sensors.
def writeLightSensors(self):
for i in range(8):
print self.LightSensors

#Purpose: Display the value of requested sensor as numbered.
def getLightSensor(self,s):
print self.LightSensors[s]
return self.LightSensors[s]

#Purpose: Reads the values of each of eight proximity sensors.
def readProxSensors(self):
self.serial.write("N\n")
reply=self.serial.readline()
self.ProxSensors=string.split(reply,",")
print self.ProxSensors
for i in range(8):
self.ProxSensors=string.atoi(self.ProxSensors[i+1])
return self.ProxSensors

#Purpose: Display the values of each of eight proximity sensors.
def writeProxSensors(self):
for i in range(8):
print self.ProxSensors

#Purpose: Display the value of requested sensor as numbered.
def getProxSensor(self,t):
print self.ProxSensors[t]
return self.ProxSensors[t]

if __name__ == '__main__':
r=Myrobot()
r.connect()

def obsFront(r):
r.readProxSensors()
if (r.getProxSensor(2)>500 and r.getProxSensor(3)>500):
return 1
else:
return 0

def obsRight(r):
r.readProxSensors()
if (r.getProxSensor(4)>200 and r.getProxSensor(5)>370):
return 1
else:
return 0

def obsLeft(r):
r.readProxSensors()
if (r.getProxSensor(0)>385 and r.getProxSensor(1)>95):
return 1
else:
return 0

def wallDistLeft(r):
r.readProxSensors()
if (375<r.getProxSensor(0)<385 and 85<r.getProxSensor(1)<95):
return 1
else:
return 0

def wallDistRight(r):
r.readProxSensors()
if (185<r.getProxSensor(4)<200 and 355<r.getProxSensor(5)<370):
return 1
else:
return 0

def nothingFront(r):
r.readProxSensors()
if (r.getProxSensor(2) < 500 and r.getProxSensor(3) < 500):
return 1
else:
return 0

def nothingRight(r):
r.readProxSensors()
if (r.getProxSensor(4) < 200 and r.getProxSensor(5) < 370):
return 1
else:
return 0

def nothingLeft(r):
r.readProxSensors()
if (r.getProxSensor(0) < 385 and r.getProxSensor(1) < 95):
return 1
else:
return 0

#stop your robot by pressing any key on the keyboard.
while not msvcrt.kbhit():

if nothingFront(r) and nothingLeft(r) and nothingRight(r):
r.moveForwardDistance(100)
time.sleep(1)
count = 0

elif obsLeft(r):
r.move(0.3,-0.1)

elif obsRight(r):
print "turn slight toward"
self.move(0.3,0.1)

#elif nothingLeft(r):
# r.move(10+count,40)
# if (count < 30):
# count = count + 1

#elif (nothingFront(r) and nothingLeft(r) and nothingRight(r)):

#time.sleep(1)
# r.move(50,50)
#else:
# r.move(40,40)
#r.turnLeft(180)
# time.sleep(3)

r.stop()
r.disconnnect()

i suggest you start with properly intending your code. that wil allow it to work and make it easier to read

i suggest you start with properly intending your code. that wil allow it to work and make it easier to read

*I had the code indented correctly when i ran it but when i copied it over it shifted all text to the left.
*What I am trying to achieve at the moment is to get the robot to realign itself should it travel away from the straight line.
*Here is a copy of my code in txt format as i do not have access to python at the moment.

Thank you again

Attachments

khepera.txt (7.97 KB)

import serial,math,string,time,msvcrt
wheelSpeeds=[5,5]

class Myrobot:
    def __init__(self,speed = 9600,port=0,stopbits=2):        
        self.speed = speed
        self.port = port
        self.stopbits = 2
        
        
    def connect(self):
        " connects the serial port "
        self.serial = serial.Serial(baudrate=self.speed,port=self.port,
                                    stopbits=self.stopbits)
        self.serial.open()
        print self.serial.isOpen()
        print self.serial
        print self.serial.parity
        self.serial.write("G,0,0\n")
        print self.serial.readline()

    def disconnect(self):
        self.serial.close()
#new function
    def reset(self):
        self.serial.write("G,0,0\n")
        print self.serial.readline()
#new function
    def readPos(self):
        self.serial.write("H\n")
        print self.serial.readline()

#Purpose: Sets the speed of wheel in mm/sec, range is 0<speed<=1000 mm/sec.
    def move(self,leftSpeed,rightSpeed):
        leftSpeed=(leftSpeed*127)/1000
        rightSpeed=(rightSpeed*127)/1000
        command = "D,"+str(leftSpeed)+","+str(rightSpeed)+"\n"
        self.serial.write(command)
        self.serial.readline()
        self.serial.write("E \n")
        self.serial.readline()

#Purpose: Maks robot move forward indefinitely.
    def moveForward(self):
        command="D,"+str(wheelSpeeds[0])+","+str(wheelSpeeds[1])+"\n"
        self.serial.write(command)
        self.serial.readline()

#Purpose: Make robot move backword indefinitely.
    def moveBackward(self):
        commmand="D,-"+str(wheelSpeeds[0])+",-"+str(wheelSpeeds[1])+"\n"
        self.serial.write(command)
        self.serial.readline()

#Purpose: Makes robot move forward n millimeters, range is 0<distance<=670000mm.
    def moveForwardDistance(self,distance):
        self.reset()
        rotations=(distance*1000)/80       
        leftCounter=rotations
        rightCounter=rotations
        command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
        self.serial.write(command)
        self.serial.readline()
        print str(leftCounter) + " " + str(rightCounter);


#Purpose: Makes robot move backward n millimeters.
    def moveBackwardDistance(self,distance):
        self.reset()
        rotations=(distance*1000)/80
        leftCounter=-rotations
        rightCounter=-rotations
        command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
        self.serial.write(command)
        self.serial.readline()
        print str(leftCounter) + " " + str(rightCounter);


#Purpose: Makes robot stop.
    def stop(self):
        command="D,0,0\n"
        self.serial.write(command)
        self.serial.readline()

#Purpose: Makes robot turn left degrees passed as parameter.
    def turnLeft(self,degree):
        self.reset()
        counter=(1035*degree)/180
        if((float(1035*degree)/180)>=90):
            counter=counter+1
        leftCounter=-counter
        rightCounter=counter
        self.serial.write("D,0,0\n")
        self.serial.readline()
        command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
        self.serial.write(command)
        self.serial.readline()
        print str(leftCounter) + " " + str(rightCounter);



#Purpose: Makes robt turn right degrees passed as parameter.
    def turnRight(self,degree):
        self.reset()
        counter=(1035*degree)/180
        if((float(1035*degree)/180)>=90):
            counter=counter+1
        leftCounter=counter
        rightCounter=-counter
        self.serial.write("D,0,0\n")
        self.serial.readline()
        command="C,"+str(leftCounter)+","+str(rightCounter)+"\n"
        self.serial.write(command)
        self.serial.readline()
        print str(leftCounter) + " " + str(rightCounter);



#Purpose: Reads the speeds of wheels.
    def wheelSpeeds(self):
        self.serial.write("E\n")
        reply=self.serial.readline()
        rawdata=string.split(reply,",")
        for i in range(2):
            speeds[i]=string.atoi(rawdata[i+1])
        for i in range(2):
            print speeds[i]
            

    def getLeftWheelSpeed(self):
        print speeds[0]

    def getRightWheelSpeed(self):
        print speeds[1]

#Purpose: Reads the values of each of eight light sensors.
    def readLightSensors(self):
        self.serial.write("O\n")
        reply=self.serial.readline()
        self.LightSensors=string.split(reply,",")
        print self.LightSensors
        for i in range(8):
            self.LightSensors[i]=string.atoi(self.LightSensors[i+1])
        return self.LightSensors

#Purpose: Display the values of each of eight light sensors.
    def writeLightSensors(self):
        for i in range(8):
            print self.LightSensors[i]

#Purpose: Display the value of requested sensor as numbered.
    def getLightSensor(self,s):
        print self.LightSensors[s]
        return self.LightSensors[s]

#Purpose: Reads the values of each of eight proximity sensors.
    def readProxSensors(self):
        self.serial.write("N\n")
        reply=self.serial.readline()
        self.ProxSensors=string.split(reply,",")
        print self.ProxSensors
        for i in range(8):
            self.ProxSensors[i]=string.atoi(self.ProxSensors[i+1])
        return self.ProxSensors     

#Purpose: Display the values of each of eight proximity sensors.
    def writeProxSensors(self):
         for i in range(8):
            print self.ProxSensors[i]

#Purpose: Display the value of requested sensor as numbered.
    def getProxSensor(self,t):
        print self.ProxSensors[t]
        return self.ProxSensors[t]

if __name__ == '__main__':
    r=Myrobot()
    r.connect()

    def obsFront(r):
        r.readProxSensors()
        if (r.getProxSensor(2)>500 and r.getProxSensor(3)>500):
            return 1
        else:
            return 0

    def obsRight(r):
        r.readProxSensors()
        if (r.getProxSensor(4)>200 and r.getProxSensor(5)>370):
            return 1
        else:
            return 0

    def obsLeft(r):
        r.readProxSensors()
        if (r.getProxSensor(0)>385 and r.getProxSensor(1)>95):
            return 1
        else:
            return 0

    def wallDistLeft(r):
        r.readProxSensors()
        if (375<r.getProxSensor(0)<385 and 85<r.getProxSensor(1)<95):
            return 1
        else:
            return 0

    def wallDistRight(r):
        r.readProxSensors()
        if (185<r.getProxSensor(4)<200 and 355<r.getProxSensor(5)<370):
            return 1
        else:
            return 0    

    def nothingFront(r):
        r.readProxSensors()
        if (r.getProxSensor(2) < 500 and r.getProxSensor(3) < 500):
            return 1
        else:
            return 0

    def nothingRight(r):
        r.readProxSensors()
        if (r.getProxSensor(4) < 200 and r.getProxSensor(5) < 370):
            return 1
        else:
            return 0

    def nothingLeft(r):
        r.readProxSensors()
        if (r.getProxSensor(0) < 385 and r.getProxSensor(1) < 95):
            return 1
        else:
            return 0

#stop your robot by pressing any key on the keyboard.
    while not msvcrt.kbhit():
        
        if nothingFront(r) and nothingLeft(r) and nothingRight(r):
            r.moveForwardDistance(100)
            time.sleep(1)
            count = 0

        elif obsLeft(r):
            r.move(0.3,-0.1)
        

        elif obsRight(r):
            print "turn slight toward"
            self.move(0.3,0.1)


        #elif nothingLeft(r):
        #    r.move(10+count,40)
        #    if (count < 30):
        #        count = count + 1

        #elif (nothingFront(r) and nothingLeft(r) and nothingRight(r)):
                
            #time.sleep(1)
        #    r.move(50,50)
        #else:
        #    r.move(40,40)
        #r.turnLeft(180)
        #    time.sleep(3)
       
    r.stop()
    r.disconnnect()

The reason your code got shifted to the left was because you didn't use code tags, another thing pointed out in posting guide lines

python