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)
