Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # This program demonstrates usage of the servos.
- # Keep the robot in a safe location before running this program,
- # as it will immediately begin moving.
- # See https://learn.adafruit.com/adafruit-16-channel-pwm-servo-hat-for-raspberry-pi/ for more details.
- import time
- import Adafruit_PCA9685
- import signal
- import math
- from encoders import resetCounts, getSpeeds, getCounts, initEncoders
- # The servo hat uses its own numbering scheme within the Adafruit library.
- # 0 represents the first servo, 1 for the second, and so on.
- LSERVO = 0
- RSERVO = 1
- # This function is called when Ctrl+C is pressed.
- # It's intended for properly exiting the program.
- def ctrlC(signum, frame):
- print("Exiting")
- # Stop the servos
- pwm.set_pwm(LSERVO, 0, 0);
- pwm.set_pwm(RSERVO, 0, 0);
- exit()
- # Attach the Ctrl+C signal interrupt
- signal.signal(signal.SIGINT, ctrlC)
- # Initialize the servo hat library.
- pwm = Adafruit_PCA9685.PCA9685()
- # 50Hz is used for the frequency of the servos.
- pwm.set_pwm_freq(50)
- # Write an initial value of 1.5, which keeps the servos stopped.
- # Due to how servos work, and the design of the Adafruit library,
- # the value must be divided by 20 and multiplied by 4096.
- pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
- def setSpeedsPWM(pwmLeft, pwmRight, t):
- pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
- time.sleep(t)
- pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
- def calibrateSpeeds():
- pwmLeft = 1.5
- pwmRight = 1.5
- pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
- time.sleep(1)
- resetCounts()
- file2write=open("calibrate.txt", 'w')
- file2write.write("LeftPWM RightPWM LeftSpeed(RPS) RightSpeed(RPS) \n")
- while pwmLeft < 1.7:
- resetCounts()
- pwmLeft += 0.01
- pwmRight += 0.01
- pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
- time.sleep(3)
- if pwmLeft == 1.51:
- print()
- else:
- file2write.write('%.4f ' % pwmLeft)
- file2write.write('%.4f ' % pwmRight)
- out = getSpeeds()
- file2write.write('%.4f %.4f \n' % out)
- resetCounts()
- time.sleep(1.5)
- pwmLeft = 1.5
- pwmRight = 1.5
- pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
- time.sleep(1.5)
- resetCounts()
- while pwmLeft > 1.3:
- resetCounts()
- pwmLeft -= 0.01
- pwmRight -= 0.01
- pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
- time.sleep(3)
- if pwmLeft == 1.49:
- print()
- else:
- file2write.write('%.4f ' % pwmLeft)
- file2write.write('%.4f ' % pwmRight)
- out = getSpeeds()
- file2write.write('%.4f %.4f \n' % out)
- resetCounts()
- pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
- file2write.close()
- def setSpeedsRPS(rpsLeft, rpsRight, t):
- pwmLeft = 1.5
- pwmRight = 1.5
- count = 0
- if rpsLeft > 0.815 or rpsRight > 0.815:
- print("The robot cannot move this quickly.")
- else:
- array = []
- file2read = open("calibrate.txt", "r")
- next(file2read)
- for line in file2read:
- fields = line.split(" ")
- pwmL = float(fields[0])
- pwmR = float(fields[1])
- speedL = float(fields[2])
- speedR = float(fields[3])
- count += 1
- if pwmL > 1.50:
- pwmRight = pwmR
- if speedL == round(rpsLeft, 4):
- pwmLeft = pwmL
- print("speedl: ", speedL)
- print("rpsLeft: ", rpsLeft)
- print("left: ", pwmLeft)
- count = 0
- if count == 19:
- pwmRight = pwmR
- if rpsRight == 0:
- pwmRight = 0
- print("Right: ", pwmRight)
- if rpsLeft == 0:
- pwmRight = pwmL
- pwmLeft = 0
- if pwmLeft == 1.5:
- print("left: Invalid RPS")
- if pwmRight == 1.5:
- print("right: Invalid RPS")
- else:
- setSpeedsPWM(pwmLeft, pwmRight, t)
- #Rectangle function
- # setSpeedsPWM(pwmRight, pwmRight - .4, t)
- '''
- if direction == 1 and pwmL > 1.5:
- if round(speedL, 1) == round(rpsLeft, 1):
- pwmLeft = round(pwmL, 3)
- print("left: ", pwmLeft)
- if round(speedR, 1) == round(rpsRight, 1):
- pwmRight = round(pwmR, 3)
- print("right: ", pwmRight)
- elif direction == 0 and pwmR > 1.5:
- if round(speedL, 1) == round(rpsLeft, 1):
- pwmLeft = round(pwmL, 3)
- print("left: ", pwmLeft)
- if round(speedR, 1) == round(rpsRight, 1):
- pwmRight = round(pwmR, 3)
- print("right: ", pwmRight)
- if pwmLeft == 1.5:
- print("left: Invalid RPS")
- if pwmRight == 1.5:
- print("right: Invalid RPS")
- else:
- setSpeedsPWM(pwmLeft, pwmRight)
- time.sleep(4)
- pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
- pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
- '''
- def setSpeedsIPS(ipsLeft, ipsRight, time):
- ipsLeft = ipsLeft / 8.2
- left = round(ipsLeft, 5)
- ipsRight = ipsRight / 8.2
- right = round(ipsRight, 5)
- if ipsLeft > .8125 or ipsRight > .8125:
- print("Speed is too great")
- if ipsLeft < .125 or ipsRight < .125:
- print("Speed is too slow.")
- else:
- setSpeedsRPS(ipsLeft, ipsRight, time)
- #def setSpeedVW(v, w):
- #v = (ipsRight + ipsLeft)/2
- #w = (ipsRight - ipsLeft)/3.95
- initEncoders()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement