def __init__(self, sensor, dOuts):
# PID
self.pidParams = PIDParams(input=0, output=0, setpoint=0)
self.pid = PID(params=self.pidParams,
kP=0,
kI=0,
kD=0,
direction=PID.DIRECT,
debugEnabled=True)
self.pid.sampleTime = self.Ts # ms
self.pid.setTunings(2.5, 0.005, 0)
self.pid.setOutputLimits(0, 100)
self.pid.debugEnabled = False
# Resistors
self.dOuts = dOuts # DigitalOutputs595 instance
self.lowerResistor = 0 # Desired power (%)
self.upperResistor = 0 # Desired power (%)
self.segmentPWM = 0 # Current segment of the PWM period
self.lastPWM = 0 # Last time of the PWM execution
# Temperature
self.sensor = sensor # Max6675 sensor
self.temps = [30.0] * self.windowSize # Temperature sliding window
self.tempsHead = 0 # Pointer to the head of the sliding window
self.temperature = 30.0 # Average temperature
self.lastRead = 0
self.t = 0
# Status
self.on = False
self.ready = False
self.starting = 0
self.turnOFF()
def PID_Test():
import pylab
from numpy import array
from PID import PID
height = []
quad = Quadcopter("001", "1.0")
height_pid = PID(40.0, 0.5, 0.0)
height_pid.set_point = 5.0
## loop = 0
## while (loop < 500):
## control = height_pid.update(quad.height)
## quad.set_motors(1000+control)
## quad.update_all(0.02)
## print control,quad.height
## loop += 1
quad.set_motors(1000)
quad.update_all(1)
#print quad.height
#quad.set_motors(1500)
#quad.update_all(1)
#print quad.height
for i in range(0, 1000):
quad.set_motors(height_pid.update(quad.height) + 1000)
quad.update_all(0.01)
#print quad.height
height.append(quad.height)
if i == 500:
height_pid.set_point = 2.0
Aheight = array(height)
pylab.plot(Aheight)
pylab.show()
def __init__(self, driveSystem, gpsDriver):
self.target_heading = 30.0
self.pid = PID(P=0.005, I=0.0, D=0.0, current_time=None)
self.gpsDriver = gpsDriver
self.driveSystem = driveSystem
self.waypoints = []
self.target_lat = 0.0
self.target_lon = 0.0
self.prev_lat = 0.0
self.prev_long = 0.0
self.waypoint_index = 0
self.forward_throttle = 1.0
self.navigating = False
#self.calculate_distance(47.730759, -117.280971, 47.691172, -117.283890)
#print(self.get_bearing(47.703185, -117.280966, 47.704542, -117.278197))
thread = Thread(target=self.HeadingCtrl)
thread.daemon = True
thread.start()
autoNavThread = Thread(target=self.AutoNavigate)
autoNavThread.daemon = True
autoNavThread.start()
def __init__(self, emu):
self.predActive = 0
self.next_stop = -1
self.actualLimit = 0
self.pid = PID(P=70, I=0.3, D=4.0, current_time=0)
self.pid.SetPoint = 30.0
self.pid.setSampleTime(0.1)
self.pid.setWindup(20.0)
self.pid_poz = PID(P=70.0, I=10.0, D=60.0, current_time=0)
self.pid_poz.setSampleTime(0.1)
self.pid_poz.setWindup(25.0)
self.pid_poz.setPoint = 0
self.predIter = 0
self.predLen = 0
#Data collection:
self.tv = []
self.sv = []
self.vv = []
self.zv = []
self.Pv = []
self.vlimv = []
self.predReal = []
self.predSets = []
def __init__(self):
self.status = ""
rospy.init_node('forward', anonymous=False)
self.pid = {
# 'az': PID(T * 0.001, (1.0/180.0), 0, 0, "Orientation"),
'az': PID(T / 1000.0, 2.0, 0.0, 1.0, "Orientation"),
'z': PID(T / 1000.0, 0.007, 0.000, 0.001, "Altitude"),
'x': PID(T / 1000.0, 0.007, 0.000, 0.001, "X Position"),
'y': PID(T / 1000.0, 0.007, 0.000, 0.001, "Y Position")
}
self.navdata = Navdata()
self.odom = Odometry()
self.mark = Marker()
self.rate = rospy.Rate(10)
self.pose = {'x': 1.0, 'y': -1.0, 'z': 1.0, 'w': 0}
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",
Empty,
queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.command = Twist()
rospy.Subscriber("/ardrone/navdata", Navdata, self.cbNavdata)
# rospy.Subscriber("/ardrone/odometry", Odometry, self.cbOdom)
rospy.Subscriber("/ground_truth/state", Odometry, self.cbOdom)
rospy.Subscriber("/visualization_marker", Marker, self.cbMarker)
self.state_change_time = rospy.Time.now()
rospy.on_shutdown(self.SendLand)
def executar():
pid = PID(KP, KI, KD)
pid.SetPoint = 0
botao = Button()
while not botao.any():
if parece_verde():
confirme_verde()
if sensor_frente.distance_centimeters < 5:
Sound.beep()
print('\n -- VI OBSTACULO! -- \n')
ultrapassar_obstaculo()
erro = get_valor_sensor('direita') - get_valor_sensor('esquerda')
pid.update(erro)
correcao = pid.output
giro_dir = sat(TP - correcao)
giro_esq = sat(TP + correcao)
esq.run_forever(speed_sp=giro_esq * (-1))
dir.run_forever(speed_sp=giro_dir * (-1))
esq.stop()
dir.stop()
def start(self):
# Init request flag.
self.request = False
self.controlling = False
self.alive = True
# Start a timer for PID.
self.timer = QtCore.QElapsedTimer()
self.timer.start()
# Init PID.
self.pid = PID(timestamp=self.timer.elapsed())
# Load tasks from NI-MAX.
self.task_ai = nidaqmx.system.storage.persisted_task.PersistedTask('TaskTemp').load()
self.task_co = nidaqmx.system.storage.persisted_task.PersistedTask('TaskPow').load()
self.task_do = nidaqmx.system.storage.persisted_task.PersistedTask('TaskDir').load()
# Start tasks.
try:
self.task_ai.start()
self.task_do.control(nidaqmx.constants.TaskMode.TASK_RESERVE)
self.task_co.control(nidaqmx.constants.TaskMode.TASK_RESERVE)
self.task_do.start()
self.task_co.start()
except Exception as err:
print(err)
def __init__(self):
self.bus = smbus.SMBus(1)
self.pwm = PwmBoard(self.bus, 0x40)
self.motor1 = Motor(self.pwm, 0)
self.motor2 = Motor(self.pwm, 1)
self.motor3 = Motor(self.pwm, 2)
self.motor4 = Motor(self.pwm, 3)
self.mpu6050 = MPU6050(self.bus, 0x68)
self.mpu6050.start()
self.networkhandler = NetworkHandler(self)
self.running = True
self.pgain = 5.0
self.igain = 0.01
self.dgain = 1.5
self.roll_pid = PID(self.pgain, self.igain, self.dgain)
self.pitch_pid = PID(self.pgain, self.igain, self.dgain)
self.yaw_pid = PID(self.pgain, self.igain, self.dgain)
self.wantedroll = 0.0
self.wantedpitch = 0.0
self.throttle = 0.0
time.sleep(1)
def __init__(self,
detect_model,
tello,
frame_width=512,
skip_frames=20,
confidence=0.4,
output_video=None):
self.detect_model = detect_model
self.tello = tello
self.frame_width = frame_width
self.skip_frames = skip_frames
self.confidence = confidence
self.output_video = output_video
self.frame = None
self.init_area = None
self.foward_pid = PID(setPoint=1.0, sample_time=0.5, P=30, I=5, D=10)
self.rotate_pid = PID(setPoint=0.5, sample_time=0.5, P=50, I=8, D=15)
self.frame_pool = Queue()
self.stopEvent = threading.Event()
self.horizontal_speed = 0
self.foward_speed = 0
self.accelerator = 0
self.rotate = 0
# meters
self.distance = 0.5
self.degree = 10
self.vedio_thread = threading.Thread(target=self._videoLoop, args=())
self.vedio_thread.start()
self.sending_command_thread = threading.Thread(
target=self._sendingCommand)
def __init__(self):
self.epsilon = 0.9
self.pid_parameter = {'kp': 0.5, 'ki': 0.5, 'kd': 0.5}
self.episodes = 500
self.take_off_procedure = 50 * 4
self.env_max_steps = 1000 # to be checked
self.alpha = 0.01
self.gamma = 0.9
self.number_of_actions = _get_number_of_actions()
self.render_every = 100
# Initialize PID controller, environment and states
self.pid = PID(self.pid_parameter['kp'], self.pid_parameter['ki'],
self.pid_parameter['kd'])
self.env = GameOfDrone()
states = self.env.reset()
# Discretize the state space
get_discrete_states = StateDiscrete(
states.dist_to_target,
states.distance_vector.optimal_heading - states.angle,
states.total_velocity)
self.discrete_states = get_discrete_states.discretize()
# Initialize the Q table
self.Q = dict()
for a in Actions:
self.Q[a] = np.zeros(get_discrete_states.state_buckets)
self.policy = np.zeros(get_discrete_states.state_buckets,
dtype=Actions)
for i in range(get_discrete_states.state_buckets[0]):
for j in range(get_discrete_states.state_buckets[1]):
for k in range(get_discrete_states.state_buckets[2]):
self.policy[i, j, k] = Actions.ENGINES_OFF
def __init__(self, sensorConversionThread):
'''
Initializes the basics of the vehicle
@param sensorConversionThread - the conversion thread
'''
self.sensorConversionThread = sensorConversionThread
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(Constants.PWM_SENSOR_FREQ)
self.velocityPID = PID(Constants.VELOCITY_PID_P,
Constants.VELOCITY_PID_I,
Constants.VELOCITY_PID_D,
Constants.VELOCITY_PID_WINDUP)
self.steeringAnglePID = PID(Constants.STEERING_ANGLE_PID_P,
Constants.STEERING_ANGLE_PID_I,
Constants.STEERING_ANGLE_PID_D,
Constants.STEERING_ANGLE_PID_WINDUP)
self.velocityDuration = -0.0
self.steeringDuration = -0.0
self.prevTotalStripCount = 0.0
self.totalStripCount = 0.0
# TODO: Wall Follow PID
self.tabs = 0
self.totalUpdateTime = 0.0
self.prevTime = time.time()
self.currentTime = time.time()
self.updateRate = gcd(Constants.VELOCITY_PID_UPDATE_RATE,
Constants.STEERING_PID_UPDATE_RATE)
def __init__(self, imuConnectString):
# go to default connect string if no string was specified
if imuConnectString == None:
imuConnectString ='tcp://localhost:10001/arduinoIMU/arduinoIMUData'
self.m1Duty = 0
self.m2Duty = 0
#largest absolute value that the motors can be set to
self.dutyMax = 5000
# control mode will be either Duty or Velocity depending on last command sent
self.controlMode = 'Duty'
self.canMeasureWheelVelocities = True
self.pidControllerR = PID(20,0,0,0,0,0,0)
self.pidControllerL = PID(-20,0,0,0,0,0,0)
# try to find and connect to NRF IMU server to get wheel velocities
try:
self.imuGateway = RRN.ConnectService(imuConnectString)
except:
print "Couldn't find NRF IMU server, unable to accept velocity commands!"
self.canMeasureWheelVelocities = False
self._lock = threading.RLock()
def callback(self, array): if len(array.data) > 0: # Body frame: z point ups; Marker frame: z points down (both right handed frames). self.Tx = array.data[1]; self.Ty = array.data[0]; self.Tz = array.data[2]; self.yaw = array.data[3]; # Access current yaw in degrees. ## Choose shortest setpoint, based on current yaw translation ## if self.first_yaw: if self.yaw > 0: self.pid_yaw = PID(-1.5, 0, -.4, .8, 0); else: self.pid_yaw = PID(-1.5, 0, -.4, .8, 0); self.first_yaw = False; self.update(); # Update PID outputs at each array callback. else: self.vel_cmd.twist.linear.x = 0; self.vel_cmd.twist.linear.y = 0; self.vel_cmd.twist.linear.z = 0; self.vel_cmd.twist.angular.z = 0; self.pub_vel.publish(self.vel_cmd);
def PID_Test():
import pylab
from numpy import array
from PID import PID
height = []
quad = Quadcopter("001","1.0")
height_pid = PID(40.0,0.5,0.0)
height_pid.set_point = 5.0
## loop = 0
## while (loop < 500):
## control = height_pid.update(quad.height)
## quad.set_motors(1000+control)
## quad.update_all(0.02)
## print control,quad.height
## loop += 1
quad.set_motors(1000)
quad.update_all(1)
#print quad.height
#quad.set_motors(1500)
#quad.update_all(1)
#print quad.height
for i in range(0,1000):
quad.set_motors(height_pid.update(quad.height)+1000)
quad.update_all(0.01)
#print quad.height
height.append(quad.height)
if i == 500:
height_pid.set_point = 2.0
Aheight = array(height)
pylab.plot(Aheight)
pylab.show()
def setUp(self):
self.pidParams = PIDParams(input=70, output=0, setpoint=90)
self.pid = PID(params=self.pidParams,
kP=5.0,
kI=0.25,
kD=1.15,
direction=PID.DIRECT,
debugEnabled=True)
def __init__(self, y: float, right: bool, end_x: float, speed=0.2, omega=0.2):
self.pid = PID(0.4, 1, 1.5, 0.2)
self.y = y
self.right = right
self.end_x = end_x
self.speed = speed
self.omega = omega
self.t_last = None
def __init__(self, name, motor, counter):
self.name = name
self.motor = motor
self.counter = counter
self.pid = PID(0.5, 1.4, 0.8, Integrator_max=300, Integrator_min=-300)
self.lastUpdateTime = time.time()
self.lastCount = self.counter.count
def __init__(self):
super().__init__()
self.plant = Plant("opc.tcp://192.168.1.23:4840")
self.plant.start()
self.pid_valv1 = PID()
self.pid_valv2 = PID()
self.is_running = False
self.u = [0, 0]
def calcTheForce(self):
# расчет сил
dx = self.XYZ_d[0] - self.XYZ[0]
dy = self.XYZ_d[1] - self.XYZ[1]
self.ABG_d[2] = np.arctan2(dy, dx)
Fx_d = 0
if -0.05 <= self.ABG_d[2] - self.ABG[2] <= 0.05:
if dx < 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
if dx > 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx < 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx > 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
# перемещения вдоль игрек
if -0.5 <= dx <= 0.5 and (self.XYZ[1] <= self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if (-0.5 <= dx <= 0.5) and (self.XYZ[1] > self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1
# перемещения вдоль икс
if -0.5 <= dy <= 0.5 and (self.XYZ[0] <= self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
if (-0.5 <= dy <= 0.5) and (self.XYZ[0] > self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1
else:
Fx_d = 0
Fy_d = 0
Fz_d = PIDc.pidUpdate(self.stab_ctrl, self.XYZ_d[2], self.XYZ[2])
# повороты: рыскание, тангаж и крен
Mx_d = PIDc.pidUpdate(self.rotation_ctrl, self.ABG_d[2],
self.ABG[2]) # вокруг Z, рыскание
My_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[1], self.ABG[1])
Mz_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[0], self.ABG[0])
forceD = [Fx_d, Fy_d, Fz_d, Mx_d, My_d, Mz_d]
s_force = vrep.simxPackFloats(forceD)
vrep.simxSetStringSignal(self.clientID, "ForceD", s_force,
vrep.simx_opmode_oneshot)
def car_control(self):
self.find_all()
####----------------Use MPC -------------------
# predict = MPC(x_start,y_start,yaw,v)
if ((abs(self.yaw))> deg2rad(86)):
a = 7
elif rad2deg(abs(self.yaw)) > deg2rad(84) :
a = 5
elif rad2deg(abs(self.yaw)) > deg2rad(78) :
a = 3
elif rad2deg(abs(self.yaw)) > deg2rad(75) :
a = 2
else:
a = -6
++i
print(i)
self.dt = self.dt+ 1/10
self.speed = self.speed + a * self.dt
###Test
#print(self.dt,'dt')
print(rad2deg(self.yaw),'yaw')
print(self.speed,'speed')
### ----------------Use PID--------------------
kp = .53
ki = 0.005
kd = 0.0003
"""40km/h"""
# kp = .92
# ki = 0.001
# kd = 0.5
# if (self.DIRECTION == 'right'):
# self.cte = abs(self.cte) +5
# else:
# self.cte = -abs(self.cte) - 5
control = PID(kp,ki,kd)
control.update_error(self.cte)
steer = control.total_error()
self.angle_steer = steer
# print(self.cte,'cte')
#####SPEED####
# speed = 30
# self.speed = speed
# print(steer,'steer')
# print(self.angle_steer,'total error')
print(steer,'fianl steer')
self.speed_pub.publish(self.speed)
self.steer_pub.publish(steer)
class LidarSteerManager:
def __init__(self):
self.pid = PID(LIDAR_PID_P, LIDAR_PID_I, LIDAR_PID_D)
self.pid.SetPoint = 0
self.pid.setSampleTime(LIDAR_PID_SAMPLE_RATE)
self.priority = "0"
def steer(self, lidarData):
error = self.error(lidarData)
self.pid.update(error)
speed = .5 + .5 * (1 - abs(self.pid.output))
return MAX_STEER * self.pid.output,speed,self.priority
def error(self, points):
FUTURE_WEIGHT = .65
PRESENT_WEIGHT = 1 - FUTURE_WEIGHT
diff_present = self.subErrorAtAngle(points, 90 - 5)
diff_future = self.subErrorAtAngle(points, 45)
diff = diff_present * PRESENT_WEIGHT + diff_future * FUTURE_WEIGHT
return LidarSteerManager.cappedMultiply(diff, LIDAR_STEER_MULT)
def subErrorAtAngle(self, lidarPoints, angle):
# angle = degrees
left = self.getPerpDistance(lidarPoints, -angle)
right = self.getPerpDistance(lidarPoints, angle)
left, right = LidarSteerManager.normalize(left, right)
# In case of emergency drop right....
if(left < .75 or right < .75):
self.priority = "3"
else:
self.priority = "1"
sub_error = right - left
return sub_error
def getPerpDistance(self, lidarPoints, angle):
# Perpendicular
index = LidarHelper.angleToLidarIndex(angle)
vectorDistance = lidarPoints[index]
return abs(LidarSteerManager.getXComponentOfVector(vectorDistance, 90 - angle))
@staticmethod
def getXComponentOfVector(magnitude, direction):
# direction = degree
degree2Radian = math.pi / 180
return magnitude * math.cos(direction * degree2Radian)
@staticmethod
def normalize(left_distance, right_distance):
max_distance = max(left_distance, right_distance)
return left_distance / max_distance, right_distance / max_distance
@staticmethod
def cappedMultiply(value, by):
if value == 0:
return 0
return min(abs(value) * by, 1) * value / abs(value)
def __init__(self, drone, picture_width, desired_move):
self.turn = PID(K_p=0.6, K_d=0.1)
self.move = PID(K_p=0.15, K_d=0.01)
self.height = PID(K_p=0.2, K_d=0.00)
self.picture_width = picture_width
self.desired_move = desired_move
self.drone = drone
time.sleep(0.05)
self.drone.takeoff()
time.sleep(0.05)
def __init__(self, X=True) :
if X :
self.pwmpin = Pin('X6', Pin.OUT_PP)
self.pwmtim = Timer(2, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('X2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('X3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('X4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('X5', Pin.IN, Pin.PULL_UP)
else :
self.pwmpin = Pin('Y1', Pin.OUT_PP)
self.pwmtim = Timer(8, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('Y2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('Y3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP)
self.pwmscale = (self.pwmtim.period() + 1) // 10000 # scale factot for permyriad(10000 as it allows to get more distinct points and avoid divisions) power
self.count_a = 0 # counter for impulses on the A output of the encoder
self.target_a = 0 # target value for the A counter (for controlled rotation)
self.count_b = 0 # counter for impulses on the B output of the encoder
self.time_a = 0 # last time we got an impulse on the A output of the encoder
self.time_b = 0 # last time we got an impulse on the B output of the encoder
self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B
self.dirsensed = 0 # direction sensed through the phase of the A and B outputs
self.rpm = 0 # current speed in rotations per second
self.rpm_last_a = 0 # value of the A counter when we last computed the rpms
self.cruise_rpm = 0 # target value for the rpms
self.walking = False # Boolean that indicates if the robot is walking or stationary
self.desiredDir = True# Boolean that indecates the desired direction of the motor for move function
self._control = PID() # PID control for the rotation of the wheels
self._control.setTunings(KP, KI, KD);
self._control.setSampleTime(1);
self._control.setOutputLimits(-10000, 10000)
self._control_distance = PID() # PID control for the cascade of the distance
self._control_distance.setTunings(KP_DISTANCE, KI_DISTANCE, KD_DISTANCE);
self._control_distance.setSampleTime(1);
self._control_distance.setOutputLimits(-MAXIMUM_VELOCITY, MAXIMUM_VELOCITY)
ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler)
ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler)
if RomiMotor.rpmtimer is None : # create only one shared timer for all instances
RomiMotor.rpmtimer = Timer(4)
RomiMotor.rpmtimer.init(freq=100, callback=RomiMotor.class_rpm_handler)
RomiMotor.rpm_handlers.append(self.rpm_handler) # register the handler for this instance
def __init__(self):
self.pid = PID(2, 0.10, 20) # 3 0.8 20 # Trial and Error values
self.pid.setWindup(75) # 100
self.top_arduino = None
self.base_arduino = None
self.base_target = None # Current target for PID
self.thread = None
self.looping = False # If thread is running
self.thread_shutdown = False
def setup():
pinMode(pin, INPUT, PULLUP)
attachInterrupt(pin, motorReadAndAdjustment, FALLING)
print "falling edge interrupt attached to P9.12 (GPIO1_28)"
pin = GPIO1_28
gpio.setup("P8_10", gpio.OUT)
set_mode(self, 1)
set_period(self, 100000000)
set_position(self, 0)
pid = PID(0.2, 0.0, 0.0)
pid.update()
class PID_Posicion(object):
"""docstring for PID_Posicion"""
def __init__(self,callback,kp = 1.5 ,ki = 0.5 ,kd = 0.0, motor_no = 4,):
self.callback = callback
self.pid = PID(kp,ki,kd)
self.motor = Motor(pines['motor'][motor_no])
def setPoint(self , setpoint):
self.pid.setPoint(setpoint)
sp = int(setpoint*(10))
self.motor.avance(sp)
def setup():
pinMode(pin, INPUT, PULLUP)
attachInterrupt(pin, motorReadAndAdjustment, FALLING)
print "falling edge interrupt attached to P9.12 (GPIO1_28)"
pin = GPIO1_28
gpio.setup("P8_10",gpio.OUT)
set_mode(self,1)
set_period(self,100000000)
set_position(self,0)
pid = PID(0.2, 0.0, 0.0)
pid.update()
def send_att(self):
rate = rospy.Rate(100) # Hz
self.att.body_rate = Vector3(0, 0, 2)
self.att.header = Header()
self.att.header.frame_id = "base_footprint"
self.att.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
self.att.thrust = 0.3
self.att.type_mask = 7 # ignore body rate
xPID = PID(kp=0.2, kd=0.2, ki=0.1)
zPID = PID(kp=0.03, kd=0.01, ki=0.01)
yPID = PID(kp=0.03, kd=0.01, ki=0.01)
while not rospy.is_shutdown():
try:
trans = (self.udrone_state.x, self.udrone_state.y,
self.udrone_state.z)
print("Estimate: ", trans)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as Ex:
print(Ex)
self.att.header.stamp = rospy.Time.now()
self.att.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0))
self.att.thrust = 0.3
self.att_pub.publish(self.att)
self.rate.sleep()
continue
x_error = xPID.run(trans[0], 0)
y_error = yPID.run(trans[1], 0)
z_error = zPID.run(trans[2], 5)
# if z_error is positive, udrone is far from boat
# so increase pitch to increase the altitude
if z_error < -1.5709:
z_error = -1.5709
elif z_error > 1.5709:
z_error = 1.5709
print(z_error)
# if y_error is positive, udrone is far from boat
# so increase pitch to increase the altitude
if y_error < -1.5709:
y_error = -1.5709
elif y_error > 1.5709:
y_error = 1.5709
if x_error < 0:
x_error = 0.0
elif x_error > 1:
x_error = 1.0
self.att.header.stamp = rospy.Time.now()
self.att.orientation = Quaternion(
*quaternion_from_euler(0, -1 * z_error, y_error))
self.att.thrust = x_error
self.att_pub.publish(self.att)
rate.sleep()
class Control:
def __init__(self, robot: Robot):
self.robot = robot
self.pid = PID(0.4, 1, 1.5, 0.2)
def state(self) -> State:
return self.robot.estimator.state()
def reset(self):
self.pid.clear()
def update_hline(self, dt, y, right):
state = self.state()
d = y - state.y
# angle is 90 for large d
theta = (pi / 2) * (1 - math.exp(-(2 * d)**2))
if d < 0:
theta = -theta
if not right:
theta = pi - theta
dtheta = norm_angle(theta - state.theta)
# reduce speed if theta is very wrong, 1 at 0, 0.2 at pi/2
speed = 0.5 * math.exp(-abs(5 * dtheta)**2)
# relax towards desired _theta
omega = dtheta # / 2
domega = 0
if abs(d) < 0.4:
domega = -self.pid.update(d, dt)
if right:
domega = -domega
else:
# TODO: reset PID? or update PID without using result?
pass
print(d, theta, state.theta, dtheta, speed, domega)
self.robot.set_speed_omega(speed, omega + domega)
self.robot.update(dt)
def arc(self, dt, radius, speed, end_theta, direction):
omega = speed / radius
if not direction:
omega = -omega
self.robot.set_speed_omega(speed, omega)
# stop at first zero crossing, but not a random jump +-pi
dtheta_last = norm_angle(self.state().theta - end_theta)
while True:
dtheta = norm_angle(self.state().theta - end_theta)
if abs(dtheta) < 0.5:
if dtheta_last < 0 and dtheta >= 0: break
if dtheta_last > 0 and dtheta <= 0: break
dtheta_last = dtheta
self.robot.update(dt)
def __init__(self, configFile, channel=0): self.configFile = configFile self.IOdevice = IOdevice self.channel = channel self.readParams() self.readTimepoints() self.CO2PID = PID() self.O3PID = PID() self.CO2enable = True self.O3enable = True self.parentPipe, self.childPipe = Pipe()
def __init__(
self,
callback,
kp=1.5,
ki=0.5,
kd=0.0,
motor_no=4,
):
self.callback = callback
self.pid = PID(kp, ki, kd)
self.motor = Motor(pines['motor'][motor_no])
def InitControl(self):
self._P = 0.8
self._I = 14.0
self._D = 0.12
self._maxIntegralOutput = 9.0
self._pid = PID(self._P, self._I, self._D)
self._pid.setWindup(100.0)
self._ctrlOutputMin = 1.0 # Volts, min motor output
self._ctrlOutputMax = 12.0 # Volts, max motor output
return
class HLineControl(Control):
def __init__(self, y: float, right: bool, end_x: float, speed=0.2, omega=0.2):
self.pid = PID(0.4, 1, 1.5, 0.2)
self.y = y
self.right = right
self.end_x = end_x
self.speed = speed
self.omega = omega
self.t_last = None
def __str__(self):
return f"HLineControl({self.right},{self.end_x})"
def reset(self):
self.pid.clear()
def update(self, t: float, state: State): # -> (speed, omega)
d = self.y - state.y
# angle is 90 for large d
theta = (pi/2) * (1 - math.exp(-(d/0.1)**2))
if d < 0:
theta = -theta
if not self.right:
theta = pi - theta
dtheta = norm_angle(theta - state.theta)
# reduce speed if theta is very wrong, 1 at 0, 0.2 at pi/2
# also reduce speed if will overshoot in one iteration:
# sin(theta) * speed * 1s <= abs(d)
# speed <= abs(d) / sin(theta)
speed = self.speed * math.exp(-abs(4*dtheta)**2)
if math.sin(theta) > 0:
speed = min(speed, abs(d) / math.sin(theta) + 0.02)
omega = math.copysign(min(abs(dtheta) + 0.02, self.omega), dtheta)
domega = 0
if False and self.t_last is not None and abs(d) < 0.4:
# only use PID if near the line
# TODO: remove this discontinuity
domega = -self.pid.update(d, t - self.t_last)
if self.right:
domega = -domega
else:
# TODO: reset PID? or update PID without using result?
pass
#print(t, d, theta, state.theta, dtheta, speed, domega)
self.t_last = t
return speed, omega + domega
def end(self, t: float, state: State) -> bool:
return self.right == (state.x >= self.end_x)
def configure(self):
self.load_config()
#self.myPID1 = PID(self.config['kp1'], self.config['ki1'], self.config['sp1'])
self.myPID1 = PID(2000, 5, 1125)
self.myPID2 = PID(self.config['kp2'], self.config['ki2'],
self.config['sp2'])
self.myPID2.stop()
self.u1 = 0
self.y01 = 0
self.u2 = 0
self.y02 = 0
self.timer = pyb.Timer(1, freq=1000)
def __init__(self, uav, height=10, radius=None, increment=5):
super(BoxSearch, self).__init__(uav, height=height)
self.radius = radius
self.increment = increment
self.last_pose = None
self.last_increment = self.increment // 2
self.last_increment_dim = "-y"
self.velocity = None
self.pidz = PID(1.2, 1, .001)
self.pidz.SetPoint = self.height
self.pidz.setSampleTime(.01)
self.pidxy = None
def __init__(self, lin_pid=PID(0.05, 0, 0.01),
ang_pid=PID(0.03, 0, 0.05),
trajectory_generator=TrajectorySCurve(r=10)):
"""
:param lin_pid: Linear PID.
:param ang_pid: Angular PID.
:param move_type; Which type of movement it should return. It can be
'vel' for MoveByVelocities or 'pow' for MoveByPowers.
:return: None.
"""
self.lin_pid = lin_pid
self.ang_pid = ang_pid
self.trajectory_generator = trajectory_generator
def __init__(self, outer_loop_params, inner_loop_params, sample_time):
self.SetPoint = 0.0
self.Output = 0.0
self.SampleTime = sample_time
self.positionPID = PID(outer_loop_params.Kp, outer_loop_params.Ki,
outer_loop_params.Kd, outer_loop_params.Limit)
self.positionPID.setSampleTime(self.SampleTime)
self.speedPID = PID(inner_loop_params.Kp, inner_loop_params.Ki,
inner_loop_params.Kd, inner_loop_params.Limit)
self.speedPID.setSampleTime(self.SampleTime)
def __init__(self):
self.temperature_samples = []
self.last_fill = 0
self.current_volume = 0
self.resistor_duty = 0 # set by LLD
self.temperatures = List_max(SAMPLE_HISTORY)
self.volumes = List_max(SAMPLE_HISTORY)
self.powers = List_max(SAMPLE_HISTORY)
self.timing = List_max(SAMPLE_HISTORY)
self.recipe_index = 0
PID.__init__(self)
Chrono.__init__(self)
def __init__(self, imu, m1, m2, m3): threading.Thread.__init__(self) self.imu = imu self.m1 = m1 self.m2 = m2 self.m3 = m3 self.pidX = PID() self.pidY = PID() self.pidZ = PID(P=0.0, I=0.0, D=0.0) self.pidX.setPoint(0.02) # in radians self.pidY.setPoint(0.03) self.pidZ.setPoint(0.0) self.start()
class MotorController: def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.PID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.motorState="turnToAngle" def stop(self): self.actuators.motorL.write(1,0) self.actuators.motorR.write(1,0) def updateMotorSpeeds(self): print self.motorState if(self.motorState=="turnToAngle"): self.updateTurnToAngle() elif(self.motorState=="wallFollow"): self.updateWallFollow() elif(self.motorState=="turnConstantRate"): self.updateTurnConstantRate() def updateTurnToAngle(self): pidResult=self.PID.valuePID(self.sensors.gyro.gyroCAngle, self.desiredAngle) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) def updateWallFollow(self): self.motorLdrive = self.fwdVel - self.wallFollowPIDResult self.motorRdrive = self.fwdVel + self.wallFollowPIDResult self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) def updateTurnConstantRate(self): self.motorLdrive = -self.turnConstantRate self.motorRdrive = self.turnConstantRate self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
class Actuator(object):
def __init__(self, drone, picture_width, desired_move):
self.turn = PID(K_p=0.6, K_d=0.1)
self.move = PID(K_p=0.15, K_d=0.01)
self.height = PID(K_p=0.2, K_d=0.00)
self.picture_width = picture_width
self.desired_move = desired_move
self.drone = drone
time.sleep(0.05)
self.drone.takeoff()
time.sleep(0.05)
def step(self, wdithmid, width):
desired_turn = self.picture_width / 2
actual_turn = wdithmid
actual_move = width
ut = self.turn.step(desired_turn, actual_turn)
um = self.move.step(self.desired_move, actual_move)
height = 550
nav_data = self.drone.get_navdata()
nav_data = nav_data[0]
uh = self.height.step(height, nav_data['altitude'])
self.drone.at(libardrone.at_pcmd, True, 0, self.moveDrone(um), self.heightDrone(uh), self.turnDrone(ut))
def turnDrone(self, u):
speed = - u / (self.picture_width / 2.)
print "move horizontal to" + str(speed)
return speed
def moveDrone(self, u):
speed = - u / (self.picture_width / 2.)
print "move near to" + str(speed)
return speed
def heightDrone(self, u):
speed = u / 500
print "height near to" + str(speed)
return speed
def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.turnToAnglePID=PID(1.2, 3.5, 0.2) self.turnAndMovePID=PID(1, 2, 0.15) self.movePID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.turnConstantRateDirection="Right" self.motorState="turnToAngle"
def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.PID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.motorState="turnToAngle"
def __init__(self):
# Time for timer
global T
T = 1000
# For debugging
global _debug
_debug = False
# For PID
# TODO: Add GUI to tune K
global Kp, Ki, Kd
Kp = 3.0
Ki = 0.4
Kd = 1.2
self.p = PID(3.0, 0.4, 1.2)
self.p.setPoint(0)
# Initialize AutoWC and WheelchairModel
self.autoWC = AutoWC()
self.wc = WheelchairModel()
self.wc.delta_t = T / 1000.
interface = gtk.Builder()
interface.add_from_file('AutoWC_GUI.glade')
interface.connect_signals(self)
self.Interface = interface.get_object('mainWindow')
self.Interface.show()
self.myPort = interface.get_object('entryPort')
self.myStatus = interface.get_object('lbStatus')
self.txtTicks = interface.get_object('textTicks')
self.LRSignal = interface.get_object('adjustmentLR')
self.FBSignal = interface.get_object('adjustmentFB')
self.entryDistance = interface.get_object('entryDistance')
self.entryAngle = interface.get_object('entryAngle')
self.entryVelocity = interface.get_object('entryVelocity')
self.cbtnStop_distance = interface.get_object('cbtnStop_distance')
self.cbtnStop_angle = interface.get_object('cbtnStop_angle')
self.entryStop_distance = interface.get_object('entryStop_distance')
self.entryStop_angle = interface.get_object('entryStop_angle')
def setup(self):
self.TicpIn = 4480 / 2.875
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
# motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1, 0)
# self.deltaL = 1
# self.motorvalL = 0
# motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorRdrive = 0
self.motorR.write(1, 0)
# self.deltaR = 1
# self.motorvalR = 0
# gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -0.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(5, 4, 0.2)
self.fwdVel = 0
self.timer = Timer()
"""
def __init__(self):
self.port = 'COM22' # change this to the com port for your Arduino
self.baud = 115200 # baudrate don't change
try:
self.ser = serial.Serial(self.port, self.baud)
print "Successfully opened Serial"
print
except serial.SerialException:
self.ser = None
print "Could not open Serial!"
print
# Initialize Joystick object
joystick.init()
pygame.display.init()
if joystick.get_count():
self.joy = joystick.Joystick(0)
self.joy.init()
else:
self.joy = None
self.x_axis = 0 # this is usually 0
self.throttle_axis = 1 # change this to the correct axis for the joystick
self.last_turn_val = 0
self.last_sheet_val = 47
self.auto_enabled = False # change to true to use PID
self.kite_tracker = KiteTracker(path='cam')
self.pos_list = []
self.pid = PID(3.0, 0.4, 1.2)
self.pid.setSetPoint(0.0) # Set-point corresponds to heading vector which has angle = 0.0 in its frame.
# create log file
self.logfile = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d_%H_%M_%S') + '_log.txt'
with open(logfile, 'w') as f:
f.write('time,command,turn_val,power_val\n')
def main(self):
# Creates the publisher for ROS. (name of message, data type(from geometry_msgs.msg), queue size)
velocity_publisher = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
velocity_vector = TwistStamped() # Ros data type for publisher
x_linear = 0
y_linear = 0
z_linear = 0
altitude_PID = PID(0.5,0.1,0.01)
x_ang = 0
y_ang = 0
z_ang = 0
# Creates a state publisher, for when we want to remotely disarm the system
state_publisher = rospy.Publisher('mavros/state', State, queue_size = 10)
state_variable = State()
# sets the rate of the loop
rate = rospy.Rate(10)
# goal altitude
altitude_goal = 3
self.altitude_current = 0
# state variable
stance = 0
stance_previous = 0
count = 0
# 0 : logical element
# 1 : waiting for offb controll
# 2 : take off
# 3 : land
# 4 : search pattern
while not rospy.is_shutdown():
count += 1
if count % 100 == 1:
rospy.loginfo("State: " + str(stance))
##########################################################################################
################# stance switch ###########################################################
##########################################################################################
if stance == 0:
x_linear = 0
y_linear = 0
z_linear = 0
x_ang = 0
y_ang = 0
z_ang = 0
stance = 1
# waiting for offb control
elif stance == 1:
rate.sleep()
if self.state_current.guided:
rospy.loginfo("I'm taking control, takeoff in " + str(countdown))
countdown -= 0.1
if countdown < 0:
rospy.loginfo("Switching to Takeoff")
stance_previous = 1
stance = 2
else:
countdown = 5
# Take off:
elif stance == 2:
altitude_goal = 2
if count % 10 == 1:
rospy.loginfo("Current altitude: " + str(self.altitude_current))
if self.altitude_current <= altitude_goal + 0.1 and self.altitude_current >= altitude_goal - 0.1:
countdown -= 0.1
if count % 10 == 1:
rospy.loginfo("At goal altitude, switching to logic state")
stance_previous = 2
stance = 0
else:
countdown = 5
if countdown < 0:
stance = 0
# Landing
elif stance == 3:
if count % 10 == 1:
rospy.loginfo(self.altitude)
z_linear = -0.02
if self.altitude_current < 0.10:
countdown -= 0.1
rospy.loginfo("WARNING: low altitude, DISARMING in: " + str(countdown))
else:
countdown = 10
if self.altitude_current < 0.05 and countdown < 0:
rospy.loginfo("DISARM-DISARM-DISARM")
state_variable.armed = False
state_variable.guided = False
state_publisher.publish(state_variable)
elif stance == 4:
pass
elif stance == 5:
pass
elif stance == 6:
pass
##########################################################################################
################# velocity finder ########################################################
##########################################################################################
if not stance == 3: # If were not landing, this works
# set the verticle speed
z_linear = altitude_PID.run(self.altitude_current, altitude_goal)
# need to do this for x_linear, y_linear, and possibly x,y, and z ang
##########################################################################################
################# velocity publisher #####################################################
##########################################################################################
# set linear to value
velocity_vector.twist.linear.x = x_linear
velocity_vector.twist.linear.y = y_linear
velocity_vector.twist.linear.z = z_linear
# Set angular to zero
velocity_vector.twist.angular.x = x_ang
velocity_vector.twist.angular.y = y_ang
velocity_vector.twist.angular.z = z_ang
# Get the time now for the velocity commands
time = rospy.Time.now()
velocity_vector.header.stamp.secs = int(time.to_sec())
velocity_vector.header.stamp.nsecs = int((time.to_sec() - int(time.to_sec())) * 1000000000)
velocity_vector.header.seq = count
# use the publisher
velocity_publisher.publish(velocity_vector)
rate.sleep()
def setup(self):
self.TicpIn = 4480/2.875
self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10)
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
# print "initial angle:"+str(self.initAngle)
self.Follow = 'Right'
self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]}
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.IRPID = PID(10, 5, .15)
self.fwdVel = 30
self.turnVel = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
class PIDDrive(SyncedSketch):
ss_pin = 10 #gyro select pin
# image_pipe = './image'
# if not os.path.exists(image_pipe):
# os.mkfifo(image_pipe)
# image_fd = os.open(image_pipe, os.O_RDONLY)
def setup(self):
self.TicpIn = 4480/2.875
self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10)
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
# print "initial angle:"+str(self.initAngle)
self.Follow = 'Right'
self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]}
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.IRPID = PID(10, 5, .15)
self.fwdVel = 30
self.turnVel = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def loop(self):
#state 0 - wait for first no block, do nothing
#transition: no block -> state 1
#state 1 - look for block
#transition: found block -> state 2
#sate 2 - drive over block
#transition: ir triggered -> state 3
#sate 3 - pick up block
#transition: color sensor done -> sate 1
if self.timer.millis() > 100:
# print 'Color'
# print self.color.c
# print self.color.r, self.color.g
if self.color.c > 800 and self.Sort == 0:
if self.color.r > self.color.g:
self.Sort = 1
else:
self.Sort = 2
# print self.uIR.val
if self.uIR.val == 0:
# self.MoveArm = True
self.State = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = self.servo.bottom
self.servo.write(self.servo.bottom)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= self.servo.top:
time.sleep(1)
self.delta = -self.servo.speed
self.Top = 1
elif self.servoval <= self.servo.bottom:
self.delta = self.servo.speed
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
if self.Sort == 1:
if self.sorterval < self.sorter.left:
self.dsorter = self.sorter.speed
elif self.sorterval >= self.sorter.left:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 2:
if self.sorterval > self.sorter.right:
self.dsorter = -self.sorter.speed
elif self.sorterval <= self.sorter.right:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 3:
if time.time() - self.SortPause > 1:
self.sorterval = self.sorter.center
self.Sort = 0
self.sorterval += self.dsorter
self.sorter.write(abs(self.sorterval))
self.servoval += self.delta
self.servo.write(abs(self.servoval))
self.initAngle += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle=self.gyro.val - self.totalDrift #corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro=self.gyro.val
self.totalDrift+=self.drift
# message = os.read(self.image_fd, 20)
# if ParseMessage(message):
# self.blockDistance, self.blockAngle = ParseMessage(message)
# print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult)
# print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val)
# print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.)
self.ir_array.update()
ir = self.ir_array.ir_value
ir90 = self.IRs[self.Follow][0]
ir30 = self.IRs[self.Follow][1]
avg = (ir[self.IRs[self.Follow][0]]+ir[self.IRs[self.Follow][1]]/2)/2
min_val = min(ir[self.IRs[self.Follow][0]], ir[self.IRs[self.Follow][1]])
if avg != float('inf'):
pidResult= -self.IRPID.valuePID(4, avg)
elif min_val != float('inf'):
pidResult= -self.IRPID.valuePID(4, min_val)
else:
pidResult = -20
if ir[self.IRs[self.Follow][2]] < 4:
pidResult = 20
# self.fwdVel = 0
else:
self.fwdVel = 30
pidResult = self.PID.valuePID(self.initAngle, cAngle)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
print 'Distance Traveled : ' + str(self.getDistanceTraveled())
self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive))
self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
'''
print "ok"
#response = self.rp.read()
#print "Got response %s" % response
'''
def ParseMessage(message):
if message:
if message[:2] == 'no':
blockDistance, blockAngle = 0, 0
return None
else:
try:
blockDistance, blockAngle = [number[:6] for number in message.split(',')]
except:
blockDistance, blockAngle = 0, 0
return blockDistance, blockAngle
else:
return None
def getDistanceTraveled(self):
"""returns Distance traveled in inches.
Call resetEncoders, drive forward until you've reached your destination"""
print 'Left: ' + str(self.encoderL.val)
print 'Right: ' + str(self.encoderR.val)
avg = (self.encoderL.val + self.encoderR.val)/2
return avg/360.
class KiteControl(object):
def __init__(self):
self.port = 'COM22' # change this to the com port for your Arduino
self.baud = 115200 # baudrate don't change
try:
self.ser = serial.Serial(self.port, self.baud)
print "Successfully opened Serial"
print
except serial.SerialException:
self.ser = None
print "Could not open Serial!"
print
# Initialize Joystick object
joystick.init()
pygame.display.init()
if joystick.get_count():
self.joy = joystick.Joystick(0)
self.joy.init()
else:
self.joy = None
self.x_axis = 0 # this is usually 0
self.throttle_axis = 1 # change this to the correct axis for the joystick
self.last_turn_val = 0
self.last_sheet_val = 47
self.auto_enabled = False # change to true to use PID
self.kite_tracker = KiteTracker(path='cam')
self.pos_list = []
self.pid = PID(3.0, 0.4, 1.2)
self.pid.setSetPoint(0.0) # Set-point corresponds to heading vector which has angle = 0.0 in its frame.
# create log file
self.logfile = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d_%H_%M_%S') + '_log.txt'
with open(logfile, 'w') as f:
f.write('time,command,turn_val,power_val\n')
def write_to_serial(self, msg):
if self.ser:
if not '/' in msg:
msg += '/'
self.ser.write(msg) # Terminating char '/' must always be appended to the end of the msg
with open('log.txt', 'a') as f:
date = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d %H:%M:%S.%f')
f.write(date + ',' + msg + ',')
if 'p' in msg:
f.write(',' + msg.strip('t/p'))
elif 't' in msg:
f.write(msg.strip('t/p') + ',')
f.write('\n')
return True
else:
if not '/' in msg:
msg += '/'
print 'Serial not available: ' + msg
return False
def turn(self, val):
val *= 30
val = int(val)
if not val == self.last_turn_val:
self.last_turn_val = val
return self.write_to_serial('t' + str(val))
def sheet(self, val):
val = (val + 1)/2 * 96 # maps the values (-1,1) to (0,96) This might need to be reversed?
val = int(val)
if not val == self.last_sheet_val:
self.last_sheet_val = int(val)
return self.write_to_serial('p' + str(val))
def run(self):
while True:
pygame.event.get()
pygame.event.pump()
if not self.auto_enabled:
if self.joy:
self.turn(self.joy.get_axis(self.x_axis))
self.sheet(self.joy.get_axis(self.throttle_axis))
self.kite_tracker.update()
else:
self.kite_tracker.update() # must be called once per loop
error = self.update_error()
#output = self.pid.update(error)
#self.turn(output)
time.sleep(0.05)
def update_error(self):
"""
Input: tuple of x,y coordinates of the kite.
Computes angle error between current heading and desired heading.
Returns: error angle in degrees.
"""
# get updated position from kite tracker and store in array
self.pos_list.append(self.kite_tracker.get_pos())
if len(self.pos_list) > 3:
last_pos = self.pos_list[-3]
pos = self.pos_list[-1]
target = self.kite_tracker.get_current_target()
if pos and last_pos and last_pos != pos:
# only keep the last 5 positions
if len(self.pos_list) > 5:
self.pos_list = self.pos_list[1:]
# compute the heading based on the last average vector over the last 2 positions
vecA = np.array([pos[0]-last_pos[0], pos[1] - last_pos[1]])
vecA = vecA/np.linalg.norm(vecA)
vecB = np.array([target[0] - last_pos[0], target[1] - last_pos[1]])
vecB = vecB/np.linalg.norm(vecB)
ctheta = vecA.dot(vecB)/(np.linalg.norm(vecA) * np.linalg.norm(vecB)) # = cos(theta)
if ctheta > 1:
ctheta = 1.0
theta = np.rad2deg(np.arccos(ctheta)) # original unsigned angle
angle = 1
rot = np.mat([[np.cos(np.deg2rad(angle)), -np.sin(np.deg2rad(angle))],
[np.sin(np.deg2rad(angle)), np.cos(np.deg2rad(angle))]])
# rotate vecA by 1 degree ccw
rot_vecA = rot * np.mat(vecA).T
rot_vecA = rot_vecA.T
# recalcluate angle
ctheta = rot_vecA.dot(vecB)/(np.linalg.norm(rot_vecA) * np.linalg.norm(vecB))
if ctheta > 1:
ctheta = 1.0
theta_new = np.rad2deg(np.arccos(ctheta)) # original unsigned angle
if theta_new < theta:
theta = -theta
print theta
return theta
else:
print 0
return 0
else:
print 0
return 0
class Quadcopter():
def __init__(self):
self.bus = smbus.SMBus(1)
self.pwm = PwmBoard(self.bus, 0x40)
self.motor1 = Motor(self.pwm, 0)
self.motor2 = Motor(self.pwm, 1)
self.motor3 = Motor(self.pwm, 2)
self.motor4 = Motor(self.pwm, 3)
self.mpu6050 = MPU6050(self.bus, 0x68)
self.mpu6050.start()
self.networkhandler = NetworkHandler(self)
self.running = True
self.pgain = 5.0
self.igain = 0.01
self.dgain = 1.5
self.roll_pid = PID(self.pgain, self.igain, self.dgain)
self.pitch_pid = PID(self.pgain, self.igain, self.dgain)
self.yaw_pid = PID(self.pgain, self.igain, self.dgain)
self.wantedroll = 0.0
self.wantedpitch = 0.0
self.throttle = 0.0
time.sleep(1)
def start(self):
lasttime = time.time()
time_diff = 0.01
while self.running:
self.mpu6050.read()
time.sleep(time_diff - 0.005)
x, y, z = self.mpu6050.getlastvalues()
delta_time = time.time() - lasttime
lasttime = time.time()
roll_pid_result = self.roll_pid.Compute(x, self.wantedroll, delta_time)
pitch_pid_result = self.pitch_pid.Compute(y, self.wantedpitch, delta_time)
#yaw_pid_result = self.yaw_pid.Compute(z, 0, delta_time)
yaw_pid_result = 0
if self.throttle != 0:
self.motor1.update((self.throttle + roll_pid_result + pitch_pid_result + yaw_pid_result))
self.motor2.update((self.throttle + roll_pid_result - pitch_pid_result - yaw_pid_result))
self.motor3.update((self.throttle - roll_pid_result - pitch_pid_result + yaw_pid_result))
self.motor4.update((self.throttle - roll_pid_result + pitch_pid_result - yaw_pid_result))
def changepidgain(self, pgain, igain, dgain):
self.roll_pid.changegain(pgain, igain, dgain)
self.pitch_pid.changegain(pgain, igain, dgain)
def setroll(self, roll):
if (roll > -50) & (roll < 50):
self.wantedroll = roll
def setpitch(self, pitch):
if (pitch > -50) & (pitch < 50):
self.wantedpitch = pitch
def setthrottle(self, throttle):
if throttle == 0:
self.throttle = 0
time.sleep(0.1)
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
else:
throttle = throttle * (((self.motor1.max - 180) - (self.motor1.min + 180)) / 100) + (self.motor1.min + 180)
if (throttle > (self.motor1.min + 180)) & (throttle < (self.motor1.max - 180)):
self.throttle = throttle
def getdata(self):
gx, gy, ax, ay, x, y, z = self.mpu6050.getextendedvalues()
return gx, gy, ax, ay, x, y, z, self.motor1.getpercent(), self.motor2.getpercent(), self.motor3.getpercent(), self.motor4.getpercent()
def stop(self):
self.running = False
self.mpu6050.stop()
time.sleep(1)
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
self.networkhandler.stop()
def Lock(con, cur):
setPoints = getSetpoints()
LaserLock_369 = PID(P=-50, I=-500, D=0)
LaserLock_399 = PID(P=20, I=20, D=0)
LaserLock_935 = PID(P=40, I=60, D=0)
LaserLock_369.setPoint(setPoints[0])
LaserLock_399.setPoint(setPoints[1])
LaserLock_935.setPoint(setPoints[2])
ADDA1.setVoltage(0,0)
ADDA1.setVoltage(1,0)
ADDA1.setVoltage(2,0)
timeFlag_1 = False
overTime = time.mktime(datetime.datetime.now().timetuple())
errorCount=0
while True:
freq = getFreqs()
t = getSetpoints()
if(t != None):
setPoints = getSetpoints()
if(LaserLock_369.set_point != setPoints[0]): LaserLock_369.setPoint(setPoints[0])
if(LaserLock_399.set_point != setPoints[1]): LaserLock_399.setPoint(setPoints[1])
if(LaserLock_935.set_point != setPoints[2]): LaserLock_935.setPoint(setPoints[2])
for i in range(len(freq)):
if freq[i]<0:
freq[i] = setPoints[i]
if abs(freq[i]-setPoints[i]) > 0.01: # > 10 Ghz or 0.01nm? probably a mode hop
freq[i] = setPoints[i] # wait to try and recover
#freq[0] = setPoints[0]
error_369 = LaserLock_369.update(freq[0])
error_399 = LaserLock_399.update(freq[1])
error_935 = LaserLock_935.update(freq[2])
#print freq
#print setPoints
print round(error_369,4), round(error_399,4), round(error_935,4)
if (error_369>=5) or(error_399>=5) or(error_935>=5) or(error_369<=-5) or(error_399<=-5) or(error_935<=-5):
errorCount += 1
else:
errorCount = 0
if errorCount > 3:
# winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS)
# winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS)
# winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS)
# winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS)
ADDA1.setVoltage(0,0)
ADDA1.setVoltage(1,0)
ADDA1.setVoltage(2,0)
print "Lock Broken!"
break
ADDA1.setVoltage(0, error_369)
ADDA1.setVoltage(1, error_399)
ADDA1.setVoltage(2, error_935)
cTime = time.mktime(datetime.datetime.now().timetuple())*1e3 + datetime.datetime.now().microsecond/1e3
#cur.execute("INSERT INTO `wavemeter`.`error`( `index`, `time`, `739`, `935`, `739w`, `935w`) VALUES (NULL, \'%s\',\'%s\',\'%s\',\'%s\',\'%s\');",(cTime,error_2,error_1, freq[1], freq[0]))
#con.commit()
cur.execute("INSERT INTO `wavemeter`.`error` (time, `369`, `399`, `935`, 369w, 399w, 935w) VALUES (\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\');",(cTime,round(error_369,4), round(error_399,4), round(error_935,4), freq[0], freq[1], freq[2]))
con.commit()
time.sleep(.2)
class PIDDrive(SyncedSketch):
ss_pin = 10 #gyro select pin
image_pipe = './image'
if not os.path.exists(image_pipe):
os.mkfifo(image_pipe)
image_fd = os.open(image_pipe, os.O_RDONLY)
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480/2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def loop(self):
#state 0 - wait for first no block, do nothing
#transition: no block -> state 1
#state 1 - look for block
#transition: found block -> state 2
#sate 2 - drive over block
#transition: ir triggered -> state 3
#sate 3 - pick up block
#transition: color sensor done -> sate 1
message = os.read(self.image_fd, 20)
if message:
# print("Recieved: '%s'" % message)
if message[:2] == 'no':
self.blockAngle = 0
if self.State == 0:
self.State = 1
if self.State == 2:
self.State = 3
else:
if self.State != 0:
self.State = 2
try:
self.blockDistance, self.blockAngle = [number[:6] for number in message.split(',')]
except:
self.blockAngle = 0
self.blockAngle = float(self.blockAngle)
if self.timer.millis() > 100:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.display.quit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
self.turnVel = -10
if event.key == pygame.K_RIGHT:
self.turnVel = 10
if event.key == pygame.K_UP:
self.fwdVel = 100
if event.key == pygame.K_DOWN:
self.fwdVel = -100
if event.key == pygame.K_1:
self.Sort = 1
if event.key == pygame.K_2:
self.Sort = 2
if event.key == pygame.K_SPACE:
self.MoveArm = True
if event.type == pygame.KEYUP:
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
self.turnVel = 0
self.turn = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
self.fwdVel = 0
print 'Color'
print self.color.c
print self.color.r, self.color.g
print
if self.color.c > 800 and self.Sort == 0:
if self.color.r > self.color.g:
self.Sort = 1
else:
self.Sort = 2
# print self.uIR.val
if self.uIR.val == 0:
self.MoveArm = True
self.State = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = self.servo.bottom
self.servo.write(self.servo.bottom)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= self.servo.top:
time.sleep(1)
self.delta = -self.servo.speed
self.Top = 1
elif self.servoval <= self.servo.bottom:
self.delta = self.servo.speed
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
if self.Sort == 1:
if self.sorterval < self.sorter.left:
self.dsorter = self.sorter.speed
elif self.sorterval >= self.sorter.left:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 2:
if self.sorterval > self.sorter.right:
self.dsorter = -self.sorter.speed
elif self.sorterval <= self.sorter.right:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 3:
if time.time() - self.SortPause > 1:
self.sorterval = self.sorter.center
self.Sort = 0
self.sorterval += self.dsorter
self.sorter.write(abs(self.sorterval))
self.servoval += self.delta
self.servo.write(abs(self.servoval))
self.turn += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle=self.gyro.val - self.totalDrift #corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro=self.gyro.val
self.totalDrift+=self.drift
# print 'State: ' + str(self.Searching)
self.newAngle = cAngle + self.blockAngle
print self.blockAngle
pidResult=self.PID.valuePID(cAngle, cAngle+self.blockAngle+self.turn)
# print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult)
# print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val)
# print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive))
self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
'''
# Preset options.
requiredTemp = 37.5
saveToLogFile = False
minTempAlarm = 0.0
maxTempAlarm = 100.0
maxTempDiff = 100.0
# Pin asignments
sensorPins = []
alarmOutputPin = 0
tempOutputPin = 0
tempPID = PID(1.0, 0.0, 0.0)
# Retrieve a sensor reading
def getSensorValues(sensorPin):
# Use the (modified) Adafruit DHT library
proc = subprocess.Popen(' '.join(["./Adafruit_DHT 22",str(sensorPin)]), stdout=subprocess.PIPE, shell=True)
(out, err) = proc.communicate()
if out == "":
raise ValueError
else:
# Output format: TT.t,HH.h
vals = out.split(",")
return float(vals[0]), float(vals[1])
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480/2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
numReadings = 0
sumReadings= 0
while time.time() < (startTimeForCalibration+10.0):
numReadings += 1
sumReadings += accelorometer.getReadingX()
meanReading = 0 - (sumReadings / numReadings)
start_time = time.time()
# Set up the PID control system
pid = PID(P=1, I=1, D=1, Derivator=0, Integrator=0, Integrator_max=50, Integrator_min=-50)
for i in range (0,1000):
r= accelorometer.getReadingX() + meanReading
print r, pid.update(r)
end_time = time.time()
print("Elapsed time was %g seconds" % (end_time - start_time))
g=numpy.matrix(numpy.diag([l/Ixx,l/Iyy,1/Izz]))
ginv=g.I
## Vectores de x deseada y su derivada
xd=numpy.array([[0],[0],[0]])
xpd=numpy.array([[0],[0],[0]])
Altd=260
## Objeto diferenciador numerico por modos deslizantes
difN = 4 ## Orden del diferenciador
dif = diffOrdN.DiffOrdN(difN,[12,8,4,3.5,2.1])
## Objeto que calcula la integral de la funcion signo
intsgn=Integral(lambda x:numpy.sign(x))
## Controlador de altitud
ctrlalt=PID(.7,.2,.1)
### Se establece configuracion con el ardrone, se apagan los motores y se cambia el modo de camara
stdout.write("Estableciendo comunicacion con el ARDrone\n")
stdout.flush()
drone=libardrone.ARDrone()
sleep(1)
print "Listo!"
stdout.write("Estableciendo configuracion inicial\n")
stdout.flush()
drone.reset()
sleep(0.1)
drone.trim()
sleep(1.5)
drone.reset()
print "Encendiendo motores"
class PIDDrive(SyncedSketch):
ss_pin = 10 # gyro select pin
def setup(self):
self.TicpIn = 4480 / 2.875
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
# motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1, 0)
# self.deltaL = 1
# self.motorvalL = 0
# motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorRdrive = 0
self.motorR.write(1, 0)
# self.deltaR = 1
# self.motorvalR = 0
# gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -0.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(5, 4, 0.2)
self.fwdVel = 0
self.timer = Timer()
"""
self.pipePath = "./vision"
print "ok1"
time.sleep(1)
try:
os.mkfifo(self.pipePath)
except OSError:
print "error"
print "ok"
self.rp = open(self.pipePath, 'r')
"""
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
# response = self.rp.read()
# print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle = self.gyro.val - self.totalDrift # corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro = self.gyro.val
self.totalDrift += self.drift
pidResult = self.PID.valuePID(cAngle, self.initAngle)
print "Angle Dif: " + str(cAngle - self.initAngle) + "\tPID RESULT: " + str(pidResult)
print "Encoders:\tR: " + str(self.encoderR.val) + "\tL: " + str(self.encoderL.val)
print "AVG: " + str((self.encoderR.val + self.encoderL.val) / 2.0)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
if self.motorLdrive >= 0:
dirL = 0
else:
dirL = 1
if self.motorRdrive >= 0:
dirR = 0
else:
dirR = 1
self.motorL.write(dirL, abs(self.motorLdrive))
self.motorR.write(dirR, abs(self.motorRdrive))
"""
class Stablizer (threading.Thread): def __init__(self, imu, m1, m2, m3): threading.Thread.__init__(self) self.imu = imu self.m1 = m1 self.m2 = m2 self.m3 = m3 self.pidX = PID() self.pidY = PID() self.pidZ = PID(P=0.0, I=0.0, D=0.0) self.pidX.setPoint(0.02) # in radians self.pidY.setPoint(0.03) self.pidZ.setPoint(0.0) self.start() def run(self): t = 0 while not config.exitStatus: try: if config.is_PID_new == True: self.pidX.setKp(config.PID[0]) self.pidX.setKi(config.PID[1]) self.pidX.setKd(config.PID[2]) self.pidX.setIntegrator(0) self.pidY.setKp(config.PID[0]) self.pidY.setKi(config.PID[1]) self.pidY.setKd(config.PID[2]) self.pidY.setIntegrator(0) except AttributeError: pass data = self.imu.getData() roll = data['complementaryX'] + 5.5 pitch = data['complementaryY'] - 3.4 yaw = data['complementaryZ'] x = self.pidX.update(math.sin(roll*math.pi/180.0)) y = self.pidY.update(math.sin(pitch*math.pi/180.0)) output = self.vector2Motor(x, y) self.m1.setSpeed(1.00*output[0]) self.m2.setSpeed(1.00*output[1]) self.m3.setSpeed(1.00*output[2]) sleep(0.02) print "Stablizer Thread: Shutting down." def vector2Motor(self, x, y): phase_offset = 4.107860468 # Motor to IMU angle offset (786) magnitude = math.sqrt(x*x + y*y) angle = math.atan(x/y) if y != 0 else math.pi/2.0 if y < 0: angle = angle + math.pi angle = angle + phase_offset return (magnitude*math.sin(angle + math.pi/3.0), magnitude*math.sin(angle + 5.0*math.pi/3.0), magnitude*math.sin(angle + math.pi))
class RoboClawState:
""" test """
def __init__(self, imuConnectString):
# go to default connect string if no string was specified
if imuConnectString == None:
imuConnectString ='tcp://localhost:10001/arduinoIMU/arduinoIMUData'
self.m1Duty = 0
self.m2Duty = 0
#largest absolute value that the motors can be set to
self.dutyMax = 5000
# control mode will be either Duty or Velocity depending on last command sent
self.controlMode = 'Duty'
self.canMeasureWheelVelocities = True
self.pidControllerR = PID(20,0,0,0,0,0,0)
self.pidControllerL = PID(-20,0,0,0,0,0,0)
# try to find and connect to NRF IMU server to get wheel velocities
try:
self.imuGateway = RRN.ConnectService(imuConnectString)
except:
print "Couldn't find NRF IMU server, unable to accept velocity commands!"
self.canMeasureWheelVelocities = False
self._lock = threading.RLock()
def setDutyMax(self, newMax):
self.dutyMax = newMax
# exposed to user through RR (implicitly sets to Duty mode)
def setMDuties(self, leftDuty, rightDuty):
print "received command"
self.controlMode = 'Duty'
global lastMessageTime
lastMessageTime = time.time()
if abs(leftDuty) > self.dutyMax:
self.m1Duty = self.dutyMax * cmp(leftDuty, 0)
else:
self.m1Duty = leftDuty
if abs(rightDuty) > self.dutyMax:
self.m2Duty = self.dutyMax * cmp(rightDuty, 0)
else:
self.m2Duty = rightDuty
# used by velocity controller to write motor duties without setting to Duty mode or updating time
def internalSetDuties(self, leftDuty, rightDuty):
if abs(leftDuty) > self.dutyMax:
self.m1Duty = self.dutyMax * cmp(leftDuty, 0)
else:
self.m1Duty = leftDuty
if abs(rightDuty) > self.dutyMax:
self.m2Duty = self.dutyMax * cmp(rightDuty, 0)
else:
self.m2Duty = rightDuty
def setMVelocities(self, leftV,rightV):
self.controlMode = 'Velocity'
global lastMessageTime
lastMessageTime = time.time()
#print leftV,rightV
self.pidControllerL.setPoint(-leftV)
self.pidControllerR.setPoint(rightV)
