try:
tic1 = pyb.micros()
while True:
dt = pyb.micros() - tic1
if dt > 5000: # sampling time is 5 msec or 50Hz
pitch, pitch_dot = pitch_estimate(pitch, dt*0.000001, alpha)
tic1 = pyb.micros()
u = pidc.get_pwm(pitch, pitch_dot)
if u > 0:
motor.A_forward( (abs(u)+motor_offset) * mWeight.report('A') )
motor.B_forward( (abs(u)+motor_offset) * mWeight.report('B') )
elif u < 0:
motor.A_back( (abs(u)+motor_offset) * mWeight.report('A') )
motor.B_back( (abs(u)+motor_offset) * mWeight.report('B') )
#else:
#motor.A_stop()
#motor.B_stop()
if mic.buffer_full(): # semaphore signal from ISR - set if buffer is full
b_LED.off() # flash off
# Get instantaneous energy
E = mic.inst_energy()
# compute moving sum of last 50 energy epochs
sum_energy = sum_energy - e_buf[e_ptr] + E
e_buf[e_ptr] = E # over-write earlest energy with most recent
e_ptr = (e_ptr + 1) % M # increment e_ptr with wraparound - 0 to M-1
# Compute ratio of instantaneous energy/average energy
oled.draw_text(0, 50, 'Target = {:5.2f}'.format(pitch_target))
oled.display()
try:
tic = pyb.micros()
while True:
b_LED.toggle()
dt = pyb.micros() - tic
if dt > 5000: # sampling time is 5 msec or 50Hz
pitch, pitch_dot = pitch_estimate(pitch, dt * 0.000001, alpha)
tic = pyb.micros()
pitch_error = pitch - pitch_target
u = K_p * pitch_error + K_d * pitch_dot
if u > 100: u = 100 # limit u to + - 100
elif u < -100: u = -100
if u > 0:
motor.A_forward(abs(u) + offset)
motor.B_forward(abs(u) + offset)
elif u < 0:
motor.A_back(abs(u) + offset)
motor.B_back(abs(u) + offset)
else:
motor.A_stop()
motor.B_stop()
finally:
motor.A_stop()
motor.B_stop()
motor_offset = 5 # remove motor deadzone
motor = MOTOR() # init motor object
pidc = PIDC(Kp=K_p, Kd=K_d, Ki=K_i,
theta_0=calibration) # init PID controller object
pidc.target_reset() # set target point for self-balance as normal to ground
try:
tic = pyb.micros()
while True:
b_LED.toggle()
dt = pyb.micros() - tic
if dt > 5000: # sampling time is 5 msec or 50Hz
pitch, pitch_dot = pitch_estimate(pitch, dt * 0.000001, alpha)
tic = pyb.micros()
u = pidc.get_pwm(pitch, pitch_dot)
if u > 0:
motor.A_forward(abs(u) + motor_offset)
motor.B_forward(abs(u) + motor_offset)
elif u < 0:
motor.A_back(abs(u) + motor_offset)
motor.B_back(abs(u) + motor_offset)
else:
motor.A_stop()
motor.B_stop()
finally: # in the event of a crash or keyboard interrupt turn of motors before exiting program
motor.A_stop()
motor.B_stop()
# Compute ratio of instantaneous energy/average energy c = E*M/sum_energy dac.write(min(int(c*4095/3), 4095)) # useful to see on scope, can remove if (pyb.millis()-tic > 545): # if more than 545 ms since last beat, value calculated using audio processing of chosen song on Matlab if (c>BEAT_THRESHOLD): # look for a beat #step = moves[danceCount] step = 'f' if step == 'f': # dance moves being sent as speed and direction instructions to motors Segway.A_forward(20) Segway.B_forward(20) danceCount+=1 if step == 'b': Segway.A_back(20) Segway.B_back(20) danceCount += 1 if step == 'B': Segway.A_back(20) Segway.B_back(20) danceCount += 1 if step == 's': Segway.A_stop() Segway.B_stop() danceCount += 1 if step == 'L': Segway.A_forward(5) Segway.B_forward(35)
elif command[2] == ord('4'):
oled.draw_text(0, 30, ' 4-key pressed')
print('4-decceleration')
Segway.dn_Aspeed(10)
Segway.dn_Bspeed(10)
elif command[2] == ord('5'):
oled.draw_text(0, 30, 'UP key pressed')
print('UP key pressed')
Segway.A_forward(20)
Segway.B_forward(20)
elif command[2] == ord('6'):
oled.draw_text(0, 30, 'DN key pressed')
print('DN key pressed')
Segway.A_back(Segway.Aspeed)
Segway.B_back(Segway.Bspeed)
elif command[2] == ord('7'):
oled.draw_text(0, 30, 'LF key pressed')
print('LF key pressed')
Segway.up_Aspeed(20)
Segway.up_Bspeed(0)
elif command[2] == ord('8'):
oled.draw_text(0, 30, 'RT key pressed')
print('RT key pressed')
Segway.up_Aspeed(0)
Segway.up_Bspeed(20)
oled.display()