/
cardifferential.cpp
118 lines (100 loc) · 3.97 KB
/
cardifferential.cpp
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
/************************************************************************/
/* */
/* This file is part of VDrift. */
/* */
/* VDrift is free software: you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation, either version 3 of the License, or */
/* (at your option) any later version. */
/* */
/* VDrift is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with VDrift. If not, see <http://www.gnu.org/licenses/>. */
/* */
/************************************************************************/
#include "cardifferential.h"
CARDIFFERENTIAL::CARDIFFERENTIAL() : final_drive(4.1), anti_slip(600.0), anti_slip_torque(0), anti_slip_torque_deceleration_factor(0), torque_split(0.5), side1_speed(0), side2_speed(0), side1_torque(0), side2_torque(0)
{
// Constructor.
}
void CARDIFFERENTIAL::DebugPrint(std::ostream & out) const
{
out << "---Differential---" << "\n";
out << "Side 1 RPM: " << side1_speed * 30.0 / 3.141593 << "\n";
out << "Side 2 RPM: " << side2_speed * 30.0 / 3.141593 << "\n";
out << "Side 1 Torque: " << side1_torque << "\n";
out << "Side 2 Torque: " << side2_torque << "\n";
}
void CARDIFFERENTIAL::SetFinalDrive(const btScalar & value)
{
final_drive = value;
}
void CARDIFFERENTIAL::SetAntiSlip(btScalar as, btScalar ast, btScalar astdf)
{
anti_slip = as;
anti_slip_torque = ast;
anti_slip_torque_deceleration_factor = astdf;
}
btScalar CARDIFFERENTIAL::CalculateDriveshaftSpeed(btScalar new_side1_speed, btScalar new_side2_speed)
{
side1_speed = new_side1_speed;
side2_speed = new_side2_speed;
return final_drive * (side1_speed + side2_speed) * 0.5;
}
btScalar CARDIFFERENTIAL::GetDriveshaftSpeed() const
{
return final_drive * (side1_speed + side2_speed) * 0.5;
}
btScalar CARDIFFERENTIAL::clamp(btScalar val, btScalar min, btScalar max) const
{
return std::max(std::min(val,max), min);
}
void CARDIFFERENTIAL::ComputeWheelTorques(btScalar driveshaft_torque)
{
// Determine torque from the anti-slip mechanism.
btScalar current_anti_slip = anti_slip;
// If torque sensitive.
if (anti_slip_torque > 0)
//TODO: add some minimum anti-slip.
current_anti_slip = anti_slip_torque*driveshaft_torque;
// Determine behavior for deceleration.
if (current_anti_slip < 0)
current_anti_slip *= -anti_slip_torque_deceleration_factor;
current_anti_slip = std::max(btScalar(0) ,current_anti_slip);
btScalar drag = clamp(current_anti_slip * (side1_speed - side2_speed),-anti_slip,anti_slip);
btScalar torque = driveshaft_torque * final_drive;
side1_torque = torque * (1 - torque_split) - drag;
side2_torque = torque * torque_split + drag;
}
const btScalar & CARDIFFERENTIAL::GetSide1Torque() const
{
return side1_torque;
}
const btScalar & CARDIFFERENTIAL::GetSide2Torque() const
{
return side2_torque;
}
const btScalar & CARDIFFERENTIAL::GetSide1Speed() const
{
return side1_speed;
}
const btScalar & CARDIFFERENTIAL::GetSide2Speed() const
{
return side2_speed;
}
btScalar CARDIFFERENTIAL::GetFinalDrive() const
{
return final_drive;
}
bool CARDIFFERENTIAL::Serialize(joeserialize::Serializer & s)
{
_SERIALIZE_(s, side1_speed);
_SERIALIZE_(s, side2_speed);
_SERIALIZE_(s, side1_torque);
_SERIALIZE_(s, side2_torque);
return true;
}