In [ ]:

    

# Boiler plate imports.
import time
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt

# Demo module for PyCon Brisbane/Australia 2015 presentation.
import spaceship as pycobr

# At this point, demos/demo_boostercube.py *must* be running!

# Connect to Azrael.
client = pycobr.PyConBrisbaneClient()

# Set the initial position and velocity of the cube.d
client.setPositionAndVelocity(pos=[0, 0, 0], vel=[0,0,0])


    

In [ ]:

    

client.reset(mass=100)

# Define the initial position of the ship.
pos_init = [0, 0, 0]
client.setPositionAndVelocity(pos=pos_init, vel=[0,0,0])

# Desired position where the ship should end up.
pos_ref = [0, 0, 10]

# Timestep in seconds.
dt = 0.2

# Periodicaly query the postion, compute the error relative to the
# desired reference position, and engage the thrusters accordingly.
pos_log = [pos_init]
for ii in range(70):
    # Wait.
    time.sleep(dt)

    # Query current position and add it to the log.
    p = client.getPosition()
    pos_log.append(p)
    
    # Compute the position error and its slope.
    err_val = pos_ref - pos_log[ii + 1]
    err_slope = pos_log[ii + 1] - pos_log[ii]
    
    # Determine the booster output with a Proportional-Differential
    # Controller.
    force = 10 * err_val - 40 * err_slope

    # Update the booster values
    client.boosterForce(force=force)

client.setPositionAndVelocity(pos=pos_init, vel=[0,0,0])
time.sleep(0.2)
client.reset()
pos_log = client.controller(pos_ref, dt, 70)
    
# Plot the cube's x/y/z position over time.
plt.plot(dt * np.arange(len(pos_log)), pos_log)
plt.grid()
plt.legend(('x', 'y', 'z'), loc='best')
plt.xlabel('Time')
plt.ylabel('Position')


    

In [ ]:

    

client.reset(mass=1000)

pos_init = [0, 0, 0]
pos_desired = [0, 0, 12]

client.setPositionAndVelocity(pos=pos_init, vel=[0, 0, 0])
time.sleep(0.2)

dt = 0.2

pos_log = client.controller(pos_desired, dt, 200)    
plt.plot(pos_log)
plt.grid()


    

In [ ]:

    

client.reset(imass=1000)

pos_init = [0,0,0]
pos_desired = [0, 0, 12]

client.setPositionAndVelocity(pos=pos_init, vel=[0,0,0])
time.sleep(0.2)

pos_log = [pos_init]

for ii in range(50):
    time.sleep(0.2)
    p = client.getPosition()
    pos_log.append(p)
    
    err_value = pos_desired - p
    err_slope = pos_log[-1] - pos_log[-2]

    force = 10 * err_value - 40 * err_slope
    client.boosterForce(force)
    

plt.plot(pos_log)
plt.grid()