def run(self, creature):
quad = Quadcopter()
quad.position = (0, 0, self.z0)
z_setpoint = self.z1 # first task: go to setpoint (0, 0, z1)
fitness = 0
while quad.t < self.total_t:
## if quad.t >= 2:
## # switch to second task
## z_setpoint = self.z2
#
inputs = [z_setpoint, quad.position.z]
outputs = creature.run_step(inputs)
assert len(outputs) == 1
pwm = outputs[0]
quad.set_thrust(pwm, pwm, pwm, pwm)
quad.step(self.dt)
fitness += self.compute_fitness(quad, z_setpoint)
self.show_step(quad)
return fitness
def main():
quad = Quadcopter()
plotter = QuadPlotter()
pd_controller = PD()
quad.position = (0, 0, 3) # put it a bit above the ground
dt = 0.01
plotter.add_marker((0, 0, 5), RED)
plotter.add_marker((0, 0, 3), YELLOW)
setpoint = 5
while plotter.show_step():
#P_controller(quad, sp_z=setpoint)
pd_controller(quad, dt, sp_z=setpoint)
#lift_controller(quad)
#yaw_controller(quad)
quad.step(dt)
if quad.t > 3:
setpoint = 3
plotter.update(quad)