diff --git a/lcec.hal b/lcec.hal new file mode 100755 index 0000000000000000000000000000000000000000..5531cf53c4b68dc51965ea0a11538da5d5ce032c --- /dev/null +++ b/lcec.hal @@ -0,0 +1,10 @@ +loadrt trivkins +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +loadusr -W lcec_conf maurice.xml +loadrt lcec + +addf lcec.read-all servo-thread +addf lcec.write-all servo-thread + +net state-op <= lcec.state-op diff --git a/lcec_simu.hal b/lcec_simu.hal new file mode 100755 index 0000000000000000000000000000000000000000..8b91ff0940b520c8c8f249b223c9d8ecaf89bf13 --- /dev/null +++ b/lcec_simu.hal @@ -0,0 +1,8 @@ +loadrt trivkins +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +loadrt lcec_simu names=lcec.0.0,lcec.0.1 +addf lcec.0.0 servo-thread +addf lcec.0.1 servo-thread + +setp iocontrol.0.emc-enable-in 1 diff --git a/lcec_simu/lcec_simu.comp b/lcec_simu/lcec_simu.comp new file mode 100755 index 0000000000000000000000000000000000000000..f9ac48d29ab00f091c91cb18d32baba51fe7bcda --- /dev/null +++ b/lcec_simu/lcec_simu.comp @@ -0,0 +1,120 @@ +// setp lcec.0.0.drivecontrol-1 1 +// setp lcec.0.0.drivecontrol-2 1 +// net state-op <= lcec.state-op +// net X-amp => lcec.0.0.drivecontrol-3 => lcec.0.0.drivecontrol-0 +// net X-poscmd => lcec.0.0.poscommand +// net X-posfb <= lcec.0.0.pos +// net X-fault <= lcec.0.0.drivestatus-3 + +// net X-stat-pos <= lcec.0.0.modestatus-3 +// net X-stat-home <= lcec.0.0.modestatus-2 +// net X-homingok <= lcec.0.0.drivestatus-12 +// net X-athome <= lcec.0.0.drivestatus-10 + +// net X-ctrl-home => lcec.0.0.modecontrol-1 => lcec.0.0.modecontrol-2 +// net X-homestart => lcec.0.0.drivecontrol-4 +// net X-ctrl-pos => lcec.0.0.modecontrol-3 + +// net X-PoT <= lcec.0.0.inputs-1 +// net X-NoT <= lcec.0.0.inputs-0 + +component lcec_simu "ESTUN drive simulator"; +author "Electrolab"; + +pin out bit state_op; + +pin in bit drivecontrol_0; +pin in bit drivecontrol_1; +pin in bit drivecontrol_2; +pin in bit drivecontrol_3; +pin in bit drivecontrol_4; + +pin in float poscommand; +pin out float pos; + +pin out bit drivestatus_3; +pin out bit drivestatus_10; +pin out bit drivestatus_12; + +pin out bit modestatus_2; +pin out bit modestatus_3; + +pin in bit modecontrol_0; +pin in bit modecontrol_1; +pin in bit modecontrol_2; +pin in bit modecontrol_3; +pin in bit modecontrol_4; + +pin out bit inputs_0; +pin out bit inputs_1; + +pin out s32 debug; +pin out s32 state; +pin out float motion_length; + +function _; +license "GPL"; // indicates GPL v2 or later +;; + +#define HOMING_LENGTH 100 +#define LATCH_LENGTH 5 +#define HOMING_SPEED_MM_S 10 +#define SERVO_PERIOD_MILI 1 + +FUNCTION(_) { + state_op = 1; + drivestatus_3 = 0; + inputs_0 = 0; + inputs_1 = 0; + + debug += 1; + + modestatus_2 = modecontrol_2; + modestatus_3 = modecontrol_3; + + if (!drivecontrol_4) { + motion_length = HOMING_LENGTH; + drivestatus_12 = 0; + state = 0; + } + + if (drivecontrol_0 && drivecontrol_1 && drivecontrol_2 && drivecontrol_3) { // Drive enable + + if (!modecontrol_2 && modecontrol_3) { // Position mode + pos = poscommand; + } + + if (modecontrol_2 && !modecontrol_3 && drivecontrol_4) { // Homing mode + switch (state) { + case 0: // idle + state = 1; + break; + case 1: // home + if (motion_length > 0) { + float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0; + motion_length -= step; + pos += step; + } + else { + motion_length = LATCH_LENGTH; + state = 2; + } + break; + case 2: // latch + if (motion_length > 0) { + float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0; + motion_length -= step; + pos -= step; + } + else { + state = 3; + } + break; + case 3: // homed + drivestatus_12 = 1; + break; + } + } + } + +} \ No newline at end of file diff --git a/lcec_simu/lcec_simu.so b/lcec_simu/lcec_simu.so new file mode 100755 index 0000000000000000000000000000000000000000..86bd8f605ec17394e8744f8cbe58816792f81dd9 Binary files /dev/null and b/lcec_simu/lcec_simu.so differ diff --git a/maurice_gmoccapy.ini b/maurice-common.ini old mode 100644 new mode 100755 similarity index 97% rename from maurice_gmoccapy.ini rename to maurice-common.ini index 256874357320fe5c0a8ea0cdcbf42e85cbf71747..6c198b2a34d98d662a9b379c9518523732dafb7e --- a/maurice_gmoccapy.ini +++ b/maurice-common.ini @@ -56,11 +56,6 @@ SERVO_PERIOD = 1000000 TASK = milltask CYCLE_TIME = 0.005 -[HAL] -HALUI = halui -HALFILE = maurice.hal -POSTGUI_HALFILE = gmoccapy_postgui.hal - [HALUI] [TRAJ] diff --git a/maurice-simu.ini b/maurice-simu.ini new file mode 100755 index 0000000000000000000000000000000000000000..c64dd9cc538934fbd1e3a6ea3620132391712acb --- /dev/null +++ b/maurice-simu.ini @@ -0,0 +1,7 @@ +#INCLUDE maurice-common.ini + +[HAL] +HALUI = halui +HALFILE = lcec_simu.hal +HALFILE = maurice.hal +POSTGUI_HALFILE = gmoccapy_postgui.hal diff --git a/maurice.hal b/maurice.hal index 5e97fd9957549585e471ff251768de7d27019878..fdef01709056541ffae48e38e0367faa25ea4402 100644 --- a/maurice.hal +++ b/maurice.hal @@ -6,8 +6,8 @@ # Components declaration #========================================== -loadrt trivkins -loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS +#loadrt trivkins +#loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS addf motion-command-handler servo-thread addf motion-controller servo-thread @@ -65,11 +65,11 @@ loadrt classicladder_rt addf classicladder.0.refresh servo-thread loadusr classicladder maurice.clp -loadusr -W lcec_conf maurice.xml -loadrt lcec +#loadusr -W lcec_conf maurice.xml +#loadrt lcec -addf lcec.read-all servo-thread -addf lcec.write-all servo-thread +#addf lcec.read-all servo-thread +#addf lcec.write-all servo-thread setp lcec.0.0.drivecontrol-1 1 setp lcec.0.0.drivecontrol-2 1 @@ -81,12 +81,6 @@ setp lcec.0.0.modecontrol-4 0 setp lcec.0.1.modecontrol-0 0 setp lcec.0.1.modecontrol-4 0 -#========================================== -# Global machine logic -#========================================== - -net state-op <= lcec.state-op - #========================================== # X axis #========================================== diff --git a/maurice.ini b/maurice.ini new file mode 100644 index 0000000000000000000000000000000000000000..c8932f27271561bdfc643791c78755898cc73210 --- /dev/null +++ b/maurice.ini @@ -0,0 +1,7 @@ +#INCLUDE maurice-common.ini + +[HAL] +HALUI = halui +HALFILE = lcec.hal +HALFILE = maurice.hal +POSTGUI_HALFILE = gmoccapy_postgui.hal