AnitSway modified and moved from rs

ssabox/doc/orm/src/os_linux/hw_x86/makefile

+include $(pwre_dir_symbols)
+ifndef variables_mk
+ -include $(pwre_kroot)/tools/bld/src/$(os_name)/$(hw_name)/variables.mk
+endif
+ifndef variables_mk
+ include $(pwre_kroot)/tools/bld/src/$(os_name)/variables.mk
+endif
+
+
+ifndef rules_mk
+ -include $(pwre_kroot)/tools/bld/src/$(os_name)/$(hw_name)/rules.mk
+endif
+ifndef rules_mk
+ include $(pwre_kroot)/tools/bld/src/$(os_name)/rules.mk
+endif
+
+
+vpath %.gif $(hw_source):$(os_source):$(co_source)
+
+source_dirs := $(hw_source) $(os_source) $(co_source)
+
+gif_sources := $(sort \
+             $(foreach file, \
+               $(foreach dir, \
+                 $(source_dirs), \
+                 $(wildcard $(dir)/*.gif) \
+               ), $(notdir $(file)) \
+             ) \
+           )
+
+html_sources := $(sort \
+             $(foreach file, \
+               $(foreach dir, \
+                 $(source_dirs), \
+                 $(wildcard $(dir)/*.html) \
+               ), $(notdir $(file)) \
+             ) \
+           )
+
+export_gif := $(patsubst %.gif,$(doc_dir)/en_us/orm/%.gif,$(gif_sources))
+export_html := $(patsubst %.html,$(doc_dir)/en_us/orm/%.html,$(html_sources))
+clean_gif := $(patsubst %.gif,clean_%.gif,$(gif_sources))
+clean_html := $(patsubst %.html,clean_%.html,$(html_sources))
+
+.SUFFIXES:
+
+$(doc_dir)/en_us/orm/%.gif : %.gif
+	@ $(log_h_h)
+	@ $(cp) $(cpflags) $(source) $(target)
+	@ $(cp) $(cpflags) $(source) $(doc_dir)/sv_se/orm
+	@ $(cp) $(cpflags) $(source) $(doc_dir)/help
+
+$(doc_dir)/en_us/orm/%.html : ../../%.html
+	@ $(log_h_h)
+	@ $(cp) $(cpflags) $(source) $(target)
+	@ $(cp) $(cpflags) $(source) $(doc_dir)/sv_se/orm
+	@ $(cp) $(cpflags) $(source) $(doc_dir)/help
+
+.PHONY : all init copy lib exe clean realclean \
+         $(clean_gif)
+
+all : init copy lib exe
+
+init :
+
+copy : $(export_gif) $(export_html)
+
+lib :
+
+exe :
+
+
+clean :
+
+realclean : clean $(clean_gif) $(clean_html) $(clean_c)
+
+$(clean_gif) : clean_%.gif : %.gif
+	@ $(rm) $(rmflags) $(doc_dir)/orm/$*.gif
+
+$(clean_html) : clean_%.html : %.html
+	@ $(rm) $(rmflags) $(doc_dir)/orm/$*.html
+
+
+
+

ssabox/lib/ssabox/src/os_linux/hw_x86/special.mk

+copy : $(doc_dir)/sv_se/orm/ssabox_plc_antisway_c.html \
+       $(doc_dir)/en_us/orm/ssabox_plc_antisway_c.html \
+       $(doc_dir)/sv_se/orm/ssabox_plc_servoreg_c.html \
+       $(doc_dir)/en_us/orm/ssabox_plc_servoreg_c.html
+
+$(doc_dir)/sv_se/orm/ssabox_plc_antisway_c.html : \
+	$(pwre_sroot)/lib/ssabox/src/ssabox_plc_antisway.c
+	@ echo "Generating html files for ssabox plc antiway code sv_se..."
+	@ co_convert -c -d $(doc_dir)/sv_se/orm "$(source)"
+
+$(doc_dir)/en_us/orm/ssabox_plc_antisway_c.html : \
+	$(pwre_sroot)/lib/ssabox/src/ssabox_plc_antisway.c
+	@ echo "Generating html files for ssabox plc antisway code en_us..."
+	@ co_convert -c -d $(doc_dir)/en_us/orm "$(source)"
+
+$(doc_dir)/sv_se/orm/ssabox_plc_servoreg_c.html : \
+	$(pwre_sroot)/lib/ssabox/src/ssabox_plc_servoreg.c
+	@ echo "Generating html files for ssabox plc servoreg code sv_se..."
+	@ co_convert -c -d $(doc_dir)/sv_se/orm "$(source)"
+
+$(doc_dir)/en_us/orm/ssabox_plc_servoreg_c.html : \
+	$(pwre_sroot)/lib/ssabox/src/ssabox_plc_servoreg.c
+	@ echo "Generating html files for ssabox plc servoreg code en_us..."
+	@ co_convert -c -d $(doc_dir)/en_us/orm "$(source)"
+

ssabox/lib/ssabox/src/ssabox_as_ode_solvers.c

+/*2005-02-13      File: AS_ODE_Solvers.c
+* Author: Jonas Haulin   email: joha7211@student.uu.se 
+* Contains functions that will integrate the discretized DE:s for sway angle, 
+* trolley position and cable length
+*/
+
+#include "ssabox_as_ode_solvers.h"
+
+void AS_thetaIntegrator(double *thetam, double *thetac, double Lc, double DLc, double ac, double dt) 
+{
+  /* Function: thetaIntegrator
+   * Simulation of sway of crane load.
+   * One-dimensional traversal and sway. Variable pendulum length, L
+   * Lagrange's equation: D^2(theta)*L + 2*D(L)*D(theta) + D^2(x)*cos(theta) + g*sin(theta) = 0
+   * 
+   * Integrates Lagrange's equation and stores the current sway angle in thetac
+   *
+   * Variables
+   * thetam - sway angle at time n-1
+   * thetac - sway angle at time n
+   * thetap - sway angle at time n+1 */
+  
+  /*Solving the discretized ODE for thetap*/
+  double thetap = (2.0*(*thetac)*Lc + (*thetam)*(dt*DLc - Lc) - (dt*dt)*(ac*cos(*thetac) + AS_GRAV_ACCEL*sin(*thetac)))/(dt*DLc + Lc);
+  *thetam=*thetac;
+  *thetac=thetap;
+}
+
+void AS_xIntegrator(double *xm, double *xc, double ac, double dt) 
+{
+  /* Calculates the position at time n+1 from acceleration and x(n), x(n-1) */
+
+  double xp=2.0*(*xc) - *xm + ac*(dt*dt);
+  *xm=*xc;
+  *xc=xp;
+}
+
+void AS_xIntegratorUA(double *xm, double *xc, double uc, double ac, double dt) 
+{
+/* Calculates the position at time n+1 from velocity and x(n) */
+
+  double xp=*xc + uc*dt + 0.5*ac*(dt*dt);
+  *xm=*xc;
+  *xc=xp;
+}
+
+void AS_xcIntegratorU(double *xc, double uc, double dt) 
+{
+  *xc += uc*dt;
+}
+
+void AS_xIntegrator2U(double *xm, double *xc, double um, double dt) 
+{
+  double xp = *xm + 2.0*um*dt;
+  *xm=*xc;
+  *xc=xp;
+}
+
+void AS_uIntegratorFwd(double *uc, double ac, double dt) 
+{
+/* Calculates velocity at time n+1 from uc and ac with a forward difference approximation of the derivative */
+
+  *uc += ac*dt;
+}
+
+void AS_LIntegratorFwd(double *Lc, double DLc, double dt) 
+{
+  *Lc += DLc*dt;
+} 

ssabox/lib/ssabox/src/ssabox_as_ode_solvers.h

+/*********************************************************************
+**********************************************************************
+*
+*       S S A B  O X E L  S U N D
+*       = = = =  = = = = = = = = =
+**********************************************************************
+*
+* Filename      : 
+*
+* Description   : This header file is included by AS_ODE_solvers.c
+*               : and ra_plc_user.h.
+*		: It contains prototypes for the discrete-time
+*               : ODE solvers.
+*                 
+*               Date    Sign           Action
+* Revision      050213  jonas_h        First edition.
+*               
+*
+*
+**********************************************************************
+**********************************************************************/
+
+#ifndef ssabox_as_ode_solvers_h
+#define ssabox_as_ode_solvers_h
+
+#include <math.h>
+
+#include "ssabox_as_phasor_methods.h"
+
+/*prototype declarations*/
+void AS_thetaIntegrator(double *, double *, double , double , double, double dt);
+void AS_xIntegrator(double *, double *, double, double dt);
+void AS_xIntegratorUA(double *, double *, double, double, double dt);
+void AS_xcIntegratorU(double *, double, double dt);
+void AS_xIntegrator2U(double *xm, double *xc, double um, double dt);
+void AS_uIntegratorFwd(double *, double, double dt);
+void AS_LIntegratorFwd(double *, double , double dt);
+
+#endif

ssabox/lib/ssabox/src/ssabox_plc_antisway.h

+/*********************************************************************
+**********************************************************************
+*
+*       S S A B  O X E L  S U N D
+*       = = = =  = = = = = = = = =
+**********************************************************************
+*
+* Filename      : 
+*
+* Description   : This header file is included by the plc modules
+*		: and allows the user to include function prototypes, 
+*		: defines, typedefs etc.
+*                 
+*               Date    Sign           Action
+* Revision      040923  SSABCJ         First edition.
+*               050213  jonas_h        Antisway
+*
+*
+**********************************************************************
+**********************************************************************/
+
+#include <stddef.h>
+#include <time.h>
+#include <sys/time.h>
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <float.h>
+#include "libiberty.h"
+
+#include "pwr_baseclasses.h"
+#include "pwr_basecomponentclasses.h"
+#include "pwr_ssaboxclasses.h"
+#include "rt_plc.h"
+#include "co_time.h"
+#include "pwr.h"
+
+#include "ssabox_as_ode_solvers.h"
+#include "ssabox_as_phasor_methods.h"
+
+
+void Ssab_AntiSway_init(pwr_sClass_Ssab_AntiSway *object);
+
+void Ssab_AntiSway_exec(plc_sThread *tp, pwr_sClass_Ssab_AntiSway *object);

ssabox/lib/ssabox/src/ssabox_plc_servoreg.c

+/*********************************************************************
+**********************************************************************
+*
+*       S S A B  O X E L  S U N D
+*       = = = =  = = = = = = = = =
+**********************************************************************
+*
+* Filename      : $pwrp_src:ra_plc_user.c
+*
+* Description   : Contains main functions for anti-sway control,
+*               : called from plc
+*                 
+*               Date    Sign           Action
+* Revision      050213  jonas_h        First edition
+*               050721  jonas_h        Improved error handling: Errors are written to an internal buffer instead of being printed with printf(..). From the buffer, errors are stored in a String80 output, that can be connected e.g. to a DSup.
+*                                      Extended functionality for automatic object: The soft maximum velocity, umaxS, can be changed while in travel (if e.g. in a low-speed area).
+*                                      Transfer of set to manual object when disabling, allowing for continued anti-sway, and smooth travel.
+*
+*
+**********************************************************************
+**********************************************************************/
+
+#include "ssabox_plc_servoreg.h"
+
+/*_*
+  @aref ssab_servoreg Ssab_ServoReg
+*/
+void Ssab_ServoReg_exec(plc_sThread *tp, pwr_sClass_Ssab_ServoReg *object)
+{
+  double aD, uD, xD, dt, control, uRamp, xDiff;
+  int delaysteps;
+
+  dt = tp->PlcThread->ScanTime; /* Constant scan time (the ideal value, not the measurement of last cycle time) */
+  object->enable = *object->enableP;
+ 
+ if (!object->enable) { //bypass the reference and reset active flags and elapsed time
+    object->uReg = *object->uRP;
+    object->RampActive = FALSE;
+    object->DZActive = FALSE;
+    object->TDZElapsedTime = 0.0;
+    object->TDZActive = FALSE;
+    return;
+  }
+
+
+  /* 1. Retrieve input */
+
+  object->aR = *object->aRP;
+  object->uR = *object->uRP;
+  control = object->uR;
+  object->xR = *object->xRP;
+  object->ac = *object->acP;
+  object->uc = *object->ucP;
+  object->xc = *object->xcP;
+  object->xCommand = *object->xCommandP;
+  xDiff = object->xCommand - object->xc;
+  object->umaxP = *object->umaxPP;
+  object->umaxN = *object->umaxNP;
+  object->positioning = *object->positioningP;
+  if (object->positioning && object->enableRamp) //uR not used - use the regulator for positioning
+    control = (xDiff > 0.0 ? object->umaxP : object->umaxN);
+
+  /* 2. PID regulator */
+
+  // Should change some stuff here: Relative gain kP, Integration time Ti, Derivation time Td. 
+
+  if (object->enablePID && !object->positioning) {
+    SR_addNewRef(SR_OBJ_LISTPP, object->aR, object->uR, object->xR);
+    delaysteps = AS_ROUND(object->DelayPID/dt);   
+    if (delaysteps < 0)
+      delaysteps = 0;
+    if (delaysteps > object->maxdelaysteps)
+      delaysteps = object->maxdelaysteps;
+    SR_extractRef(SR_OBJ_LISTPP, delaysteps, &aD, &uD, &xD);
+    control = uD + object->kPID[0]*(uD - object->uc) + object->kPID[1]*(xD - object->xc) + object->kPID[2]*(aD - object->ac);
+  }
+
+
+  /* 3. Square root ramp */
+  //This will give erroneous behavior if the command position is changed to a position close to the current in the midst of a travel.
+  //Possibly solve this problem by taking into account further conditions. 
+
+  if (object->enableRamp) {
+    uRamp = AS_SIGN(xDiff) * ( -object->DelayRamp*object->amaxS + 
+	   sqrt(object->DelayRamp*object->DelayRamp*object->amaxS*object->amaxS + 2.0*fabs(xDiff)*object->amaxS) );
+    if ( (xDiff >= 0.0 && control > uRamp) || (xDiff < 0.0 && control < uRamp) ) {
+      control = uRamp;
+      object->RampActive = TRUE;
+    }
+    else
+      object->RampActive = FALSE;
+  }
+  else
+    object->RampActive = FALSE;
+   
+
+  /* 4. Dead zone */
+  
+  if (object->enableDZ && (object->uR == 0.0) && (fabs(xDiff) < object->DeadZone)) {
+    control = 0.0;
+    object->DZActive = TRUE;
+  }
+  else 
+    object->DZActive = FALSE;
+
+
+
+  /* 5. Timer dead zone */
+
+  if (object->enableTDZ && (object->uR == 0.0) && (fabs(xDiff) < object->TimerDeadZone)) {
+    object->TDZElapsedTime += *(object->ScanTime);
+    if (object->TDZElapsedTime > object->TDZTime) {
+      control = 0.0;
+      object->TDZActive = TRUE;
+      object->TDZElapsedTime = object->TDZTime;
+    }
+    else
+      object->TDZActive = FALSE;
+  }
+  else {
+    object->TDZElapsedTime = 0.0;
+    object->TDZActive = FALSE;
+  }
+
+
+  /* 6. Check bounds */
+  
+  if (control > object->umaxP)
+    control = object->umaxP;
+  if (control < object->umaxN)
+    control = object->umaxN;
+
+  
+  /* 8. Set output */
+
+  object->uReg = control;
+  
+} 
+/* End of ServoReg */
+
+

ssabox/lib/ssabox/src/ssabox_plc_servoreg.h

+/*********************************************************************
+**********************************************************************
+*
+*       S S A B  O X E L  S U N D
+*       = = = =  = = = = = = = = =
+**********************************************************************
+*
+* Filename      : $pwrp_inc:ra_plc_user.h
+*
+* Description   : This header file is included by the plc modules
+*		: and allows the user to include function prototypes, 
+*		: defines, typedefs etc.
+*                 
+*               Date    Sign           Action
+* Revision      040923  SSABCJ         First edition.
+*               050213  jonas_h        Antisway
+*
+*
+**********************************************************************
+**********************************************************************/
+
+#include <stddef.h>
+#include <time.h>
+#include <sys/time.h>
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <float.h>
+#include "libiberty.h"
+
+#include "pwr_baseclasses.h"
+#include "pwr_basecomponentclasses.h"
+#include "pwr_ssaboxclasses.h"
+#include "rt_plc.h"
+#include "co_time.h"
+#include "pwr.h"
+
+#include "ssabox_sr_regulator_methods.h"
+
+
+void Ssab_ServoReg_exec(plc_sThread *tp, pwr_sClass_Ssab_ServoReg *object);

ssabox/lib/ssabox/src/ssabox_sr_regulator_methods.c

+/*2005-09-14      File: SR_regulator_methods.c
+* Author: Jonas Haulin   email: jonas@haulin.se 
+* Contains internal and engine functions for antisway objects. 
+*/
+
+#include "ssabox_sr_regulator_methods.h"
+
+
+/* Prototype declarations of internal functions */
+static void remove_tail(SR_RefList *tail);
+
+
+void SR_addNewRef(SR_RefList **objectRefListpp, double aR, double uR, double xR)
+{
+  SR_RefList *newest;
+
+  newest = (SR_RefList *) malloc(sizeof(SR_RefList));
+  if (newest == NULL) {
+    fprintf(stderr, "SR_addNewRef: Could not allocate memory!\n");
+    exit(99);
+  }
+  newest->older = *objectRefListpp; //ok if this happens to be NULL
+  newest->a = aR;
+  newest->u = uR;
+  newest->x = xR;
+
+  *objectRefListpp = newest; //the object RefList pointer always points at the newest reference.
+}
+
+void SR_extractRef(SR_RefList **objectRefListpp, int delay, double *aDelayp, double *uDelayp, double *xDelayp)
+{
+  int i = 0;
+  SR_RefList *sought;
+ 
+  sought = *objectRefListpp;
+  if (sought == NULL) //This should not happen..
+    return;
+
+  /* Find delayed references */
+  while ((sought->older != NULL) && (i++ < delay))
+    sought = sought->older;   
+
+  /* Extract delayed references */
+  *aDelayp = sought->a;
+  *uDelayp = sought->u;
+  *xDelayp = sought->x;
+
+  if ((i > delay) && (sought->older != NULL)) { // Remove tail of post-delay references if it exists
+    remove_tail(sought->older);
+    sought->older = NULL;
+  }
+}
+
+static void remove_tail(SR_RefList *tail) //recursive function to remove tail of list.
+{
+  if (tail->older != NULL)
+    remove_tail(tail->older);
+    
+  free(tail);
+}
+

ssabox/lib/ssabox/src/ssabox_sr_regulator_methods.h

+#ifndef ssabox_sr_regulator_methods_h
+#define ssabox_sr_regulator_methods_h 
+
+#include "pwr_baseclasses.h"
+#include "pwr_basecomponentclasses.h"
+#include "pwr_ssaboxclasses.h"
+#include "ssabox_as_phasor_methods.h" //included to get max, min macros etc.
+
+/* Macros */
+
+#define SR_OBJ_LISTPP ((SR_RefList **) &object->RefListp)
+
+
+
+
+/* Typedefs / structs */
+
+#ifndef ssabox_sr_servoreg_structs
+#define ssabox_sr_servoreg_structs 
+
+typedef struct SR_RefList {
+  double a;
+  double u;
+  double x;
+  struct SR_RefList *older;
+} SR_RefList;
+
+#endif
+
+/* Prototype declarations of externally visible regulator methods.*/
+
+void SR_addNewRef(SR_RefList **objectRefListp, double aR, double uR, double xR);
+void SR_extractRef(SR_RefList **objectRefListp, int delay, double *aDelayp, double *uDelayp, double *xDelayp);
+
+
+#endif