def headTo(targetX, targetY, targetZ, maxTimeToGetThere):
startTime = timer()
print([targetX, targetY, targetZ])
while (robot.step(timestep) != -1
and (timer() - startTime < maxTimeToGetThere)):
#print("im flying")
led_state = int(robot.getTime()) % 2
front_left_led.set(led_state)
front_right_led.set(int(not (led_state)))
roll = imu.getRollPitchYaw()[0] + M_PI / 2.0
pitch = imu.getRollPitchYaw()[1]
yaw = compass.getValues()[0]
roll_acceleration = gyro.getValues()[0]
pitch_acceleration = gyro.getValues()[1]
xGPS = gps.getValues()[2]
yGPS = gps.getValues()[0]
zGPS = gps.getValues()[1]
throttlePID.setpoint = targetZ
vertical_input = throttlePID(zGPS)
yaw_input = yawPID(yaw)
rollPID.setpoint = targetX
pitchPID.setpoint = targetY
roll_input = float(
params["k_roll_p"]) * roll + roll_acceleration + rollPID(xGPS)
pitch_input = float(
params["k_pitch_p"]) * pitch - pitch_acceleration + pitchPID(-yGPS)
front_left_motor_input = float(
params["k_vertical_thrust"]
) + vertical_input - roll_input - pitch_input + yaw_input
front_right_motor_input = float(
params["k_vertical_thrust"]
) + vertical_input + roll_input - pitch_input - yaw_input
rear_left_motor_input = float(
params["k_vertical_thrust"]
) + vertical_input - roll_input + pitch_input - yaw_input
rear_right_motor_input = float(
params["k_vertical_thrust"]
) + vertical_input + roll_input + pitch_input + yaw_input
mavic2proHelper.motorsSpeed(robot, front_left_motor_input,
-front_right_motor_input,
-rear_left_motor_input,
rear_right_motor_input)
#print("im not flying anymore")
return
params[line[0]] = line[1]
TIME_STEP = int(params["QUADCOPTER_TIME_STEP"])
TAKEOFF_THRESHOLD_VELOCITY = int(params["TAKEOFF_THRESHOLD_VELOCITY"])
M_PI = 3.1415926535897932384626433
robot = Robot()
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = mavic2proHelper.getMotorAll(robot)
timestep = int(robot.getBasicTimeStep())
mavic2proMotors = mavic2proHelper.getMotorAll(robot)
mavic2proHelper.initialiseMotors(robot, 0)
mavic2proHelper.motorsSpeed(robot, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY)
front_left_led = LED("front left led")
front_right_led = LED("front right led")
gps = GPS("gps")
gps.enable(TIME_STEP)
imu = InertialUnit("inertial unit")
imu.enable(TIME_STEP)
compass = Compass("compass")
compass.enable(TIME_STEP)
gyro = Gyro("gyro")
gyro.enable(TIME_STEP)
#cameraRobot = Robot("cameraRobot")
#camera = cameraRobot.getCamera("eagleCamera")
#camera = Camera("eagleCamera")
#camera.enable(TIME_STEP)
lines = csv.reader(f)
for line in lines:
params[line[0]] = line[1]
TIME_STEP = int(params["QUADCOPTER_TIME_STEP"])
TAKEOFF_THRESHOLD_VELOCITY = int(params["TAKEOFF_THRESHOLD_VELOCITY"])
M_PI = 3.1415926535897932384626433
robot = Robot()
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = mavic2proHelper.getMotorAll(robot)
timestep = int(robot.getBasicTimeStep())
mavic2proMotors = mavic2proHelper.getMotorAll(robot)
mavic2proHelper.initialiseMotors(robot, 0)
mavic2proHelper.motorsSpeed(robot, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY, TAKEOFF_THRESHOLD_VELOCITY)
front_left_led = LED("front left led")
front_right_led = LED("front right led")
gps = GPS("gps")
gps.enable(TIME_STEP)
imu = InertialUnit("inertial unit")
imu.enable(TIME_STEP)
compass = Compass("compass")
compass.enable(TIME_STEP)
gyro = Gyro("gyro")
gyro.enable(TIME_STEP)
yaw_setpoint=-1
pitchPID = PID(float(params["pitch_Kp"]), float(params["pitch_Ki"]), float(params["pitch_Kd"]), setpoint=0.0)