m_dMaxSpeed is again a parameter in WaypointControlLaw

src/BelugaControl.cpp

@@ -27,10 +27,11 @@ BelugaBoundaryControlLaw* belugaBoundaryControlLawFactory(unsigned int bot_num,
 
 BelugaWaypointControlLaw::BelugaWaypointControlLaw()  // includes HITL timing control (instead of having a separate ControlLaw)
     : mt_ControlLaw(3 /* # control inputs */,
-                    3 /* # parameters */),
+                    4 /* # parameters */),
       m_bActive(false),
-      m_dTiming(7500),		  // time for robot to travel between waypoints (msec), set in js
-      m_dDistThreshold(0.5),  // distance from waypoint at which robot starts to decrease speed from max (m)
+      m_dTiming(7500),	      // time for robot to travel between waypoints (msec), set in js and updated during IPC exchange
+      m_dMaxSpeed(1.28),      // speed robot travels when more than 'm_dDistThreshold' away from waypoint (m/s), updated during IPC exchange
+	  m_dDistThreshold(0.5),  // distance from waypoint at which robot starts to decrease speed from max (m)
       m_dTurningGain(10.0)
 {
 }
@@ -83,14 +84,13 @@ mt_dVector_t BelugaWaypointControlLaw::doControl(const mt_dVector_t& state,
     }
     else
     {
-        double maxSpeed = 1.5*((2*DEFAULT_TANK_RADIUS)/m_dTiming)*1000;  // can play with 1.5 if not Beluga is slow or fast
 		if (d > m_dDistThreshold)
         {
-            u_speed = maxSpeed;
+            u_speed = m_dMaxSpeed;
         }
         else
         {
-            u_speed = maxSpeed*(d/m_dDistThreshold)*fabs(cos(dth));
+            u_speed = m_dMaxSpeed*(d/m_dDistThreshold)*fabs(cos(dth));
         }
         u_turn = -m_dTurningGain*sin(dth);
 		if (dth > 1.57)
@@ -203,7 +203,7 @@ mt_dVector_t BelugaLowLevelControlLaw::doControl(const mt_dVector_t& state,
 
 BelugaBoundaryControlLaw::BelugaBoundaryControlLaw()
 	: mt_ControlLaw(3 /* # control inputs */,
-					1 /* # parameters */),
+					7 /* # parameters */),
 	  m_bActive(true)
 {
 	/* map tank at depth z */

src/BelugaControl.h

@@ -15,6 +15,7 @@ class BelugaWaypointControlLaw : public mt_ControlLaw
     bool doActivate(bool value = true){m_bActive = value;  return m_bActive;};
 
 	double m_dTiming;
+	double m_dMaxSpeed;
 	double m_dDistThreshold;
     double m_dTurningGain;
 

src/BelugaTrackerGUI.cpp

@@ -423,14 +423,16 @@ void BelugaTrackerFrame::doIPCExchange()
         bool r = m_IPCClient.setAllPositions(&X, &Y, &Z);
         r &= m_IPCClient.getControls(robots, &mode, &X, &Y, &Z);
 
-		// set timing parameter for WaypointControlLaw (m_dTiming) from IPC
+		// set timing and maxSpeed parameters for WaypointControlLaw (m_dTiming & m_dMaxSpeed) from IPC
 		double timing = 0;
 		std::string params("");
 		r &= m_IPCClient.getParams(&params);
 		sscanf(params.c_str(), "%f", &timing);
+		double maxSpeed = 1.5*((2*DEFAULT_TANK_RADIUS)/timing)*1000;
 		for(unsigned int i = 0; i < 4; i++)
 		{
 			m_apWaypointController[i]->m_dTiming = timing;
+			m_apWaypointController[i]->m_dMaxSpeed = maxSpeed;
 		}
 
         if(!r)

test/test_Controller.cpp

@@ -24,7 +24,7 @@ int testBelugaWaypointControlLaw()
     u_in[BELUGA_WAYPOINT_X] = state[BELUGA_STATE_X];
     u_in[BELUGA_WAYPOINT_Y] = state[BELUGA_STATE_Y] + 1.1*control_law.m_dDistThreshold;
     u_out = control_law.doControl(state, u_in);
-    if(!eq_wf(u_out[BELUGA_CONTROL_FWD_SPEED], 1.5))
+    if(!eq_wf(u_out[BELUGA_CONTROL_FWD_SPEED], control_law.m_dMaxSpeed))
         RETURN_ERROR_ELSE_OK("Control did not saturate, was " << u_out[BELUGA_CONTROL_FWD_SPEED]);
 
     return OK;