engine-sim/src/transmission.cpp at e55992d28c69bfc5982250e3802accf6c8e3fb3e · elevatorguy/engine-sim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "../include/transmission.h"

#include "../include/units.h"

#include <cmath>

Transmission::Transmission() {
    m_gear = -1;
    m_newGear = -1;
    m_gearCount = 0;
    m_gearRatios = nullptr;
    m_maxClutchTorque = units::torque(1000.0, units::ft_lb);
    m_rotatingMass = nullptr;
    m_vehicle = nullptr;
    m_clutchPressure = 0.0;
    m_diskPosition = 1.0;
    m_engine = nullptr;
    maySlide = false;
    m_diskMin = 0.7;
    m_diskMax = 3.1;
}

Transmission::~Transmission() {
    if (m_gearRatios != nullptr) {
        delete[] m_gearRatios;
    }

    m_gearRatios = nullptr;
}

void Transmission::initialize(const Parameters &params) {
    m_gearCount = params.GearCount;
    m_maxClutchTorque = params.MaxClutchTorque;
    m_gearRatios = new double[params.GearCount];
    memcpy(m_gearRatios, params.GearRatios, sizeof(double) * m_gearCount);
    if (params.GearCount > 1)
    {
        m_diskMax = m_gearRatios[0];
        m_diskMin = m_gearRatios[params.GearCount - 1];
    }
    if (params.variable) {
        enableSlidingDisk();
    }
}

void Transmission::update(double dt) {
    if (m_gear == -1) {
        m_clutchConstraint.m_minTorque = 0;
        m_clutchConstraint.m_maxTorque = 0;
    }
    else {
        m_clutchConstraint.m_minTorque = -m_maxClutchTorque * m_clutchPressure;
        m_clutchConstraint.m_maxTorque = m_maxClutchTorque * m_clutchPressure;
    }

    if (maySlide) {
        slideGear();
    }
}

void Transmission::addToSystem(
    atg_scs::RigidBodySystem *system,
    atg_scs::RigidBody *rotatingMass,
    Vehicle *vehicle,
    Engine *engine)
{
    m_rotatingMass = rotatingMass;
    m_vehicle = vehicle;

    m_clutchConstraint.setBody1(&engine->getOutputCrankshaft()->m_body);
    m_clutchConstraint.setBody2(m_rotatingMass);

    system->addConstraint(&m_clutchConstraint);

    m_engine = engine;
}

void Transmission::enableSlidingDisk() {
    maySlide = true;
    if(m_gear > -1) {
        m_gear = 0;
    }
}

void Transmission::changeGear(int newGear) {
    if (maySlide && newGear > 0) return;
    if (newGear < -1 || newGear >= m_gearCount) return;
    else if (newGear != -1) {
        const double m_car = m_vehicle->getMass();
        const double gear_ratio = m_gearRatios[newGear];
        const double diff_ratio = m_vehicle->getDiffRatio();
        const double tire_radius = m_vehicle->getTireRadius();
        const double f = tire_radius / (diff_ratio * gear_ratio);

        const double new_I = m_car * f * f;
        const double E_r =
            0.5 * m_rotatingMass->I * m_rotatingMass->v_theta * m_rotatingMass->v_theta;
        const double new_v_theta = m_rotatingMass->v_theta < 0
            ? -std::sqrt(E_r * 2 / new_I)
            : std::sqrt(E_r * 2 / new_I);

        m_rotatingMass->I = new_I;
        m_rotatingMass->p_x = m_rotatingMass->p_y = 0;
        m_rotatingMass->m = m_car;
        m_rotatingMass->v_theta = new_v_theta;
    }

    m_gear = newGear;
}

void Transmission::slideGear(void) {
    double current_rpm = m_engine->getRpm();
    double redline = m_engine->getRedline(); //todo: algorithm

    const double m_car = m_vehicle->getMass();
    const double diff_ratio = m_vehicle->getDiffRatio();
    const double tire_radius = m_vehicle->getTireRadius();
    const double f = tire_radius / (diff_ratio * (clamp(m_diskPosition)*(m_diskMax - m_diskMin) + m_diskMin));

    const double new_I = m_car * f * f;
    const double E_r = 0.5 * m_rotatingMass->I * m_rotatingMass->v_theta * m_rotatingMass->v_theta;
    const double new_v_theta = m_rotatingMass->v_theta < 0 ? -std::sqrt(E_r * 2 / new_I) : std::sqrt(E_r * 2 / new_I);

    m_rotatingMass->I = new_I;
    m_rotatingMass->p_x = m_rotatingMass->p_y = 0;
    m_rotatingMass->m = m_car;
    m_rotatingMass->v_theta = new_v_theta;
}