def set(self, colorselect):
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 == "white"):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_START_CAL, 1)
elif (colorselect == "black"):
wirebk.WriteBk(Define.SLAVE_ADDRESS_COLORSENSOR,
Define.REGISTER_START_CAL, 2)
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
def configPID(self,P_val,I_val,D_val,dead_band):
error_com = wirebk.Port_Enabled(self.portnum);
if(error_com==0):
highbyte= P_val>>8 & 0xff
lowbyte= P_val & 0xff
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.P_GAIN_HI,[highbyte,lowbyte])
highbyte= I_val>>8 & 0xff
lowbyte= I_val & 0xff
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.I_GAIN_HI,[highbyte,lowbyte])
highbyte= D_val>>8 & 0xff
lowbyte= D_val & 0xff
wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.D_GAIN_HI,[highbyte,lowbyte])
highbyte= dead_band>>8 & 0xff
lowbyte= dead_band & 0xff
statusvar = wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.DEAD_BAND,[highbyte,lowbyte])
if(statusvar=="error"):
print("motor {} not working".format(self.portnum))
else:
print("comunication i2c not working")
wirebk.Port_Enabled(0)
def configPID(self,P_val,I_val,D_val,dead_band):
error_com = wirebk.Port_Enabled(self.portnum);
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)
