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()