corso2013.ctrl

frequency 1000

blocks {
	{ "ec",              "ethercat",    "eth0", 6, 0 }
	{ "beckhoff_el2xxx", "output_top",  "EL2008", 1 }
	{ "beckhoff_el2xxx", "output_low",  "EL2008", 2 }
	{ "beckhoff_el1xxx", "inputblock1", "EL1008", 4 }
	{ "beckhoff_el3xxx", "analog_in",   "EL3102", 5 }

	{ "value_bool",      "false"       }
	{ "value_bool",      "true"        }
	{ "value",           "zero"        }

	{ "filter_lp", "distance_l_lp" }
	{ "filter_lp", "distance_r_lp" }

	{ "inverse_proportional", "distance_l" }
	{ "inverse_proportional", "distance_r" }
	
	{ "matrix_2x2", "input_matrix" }

	{ "rangecheck", "diff_check" }
	{ "rangecheck", "diff_deadband" }

	{ "trigger", "spg_enable" }
	{ "trigger", "spg_disable" }
	{ "setreset", "spg_enabled" }
	{ "or3", "panic_or" }
	{ "not", "panic" }
	{ "not", "disabled" }
	{ "and2", "diff_enable" }

	{ "setpoint_generator", "lift_spg", "lift_spg", "m" }
	{ "subtract", "distance_error" }
	{ "pid_aw", "distance_pid" }
	{ "pid_aw", "diff_pid" }
	
	{ "add", "lift_add" }

	{ "matrix_2x2", "output_matrix" }
	
	{ "bridge_pwm",      "lift_ll_pwm" }
	{ "bridge_pwm",      "lift_lh_pwm" }
	{ "bridge_pwm",      "lift_rl_pwm" }
	{ "bridge_pwm",      "lift_rh_pwm" }
}


links {
	{ "false", "value",             "output_low", "output1", true }
	{ "false", "value",             "output_low", "output2", true }
	{ "false", "value",             "output_low", "output5", true }
	{ "false", "value",             "output_low", "output6", true }
	{ "false", "value",             "output_low", "output7", true }
	{ "false", "value",             "output_low", "output8", true }
	
	{ "spg_enable", "q",            "spg_enabled", "set",    true }
	{ "inputblock1", "input3",      "panic", "input",        true }
	{ "panic", "output",            "panic_or", "a",         true }
	{ "spg_disable", "q",           "panic_or", "b",         true }
	{ "panic_or", "q",              "spg_enabled", "reset",  true }
	{ "panic_or", "q",              "output_low", "output3", true }
	{ "spg_enabled", "output",      "disabled", "input",     true }
	{ "spg_enabled", "output",      "output_low", "output4", true }

	{ "analog_in", "input1",        "distance_l_lp", "in",   true }
	{ "analog_in", "input2",        "distance_r_lp", "in",   true }

	{ "distance_l_lp", "out",       "distance_l", "in",      true }

	{ "distance_r_lp", "out",       "distance_r", "in",      true }
	
	{ "distance_l", "out",          "input_matrix", "in0",   true }
	{ "distance_r", "out",          "input_matrix", "in1",   true }

	{ "disabled", "output",         "lift_spg", "reset",     true }
	{ "input_matrix", "out0",       "lift_spg", "reset_x",   true }
	
	{ "lift_spg", "x",              "distance_error", "positive", true }
	{ "input_matrix", "out0",       "distance_error", "negative", true }
	
	{ "input_matrix", "out1",       "diff_check", "in",      true }
	{ "diff_check", "invalid",      "panic_or", "c",         true }
	{ "input_matrix", "out1",       "diff_deadband", "in",   true }
	{ "diff_check", "valid",        "diff_enable", "a",      true }
	{ "spg_enabled", "output",      "diff_enable", "b",      true }
	
	{ "distance_error", "difference",  "distance_pid", "in",     true }
	{ "spg_enabled", "output",      "distance_pid", "enable", true }
	
	{ "lift_spg", "v",              "lift_add", "in0",       true }
	{ "distance_pid", "out",        "lift_add", "in1",       true }

	{ "lift_add", "out",            "output_matrix", "in0",  true }

	{ "input_matrix", "out1",       "diff_pid", "in",        true }
	{ "diff_pid", "out",            "output_matrix", "in1",  true }
	{ "diff_enable", "q",           "diff_pid", "enable",    true }
	
	{ "output_matrix", "out0",      "lift_ll_pwm", "in",     true }
	{ "output_matrix", "out0",      "lift_lh_pwm", "in",     true }
	{ "output_matrix", "out1",      "lift_rl_pwm", "in",     true }
	{ "output_matrix", "out1",      "lift_rh_pwm", "in",     true }

	{ "lift_ll_pwm", "b",           "output_top", "output1", true }
	{ "lift_ll_pwm", "a",           "output_top", "output2", true }
	{ "lift_lh_pwm", "b",           "output_top", "output3", true }
	{ "lift_lh_pwm", "a",           "output_top", "output4", true }
	{ "lift_rl_pwm", "b",           "output_top", "output5", true }
	{ "lift_rl_pwm", "a",           "output_top", "output6", true }
	{ "lift_rh_pwm", "b",           "output_top", "output7", true }
	{ "lift_rh_pwm", "a",           "output_top", "output8", true }
}

