Corrected SteeringRecord model and updated DriverController

TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl

@@ -11,7 +11,7 @@ component Mastercomponent {
    instance Drivercontroller driverController;
    instance Unnormalizer unnorm;
    //instance KFMastercomponent kfm;
-   instance AngleBuffer angleBuffer;
+   instance SteeringBuffer steeringBuffer;
 
    connect imageIn -> dpnet.data;
    connect dpnet.predictions -> unnorm.normalizedPredictions;
@@ -19,10 +19,9 @@ component Mastercomponent {
    //connect un.affordance -> kfm.predictions;
    //connect kfm.predictionsSmoothed -> driverController.affordanceIn;
 
-   connect unnorm.affordance -> angleBuffer.inputAffordance;
    connect unnorm.affordance -> driverController.affordanceIn;
-   connect angleBuffer.outputBuffer -> driverController.anglesIn;
-
+   connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
    connect speedIn -> driverController.speedIn;
    connect driverController.commandsOut -> commandsOut;
+   connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
 }
\ No newline at end of file

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Commands.struct

-package dp.subcomponents;
-
-struct Commands {
-    Q(0:1) acceleration;
-    Q(0:1) brake;
-    Q(-1:1) steering;
-}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl

@@ -2,21 +2,26 @@ package dp.subcomponents;
 
 component Drivercontroller {
     ports
-        in Q^{5} anglesIn,
+        in Q^{5} steeringRecordIn,
 	    in Affordance affordanceIn,
 	    in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
-	    out Q(-1:1)^{3} commandsOut;
+	    out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
+	    out Q steerCmd;
 
 	implementation Math {
         Q roadWidth = 8;
-
-	    // Acceleration
-	    Q slowDown = 100;
+        Q centerLine;
+        Q coeSteer = 1.0;
+        Q preML;
+        Q preMR;
+        Q desiredSpeed;
+	    Q slowDown = 100; // Acceleration
 
 	    if (affordanceIn.distMM < 15)
             Q vMax = 20;
             Q vC = 2.772;
             Q vD = -0.693;
+            // optimal velocity car-following model
             slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
 
             if (slowDown < 0)
@@ -24,10 +29,6 @@ component Drivercontroller {
             end
         end
 
-        Q centerLine;
-        Q coeSteer;
-        Q preML;
-        Q preMR;
         if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
             coeSteer = 1.5;
             centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
@@ -47,16 +48,17 @@ component Drivercontroller {
 
         Q steerCmd = 0;
         // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
-	    steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
+	    steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
+
 	    if (coeSteer > 1 && steerCmd > 0.1)   // reshape the steering control curve
             steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
         end
         commandsOut(2) = steerCmd;
 
-        Q desiredSpeed = 20;
-	    //if (affordanceIn.fast != 1)
-	    //    desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
-	    //end
+        desiredSpeed = 20;
+	    if (affordanceIn.fast != 1)
+	        desiredSpeed = 20 - abs(steeringRecordIn(0) + steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4)) * 4.5;
+	    end
 
 	    if (desiredSpeed < 10)
 	        desiredSpeed = 10;
@@ -75,15 +77,15 @@ component Drivercontroller {
             end
 
             commandsOut(1) = accelCmd;
-            commandsOut(3) = 0;
+            commandsOut(3) = 0; // brakeCmd
         else
             Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
             if (brakeCmd > 1)
-                commandsOut(3) = 1;
+                brakeCmd = 1;
             end
 
             commandsOut(3) = brakeCmd;
-            commandsOut(1) = 0;
+            commandsOut(1) = 0; // accelCmd
         end
     }
 }
\ No newline at end of file

TorcsEMAMGenerator/src/main/models/dp/subcomponents/AngleBuffer.emadl → TorcsEMAMGenerator/src/main/models/dp/subcomponents/SteeringBuffer.emadl

 package dp.subcomponents;
 
-component AngleBuffer {
-    ports in Affordance inputAffordance,
+component SteeringBuffer {
+    ports in Q inSteerCmd,
           out Q^{5} outputBuffer;
 
     implementation Math {
@@ -11,7 +11,7 @@ component AngleBuffer {
             buffer(4) = buffer(3);
             buffer(3) = buffer(2);
             buffer(2) = buffer(1);
-            buffer(1) = inputAffordance.angle;
+            buffer(1) = inSteerCmd;
 
             outputBuffer(1) = buffer(1);
             outputBuffer(2) = buffer(2);