method. Modes are =================== ===================================================================== Mode Description =================== ===================================================================== AutoDirect This is a mode for Haulin and Payout without PID controller. The velocity is taken from the PositionGenerator. This mode is used for simple winches without the demand for exact positioning Auto This mode for Haulin and Payout is for exact positing under inclusion of AHC. ManualMode Control of velocity of a winch via manual devices ManualModeAHC Control of position of a winch via manual devices which includes AHC PassivePayout Control of the trajectory of a payout via tension, which demands an external force which is pulling the wire =================== ===================================================================== Methods And Properties ====================== *) FUNCTION_BLOCK LocalEWinch IMPLEMENTS IWinch VAR_INPUT Config : REFERENCE TO ConfigElectricWinch; OpValues : REFERENCE TO OpValueWinch; Drive : p7elec.IDrive; Encoder : p7elec.IEncoder; HCController : IHeaveCompController; SpoolHeadPosition : LREAL; BrakeHPU : p7type.IHPUBrake; END_VAR VAR {attribute 'hide'} setpointPID : LREAL; {attribute 'hide'} torqueLimit : LREAL; {attribute 'hide'} errorLimit : LREAL; {attribute 'hide'} sign : LREAL; {attribute 'hide'} driveOk : BOOL; {attribute 'hide'} driveRun : BOOL; {attribute 'hide'} cmd : WinchCMD; {attribute 'hide'} is_ready : BOOL; {attribute 'hide'} timerPrepare : TON; {attribute 'hide'} timerBrakeHPU : TON; // new spooling variables _InboundVirtualWireDirection : BOOL; // used in virtual wire and offset calculations _pulsesPerLayer : LINT; // used in virtual wire and offset calculations _singleThread_virtualWirePosition : LREAL; // NEW value END_VAR VAR {attribute 'hide'} _started : BOOL; // // Basic vars // {attribute 'hide'} PID_cycle_timer : p6com.CycleGenerator; // // // {attribute 'hide'} _meas_velocity : LREAL; {attribute 'hide'} _meas_acceleration : LREAL; {attribute 'hide'} _meas_position : LREAL; {attribute 'hide'} _meas_radius : LREAL; // current radius where we are now {attribute 'hide'} _meas_tension : LREAL; {attribute 'hide'} _meas_speed : LREAL; {attribute 'hide'} _meas_break_failing : BOOL; // // Spooling // {attribute 'hide'} _diamondScrew_VirtualWirePosition : LREAL; {attribute 'hide'} _spool_layer : DINT; {attribute 'hide'} _loopsPerLayer : LREAL; {attribute 'hide'} _pulses_per_meter : LREAL; // // State machine // {attribute 'hide'} _state : EWinchSM := EWInchSM.IDLE; {attribute 'hide'} _stateclass : EWinchSMClass; {attribute 'hide'} _event : BOOL; {attribute 'hide'} _eventId : p6com.EventIdentifier; {attribute 'hide'} _cmd : OneShotWinchCMD; {attribute 'hide'} _opstate : p6com.P6OpState; // // Mode related vars // // _parameter vars are interpreted according to their mode // Research: // _parameter1 : Max velocity // _parameter2 : Acceleration // _parameter3: Max tension // // Fishery: // _parameter1: Max velocity // _parameter2: Max tension // _parameter3: Min tension // _parameter4: rampdown time {attribute 'hide'} _parameter1 : LREAL; {attribute 'hide'} _parameter2 : LREAL; {attribute 'hide'} _parameter3 : LREAL; {attribute 'hide'} _parameter4 : LREAL; {attribute 'hide'} _setpoint : LREAL; {attribute 'hide'} _ahc : BOOL; {attribute 'hide'} _ahcposition : LREAL; {attribute 'hide'} _ahcvelocity : LREAL; {attribute 'hide'} _ahcacceleration : LREAL; {attribute 'hide'} _velocity : LREAL; {attribute 'hide'} _velocityRel : LREAL; {attribute 'hide'} _torque : LREAL; {attribute 'hide'} _accleration : LREAL; //{attribute 'hide'} _tensionMax : LREAL; {attribute 'hide'} _tensionRel : LREAL; {attribute 'hide'} _tensionMin : LREAL; {attribute 'hide'} _speed : LREAL; {attribute 'hide'} _torqueRel : LREAL; // // Signals // {attribute 'hide'} ready_auto : BOOL; {attribute 'hide'} stop_cond : BOOL; {attribute 'hide'} errorBrake : BOOL; // // Property related values // {attribute 'hide'} _lengthError : LREAL; // // misc // END_VAR ]]> 0 THEN Encoder.DirectionValue := config.Option_SetEncoderDirection; END_IF // The process timer has to be used for analog signals, // to run the pid controllers and the calculation for velocity PID_cycle_timer.update(In:=p6com.CycleTime_PID0, PLCTask_CycleTime := p6com.CycleTime_Task0); IF (NOT _started) THEN _started := TRUE; END_IF; // // Calculcate values of the winch like length and tension // _meas_speed := Drive.getSpeed(); IF config.Option_DoCalcs THEN _calcPositionVelocityRadiusTension2(); END_IF IF config.Option_HasSpooling THEN _calcHeadPositionForSpooling(); // old function, _calcVirtualWirePosition(); // new code, made on Skreigrunn for single thread screw _checkAndChangeSpoolDirection(); // new code, will change virtual spool direction if bit is toggled. END_IF _meas_break_failing := _isBrakeFailing(); // // Reading of AHC values // IF (HCController <> 0) AND config.Option_HasAHC THEN _ahcposition := HCController.HCPosition; _ahcvelocity := HCController.HCVelocity; IF _ahc AND (HCController.OpState <> p6com.P6OpState.RUN) THEN HCController.setOperation(HeaveCompCMD.START); ELSIF (NOT _ahc) AND (HCController.OpState = p6com.P6OpState.RUN) THEN HCController.setOperation(HeaveCompCMD.STOP); END_IF ELSE _ahcposition := 0.0; _ahcvelocity := 0.0; END_IF // // Signals // driveOk := Drive.State = p6com.P6State.OP; driveRun := Drive.OpState = p6com.P6OpState.RUN; timerPrepare(IN:=_opstate = p6com.P6OpState.PREPARE, PT:=T#200MS); timerBrakeHPU(IN:= _opstate = p6com.P6OpState.PREPARE, PT:=config.TimeForBrakePressure); IF BrakeHPU <> 0 THEN errorBrake := BrakeHPU.Error; ELSE errorBrake := FALSE; END_IF; cmd := _cmd.Get(); // // State machine // IF cmd = WinchCMD.FASTSTOP THEN _state := EWinchSM.FASTSTOP; ELSIF cmd = WinchCMD.FASTSTOP_OFF THEN _state := EWinchSM.IDLE; END_IF; CASE _state OF EWinchSM.FASTSTOP: _state := EWinchSM.FASTSTOP_IDLE; IF config.Option_ActivateOnEStop AND Drive.OpState = p7type.P6OpState.STOPPED THEN Drive.setOperation(DriveCMD.ACTIVATE); END_IF; EWinchSM.FASTSTOP_IDLE: ; // // Idle // EWinchSM.Idle: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.IDLE_RUN; Drive.setOperation(DriveCMD.STOP); EWInchSM.IDLE_RUN: _opstate := p6com.P6OpState.STOPPED; IF (NOT driveOk) THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSE CASE cmd OF WinchCMD.MANUAL: _state := EWinchSM.MANUAL; _opstate := p6com.P6OpState.PREPARE; WinchCMD.AUTO: _state := EWinchSM.AUTO; _opstate := p6com.P6OpState.PREPARE; WinchCMD.TORQUE: _state := EWinchSM.TORQUE; _opstate := p6com.P6OpState.PREPARE; WinchCMD.SLAVE: _state := EWinchSM.SLAVEMODE; _opstate := p6com.P6OpState.PREPARE; END_CASE END_IF // // Manual // // Parameters: // setpoint : -1 .. 1 related to SpeedMax // Parameter1 : Relative Max Torque // Parameter2 : - // Parameter3 : - // Parameter4 : - EWinchSM.MANUAL: _opstate := p6com.P6OpState.PREPARE; IF Drive.isActivationPossible() AND _brakePressureOK() THEN _state := EWinchSM.MANUAL_PREOP1; Drive.setOperation(DriveCMD.ACTIVATE); ELSIF (errorBrake OR timerBrakeHPU.Q) THEN _state := EWinchSM.IDLE; END_IF; EWinchSM.MANUAL_PREOP1: _opstate := p6com.P6OpState.PREPARE; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF drive.OpState = p6com.P6OpState.STOPPED AND timerPrepare.Q THEN _state := EWinchSM.IDLE; ELSIF errorBrake THEN _state := EWinchSM.IDLE; _opstate := p6com.P6OpState.STOPPED; ELSIF driveRun AND timerPrepare.Q THEN _state := EWinchSM.MANUAL_OP; END_IF; EWinchSM.MANUAL_OP: _opstate := p6com.P6OpState.RUN; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF NOT driveRun THEN _state := EWinchSM.IDLE; ELSE CASE cmd OF WinchCMD.FASTSTOP: _state := EWinchSM.IDLE; _opstate := p6com.P6OpState.NOTAVAILABLE; WinchCMD.STOP: _state := EWinchSM.MANUAL_EXIT1; WinchCMD.AUTO: _state := EWinchSM.AUTO; _opstate := p6com.P6OpState.PREPARE; WinchCMD.TORQUE: _state := EWinchSM.TORQUE; _opstate := p6com.P6OpState.PREPARE; WinchCMD.SLAVE: _state := EWinchSM.SLAVEMODE; _opstate := p6com.P6OpState.PREPARE; END_CASE END_IF EWinchSM.MANUAL_EXIT1: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.MANUAL_EXIT2; EWinchSM.MANUAL_EXIT2: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.IDLE; // // Constant Torque // // Parameters: // setpoint : 0 .. 1 related to Drive.TorqueRelMax() // Att: Boosting can lowering the value after a time period // Parameter1 : Max speed [RPM] for the case no force is applied on the wire. This is related to the RPM after the gear! // In any case is the speed limited in a Drive instance. If zero, mode is stopping // Parameter2 : - // Parameter3 : - // Parameter4 : - EWinchSM.TORQUE: _opstate := p6com.P6OpState.PREPARE; IF errorBrake OR timerBrakeHPU.Q THEN _state := EWinchSM.IDLE; ELSIF Drive.isActivationPossible() AND _brakePressureOK() THEN _state := EWinchSM.TORQUE_PREOP1; Drive.setOperation(DriveCMD.ACTIVATE); END_IF; EWinchSM.TORQUE_PREOP1: _opstate := p6com.P6OpState.PREPARE; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF driveRun AND timerPrepare.Q THEN _state := EWinchSM.TORQUE_PREOP2; ELSIF drive.OpState = p6com.P6OpState.STOPPED AND timerPrepare.Q THEN _state := EWinchSM.IDLE; END_IF; EWinchSM.TORQUE_PREOP2: _opstate := p6com.P6OpState.PREPARE; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF driveRun THEN _state := EWinchSM.TORQUE_OP; ELSIF drive.OpState = p6com.P6OpState.STOPPED THEN _state := EWinchSM.IDLE; END_IF; EWinchSM.TORQUE_OP: _opstate := p6com.P6OpState.RUN; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF NOT driveRun THEN _state := EWinchSM.IDLE; ELSE CASE cmd OF WinchCMD.FASTSTOP: _state := EWinchSM.IDLE; WinchCMD.STOP: _state := EWinchSM.TORQUE_EXIT1; WinchCMD.MANUAL: _state := EwinchSM.MANUAL; END_CASE END_IF EWinchSM.TORQUE_EXIT1: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.TORQUE_EXIT2; EWinchSM.TORQUE_EXIT2: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.IDLE; // // Slave Mode // // Parameters: // setpoint : Speed [RPM] related to Drive.SpeedMax() // Parameter1 : Torque Relative 0..Drive.TorqueRelMax() // Parameter2 : - // Parameter3 : - // Parameter4 : - EWinchSM.SLAVEMODE: _opstate := p6com.P6OpState.PREPARE; IF Drive.isActivationPossible() AND _brakePressureOK() THEN _state := EWinchSM.SLAVEMODE_PREOP1; Drive.setOperation(DriveCMD.ACTIVATE); ELSIF errorBrake OR timerBrakeHPU.Q THEN _state := EWinchSM.IDLE; //{warning 'Check this piece of code!!!!'} ELSIF driveRun THEN // don't waste time if drive is running _state := EWinchSM.SLAVEMODE_OP; END_IF; EWinchSM.SLAVEMODE_PREOP1: _opstate := p6com.P6OpState.PREPARE; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF driveRun AND timerPrepare.Q THEN _state := EWinchSM.SLAVEMODE_PREOP2; ELSIF drive.OpState = p6com.P6OpState.STOPPED AND timerPrepare.Q THEN _state := EWinchSM.IDLE; END_IF; EWinchSM.SLAVEMODE_PREOP2: _opstate := p6com.P6OpState.PREPARE; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF driveRun THEN _state := EWinchSM.SLAVEMODE_OP; ELSIF drive.OpState = p6com.P6OpState.STOPPED THEN _state := EWinchSM.IDLE; END_IF; EWinchSM.SLAVEMODE_OP: _opstate := p6com.P6OpState.RUN; IF NOT driveOk THEN _setFault(EWinchSM.ERROR, p6com.EventIdentifier.WINCHBASE_DRIVE_IS_NOT_AVAILABLE); ELSIF errorBrake THEN _state := EWinchSM.IDLE; ELSIF NOT driveRun THEN _state := EWinchSM.IDLE; ELSE CASE cmd OF WinchCMD.STOP: _state := EWinchSM.SLAVEMODE_EXIT1; WinchCMD.MANUAL: _state := EWinchSM.MANUAL_OP; END_CASE END_IF EWinchSM.SLAVEMODE_EXIT1: _opstate := p6com.P6OpState.STOPPED; _state := EWinchSM.MANUAL_OP; // // Error // EWinchSM.ERROR: _opstate := p6com.P6OpState.NOTAVAILABLE; IF cmd = WinchCMD.RESET THEN Drive.setOperation(p7hyd.DriveCMD.RESET); ELSIF driveOk THEN _event := FALSE; _state := EWinchSM.IDLE; END_IF END_CASE // // Statemachine output // _stateclass := _state / 100; CASE _stateclass OF EWinchSMClass.Idle: _lengthError := 0.0; Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); EWinchSMClass.FASTSTOP: _lengthError := 0.0; Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); // // Manual // EWinchSMClass.MANUALPREOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF _lengthError := 0.0; Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); EWinchSMClass.MANUALOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF _lengthError := 0.0; IF config.Option_TensionLimitManMode THEN // get the maximum torqueRel setting for the maximum tension (in ton), does also limit to 1% minimum // do not limit torqueLimit := _tensionToTorqueRel(_parameter1 * Config.TensionMax); //1 (or potmeter value) * inputs max tension in tons ELSE // Never go under 1% torque - Skreigrunn justert til 0 igjen... // Liaoyou and all other projects needs this to be 1% torqueLimit := LIMIT(0.01, _parameter1, 1.0) * Drive.getTorqueRelMax(); END_IF Drive.setProcessValues( LIMIT(-1,_setpoint,1) * Drive.getSpeedMax(), torqueLimit ); // // Constant Torque // EWinchSMClass.TORQUEPREOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); EWinchSMClass.TORQUEOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF Drive.setProcessValues( Speed := _parameter1 ,// -ABS(_parameter1), TorqueRel := LIMIT(-1.0, _setpoint, 1.0) * Drive.getTorqueRelMax(), // allow negative torque ); // // Slave Mode // EWinchSMClass.SLAVEMODEPREOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); // torqueLimit ); EWinchSMClass.SLAVEMODEOP: IF BrakeHPU <> 0 THEN BrakeHPU.setActive(); END_IF // Limit torque (_parameter1) in auto mode. // new on Liaoyou 050923 EH IF config.Option_TensionLimitAutoModes THEN // get the maximum torqueRel setting for the maximum tension (in ton) torqueLimit := LIMIT(0.01, _parameter1, _tensionToTorqueRel(Config.TensionMax)); ELSE // Never go under 1% torque torqueLimit := LIMIT(0.01, _parameter1, Drive.getTorqueRelMax()) ; // I presume torque input already is scaled to torqueRelMax END_IF Drive.setProcessValues( LIMIT(-1,_setpoint,1) * Drive.getSpeedMax(), torqueLimit //_parameter1 ); // // Idle // ELSE _lengthError := 0.0; Drive.setProcessValues(0.0, Drive.getTorqueRelMax()); END_CASE ]]> 0 THEN // invert := -1; //ELSE // invert := 1; //END_IF // add or subtract offset pulses to existing offset based on virtual wire direction IF _InboundVirtualWireDirection THEN OpValues.encoder_offset_wire_calc := OpValues.encoder_offset_wire_calc - offsetPulses ; //* invert; // (was '+') ELSE OpValues.encoder_offset_wire_calc := OpValues.encoder_offset_wire_calc + offsetPulses ; //* invert; // (was '-') END_IF Local_number := OpValues.encoder_offset_wire_calc ; ]]> 0 THEN IsRotating := NOT THIS^.Drive.isAtZeroSpeed(); ELSE IsRotating := FALSE; END_IF ]]> 0 THEN IF Config.Option_DrumEncoder THEN _pulses_per_meter := THIS^.Encoder.Resolution * LoopsPerLayer / config.Width; ELSE _pulses_per_meter := THIS^.Encoder.Resolution * LoopsPerlayer * Drive.getGearRatio()/ config.Width; END_IF ELSE _pulses_per_meter := 0; END_IF PulsesPerMeter := _pulses_per_meter;]]> THIS^.Encoder.Resolution / 30; ELSE _isBrakeFailing := FALSE; state := 0; END_IF ELSE state := 0; END_CASE ]]> 0 THEN IF Config.Option_DrumEncoder THEN _pulses_per_meter := THIS^.Encoder.Resolution * LoopsPerLayer / config.Width; ELSE _pulses_per_meter := ResolutionTurn * LoopsPerlayer / config.Width; END_IF ELSE _pulses_per_meter := 0; END_IF ResolutionLayer := TRUNC(ResolutionTurn * LoopsPerLayer); IF ResolutionLayer = 0 THEN // inconsistent config _diamondScrew_VirtualWirePosition := 0.5; RETURN; END_IF Layer_before_offset:= THIS^.Encoder.ValueRaw / ResolutionLayer; IF THIS^.Encoder.ValueRaw < 0 THEN Layer_before_offset := Layer_before_offset - 1; END_IF; // Layer position in pulses LayerPosition := ABS(THIS^.Encoder.ValueRaw) MOD ResolutionLayer; // Normalizing LayerPositionNorm := LINT_TO_LREAL(LayerPosition) / LINT_TO_LREAL(ResolutionLayer); IF THIS^.Encoder.ValueRaw < 0 THEN LayerPositionNorm := 1.0 - LayerPositionNorm; END_IF; // Add an offset because the initializing position is not null LayerPositionNorm := LayerPositionNorm + OpValues.HeadOffset; Layer := Layer_before_offset ; //can be changed in if's below IF LayerPositionNorm >= 1.0 THEN LayerPositionNorm := LayerPositionNorm - 1.0; Layer := Layer_before_offset + 1; ELSIF LayerPositionNorm < 0.0 THEN LayerPositionNorm := LayerPositionNorm + 1.0; Layer := Layer_before_offset - 1; END_IF; _spool_layer := LINT_TO_DINT(Layer); // // Calc now the position for the head. The deadzone has to be included into the calculation // Deadzone := (1.0 / LoopsPerLayer) * config.DeadzoneWidth; // spooling will not move on the start of each layer LayerPositionNormWithDeadzoneNull := SEL(LayerPositionNorm>Deadzone, 0.0, LayerPositionNorm); HeadPositionPre := LIMIT(0.0, (LayerPositionNormWithDeadzoneNull - Deadzone) / ((LoopsPerLayer-1.0)*Deadzone), 1.0); IF config.Switch_EncoderSpoolDir THEN // change dir IF (ABS(Layer) MOD 2) = 0 THEN // even layer HeadPosition := 1.0 - HeadPositionPre; ELSE HeadPosition := HeadPositionPre; // odd layer END_IF ELSE // norm dir IF (ABS(Layer) MOD 2) = 1 THEN // odd layer HeadPosition := 1.0 - HeadPositionPre; ELSE HeadPosition := HeadPositionPre; // even layer END_IF END_IF HeadPosition := LIMIT(0.0, HeadPosition, 1.0); _diamondScrew_VirtualWirePosition := HeadPosition; ]]> 0.0 THEN // input is max tons we should limit to _maxTorqueRequested := Tension*1000.0*9.81*_meas_radius ; // =Nm _relativeToMaxTorque := _maxTorqueRequested / Drive.getTorqueMax(); // result should be 0-1 _TorqueRelOutput := _relativeToMaxTorque * Drive.getTorqueRelMax(); // Multiply with maximum torqueRel, _tensionToTorqueRel := LIMIT(0.01, _TorqueRelOutput, Drive.getTorqueRelMax()); // limit in case user asks for more tension than winch can supply ELSE _tensionToTorqueRel := 0; END_IF ]]> 0.0 THEN _velocityToRPM := 60.0 * Velocity / (p6com.Math.PI2 * _meas_radius); ELSE _velocityToRPM := 0.0; END_IF ]]> Config._Circumference THEN state := 10; END_IF 10: // Circumference changed Config.WireLength := WinchCalcs.WinchDataToLength(Config.Circumference, Config.CoreDiameter, Config.Width, Config.WireDiameter, Config.WireDensity); // Store the new past Config._Circumference := Config.Circumference; // store the new operational value Opvalues.circumference := Config.Circumference; // New length is now zero THIS^.Encoder.setValueTo(0); state := 0; END_CASE ]]> 0 THEN isReadyToActivate := Drive.isActivationPossible() AND _opstate=p6com.P6OpState.STOPPED; ELSE isReadyToActivate := FALSE; END_IF]]> 0 THEN IF config.Option_DrumEncoder THEN EncoderResolution := THIS^.Encoder.Resolution; ELSE EncoderResolution := THIS^.Encoder.Resolution * Drive.getGearRatio(); END_IF ELSE EncoderResolution := 0; END_IF IF EncoderResolution < 1 THEN RETURN; END_IF noLoopsPerLayer := _NoLoopPerLayer(); layerHeight := config.WireDensity * config.WireDiameter; rotations := LINT_TO_LREAL((THIS^.Encoder.Value * 1000 / LREAL_TO_LINT(EncoderResolution)))/1000.0; IF config.Option_DiameterIsConstant THEN _meas_radius := config.Circumference / p6com.Math.PI2; radius_avg := _meas_radius; ELSE radius0 := Opvalues.circumference / p6com.Math.PI2 - config.WireDiameter/2 + layerHeight/2 ; // 2; // radius on last winding on layer (on full layer) IF config.FirstLayerUsage < 1 AND opvalues.circumference = config.Circumference AND noLoopsPerLayer > 0 THEN // subtract up to one layer height if top layer is not full radius0 := radius0 - ((LIMIT(0, 1-config.FirstLayerUsage, 1) * noLoopsPerLayer) / noLoopsPerLayer) * layerHeight ; // bugfix 140923 Erlend END_IF _meas_radius := radius0 - layerHeight * rotations / noLoopsPerLayer; radius_avg := (radius0 + _meas_radius) / 2.0; END_IF IF PID_cycle_timer.Q THEN // Store the actual lenght for usage in the next cycle _meas_position_1 := _meas_position; // Calculates the new length in meter _meas_position := radius_avg * p6com.Math.PI2 * rotations; // low pass filter 1. deg for the position difference filter(Value:= _meas_position - _meas_position_1, Coefficient := config.Alpha, Out=>_deltaLen); IF ABS(_deltaLen) < 0.0001 THEN _deltaLen := 0.0; END_IF // Calc the new velocity _meas_velocity_1 := _meas_velocity; _meas_velocity := _deltaLen / (TIME_TO_LREAL(p6com.CycleTime_PID0) / 1000.0); _meas_acceleration := (_meas_velocity - _meas_velocity_1) / (TIME_TO_LREAL(p6com.CycleTime_PID0) / 1000.0); END_IF; IF _meas_radius > 0.01 THEN _meas_tension := Drive.getTorque() / ( 9.81 * _meas_radius); ELSE _meas_tension := 0.0; END_IF ]]> 0 THEN _brakePressureOK := THIS^.BrakeHPU.PressureOK; ELSE _brakePressureOK := TRUE; END_IF ]]> 0 THEN EncoderResolution := THIS^.Encoder.Resolution * Drive.getGearRatio(); ELSE EncoderResolution := 0; END_IF IF EncoderResolution < 1 THEN RETURN; END_IF IF config.Option_DiameterIsConstant THEN _meas_radius := config.Circumference / p6com.Math.PI2; A := config.Circumference / EncoderResolution; ELSE // A is the cicrcumference divided by enc-resolution (circumference per encoder pulse) at 0m length A := (config.Circumference - config.WireDiameter) / EncoderResolution; // B is the part which becomes subtracted from A every counter pulse B := - (config.WireDensity * Math.PI * EXPT(config.WireDiameter,2)) / (config.Width * EXPT(EncoderResolution,2)); _meas_radius := (A + LREAL#2.0 * B * LINT_TO_LREAL(THIS^.Encoder.Value)) * EncoderResolution / p6com.Math.PI2; END_IF IF PID_cycle_timer.Q AND p6com.CycleTime_PID0 > T#0S THEN // Store the actual lenght for usage in the next cycle _meas_position_1 := _meas_position; // Calculates the new length in meter IF config.Option_DiameterIsConstant THEN _meas_position := A * LINT_TO_REAL(Encoder.Value); ELSE // The calculation is based on a 2nd order polynom _meas_position := A * LINT_TO_REAL(Encoder.Value) + B * EXPT(LINT_TO_LREAL(Encoder.Value),2); END_IF // Check for measurment jumps because of switching the PLC IF ABS(_meas_position_1 - _meas_position) > 4.0 THEN _meas_position_1 := _meas_position; END_IF // low pass filter 1. deg for the position difference filter(Value:= _meas_position - _meas_position_1, Coefficient := config.Alpha, Out=>_deltaLen); IF ABS(_deltaLen) < 0.0001 THEN _deltaLen := 0.0; END_IF // Store velocity for the next cycle _meas_velocity_1 := _meas_velocity; // Calc the new velocity _meas_velocity := _deltaLen / (TIME_TO_LREAL(p6com.CycleTime_PID0) / 1000.0); _meas_acceleration := (_meas_velocity - _meas_velocity_1) / (TIME_TO_LREAL(p6com.CycleTime_PID0) / 1000.0); END_IF; IF _meas_radius <> 0.0 THEN _meas_tension := Drive.getTorque() / ( 9.81 * _meas_radius); ELSE _meas_tension := 0.0; END_IF ]]> = 5.0 THEN _NoLoopPerLayer := config.NoLoopPerLayer; ELSIF config.WireDiameter > 0 THEN _NoLoopPerLayer := config.Width / config.WireDiameter; ELSE _NoLoopPerLayer := 1; // you fucked up! END_IF; ]]> 0 THEN // invert := -1; //ELSE // invert := 1; //END_IF // this will move the virtual layer to next/previous layer withot changing the absolute spool position IF _InboundVirtualWireDirection THEN // running back to start OpValues.encoder_offset_wire_calc := OpValues.encoder_offset_wire_calc + pulseCountChange; // * invert; // (was '-') ELSE // forward OpValues.encoder_offset_wire_calc := OpValues.encoder_offset_wire_calc - pulseCountChange; // * invert; // (was '+'); END_IF lastState := Config.Switch_EncoderSpoolDir;]]> 0 THEN IsSpeedNull := THIS^.Drive.isAtZeroSpeed(); ELSE IsSpeedNull := 0; END_IF]]> 0 THEN offset_pre := offset_pre * (1.0 - 1.0/config.NoLoopPerLayer); ELSE offset_pre := 0.0; END_IF IF (_spool_layer MOD 2) = 0 THEN offset_pre := -offset_pre; END_IF OpValues.HeadOffset := LREAL_TO_REAL(offset_pre); ]]>