set motor_speed 0.018

params {
	{ "true", "value", true }
	
	{ "lift_spg", "setpoint",    (float)0.0     }
	{ "lift_spg", "t",           (float)0.001   }
	{ "lift_spg", "max_x",       (float)0.8     }
	{ "lift_spg", "min_x",       (float)0.0     }
	{ "lift_spg", "max_v",       (float)0.015   }
	{ "lift_spg", "max_a",       (float)0.01    }
	{ "lift_spg", "max_j",       (float)0.01    }
	{ "lift_spg", "precision_x", (float)0.005   }
	{ "lift_spg", "precision_v", (float)0.005   }
	{ "lift_spg", "precision_a", (float)0.005   }
	
	{ "distance_l_lp", "t",   (float)0.001 }
	{ "distance_l_lp", "tau", (float)2.0   }
	{ "distance_r_lp", "t",   (float)0.001 }
	{ "distance_r_lp", "tau", (float)2.0   }

	{ "distance_l", "c0", (float)-0.495196 }
	{ "distance_l", "c1", (float)1.42175   }
	{ "distance_l", "c2", (float)0.515971  }

	{ "distance_r", "c0", (float)-0.407968 }
	{ "distance_r", "c1", (float)1.18524}
	{ "distance_r", "c2", (float)0.374365  }

	{ "input_matrix", "constants", (float) { 0.5,  0.5 },
                                       (float) { 1.0, -1.0 } }

	{ "diff_check",    "max", (float)  0.10  }
	{ "diff_check",    "min", (float) -0.10  }
	{ "diff_deadband", "max", (float)  0.06 }
	{ "diff_deadband", "min", (float) -0.06 }

	{ "distance_pid", "kp",   (float)0.05  }
	{ "distance_pid", "ki",   (float)0.0    }
	{ "distance_pid", "kd",   (float)0.0    }
	{ "distance_pid", "maxw", (float)0.005  }
	{ "distance_pid", "minw", (float)-0.005 }

	{ "diff_pid", "kp",   (float)0.0  }
	{ "diff_pid", "ki",   (float)0.0    }
	{ "diff_pid", "kd",   (float)0.0    }
	{ "diff_pid", "maxw", (float)0.005  }
	{ "diff_pid", "minw", (float)-0.005 }

	{ "output_matrix", "constants", (float) { 1.0/$(motor_speed), -0.5/$(motor_speed) },
                                        (float) { 1.0/$(motor_speed), 0.5/$(motor_speed) } }

	{ "lift_ll_pwm", "divider",       10   }
	{ "lift_ll_pwm", "continuous_on", false }
	{ "lift_lh_pwm", "divider",       10   }
	{ "lift_lh_pwm", "continuous_on", false }
	{ "lift_rl_pwm", "divider",       10   }
	{ "lift_rl_pwm", "continuous_on", false }
	{ "lift_rh_pwm", "divider",       10   }
	{ "lift_rh_pwm", "continuous_on", false }
}

traces {
	{ "distance",   "m/s",     "input_matrix", "out0"   }
}