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)