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)