CPAL Playground

/* Copyright (C) 2015, RealTime-at-Work, All Rights Reserved
 *
 * You may use, copy, reproduce, and distribute this sample cource code for any non-commercial purpose.
 * Some purposes which can be non-commercial are teaching, academic research,
 * public demonstrations and personal experimentation. You may also distribute this Software
 * with books or other teaching materials, or publish the Software on websites,
 * that are intended to teaching for academic or other non-commercial purposes.
 *
 * In return, we simply require that you agree: 
 * 1. That you will not remove any copyright or other notices from the Software.
 * 2. THAT THE SOFTWARE COMES "AS IS", WITH NO WARRANTIES. THIS MEANS NO EXPRESS, 
 *    IMPLIED OR STATUTORY WARRANTY.
 *
 * @keywords IMU; frdm k64f; PWM
 *
 * A control-command to keep a direction on a moving balance.
 */

struct Imu {
  int16: x;
  int16: y;
  int16: z;
  /* acceleration in multiple of 0.488mg */
  int16: accel_x;
  int16: accel_y;
  int16: accel_z;
};

struct Vector_int32 {
  int32: x;
  int32: y;
  int32: z;
};

struct Vector_int16 {
  int16: x;
  int16: y;
  int16: z;
};

processdef Imu_With_Low_Pass_Filter(
  in Imu: raw,
  out Imu: filtered)
{
  const uint32: k = 2;
  static var Vector_int32: reg_mag = {0, 0, 0};
  static var Vector_int32: reg_accel = {0, 0, 0};
  
  state Init {
    var Vector_int32: c_mag;
    var Vector_int32: c_accel;
    c_mag.x = int32.as(raw.x);
    c_mag.y = int32.as(raw.y);
    c_mag.z = int32.as(raw.z);
    c_accel.x = int32.as(raw.accel_x);
    c_accel.y = int32.as(raw.accel_y);
    c_accel.z = int32.as(raw.accel_z);
    IO.println("Initialising...");
    reg_mag.x = c_mag.x - (c_mag.x >> k) + c_mag.x;
    reg_mag.y = c_mag.y - (c_mag.y >> k) + c_mag.y;
    reg_mag.z = c_mag.z - (c_mag.z >> k) + c_mag.z;
    reg_accel.x = c_accel.x - (c_accel.x >> k) + c_accel.x;
    reg_accel.y = c_accel.y - (c_accel.y >> k) + c_accel.y;
    reg_accel.z = c_accel.z - (c_accel.z >> k) + c_accel.z;
    filtered.x = raw.x;
    filtered.y = raw.y;
    filtered.z = raw.z;
    filtered.accel_x = raw.accel_x;
    filtered.accel_y = raw.accel_y;
    filtered.accel_z = raw.accel_z;
  }
  on (true) to Main;
  state Main {
    IO.println("Running...");
    /* Update filter with current sample */
  	reg_mag.x = reg_mag.x - (reg_mag.x >> k) + int32.as(raw.x);
    reg_mag.y = reg_mag.y - (reg_mag.y >> k) + int32.as(raw.y);
    reg_mag.z = reg_mag.z - (reg_mag.z >> k) + int32.as(raw.z);
    reg_accel.x = reg_accel.x - (reg_accel.x >> k) + int32.as(raw.accel_x);
    reg_accel.y = reg_accel.y - (reg_accel.y >> k) + int32.as(raw.accel_y);
    reg_accel.z = reg_accel.z - (reg_accel.z >> k) + int32.as(raw.accel_z);
    /* Scale output for unity gain */
    filtered.x = int16.as(reg_mag.x >> k);
    filtered.y = int16.as(reg_mag.y >> k);
    filtered.z = int16.as(reg_mag.z >> k);
    filtered.accel_x = int16.as(reg_accel.x >> k);
    filtered.accel_y = int16.as(reg_accel.y >> k);
    filtered.accel_z = int16.as(reg_accel.z >> k);
  }
}

const time64: MAX = 1940us;
const time64: MIN = 1100us;
const int16: G = 2048;

processdef Pendulum_Aligner(in Imu: imu, out time64: pwm) {
  state Main {
    var time64: p  = time64.as(G + imu.accel_y);
    var uint64: v1 = uint64.as(MAX - MIN);
    var time64: v2 = p * v1;
    var uint64: v3 = uint64.as(2 * G);
    var time64: v4 = v2 / v3;
    IO.println("Aligning... %i", imu.accel_y);
    pwm = MIN + v4;
  }
}

