def move(self,velocity,degrees):
if(velocity>255):
velocity=255
if(velocity<-255):
velocity=-255
if(velocity<0):
velocity=velocity*-1
degrees=degrees*-1
error_com = wirebk.Port_Enabled(self.portnum);
if(error_com==0):
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_ADDRESS_POINTER,Define.REGISTER_COUNT_PER_TURN)
pulses = wirebk.ReadBk(Define.SLAVE_ADDRESS_MOTOR,2)
if(pulses[0]!="error"):
pulsesjoin = pulses[0]<<8 | pulses[1]
velocity = int(velocity)
velocity= velocity & 0xff
degrees = int((degrees*pulsesjoin)/360)
data_degrees = [degrees>>24 & 0xff,degrees>>16 & 0xff,degrees>>8 & 0xff,degrees & 0xff]
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_MAX_PWM_POS,velocity)
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_POS_VALUE,data_degrees)
else:
print("motor {} not working".format(self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
def read(self):
distance = "error"
error_com = wirebk.Port_Enabled(self.portnum)
if (error_com == 0):
distance = wirebk.ReadBk(Define.SLAVE_ADDRESS_ULTRASONIC, 1)[0]
if (distance == "error"):
print("distance sensor {} not working".format(self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
time.sleep(0.05)
return (distance)
def wait(self):
error_com = wirebk.Port_Enabled(self.portnum);
if(error_com==0):
statusvar=1
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_ADDRESS_POINTER,Define.REGISTER_BUSY)
while(statusvar==1):
statusvar=wirebk.ReadBk(Define.SLAVE_ADDRESS_MOTOR,1)[0]
time.sleep(0.01)
if(statusvar=="error"):
print("motor {} not working".format(self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
def read(self):
sensordata="error"
error_com = wirebk.Port_Enabled(self.portnum);
if(error_com==0):
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_ADDRESS_POINTER,Define.READ_ENCODER)
sensordata = wirebk.ReadBk(Define.SLAVE_ADDRESS_MOTOR,4)
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
if(sensordata[0]=="error"):
print("distance sensor {} not working".format(self.portnum))
return(sensordata)
else:
return(struct.unpack(">i", bytes(sensordata))[0])
def read(self, *argsx):
if (len(argsx) != 0): ##para checar si esta vacio o no
colorselect = argsx[0]
if (colorselect == "white"):
colorret = "error"
error_com = wirebk.Port_Enabled(self.portnum)
if (error_com == 0):
if (self.modestate != Define.MODE_LIGHT):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_MODE, Define.MODE_LIGHT)
self.modestate = Define.MODE_LIGHT
time.sleep(0.02)
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_AMBIENT)
colorret = wirebk.ReadBk(Define.SLAVE_ADDRESS_COLORSENSOR,
1)[0]
if (colorret == "error"):
print("color sensor {} not working".format(
self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
return (colorret)
elif (colorselect == "white2"):
colorret = "error"
error_com = wirebk.Port_Enabled(self.portnum)
if (error_com == 0):
if (self.modestate != Define.MODE_LIGHT2):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_MODE,
Define.MODE_LIGHT2)
self.modestate = Define.MODE_LIGHT2
time.sleep(0.02)
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_AMBIENT)
colorret = wirebk.ReadBk(Define.SLAVE_ADDRESS_COLORSENSOR,
1)[0]
if (colorret == "error"):
print("color sensor {} not working".format(
self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
return (colorret)
else:
colorret = "error"
error_com = wirebk.Port_Enabled(self.portnum)
if (error_com == 0):
if (self.modestate != Define.MODE_COLOR):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_MODE, Define.MODE_COLOR)
self.modestate = Define.MODE_COLOR
time.sleep(0.02)
if (colorselect == "red"):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_SCALED_RED)
elif (colorselect == "green"):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_SCALED_GREEN)
elif (colorselect == "blue"):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_SCALED_BLUE)
colorret = wirebk.ReadBk(Define.SLAVE_ADDRESS_COLORSENSOR,
1)[0]
if (colorret == "error"):
print("color sensor {} not working".format(
self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
return (colorret)
else:
colorret = "error"
error_com = wirebk.Port_Enabled(self.portnum)
if (error_com == 0):
if (self.modestate != Define.MODE_COLOR):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_MODE, Define.MODE_COLOR)
self.modestate = Define.MODE_COLOR
time.sleep(0.02)
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_ADDR_POINTER,
Define.REGISTER_COLOR)
colorret = wirebk.ReadBk(Define.SLAVE_ADDRESS_COLORSENSOR,
1)[0]
if (str(colorret) in self.colorcases):
colorret = self.colorcases[str(colorret)]
if (colorret == "error"):
print("color sensor {} not working".format(
self.portnum))
else:
colorret = "error"
print("color sensor {} not working".format(self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
return (colorret)
