# missile - By: Jiarui - 周三 1月 15 2020
import sensor, image, time, math, imu
from pyb import Servo
from mpu9250 import MPU9250
IMU = MPU9250('X')
time_shut_down = 5 #the time to shut down under no condition (in unit of s)
limit_ready_to_launch = 5 #the longest time between ready and launch (in unit of s)
limit_found_to_hit = 5 #the longest time between found and hit (in unit of s)
green_threshold = ( 85, 90,-75,-55, 5, 35)
s1 = Servo(1) #aileron 1 horizontal left
s2 = Servo(2) #aileron 2 horizontal right
s3 = Servo(3) #aileron 3 vertical tail
time_found = 0.0
time_ready = 0.0
time_launch = 0.0 #use global variables to write down switch points
K = 50 #the value should be measured
X = 160 #total pixels in x
Y = 120 #total pixels in y
KP = 1 #the value should be measured
k_pitch = 1 #the value should be measured
k_yaw = 1 #the value should be measured
k_roll = 1 #the value should be measured
last_y_accel=0
last_z_accel=0
last_state="stop"
'''
in the whole program, define x-row, y-pitch, z-yaw.
all positive direction are same.
in case correction needed.
# Untitled - By: Gehaha - 周五 3月 8 2019 import sensor ,image,time from pid import PID from pyb import Servo pan_servo = Servo(1) tilt_servo = Servo(2) red_threshold = (13,49,18,61,6,47) pan_pid = PID(P = 0.07,i= 0,imax = 90) #脱机运行或者图像传输，使用这个pid tilt_pid = PID(p = 0.05,i = 0,imax = 90) #脱机运行或者禁用图像传输，使用这个PID # initialize the camera sensor sensor.reset() sensor.set_contrast(1) sensor.set_gainceiling(16) #use RGB565 sensor.set_pixformat(sensor.GRAYSCALE) sensor.set_framesize(sensor.QQQVGA) sensor.set_vflip(True) sensor.skip_frames(10) sensor.set_auto_whitebal(False) clock = time.clock()
# Servo Control Example
#
# This example shows how to use your OpenMV Cam to control servos.
import micropython, time
from pyb import Servo
micropython.alloc_emergency_exception_buf(100)
s1 = Servo(1) # P7
#s1.calibration(1000, 1500, 1100)
s1.calibration(640, 2420, 1500)
s1.angle(0)
time.sleep(2000)
print(s1.pulse_width())
print(s1.calibration())
i = -45
while (i < 45):
print(i)
s1.angle(i)
print(s1.pulse_width())
time.sleep(100)
i += 1
# Car seems to start at 1390 PW which is speed -14 on default scale
# PWM:
# at 200hz with pw%=10, pw = 2160
