- Many fixes to the four circle codes during taking the new code into

operation.
- Fixed some missing output
- Second generation histogram memory and velocity selector objects
- Fixed a problem in diffscan

velosec.c

@@ -0,0 +1,157 @@
+/**
+ * This is the implementation file for a second generation velocity selector 
+ * object. This is an extension to the drivable object. It mainly adds handling of 
+ * the tilt motor to the system.
+ * 
+ * copyright: see file COPYRIGHT
+ * 
+ * Mark Koennecke, April 2009
+ */
+#include <stdlib.h>
+#include <sics.h>
+#include <motor.h>
+#include <sicsobj.h>
+#include <scriptcontext.h>
+#include <sicshipadaba.h>
+#include <drive.h>
+
+#define ABS(x) (x < 0 ? -(x) : (x))
+/*
+ * from sctdriveobj.c 
+ */
+pSICSOBJ MakeSctDriveObj(pHdb node, char *class, SctController * c,
+                         int doNotKillNode);
+/*--------------------------------------------------------------------------*/
+static hdbCallbackReturn tiltCallback(pHdb node, void *userData, 
+		pHdbMessage message)
+{
+	pHdbDataMessage get = NULL, set = NULL;
+	pMotor tilt = (pMotor)userData;
+	float val, rot;
+	char nvsName[80];
+	SConnection *pCon, *mine;
+	void *pNVS = NULL;
+	int status;
+	
+	assert(tilt != NULL);
+	
+	if((get = GetHdbGetMessage(message)) != NULL){
+		if(get->callData != NULL){
+			val = tilt->pDrivInt->GetValue(tilt,(SConnection *)get->callData);
+		} else {
+			val = tilt->pDrivInt->GetValue(tilt,pServ->dummyCon);
+		}
+		node->value.v.doubleValue = val;
+	}
+	if((set = GetHdbGetMessage(message)) != NULL){
+		if(set->callData != NULL){
+			pCon = (SConnection *)set->callData;
+		} else {
+			pCon = pServ->dummyCon;
+		}
+		/*
+		 * do we need to do anything?
+		 */
+		GetDrivablePosition(tilt,pCon,&val);
+		if(ABS(val - set->v->v.doubleValue) < .01 ){
+			SCWrite(pCon,"tilt already at position", eValue);
+			return hdbContinue;
+		}
+		GetHdbProperty(node,"NVS", nvsName, 80);
+
+		pNVS = FindCommandData(pServ->pSics,nvsName,"NVS");
+		assert(pNVS != NULL);
+		GetDrivablePosition(pNVS, pCon, &rot);
+		
+		SCWrite(pCon,"Wait for NVS to stop...", eLog);
+		status = Drive(pCon, pServ->pSics, nvsName, .0);
+		if(status != 1){
+			SCWrite(pCon,"ERROR: failed to stop NVS, driving tilt aborted", eError);
+			return hdbContinue;
+		}
+		
+		SCWrite(pCon,"Actually driving tilt...", eLog);
+		mine = SCCopyConnection(pCon);
+		SCSetRights(mine, usInternal);
+		status = Drive(pCon, pServ->pSics, tilt->name, (float)set->v->v.doubleValue);
+		if(status != 1){
+			SCWrite(pCon,"ERROR: failed to drive tilt, NVS stopped", eError);
+			return hdbContinue;
+		}
+		SCDeleteConnection(mine);
+		GetDrivablePosition(tilt,pCon,&val);
+		UpdateHipadabaPar(node, MakeHdbFloat(val), pCon);
+		
+		SCWrite(pCon,"Wait even more for NVS to get to speed again...", eLog);
+		status = Drive(pCon, pServ->pSics, nvsName, rot);
+		if(status != 1){
+			SCWrite(pCon,"ERROR: failed to run NVS to speed", eError);
+			return hdbContinue;
+		}
+		SCWrite(pCon,"Done", eValue);
+	}
+	return hdbContinue;
+}
+/*--------------------------------------------------------------------------*/
+int MakeSecNVS(SConnection * pCon, SicsInterp * pSics,
+                       void *object, int argc, char *argv[])
+{
+	pMotor tilt = NULL;
+	pSICSOBJ pNew = NULL;
+	SctController *sct = NULL;
+	pHdb node = NULL, tilli;
+	int status;
+	
+	if(argc < 4){
+		SCWrite(pCon,
+		"ERROR: missing required parameters, Usage: MakeNVS name tiltmotorname sctcontroller", 
+		eError);
+		return 0;
+	}
+	
+	tilt = (pMotor)FindCommandData(pSics,argv[2],"Motor");
+	if(tilt == NULL){
+		SCPrintf(pCon,eError, "ERROR: tilt motor %s not found", argv[2]);
+		return 0;
+	}
+	
+	sct = (SctController *)FindCommandData(pSics, argv[3],"ScriptContext");
+	if(sct == NULL){
+		SCPrintf(pCon,eError, "ERROR: scriptcontext %s not found", argv[3]);
+		return 0;
+	}
+	
+	node = MakeHipadabaNode(argv[1],HIPFLOAT,1);
+	SetHdbProperty(node,"sicsdev",argv[1]);
+	SetHdbProperty(node,"priv", "user");
+	
+	pNew = MakeSctDriveObj(node, "NVS", sct, 0);
+	if(pNew == NULL || node == NULL){
+		SCWrite(pCon,"ERROR: failed to create NVS, out of memory?", 
+				eError);
+		return 0;
+	}
+	
+	
+	tilli = MakeSICSHdbPar("tilt", usMugger, MakeHdbFloat(.0));
+	if(tilli == NULL){
+		KillSICSOBJ(pNew);
+		SCWrite(pCon,"ERROR: failed to create tilt node, out-of-memory?", eError);
+		return 0;
+	}
+	/*
+	 * In the set callback I need the name of the NVS because I need to stop and 
+	 * drive it up again. Rather then have another data structure I pass this name
+	 * as a property to the tilt node.
+	 */
+	SetHdbProperty(tilli,"NVS", argv[1]);
+	
+	MotorSetPar(tilt,pCon,"accesscode", (float)usInternal);
+	AppendHipadabaCallback(tilli, MakeHipadabaCallback(tiltCallback, tilt,NULL));
+	AddHipadabaChild(node, tilli, pCon);
+	
+	status = AddCommand(pSics,
+	                      argv[1], InterInvokeSICSOBJ, KillSICSOBJ, pNew);
+
+	return status;
+}