diff --git a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl index 83ee744d5015903c8cf8f8e978b3c312bf412c06..f73ee7a28f34880ab5a493e577d9a8f45900518e 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl +++ b/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 diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Commands.struct b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Commands.struct deleted file mode 100644 index 72f2402333fc60e27012adddf83f0972d14ccaa0..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Commands.struct +++ /dev/null @@ -1,7 +0,0 @@ -package dp.subcomponents; - -struct Commands { - Q(0:1) acceleration; - Q(0:1) brake; - Q(-1:1) steering; -} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl index 1c6765f7cc5c5476e339adcabe9dec1c822fca73..db4c2f469a41bbfc12833318d852e425e0ef67a2 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl +++ b/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 diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/AngleBuffer.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/SteeringBuffer.emadl similarity index 81% rename from TorcsEMAMGenerator/src/main/models/dp/subcomponents/AngleBuffer.emadl rename to TorcsEMAMGenerator/src/main/models/dp/subcomponents/SteeringBuffer.emadl index 76b67a38737537732ca22c041de6a4632b228c88..3bb722ede2fb8ded646a41f27ab28350f8b8cdd9 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/AngleBuffer.emadl +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/SteeringBuffer.emadl @@ -1,7 +1,7 @@ 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);