def motorSetup():
board = Board(1, 0x10)
board_detect(board)
while board.begin() != board.STA_OK: # Board begin and check board status
print_board_status()
print("board begin faild")
time.sleep(2)
print("board begin success")
# Set selected DC motor encoder enable
board.set_encoder_enable(board.ALL)
# board.set_encoder_disable(board.ALL) # Set selected DC motor encoder disable
# Set selected DC motor encoder reduction ratio, test motor reduction ratio is 43.8
board.set_encoder_reduction_ratio(board.ALL, 40)
# Set DC motor pwm frequency to 1000HZ
board.set_moter_pwm_frequency(1000)
return board
def main():
head_gyro = Gyro(0x50)
board = Board(1, 0x10)
board.set_encoder_enable(board.ALL) # Set selected DC motor encoder enable
board.set_encoder_reduction_ratio(
board.ALL, 30
) # Set selected DC motor encoder reduction ratio, test motor reduction ratio is 43.8
board.set_moter_pwm_frequency(1000) # Set DC motor pwm frequency to 1000HZ
address = board.detecte()
print("Board list conform:")
print(address)
while board.begin() != board.STA_OK: # Board begin and check board status
print_board_status()
print("board begin faild")
time.sleep(2)
print("board begin success")
offseterror = 0.0
kp = 0
ki = 0
kd = 0
Lmin = 4 #M1
Rmin = 4 #M2
LminR = 4 #M1
RminR = 4 #M2
lastError = 0.000000000000
lastError_1 = 0.000000000000
lastError_2 = 0.000000000000
integral = 0.0
flag = 0
try:
while (True):
button_state = GPIO.input(PIDButton) #offsetBTDEE
button_stateADD = GPIO.input(offsetBTADD)
button_stateDEE = GPIO.input(offsetBTDEE)
if button_state == 1:
sleep(0.5)
if flag == 0:
flag = 1
else:
flag = 0
if flag == 0:
integral = 0.0
board.motor_stop(board.ALL)
continue
angle = float(repr(head_gyro.get_angle())) + offseterror
gyro = float(repr(head_gyro.get_gyro()))
if angle > 80 or angle < -80:
integral = 0.0
flag = 0
board.motor_stop(board.ALL)
continue
error = angle
proportional = kp * error
integral += ki * error
derivative = kd * (error - lastError_1)
PID = int((proportional + integral + derivative)) #
if PID > 50:
PID = 50
if PID < -50:
PID = -50
print("Angle:" + repr(head_gyro.get_angle()) + " gyro:" +
repr(head_gyro.get_gyro()) + " offset angle:" + str(PID) +
"offseterror:" + str(offseterror) + "\n")
if PID > 0:
board.motor_movement(
[board.M1], board.CCW, Lmin +
abs(PID)) # DC motor 1 movement, orientation clockwise
board.motor_movement(
[board.M2], board.CW, Rmin + abs(PID)
) # DC motor 2 movement, orientation count-clockwise
else:
board.motor_movement(
[board.M1], board.CW, LminR +
abs(PID)) # DC motor 1 movement, orientation clockwise
board.motor_movement(
[board.M2], board.CCW, RminR + abs(PID)
) # DC motor 2 movement, orientation count-clockwise
lastError_2 = lastError_1
lastError_1 = error
except KeyboardInterrupt:
board.motor_stop(board.ALL)
import time
from DFRobot_RaspberryPi_DC_Motor import DFRobot_DC_Motor_IIC as Board
board = Board(1, 0x10) # Select bus 1, set address to 0x10
def board_detect():
l = board.detecte()
print("Board list conform Maybe?:")
print(l)
''' print last operate status, users can use this variable to determine the result of a function call. '''
def print_board_status():
if board.last_operate_status == board.STA_OK:
print("board status: everything ok")
elif board.last_operate_status == board.STA_ERR:
print("board status: unexpected error")
elif board.last_operate_status == board.STA_ERR_DEVICE_NOT_DETECTED:
print("board status: device not detected")
elif board.last_operate_status == board.STA_ERR_PARAMETER:
print("board status: parameter error, last operate no effective")
elif board.last_operate_status == board.STA_ERR_SOFT_VERSION:
print("board status: unsupport board framware version")
def initialiseBoard():
board_detect(
from DFRobot_RaspberryPi_DC_Motor import DFRobot_DC_Motor_IIC as Board
import RPi.GPIO as GPIO
import time
#GPIO setup
AUDIO_CHANNEL = 17
GPIO.setmode(GPIO.BCM)
GPIO.setup(AUDIO_CHANNEL, GPIO.IN)
#Motor setup
board = Board(1, 0x10)
def startProgram() :
#First, setup the board
setupMotors()
#Create the GPIO callback for when noise happens
GPIO.add_event_detect(AUDIO_CHANNEL, GPIO.BOTH, bouncetime=300)
GPIO.add_event_callback(AUDIO_CHANNEL, audioOccured)
def setupMotors() :
#Print the list of boards available
list = board.detecte()
print("List of available board:")
print(list)
#Ensure that the board is setup properly.
while board.begin() != board.STA_OK:
print("Failed to setup board!")
time.sleep(2)
print("Successfully setup the board!")