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