Commit 8c091fdf authored by Sascha Niklas Schneiders's avatar Sascha Niklas Schneiders
Browse files

updated Simulator model

parent 46654b8e
#ifndef SIMULATOR_MAINCONTROLLER_BRAKECONTROLLER1
#define SIMULATOR_MAINCONTROLLER_BRAKECONTROLLER1
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "simulator_mainController_brakeController1_speedLimitChecker1.h"
class simulator_mainController_brakeController1{
public:
double currentSpeedLimit;
double currentVelocity;
double brakeForce;
simulator_mainController_brakeController1_speedLimitChecker1 speedLimitChecker1;
void init()
{
speedLimitChecker1.init();
}
void execute()
{
speedLimitChecker1.currentSpeedLimit = currentSpeedLimit;
speedLimitChecker1.currentVelocity = currentVelocity;
speedLimitChecker1.execute();
if((speedLimitChecker1.speedLimitSurpassed)){
brakeForce = 1/2 ;
}
else {
brakeForce = 0/1 ;
}
}
};
#endif
#ifndef SIMULATOR_MAINCONTROLLER_BRAKECONTROLLER1_SPEEDLIMITCHECKER1
#define SIMULATOR_MAINCONTROLLER_BRAKECONTROLLER1_SPEEDLIMITCHECKER1
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
class simulator_mainController_brakeController1_speedLimitChecker1{
public:
double currentVelocity;
double currentSpeedLimit;
bool speedLimitSurpassed;
void init()
{
}
void execute()
{
if(((currentVelocity > currentSpeedLimit))){
speedLimitSurpassed = true;
}
else {
speedLimitSurpassed = false;
}
}
};
#endif
......@@ -30,16 +30,11 @@ steeringCalculator.y[1] = y[1];
steeringCalculator.gpsX = gpsX;
steeringCalculator.gpsY = gpsY;
steeringCalculator.orientation = orientation;
steeringCalculator.minSteeringAngle = minSteeringAngle;
steeringCalculator.maxSteeringAngle = maxSteeringAngle;
steeringCalculator.currentSteeringAngle = currentSteeringAngle;
steeringCalculator.execute();
double angle = steeringCalculator.newSteeringAngle;
if((angle < minSteeringAngle)){
angle = minSteeringAngle;
}
else if((angle > maxSteeringAngle)){
angle = maxSteeringAngle;
}
steeringAngle = -angle;
steeringAngle = steeringCalculator.newSteeringAngle;
}
};
......
......@@ -4,7 +4,6 @@
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "simulator_mainController_steerController1_steeringCalculator_distanceCalculator.h"
#include "Helper.h"
#include "octave/builtin-defun-decls.h"
class simulator_mainController_steerController1_steeringCalculator{
......@@ -15,26 +14,28 @@ double gpsX;
double gpsY;
double orientation;
double currentSteeringAngle;
double minSteeringAngle;
double maxSteeringAngle;
double newSteeringAngle;
simulator_mainController_steerController1_steeringCalculator_distanceCalculator distanceCalculator;
void init()
{
distanceCalculator.init();
}
void execute()
{
distanceCalculator.x[0] = x[0];
distanceCalculator.x[1] = x[1];
distanceCalculator.y[0] = y[0];
distanceCalculator.y[1] = y[1];
distanceCalculator.gpsX = gpsX;
distanceCalculator.gpsY = gpsY;
distanceCalculator.execute();
double distance;
double res = (y[2/1 -1]-y[1/1 -1])*gpsX;
res -= (x[2/1 -1]-x[1/1 -1])*gpsY;
res += x[2/1 -1]*y[1/1 -1];
res -= y[2/1 -1]*x[1/1 -1];
double xDiff = x[1/1 -1]-x[2/1 -1];
double yDiff = y[1/1 -1]-y[2/1 -1];
res /= (Helper::getDoubleFromOctaveListFirstResult(Fsqrt(Helper::convertToOctaveValueList(xDiff*xDiff+yDiff*yDiff),1)));
distance = res;
double globalOrientation = orientation*(M_PI/180/1 );
if(((globalOrientation > M_PI))){
globalOrientation -= 2/1 *M_PI;
}
double orientedDistance = distanceCalculator.distance;
double orientedDistance = distance;
double angleTowardsTrajectory = (Helper::getDoubleFromOctaveListFirstResult(Fatan(Helper::convertToOctaveValueList(orientedDistance/2/1 ),1)));
double orientationOfTrajectory;
double v1 = x[2/1 -1]-x[1/1 -1];
......@@ -56,6 +57,14 @@ else if(((finalAngle < -M_PI))){
finalAngle += 2/1 *M_PI;
}
newSteeringAngle = finalAngle*(180/1 /M_PI);
angle = newSteeringAngle;
if((angle < minSteeringAngle)){
angle = minSteeringAngle;
}
else if((angle > maxSteeringAngle)){
angle = maxSteeringAngle;
}
newSteeringAngle = -angle;
}
};
......
#ifndef SIMULATOR_MAINCONTROLLER_STEERCONTROLLER1_STEERINGCALCULATOR_DISTANCECALCULATOR
#define SIMULATOR_MAINCONTROLLER_STEERCONTROLLER1_STEERINGCALCULATOR_DISTANCECALCULATOR
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "Helper.h"
#include "octave/builtin-defun-decls.h"
class simulator_mainController_steerController1_steeringCalculator_distanceCalculator{
public:
double x[2];
double y[2];
double gpsX;
double gpsY;
double distance;
void init()
{
}
void execute()
{
double res = (y[2/1 -1]-y[1/1 -1])*gpsX;
res -= (x[2/1 -1]-x[1/1 -1])*gpsY;
res += x[2/1 -1]*y[1/1 -1];
res -= y[2/1 -1]*x[1/1 -1];
double xDiff = x[1/1 -1]-x[2/1 -1];
double yDiff = y[1/1 -1]-y[2/1 -1];
res /= (Helper::getDoubleFromOctaveListFirstResult(Fsqrt(Helper::convertToOctaveValueList(xDiff*xDiff+yDiff*yDiff),1)));
distance = res;
}
};
#endif
package simulator;
component DistanceToTrajectoryCalculator{
ports in Q(-90°:90°) x[2],
in Q(-180°:180°) y[2],
in Q(-90°:90°) gpsX,
in Q(-180°:180°) gpsY,
out Q(-1000 m : 1000m) distance;
implementation Math{
Q res =(y(2) - y(1))*gpsX;
res -= (x(2) - x(1))*gpsY;
res += x(2) * y(1);
res -= y(2) * x(1);
Q xDiff = x(1) - x(2);
Q yDiff = y(1) - y(2);
res /= sqrt(xDiff*xDiff+yDiff*yDiff);
distance = res;
}
}
\ No newline at end of file
......@@ -18,18 +18,8 @@ component SteerController{
connect gpsX -> steeringCalculator.gpsX;
connect gpsY -> steeringCalculator.gpsY;
connect orientation -> steeringCalculator.orientation;
connect minSteeringAngle -> steeringCalculator.minSteeringAngle;
connect maxSteeringAngle -> steeringCalculator.maxSteeringAngle;
connect currentSteeringAngle -> steeringCalculator.currentSteeringAngle;
implementation Math{
Q angle = steeringCalculator.newSteeringAngle;
if angle < minSteeringAngle{
angle = minSteeringAngle;
}
elseif angle > maxSteeringAngle{
angle = maxSteeringAngle;
}
steeringAngle = -angle;
}
connect steeringCalculator.newSteeringAngle -> steeringAngle;
}
......@@ -7,54 +7,70 @@ component SteeringAngleCalculator{
in Q(-180°:180°) gpsY,
in Q(-180°:180°) orientation,
in Q(-180°:180°) currentSteeringAngle,
in Q(-180°:180°) minSteeringAngle,
in Q(-180°:180°) maxSteeringAngle,
out Q(-180°:180°) newSteeringAngle;
instance DistanceToTrajectoryCalculator distanceCalculator;
connect x[:] -> distanceCalculator.x[:];
connect y[:] -> distanceCalculator.y[:];
connect gpsX -> distanceCalculator.gpsX;
connect gpsY -> distanceCalculator.gpsY;
implementation Math{
Q globalOrientation = orientation*(M_PI/180);
if (globalOrientation > M_PI) {
globalOrientation -= 2 * M_PI;
}
//calculate distance
Q distance;
Q res =(y(2) - y(1))*gpsX;
res -= (x(2) - x(1))*gpsY;
res += x(2) * y(1);
res -= y(2) * x(1);
Q xDiff = x(1) - x(2);
Q yDiff = y(1) - y(2);
res /= sqrt(xDiff*xDiff+yDiff*yDiff);
Q orientedDistance = distanceCalculator.distance;
Q angleTowardsTrajectory = atan(orientedDistance / 2);
Q orientationOfTrajectory;
distance = res;
//calculate base steering angle
Q globalOrientation = orientation*(M_PI/180);
if (globalOrientation > M_PI) {
globalOrientation -= 2 * M_PI;
}
Q v1 = x(2) - x(1);
Q v2 = y(2) - y(1);
Q cosineAngle = v2 / sqrt(v1 * v1 + v2 * v2);
Q angle = acos(cosineAngle);
Q orientedDistance = distance;
Q angleTowardsTrajectory = atan(orientedDistance / 2);
Q orientationOfTrajectory;
if (v1 > 0) {
orientationOfTrajectory = -angle;
} else {
orientationOfTrajectory = angle;
}
end
Q angleTrajectoryAndCarDirection = orientationOfTrajectory - globalOrientation;
//the resulting angle is the angle needed to steer the car parallel to the trajectory
// plus the angle towards the trajectory
Q finalAngle = angleTrajectoryAndCarDirection + angleTowardsTrajectory;
Q v1 = x(2) - x(1);
Q v2 = y(2) - y(1);
Q cosineAngle = v2 / sqrt(v1 * v1 + v2 * v2);
Q angle = acos(cosineAngle);
if (v1 > 0) {
orientationOfTrajectory = -angle;
} else {
orientationOfTrajectory = angle;
}
end
Q angleTrajectoryAndCarDirection = orientationOfTrajectory - globalOrientation;
//the resulting angle is the angle needed to steer the car parallel to the trajectory
// plus the angle towards the trajectory
Q finalAngle = angleTrajectoryAndCarDirection + angleTowardsTrajectory;
//correct angle
if (finalAngle > M_PI){
finalAngle -= 2 * M_PI;
}elseif (finalAngle < -M_PI){
finalAngle += 2 * M_PI;
}
end
newSteeringAngle = finalAngle*(180/M_PI);
if (finalAngle > M_PI){
finalAngle -= 2 * M_PI;
}elseif (finalAngle < -M_PI){
finalAngle += 2 * M_PI;
//correct angle depending on car
angle = newSteeringAngle;
if angle < minSteeringAngle{
angle = minSteeringAngle;
}
elseif angle > maxSteeringAngle{
angle = maxSteeringAngle;
}
end
newSteeringAngle = finalAngle*(180/M_PI);
newSteeringAngle = -angle;
}
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment