11/07/2016 09:37 AM, Marius Alksnys rašė:
Hello, Jan,
this is my carousel HAL component version. I made it more reliable, was
tested a lot and is working on a real machine. Sense-pocket is low when
carousel is fixed and high between positions. Sense-index occurs once per
revolution between
positions AFAIR.
Might require changes because of hard-coded sensor states it expects. I
used it with M6 re-mapping, thus used analog-out for socket number.
The component source file is attached.
component mycarousel "Orient a tool drum custom component";
// partly derived from andy-pugh carousel HAL component
pin in float pocket-number-f "The pocket to move to when the .enable pin goes
high. Float to be compatible with motion.analog-out";
pin in bit enable "Set this pin high to start movement. Setting it low will
stop movement";
pin in bit unhome "Unhomes the component and forces to find index again";
pin out bit active "indicates that the component is active";
pin out bit ready "This pin goes high when the drum is in-position";
pin in bit sense-index "Drum index position feedback pin";
pin in bit sense-pocket = true "Drum pocket position feedback pin";
pin out bit motor-fwd "Indicates the motor should run forwards (bigger
numbers)";
pin out bit motor-rev "Indicates the motor should run reverse";
pin out bit is-ok = true "No fault output";
pin out signed pos = 0 "This pin indicates the current position feedback";
pin out bit homed = false "Shows that homing is complete.";
param r signed state = 0 "Current component state";
param rw signed home-pocket = 19 "Pocket number at index.";
param rw signed pockets = 21 "Pocket count in drum";
param rw signed timeout-one = 60 "Maximum time to wait for next pocket in
thread periods";
param rw signed timeout-all = 1600 "Maximum time to rotate to any pocket in
thread periods";
license "GPL";
author "andy pugh, Marius Alksnys";
variable hal_bit_t old_sense = false;
variable hal_bit_t old_index = false;
variable hal_s32_t timeone = 3;
variable hal_s32_t timeall = 3;
variable hal_s32_t pocket_number = 0;
function _ fp;
option singleton yes;
;;
#include <rtapi_math.h>
#define ST_WAITING 0
#define ST_HOMING_INDEX 10
#define ST_FINISH_HOMING 20
#define ST_CHOOSE_DIR 30
#define ST_MOVING 40
#define ST_STOP_AT_POCKET 50
#define ST_DONE 60
#define ST_HALT 96
#define ST_HALTED 99
#define MSG_SOURCE "Tool carousel: "
FUNCTION(_) {
if (unhome) homed = false;
if (sense_pocket && !old_sense) { // Pocket sensor signal just rised!
timeone = timeout_one;
if (homed) {
if (motor_fwd) {
pos += 1;
if (pos > pockets) pos -= pockets;
}
if (motor_rev) {
pos -= 1;
if (pos < 1) pos += pockets;
}
if (!motor_fwd && !motor_rev) {
homed = false;
state = ST_WAITING;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "got socket sensor
signal, while no command to turn. Will re-home.");
}
}
}
old_sense = sense_pocket;
if (sense_index && !old_index) { // Index sensor signal just rised!
if (homed) {
if ((motor_fwd && (pos != home_pocket)) || (motor_rev && (pos !=
home_pocket + 1))) {
state = ST_HALT;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "home sensor signal
at wrong time (very big risk to brake equipment), component will halt!!!");
}
}
}
old_index = sense_index;
if (is_ok) {
if (sense_pocket && sense_index) {
state = ST_HALT;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "both sensors active
(very big risk to brake equipment), component will halt!!!");
}
if (active) { // Checking for timeouts
if (timeone-- < 0) {
motor_fwd = false;
motor_rev = false;
active = false;
is_ok = false;
state = ST_DONE;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "time-out waiting for
one position!");
}
if (timeall-- < 0) {
is_ok = false;
state = ST_STOP_AT_POCKET;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "requested position
time-out!");
}
}
}
switch (state) {
case ST_WAITING: // waiting at start
if (! enable) return;
timeall = timeout_all;
if (! sense_pocket) {
homed = false;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "carousel was not fixed
before rotating. Will re-home.");
}
active = true;
pocket_number = round(pocket_number_f);
if (!homed) {
motor_fwd = false;
motor_rev = true;
timeone = timeout_one;
state = ST_HOMING_INDEX;
break;
}
state = ST_CHOOSE_DIR;
ready = false;
case ST_CHOOSE_DIR: // choose direction
if (pocket_number < 1 || pocket_number > pockets) {
motor_fwd = false;
motor_rev = false;
active = false;
is_ok = false;
state = ST_DONE;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "there is no such
socket!");
return;
}
if (pos < pocket_number) motor_fwd = !(pocket_number - pos > (pockets /
2));
else motor_fwd = (pos - pocket_number > (pockets / 2));
motor_rev = !motor_fwd;
timeone = timeout_one;
state = ST_MOVING;
case ST_MOVING: // moving
if (enable && !unhome) {
if (pos == pocket_number) {
motor_fwd = false;
motor_rev = false;
active = false;
ready = true;
state = ST_DONE;
}
}
else {
timeone = timeout_one;
state = ST_STOP_AT_POCKET;
}
break;
case ST_STOP_AT_POCKET: // waiting for sense_pocket or another enable
if (enable || sense_pocket) {
motor_fwd = false;
motor_rev = false;
active = enable;
state = ST_WAITING;
}
break;
case ST_DONE: //waiting for enable to go false
if (!enable) {
ready = false;
is_ok = true;
state = ST_WAITING;
}
break;
case ST_HOMING_INDEX: // waiting for index
if (sense_index) { // index found
state = ST_FINISH_HOMING;
}
break;
case ST_FINISH_HOMING: // waiting for pulse
if (sense_pocket) {
pos = home_pocket;
homed = true;
active = false;
state = ST_WAITING;
}
break;
case ST_HALT:
motor_fwd = false;
motor_rev = false;
active = false;
ready = false;
is_ok = false;
homed = false;
state = ST_HALTED;
break;
case ST_HALTED:
break;
default:
state = ST_HALT;
rtapi_print_msg(RTAPI_MSG_ERR, MSG_SOURCE "unknown state, component
will halt!!!");
}
}
------------------------------------------------------------------------------
Developer Access Program for Intel Xeon Phi Processors
Access to Intel Xeon Phi processor-based developer platforms.
With one year of Intel Parallel Studio XE.
Training and support from Colfax.
Order your platform today. http://sdm.link/xeonphi
_______________________________________________
Emc-users mailing list
Emc-users@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/emc-users
