def tracer_grille(grille, agrandissement, angle, origine):
n = 4000
calibre = 0.1
pwm_X = PWM(0)
pwm_Y = PWM(
1) # il n'existe que 2 canaux pour implementer les PWMs (0 et 1)
pwm_X.export()
pwm_Y.export()
pwm_X.period = 100000 # les unites sont en nanosecondes, ainsi dans cet exemple la periode vaut 100000*1ns = 100 us
pwm_Y.period = 100000
GPIO.setmode(
GPIO.BOARD) # On lit les pattes dans l'ordre classique en électronique
GPIO.setup(35, GPIO.OUT) # La broche 35 est configurée en sortie
##La fréquence max du lever/baisser de stylo et 4Hz. Donc 250ms.
### Ecriture de la grille
for i in range(0, len(grille)):
for j in range(0, len(grille[0])):
if (grille[i][j] != 0):
seq_chiffre = choix(grille[i][j], i, j, agrandissement, angle)
x = seq_chiffre[0]
y = seq_chiffre[1]
haut = seq_chiffre[2]
for k in range(0, len(x)):
x[k] += origine[0] * calibre
y[k] += origine[1] * calibre
stylo_haut()
for k in range(0, len(x)):
if (k != 0):
lever_stylo(haut[k], haut[k - 1])
pwm_X.duty_cycle = int(x[k] * pwm_X.period / 3.3)
pwm_Y.duty_cycle = int(y[k] * pwm_Y.period / 3.3)
pwm_X.enable = True
pwm_Y.enable = True
stylo_haut()
#Lorsque l'on a termine avec la table tracante
pwm_X.enable = False # On desactive la PWM
pwm_Y.enable = False
pwm_X.unexport()
pwm_Y.unexport()
GPIO.cleanup(
) # A la fin du programme on remet à 0 les broches du Rasberry PI
return (0)
def calibrage():
GPIO.setmode(
GPIO.BOARD) # On lit les pattes dans l'ordre classique en électronique
GPIO.setup(35, GPIO.OUT) # La broche 35 est configurée en sortie
pwm_X = PWM(0)
pwm_Y = PWM(
1) # il n'existe que 2 canaux pour implementer les PWMs (0 et 1)
pwm_X.export()
pwm_Y.export()
pwm_X.period = 100000 # les unites sont en nanosecondes, ainsi dans cet exemple la periode vaut 100000*1ns = 100 us
pwm_Y.period = 100000
GPIO.output(35, GPIO.HIGH)
for i in range(0, int(pwm_Y.period / 5)):
pwm_Y.duty_cycle = 5 * i
pwm_Y.enable = True
for i in range(0, int(pwm_X.period / 5)):
pwm_X.duty_cycle = 5 * i
pwm_X.enable = True
for i in range(0, int(pwm_Y.period / 5)):
pwm_Y.duty_cycle = pwm_Y.period - 5 * i
pwm_Y.enable = True
for i in range(0, int(pwm_X.period / 5)):
pwm_X.duty_cycle = pwm_X.period - 5 * i
pwm_X.enable = True
pwm_X.enable = False
pwm_Y.enable = False
stylo_haut()
time.sleep(1)
pwm_X.duty_cycle = int(pwm_X.period / 2)
pwm_X.enable = True
time.sleep(1)
camera = picamera.PiCamera()
camera.capture('calibrage.jpg')
time.sleep(1)
pwm_X.enable = False
pwm_X.unexport()
pwm_Y.unexport()
GPIO.cleanup()
return 1
from pwm import PWM pwm0 = PWM(0) pwm1 = PWM(1) pwm0.export() pwm1.export() pwm0.period = 20000000 pwm1.period = 20000000 pwm0.duty_cycle = 1000000 pwm0.enable = False pwm1.duty_cycle = 1465000 pwm1.enable = False
import matplotlib.pyplot as plt import numpy as np from pwm import PWM """### Motor initialization and configuration Motor speed range [0 - stop, 1 - full speed] """ # Enable servo # object for servo MOTOR_BRAKE = 1000000 motor = PWM(0) motor.period = 20000000 motor.duty_cycle = MOTOR_BRAKE motor.enable = True #!! Wait for 3 seconds until the motor stops beeping """# Upon start of device, motor speed must be set to 0 for at least 5 seconds""" motor.duty_cycle = MOTOR_BRAKE + 120000 # Motor should run at low speed motor.duty_cycle = MOTOR_BRAKE + 1000000 # Motor full speed motor.duty_cycle = MOTOR_BRAKE # stop motor """### Servo configuration and calibration Servo angle range [-1 - full left, 0 - center, 1 - full right] * Physical calibration of steering
camera.sensor_mode = 7
camera.resolution = res
camera.framerate = FRAMERATE
rawCapture = PiYUVArray(camera, size=res)
stream = camera.capture_continuous(rawCapture,
format="yuv",
use_video_port=True)
# Enable servo
SERVO_MIDDLE = 1500000
SERVO_MAX = 2000000
SERVO_MIN = 1000000
servo = PWM(1)
servo.period = 20000000
servo.duty_cycle = SERVO_MIDDLE
servo.enable = True
# Enable motor
MOTOR_BRAKE = 1000000
DECREASE_MAX = SPEED - SPEED_MIN
motor = PWM(0)
motor.period = 20000000
motor.duty_cycle = MOTOR_BRAKE
motor.enable = True
# Vision
CAMERA_CENTER = res[0] // 2 + CENTER_OFFSET
# To filter the noise in the image we use a 3rd order Butterworth filter
# Wn = 0.02, the cut-off frequency, acceptable values are from 0 to 1
from pwm import PWM pwm0 = PWM(0) pwm0.export() pwm0.period = 20000000 pwm0.duty_cycle = 1000000 pwm0.enable = True pwm0.duty_cycle = 1000000
#_______________________Initialize_________________________________________________
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (320, 240)
camera.framerate = 120
rawCapture = PiRGBArray(camera, size=(320, 240))
# allow the camera to warmup
time.sleep(1)
#initialize Motor
pwm0 = PWM(1)
pwm0.export()
pwm0.period = 20000000
print("pwm0 changed 1150000_______________________________________________")
pwm0.duty_cycle = 1000000
pwm0.enable = True
#initialize Servo
pwm1 = PWM(0)
pwm1.export()
pwm1.period = 20000000
pwm1.duty_cycle = 1500000
pwm1.enable = True
#____________________________________________________________________________________
time.sleep(10)
#visual servo check
pwm1.duty_cycle = 1000000
time.sleep(1)
if len(sys.argv) >= 2 and sys.argv[1] == "P":
debug = False
else:
debug = True
b, a = butter(3, 0.007)
minSpeed = 1000000 # 0 speed
zeroRot = 1000000
maxRot = 500000
rot = zeroRot
pwm0 = PWM(0) # motor
pwm0.export()
pwm0.period = 20000000
pwm0.duty_cycle = minSpeed
pwm1 = PWM(1) # servo
pwm1.export()
pwm1.period = 20000000
pwm1.duty_cycle = zeroRot
pwm0.enable = True
pwm1.enable = True
if not debug:
print("waiting for moter")
sleep(3)
print("motor ready")
speed = 1200000
from pwm import PWM pwm0 = PWM(0) pwm1 = PWM(1) pwm0.export() pwm1.export() pwm0.period = 20000000 pwm1.period = 20000000 pwm0.duty_cycle = 1000000 pwm1.duty_cycle = 1500000 pwm0.enable = True pwm1.enable = True pwm1_ref = 1465000 pwm1.duty_cycle = pwm1_ref # In[6]: pwm_s = 1200000 pwm0.duty_cycle = pwm_s
# In[5]: '''#hardcoding speed pwm0.duty_cycle = 11000 #PWM duty cycle for straightline motion: 1500000 pwm1_ref = 1465000 pwm1.duty_cycle = pwm1_ref''' pwm0 = PWM(0) pwm1 = PWM(1) pwm0.export() pwm1.export() pwm0.period = 20000000 pwm1.period = 20000000 pwm0.duty_cycle = 1000000 pwm1.duty_cycle = 1500000 pwm0.enable = True pwm1.enable = True pwm1_ref = 1465000 pwm1.duty_cycle = pwm1_ref # In[6]: pwm0.duty_cycle = pwm_s # In[7]:
b, a = butter(3, 0.02) pwm0 = PWM(0) pwm1 = PWM(1) pwm0.period = 20000000 pwm1.period = 20000000 pwm0.enable = True pwm1.enable = True # In[2]: pwm0.duty_cycle = 1000000 pwm1.duty_cycle = 1600000 time.sleep(1) pwm1.duty_cycle = 1100000 time.sleep(1) pwm1.duty_cycle = 2100000 time.sleep(1) pwm1.duty_cycle = 1600000 # In[3]: # max speed that this algo works # ucla = 1120000 # sonic = 1250000
The values specified period and duty cycle ar in nanoseconds. Servo and motor controlled follow the following protocol: 1. Control signal is read 50 times per second by each device (servo and speed controller). Thus period is 20 ms => 20000000 ns 2. To send the minimum value (full left for servo and break for motor) the duty cycle should be 1 ms => 1000000 ns 3. Maximum value corresponds to a duty cycle of 2 ms => 2000000 ns However, these devices are not perfect and you can set a duty cycle outside this range and still get a response. I would recoment to play with it and find are the limits in your case. Pay attention that servo is limited by the mechanical construction how much it can turn the wheels. Do not set these limits to a value which servo cannot execute as it will try to do that and eventually burn. """ # Enable servo SERVO_MIDDLE = 1500000 servo = PWM(1) servo.period = 20000000 servo.duty_cycle = SERVO_MIDDLE servo.enable = True # Enable servo MOTOR_BRAKE = 1000000 motor = PWM(0) motor.period = 20000000 motor.duty_cycle = MOTOR_BRAKE motor.enable = True motor.duty_cycle = MOTOR_BRAKE """## The self driving code Quick explanation on how this algorithm works: 1. Initialize the camera, use a frame rate of 10 to 20 fps
import numpy as np import time from pwm import PWM from ipywidgets import interact import PID #global variables line = 200 Kp = 1560 Kd = 0 Ki = 43 #setup servo and motor contol pwm0 = PWM(0) pwm0.period = 20000000 pwm0.duty_cycle = 10000000 pwm0.enable = True pwm1 = PWM(1) pwm1.period = 20000000 pwm1.duty_cycle = 1500000 pwm1.enable = True #Setup PID pid = PID.PID(Kp, Kd, Ki) #Kp is set to 2000, I & D are disabled pid.SetPoint = 320 pid.setSampleTime(0.01) #sample time is 10ms # set default value line = 200
import numpy as np # Camera resolution res = (640, 480) CAMERA_CENTER = res[0] // 2 from pwm import PWM # Enable servo SERVO_MIDDLE = 1500000 servo = PWM(1) servo.period = 20000000 servo.duty_cycle = SERVO_MIDDLE servo.enable = True # Enable motor MOTOR_BRAKE = 1000000 motor = PWM(0) motor.period = 20000000 motor.duty_cycle = MOTOR_BRAKE motor.enable = True motor.duty_cycle = MOTOR_BRAKE # Run a track detection algorithm on a single horizontal line. # Uses YUV420 image format as the Y component corresponds to image intensity (gray image) # and thus there is no need to convert from RGB to BW
# In[5]: '''#hardcoding speed pwm0.duty_cycle = 11000 PWM duty cycle for straightline motion: 1500000 pwm1_ref = 1465000 pwm1.duty_cycle = pwm1_ref''' pwm0 = PWM(0) pwm1 = PWM(1) pwm0.export() pwm1.export() pwm0.period = 20000000 pwm1.period = 20000000 pwm0.duty_cycle = 1000000 pwm1.duty_cycle = 1500000 pwm0.enable = True pwm1.enable = True pwm1_ref = 1465000 pwm1.duty_cycle = pwm1_ref # In[6]: #Setting parameters res = (320, 240) straight_threshold = 30 row = 100 peak_ref = 160 prev_peak_pos = 160 time_th = 0.001 peak_diff_th = 70