/* IOs */
var time64: pin_pwm_c09 = MIN;
var Imu: pin_imu;

/* links */
var Imu: filtered_imu;

/* tasks */
process Imu_With_Low_Pass_Filter: imu_filter[50ms](pin_imu, filtered_imu);
process Pendulum_Aligner: aligner[50ms](filtered_imu, pin_pwm_c09);

for ms or try the desktop editor with advanced diagram views

Simulation
Tasks diagram


Quiet mode disabled
[0.000000000000:ASSIGN] Assign cpal_system new value: Scheduling_Policy.FIFO
[0.000000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.000000000000:STATE] process "Imu_With_Low_Pass_Filter", instance "imu_filter", state "Init"
[0.000000000000:PRINTLN] Initialising...
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:ASSIGN] Assign filtered_imu new value: 0
[0.000000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.000000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.000000000000:STATE] process "Pendulum_Aligner", instance "aligner", state "Main"
[0.000000000000:PRINTLN] Aligning... 0
[0.000000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.000000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.050000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.050000000000:STATE] process "Imu_With_Low_Pass_Filter", instance "imu_filter", state "Main"
[0.050000000000:PRINTLN] Running...
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:ASSIGN] Assign filtered_imu new value: 0
[0.050000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.050000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.050000000000:PRINTLN] Aligning... 0
[0.050000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.050000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.100000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.100000000000:PRINTLN] Running...
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:ASSIGN] Assign filtered_imu new value: 0
[0.100000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.100000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.100000000000:PRINTLN] Aligning... 0
[0.100000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.100000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.150000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.150000000000:PRINTLN] Running...
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:ASSIGN] Assign filtered_imu new value: 0
[0.150000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.150000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.150000000000:PRINTLN] Aligning... 0
[0.150000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.150000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.200000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.200000000000:PRINTLN] Running...
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:ASSIGN] Assign filtered_imu new value: 0
[0.200000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.200000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.200000000000:PRINTLN] Aligning... 0
[0.200000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.200000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.250000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.250000000000:PRINTLN] Running...
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:ASSIGN] Assign filtered_imu new value: 0
[0.250000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.250000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.250000000000:PRINTLN] Aligning... 0
[0.250000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.250000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.300000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.300000000000:PRINTLN] Running...
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:ASSIGN] Assign filtered_imu new value: 0
[0.300000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.300000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.300000000000:PRINTLN] Aligning... 0
[0.300000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.300000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.350000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.350000000000:PRINTLN] Running...
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:ASSIGN] Assign filtered_imu new value: 0
[0.350000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.350000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.350000000000:PRINTLN] Aligning... 0
[0.350000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.350000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.400000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.400000000000:PRINTLN] Running...
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:ASSIGN] Assign filtered_imu new value: 0
[0.400000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.400000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.400000000000:PRINTLN] Aligning... 0
[0.400000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.400000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.450000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.450000000000:PRINTLN] Running...
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:ASSIGN] Assign filtered_imu new value: 0
[0.450000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.450000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.450000000000:PRINTLN] Aligning... 0
[0.450000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.450000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.500000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.500000000000:PRINTLN] Running...
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:ASSIGN] Assign filtered_imu new value: 0
[0.500000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.500000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.500000000000:PRINTLN] Aligning... 0
[0.500000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.500000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.550000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.550000000000:PRINTLN] Running...
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:ASSIGN] Assign filtered_imu new value: 0
[0.550000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.550000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.550000000000:PRINTLN] Aligning... 0
[0.550000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.550000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.600000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.600000000000:PRINTLN] Running...
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:ASSIGN] Assign filtered_imu new value: 0
[0.600000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.600000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.600000000000:PRINTLN] Aligning... 0
[0.600000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.600000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.650000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.650000000000:PRINTLN] Running...
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:ASSIGN] Assign filtered_imu new value: 0
[0.650000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.650000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.650000000000:PRINTLN] Aligning... 0
[0.650000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.650000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.700000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.700000000000:PRINTLN] Running...
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:ASSIGN] Assign filtered_imu new value: 0
[0.700000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.700000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.700000000000:PRINTLN] Aligning... 0
[0.700000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.700000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.750000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.750000000000:PRINTLN] Running...
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:ASSIGN] Assign filtered_imu new value: 0
[0.750000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.750000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.750000000000:PRINTLN] Aligning... 0
[0.750000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.750000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.800000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.800000000000:PRINTLN] Running...
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:ASSIGN] Assign filtered_imu new value: 0
[0.800000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.800000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.800000000000:PRINTLN] Aligning... 0
[0.800000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.800000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.850000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.850000000000:PRINTLN] Running...
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:ASSIGN] Assign filtered_imu new value: 0
[0.850000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.850000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.850000000000:PRINTLN] Aligning... 0
[0.850000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.850000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.900000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.900000000000:PRINTLN] Running...
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:ASSIGN] Assign filtered_imu new value: 0
[0.900000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.900000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.900000000000:PRINTLN] Aligning... 0
[0.900000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.900000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[0.950000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[0.950000000000:PRINTLN] Running...
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:ASSIGN] Assign filtered_imu new value: 0
[0.950000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[0.950000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[0.950000000000:PRINTLN] Aligning... 0
[0.950000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[0.950000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.000000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.000000000000:PRINTLN] Running...
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:ASSIGN] Assign filtered_imu new value: 0
[1.000000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.000000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.000000000000:PRINTLN] Aligning... 0
[1.000000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.000000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.050000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.050000000000:PRINTLN] Running...
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:ASSIGN] Assign filtered_imu new value: 0
[1.050000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.050000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.050000000000:PRINTLN] Aligning... 0
[1.050000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.050000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.100000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.100000000000:PRINTLN] Running...
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:ASSIGN] Assign filtered_imu new value: 0
[1.100000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.100000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.100000000000:PRINTLN] Aligning... 0
[1.100000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.100000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.150000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.150000000000:PRINTLN] Running...
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:ASSIGN] Assign filtered_imu new value: 0
[1.150000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.150000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.150000000000:PRINTLN] Aligning... 0
[1.150000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.150000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.200000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.200000000000:PRINTLN] Running...
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:ASSIGN] Assign filtered_imu new value: 0
[1.200000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.200000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.200000000000:PRINTLN] Aligning... 0
[1.200000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.200000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.250000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.250000000000:PRINTLN] Running...
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:ASSIGN] Assign filtered_imu new value: 0
[1.250000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.250000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.250000000000:PRINTLN] Aligning... 0
[1.250000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.250000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.300000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.300000000000:PRINTLN] Running...
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:ASSIGN] Assign filtered_imu new value: 0
[1.300000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.300000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.300000000000:PRINTLN] Aligning... 0
[1.300000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.300000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.350000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.350000000000:PRINTLN] Running...
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:ASSIGN] Assign filtered_imu new value: 0
[1.350000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.350000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.350000000000:PRINTLN] Aligning... 0
[1.350000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.350000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.400000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.400000000000:PRINTLN] Running...
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:ASSIGN] Assign filtered_imu new value: 0
[1.400000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.400000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.400000000000:PRINTLN] Aligning... 0
[1.400000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.400000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.450000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.450000000000:PRINTLN] Running...
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:ASSIGN] Assign filtered_imu new value: 0
[1.450000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.450000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.450000000000:PRINTLN] Aligning... 0
[1.450000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.450000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.500000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.500000000000:PRINTLN] Running...
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:ASSIGN] Assign filtered_imu new value: 0
[1.500000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.500000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.500000000000:PRINTLN] Aligning... 0
[1.500000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.500000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.550000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.550000000000:PRINTLN] Running...
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:ASSIGN] Assign filtered_imu new value: 0
[1.550000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.550000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.550000000000:PRINTLN] Aligning... 0
[1.550000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.550000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.600000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.600000000000:PRINTLN] Running...
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:ASSIGN] Assign filtered_imu new value: 0
[1.600000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.600000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.600000000000:PRINTLN] Aligning... 0
[1.600000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.600000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.650000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.650000000000:PRINTLN] Running...
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:ASSIGN] Assign filtered_imu new value: 0
[1.650000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.650000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.650000000000:PRINTLN] Aligning... 0
[1.650000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.650000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.700000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.700000000000:PRINTLN] Running...
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:ASSIGN] Assign filtered_imu new value: 0
[1.700000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.700000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.700000000000:PRINTLN] Aligning... 0
[1.700000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.700000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.750000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.750000000000:PRINTLN] Running...
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:ASSIGN] Assign filtered_imu new value: 0
[1.750000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.750000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.750000000000:PRINTLN] Aligning... 0
[1.750000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.750000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.800000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.800000000000:PRINTLN] Running...
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:ASSIGN] Assign filtered_imu new value: 0
[1.800000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.800000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.800000000000:PRINTLN] Aligning... 0
[1.800000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.800000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.850000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.850000000000:PRINTLN] Running...
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:ASSIGN] Assign filtered_imu new value: 0
[1.850000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.850000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.850000000000:PRINTLN] Aligning... 0
[1.850000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.850000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.900000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.900000000000:PRINTLN] Running...
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:ASSIGN] Assign filtered_imu new value: 0
[1.900000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.900000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.900000000000:PRINTLN] Aligning... 0
[1.900000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.900000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[1.950000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[1.950000000000:PRINTLN] Running...
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:ASSIGN] Assign filtered_imu new value: 0
[1.950000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[1.950000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[1.950000000000:PRINTLN] Aligning... 0
[1.950000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[1.950000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.000000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.000000000000:PRINTLN] Running...
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:ASSIGN] Assign filtered_imu new value: 0
[2.000000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.000000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.000000000000:PRINTLN] Aligning... 0
[2.000000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.000000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.050000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.050000000000:PRINTLN] Running...
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:ASSIGN] Assign filtered_imu new value: 0
[2.050000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.050000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.050000000000:PRINTLN] Aligning... 0
[2.050000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.050000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.100000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.100000000000:PRINTLN] Running...
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:ASSIGN] Assign filtered_imu new value: 0
[2.100000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.100000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.100000000000:PRINTLN] Aligning... 0
[2.100000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.100000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.150000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.150000000000:PRINTLN] Running...
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:ASSIGN] Assign filtered_imu new value: 0
[2.150000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.150000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.150000000000:PRINTLN] Aligning... 0
[2.150000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.150000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.200000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.200000000000:PRINTLN] Running...
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:ASSIGN] Assign filtered_imu new value: 0
[2.200000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.200000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.200000000000:PRINTLN] Aligning... 0
[2.200000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.200000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.250000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.250000000000:PRINTLN] Running...
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:ASSIGN] Assign filtered_imu new value: 0
[2.250000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.250000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.250000000000:PRINTLN] Aligning... 0
[2.250000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.250000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.300000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.300000000000:PRINTLN] Running...
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:ASSIGN] Assign filtered_imu new value: 0
[2.300000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.300000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.300000000000:PRINTLN] Aligning... 0
[2.300000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.300000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.350000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.350000000000:PRINTLN] Running...
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:ASSIGN] Assign filtered_imu new value: 0
[2.350000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.350000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.350000000000:PRINTLN] Aligning... 0
[2.350000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.350000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.400000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.400000000000:PRINTLN] Running...
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:ASSIGN] Assign filtered_imu new value: 0
[2.400000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.400000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.400000000000:PRINTLN] Aligning... 0
[2.400000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.400000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.450000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.450000000000:PRINTLN] Running...
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:ASSIGN] Assign filtered_imu new value: 0
[2.450000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.450000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.450000000000:PRINTLN] Aligning... 0
[2.450000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.450000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.500000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.500000000000:PRINTLN] Running...
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:ASSIGN] Assign filtered_imu new value: 0
[2.500000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.500000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.500000000000:PRINTLN] Aligning... 0
[2.500000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.500000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.550000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.550000000000:PRINTLN] Running...
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:ASSIGN] Assign filtered_imu new value: 0
[2.550000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.550000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.550000000000:PRINTLN] Aligning... 0
[2.550000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.550000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.600000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.600000000000:PRINTLN] Running...
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:ASSIGN] Assign filtered_imu new value: 0
[2.600000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.600000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.600000000000:PRINTLN] Aligning... 0
[2.600000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.600000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.650000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.650000000000:PRINTLN] Running...
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:ASSIGN] Assign filtered_imu new value: 0
[2.650000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.650000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.650000000000:PRINTLN] Aligning... 0
[2.650000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.650000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.700000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.700000000000:PRINTLN] Running...
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:ASSIGN] Assign filtered_imu new value: 0
[2.700000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.700000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.700000000000:PRINTLN] Aligning... 0
[2.700000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.700000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.750000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.750000000000:PRINTLN] Running...
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:ASSIGN] Assign filtered_imu new value: 0
[2.750000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.750000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.750000000000:PRINTLN] Aligning... 0
[2.750000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.750000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.800000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.800000000000:PRINTLN] Running...
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:ASSIGN] Assign filtered_imu new value: 0
[2.800000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.800000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.800000000000:PRINTLN] Aligning... 0
[2.800000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.800000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.850000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.850000000000:PRINTLN] Running...
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:ASSIGN] Assign filtered_imu new value: 0
[2.850000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.850000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.850000000000:PRINTLN] Aligning... 0
[2.850000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.850000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.900000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.900000000000:PRINTLN] Running...
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:ASSIGN] Assign filtered_imu new value: 0
[2.900000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.900000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.900000000000:PRINTLN] Aligning... 0
[2.900000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.900000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[2.950000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[2.950000000000:PRINTLN] Running...
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:ASSIGN] Assign filtered_imu new value: 0
[2.950000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[2.950000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[2.950000000000:PRINTLN] Aligning... 0
[2.950000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[2.950000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
[3.000000000000:pendulum-aligner:STARTING_TASK] 'imu_filter'
[3.000000000000:PRINTLN] Running...
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:ASSIGN] Assign filtered_imu new value: 0
[3.000000000000:pendulum-aligner:TASK_FINISHED] 'imu_filter'
[3.000000000000:pendulum-aligner:STARTING_TASK] 'aligner'
[3.000000000000:PRINTLN] Aligning... 0
[3.000000000000:ASSIGN] Assign pin_pwm_c09 new value: 1ms520us
[3.000000000000:pendulum-aligner:TASK_FINISHED] 'aligner'
EXIT (SUCCESS) AT 3s (3000000000000)

PID	start	end	name	state
0	0	0	imu_filter	Init
1	0	0	aligner	Main
0	50000000000	50000000000	imu_filter	Main
1	50000000000	50000000000	aligner	Main
0	100000000000	100000000000	imu_filter	Main
1	100000000000	100000000000	aligner	Main
0	150000000000	150000000000	imu_filter	Main
1	150000000000	150000000000	aligner	Main
0	200000000000	200000000000	imu_filter	Main
1	200000000000	200000000000	aligner	Main
0	250000000000	250000000000	imu_filter	Main
1	250000000000	250000000000	aligner	Main
0	300000000000	300000000000	imu_filter	Main
1	300000000000	300000000000	aligner	Main
0	350000000000	350000000000	imu_filter	Main
1	350000000000	350000000000	aligner	Main
0	400000000000	400000000000	imu_filter	Main
1	400000000000	400000000000	aligner	Main
0	450000000000	450000000000	imu_filter	Main
1	450000000000	450000000000	aligner	Main
0	500000000000	500000000000	imu_filter	Main
1	500000000000	500000000000	aligner	Main
0	550000000000	550000000000	imu_filter	Main
1	550000000000	550000000000	aligner	Main
0	600000000000	600000000000	imu_filter	Main
1	600000000000	600000000000	aligner	Main
0	650000000000	650000000000	imu_filter	Main
1	650000000000	650000000000	aligner	Main
0	700000000000	700000000000	imu_filter	Main
1	700000000000	700000000000	aligner	Main
0	750000000000	750000000000	imu_filter	Main
1	750000000000	750000000000	aligner	Main
0	800000000000	800000000000	imu_filter	Main
1	800000000000	800000000000	aligner	Main
0	850000000000	850000000000	imu_filter	Main
1	850000000000	850000000000	aligner	Main
0	900000000000	900000000000	imu_filter	Main
1	900000000000	900000000000	aligner	Main
0	950000000000	950000000000	imu_filter	Main
1	950000000000	950000000000	aligner	Main
0	1000000000000	1000000000000	imu_filter	Main
1	1000000000000	1000000000000	aligner	Main
0	1050000000000	1050000000000	imu_filter	Main
1	1050000000000	1050000000000	aligner	Main
0	1100000000000	1100000000000	imu_filter	Main
1	1100000000000	1100000000000	aligner	Main
0	1150000000000	1150000000000	imu_filter	Main
1	1150000000000	1150000000000	aligner	Main
0	1200000000000	1200000000000	imu_filter	Main
1	1200000000000	1200000000000	aligner	Main
0	1250000000000	1250000000000	imu_filter	Main
1	1250000000000	1250000000000	aligner	Main
0	1300000000000	1300000000000	imu_filter	Main
1	1300000000000	1300000000000	aligner	Main
0	1350000000000	1350000000000	imu_filter	Main
1	1350000000000	1350000000000	aligner	Main
0	1400000000000	1400000000000	imu_filter	Main
1	1400000000000	1400000000000	aligner	Main
0	1450000000000	1450000000000	imu_filter	Main
1	1450000000000	1450000000000	aligner	Main
0	1500000000000	1500000000000	imu_filter	Main
1	1500000000000	1500000000000	aligner	Main
0	1550000000000	1550000000000	imu_filter	Main
1	1550000000000	1550000000000	aligner	Main
0	1600000000000	1600000000000	imu_filter	Main
1	1600000000000	1600000000000	aligner	Main
0	1650000000000	1650000000000	imu_filter	Main
1	1650000000000	1650000000000	aligner	Main
0	1700000000000	1700000000000	imu_filter	Main
1	1700000000000	1700000000000	aligner	Main
0	1750000000000	1750000000000	imu_filter	Main
1	1750000000000	1750000000000	aligner	Main
0	1800000000000	1800000000000	imu_filter	Main
1	1800000000000	1800000000000	aligner	Main
0	1850000000000	1850000000000	imu_filter	Main
1	1850000000000	1850000000000	aligner	Main
0	1900000000000	1900000000000	imu_filter	Main
1	1900000000000	1900000000000	aligner	Main
0	1950000000000	1950000000000	imu_filter	Main
1	1950000000000	1950000000000	aligner	Main
0	2000000000000	2000000000000	imu_filter	Main
1	2000000000000	2000000000000	aligner	Main
0	2050000000000	2050000000000	imu_filter	Main
1	2050000000000	2050000000000	aligner	Main
0	2100000000000	2100000000000	imu_filter	Main
1	2100000000000	2100000000000	aligner	Main
0	2150000000000	2150000000000	imu_filter	Main
1	2150000000000	2150000000000	aligner	Main
0	2200000000000	2200000000000	imu_filter	Main
1	2200000000000	2200000000000	aligner	Main
0	2250000000000	2250000000000	imu_filter	Main
1	2250000000000	2250000000000	aligner	Main
0	2300000000000	2300000000000	imu_filter	Main
1	2300000000000	2300000000000	aligner	Main
0	2350000000000	2350000000000	imu_filter	Main
1	2350000000000	2350000000000	aligner	Main
0	2400000000000	2400000000000	imu_filter	Main
1	2400000000000	2400000000000	aligner	Main
0	2450000000000	2450000000000	imu_filter	Main
1	2450000000000	2450000000000	aligner	Main
0	2500000000000	2500000000000	imu_filter	Main
1	2500000000000	2500000000000	aligner	Main
0	2550000000000	2550000000000	imu_filter	Main
1	2550000000000	2550000000000	aligner	Main
0	2600000000000	2600000000000	imu_filter	Main
1	2600000000000	2600000000000	aligner	Main
0	2650000000000	2650000000000	imu_filter	Main
1	2650000000000	2650000000000	aligner	Main
0	2700000000000	2700000000000	imu_filter	Main
1	2700000000000	2700000000000	aligner	Main
0	2750000000000	2750000000000	imu_filter	Main
1	2750000000000	2750000000000	aligner	Main
0	2800000000000	2800000000000	imu_filter	Main
1	2800000000000	2800000000000	aligner	Main
0	2850000000000	2850000000000	imu_filter	Main
1	2850000000000	2850000000000	aligner	Main
0	2900000000000	2900000000000	imu_filter	Main
1	2900000000000	2900000000000	aligner	Main
0	2950000000000	2950000000000	imu_filter	Main
1	2950000000000	2950000000000	aligner	Main
0	3000000000000	3000000000000	imu_filter	Main
1	3000000000000	3000000000000	aligner	Main