class CycleComputer():
def __init__(self):
self.lcd = LS027B4DH01(
SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)), Pin(32, Pin.OUT), Pin(33, Pin.OUT),
Pin(25, Pin.OUT))
self.button = Joystick(Pin(34), Pin(35), Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Home(self),
'record': Record(self),
'settings': Settings(self)
}
self.state = 'home'
def run(self):
obj = self.states[self.state]
obj.control(self.button.read())
obj.draw()
class GUI_test():
def __init__(self):
self.lcd = LS027B4DH01(spi=SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)),
scs=Pin(32, Pin.OUT),
extcomin=Pin(33, Pin.OUT),
disp=Pin(25, Pin.OUT))
self.button = Joystick(x=Pin(34), y=Pin(35), s=Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Ui_home(self),
'acceleration': Ui_acceleration(self),
'gyro': Ui_gyro(self),
'geomagnetism': Ui_geomagnetism(self)
}
self.state = 'home'
def run(self):
obj = self.states[self.state]
obj.control(self.button.read())
obj.draw()
def change_state(self, state):
self.state = state
lastTime = 0.
pidX = PID(-0.0005, 0.0, 0.000001, setpoint=0.)
pidY = PID(0.0005, 0., 0., setpoint=0.)
pidX.sample_time = 0.02 # update every 0.01 seconds
pidY.sample_time = 0.02
T = 0.5 #period of time (in seconds) of every step
offset = np.array([
0.5, 0., 0., 0.5
]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
while (True):
startTime = time.time()
pos, orn, L, angle, Lrot, T = pybulletDebug.cam_and_robotstates(boxId)
L, angle, Lrot, T = joystick.read()
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet, s = trot.loop(L, angle, Lrot, T, offset, bodytoFeet0)
arduinoLoopTime, realRoll, realPitch = arduino.serialRecive(
) #recive serial message
pos[0] = 0.015 + pidX(realPitch)
pos[1] = pidY(realRoll)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
FR_angles, FL_angles, BR_angles, BL_angles, transformedBodytoFeet = robotKinematics.solve(
orn, pos, bodytoFeet)
pidX = PID(-0.0005, 0.0, 0.000001, setpoint=0.)
pidY = PID(0.0005, 0., 0., setpoint=0.)
pidX.sample_time = 0.02 # update every 0.02 seconds
pidY.sample_time = 0.02
T = 0.4 #period of time (in seconds) of every step
offset = np.array([
0.5, 0., 0., 0.5
]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
p.setRealTimeSimulation(1)
p.setTimeStep(0.002)
while (True):
loopTime = time.time()
_, _, _, _, _, _ = pybulletDebug.cam_and_robotstates(boxId)
commandCoM, V, angle, Wrot, T, compliantMode, yaw, pitch = joystick.read()
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet = trot.loop(V, angle, Wrot, T, offset, bodytoFeet0)
arduinoLoopTime, Xacc, Yacc, realRoll, realPitch = arduino.serialRecive(
) #recive serial message
pos[0] = pidX(realPitch)
pos[1] = pidY(realRoll)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
FR_angles, FL_angles, BR_angles, BL_angles, transformedBodytoFeet = robotKinematics.solve(
orn, pos + commandCoM, bodytoFeet)
