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 __init__(self): wirebk.WriteBk(Define.SLAVE_ADDRESS_POWERMODULE, Define.REGISTER_CONFIGURATION, [0x44, 0xDF]) calibrationvar = 1024 ##1ma por bit highbyte = calibrationvar >> 8 & 0xff lowbyte = calibrationvar & 0xff error_com = wirebk.WriteBk(Define.SLAVE_ADDRESS_POWERMODULE, Define.REGISTER_CALIBRATION, [highbyte, lowbyte]) if (error_com == "error"): print("Error in power sensor initialization")
def set(self,velocity): if(velocity>255): velocity=255 if(velocity<-255): velocity=-255 error_com = wirebk.Port_Enabled(self.portnum); if(error_com==0): velocity = int(velocity) highbyte= velocity>>8 & 0xff lowbyte= velocity & 0xff wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_MAX_PWM_POS,0xff) statusvar = wirebk.WriteBk(Define.SLAVE_ADDRESS_MOTOR,Define.REGISTER_DIRECTION_SPEED,[highbyte,lowbyte]) if(statusvar=="error"): print("motor {} not working".format(self.portnum)) else: print("comunication i2c not working") wirebk.Port_Enabled(0)
def __init__(self): #configure gyroscope range wirebk.WriteBk(Define.SLAVE_ADDRESS_IMU, 27, 0x08) #500 dps #configure accelerometer range wirebk.WriteBk(Define.SLAVE_ADDRESS_IMU, 28, 0x08) #+-4g #Set by pass mode for the magnetometer wirebk.WriteBk(Define.SLAVE_ADDRESS_IMU, 0x37, 0x02) # Magnetometer configuration wirebk.WriteBk(Define.SLAVE_ADDRESS_MAG, 0x0A, 0x00) # power down magnetometer time.sleep(.01) wirebk.WriteBk(Define.SLAVE_ADDRESS_MAG, 0x0A, 0x0F) # fuse ROM access mode time.sleep(.01) self.magsensitivity = wirebk.ReadBk2( Define.SLAVE_ADDRESS_MAG, 3, 0x10) # Read mag sensitivity values self.magsensitivity[0] = ((self.magsensitivity[0] - 128) / 256) + 1 self.magsensitivity[1] = ((self.magsensitivity[1] - 128) / 256) + 1 self.magsensitivity[2] = ((self.magsensitivity[2] - 128) / 256) + 1 wirebk.WriteBk(Define.SLAVE_ADDRESS_MAG, 0x0A, 0x00) # power down magnetometer time.sleep(.01) # 0x16 Continuous measurement mode 2 (100 Hz) and 16 bit mode # 0x12 Continuous measurement mode 1 (8 Hz) and 16 bit mode wirebk.WriteBk(Define.SLAVE_ADDRESS_MAG, 0x0A, 0x16) time.sleep(.01)
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 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)