def __init__(self):
index = 1 # for cf1
initialPosition = [0, 0, 0] # x,y,z coordinate for this crazyflie
self.cfs = CrazyflieParser(index, initialPosition)
self.cf = self.cfs.crazyflies[0]
self.time = self.cfs.timeHelper
self.number = 5
self.cf.setParam("commander/enHighLevel", 1)
self.cf.setParam("stabilizer/estimator", 2) # Use EKF
self.cf.setParam("stabilizer/controller",
2) # Use mellinger controller
#cf.setParam("ring/effect", 7)
self.input = [] # Set it as cmdposition ow
self.state = [] # unsampled X
self.rank = [] # check the rank
self.prediction = []
self.new_cmd = []
self.motor_in = []
# 34:1.4 40:1.2 48:1.0 60:0.8 80:0.6 100:0.48 120:0.4 150:0.32
self.k = 60 # number of time step
self.t = 20 # number of training time
self.m = 3 # number of inputs
self.n = 2 # number of states
self.hz = 20
self.height = 0.165 + 0.023 * 3
self.Xr = np.zeros((self.number * (self.k + 20) + 1, self.m))
self.difference = np.zeros((self.k, self.n))
self.i = 0.0
self.reference() # set the reference
# Phase1: for test
self.cf.takeoff(targetHeight=self.height + 0.01, duration=3.0)
self.time.sleep(3.0)
while not rospy.is_shutdown():
if self.i < self.t:
self.cmd_u = np.array([self.Xr[self.i, 0], self.Xr[self.i, 1]])
self.cf.cmdPosition(
np.array([self.cmd_u[0], 0, self.cmd_u[1]]), 0)
self.i = self.i + 1
#self.rate.sleep()
self.time.sleep(0.1)
elif self.i < self.number * (self.k + 20):
self.update_model()
self.controller()
self.i = self.i + 1
elif self.i == self.number * (self.k + 20):
self.cf.land(targetHeight=0.0, duration=5.0)
self.time.sleep(5.0)
self.graph()
self.odom_information()
self.input.append(self.cmd_u)
#!/usr/bin/env python
import math
import numpy as np
from crazyflieParser import CrazyflieParser
if __name__ == '__main__':
index = 1 # for cf1
initialPosition = [0, 1.5, 0] # x,y,z coordinate for this crazyflie
cfs = CrazyflieParser(index, initialPosition)
cf = cfs.crazyflies[0]
time = cfs.timeHelper
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
#cf.setParam("ring/effect", 7)
cf.takeoff(targetHeight=0.5, duration=3.0)
time.sleep(3.0)
# FILL IN YOUR CODE HERE
# Please try both goTo and cmdPosition
cf.goTo([1, 1.5, 0.5], yaw=0.0, duration=5.0)
time.sleep(5.0)
cf.goTo([1, 1.5, 1], yaw=0.0, duration=3.0)
time.sleep(3.0)
cf.goTo([0, 1.5, 1], yaw=0.0, duration=5.0)
time.sleep(5.0)
cf.goTo([0, 1.5, 0.5], yaw=0.0, duration=5.0)