diff --git a/Wheel_Simulation/Wheel_Simulation/App/Wheel_Simulation.exe b/Wheel_Simulation/Wheel_Simulation/App/Wheel_Simulation.exe new file mode 100644 index 0000000..c6bff9e Binary files /dev/null and b/Wheel_Simulation/Wheel_Simulation/App/Wheel_Simulation.exe differ diff --git a/Wheel_Simulation/Wheel_Simulation/Main.cpp b/Wheel_Simulation/Wheel_Simulation/Main.cpp index d38f13c..155612b 100644 --- a/Wheel_Simulation/Wheel_Simulation/Main.cpp +++ b/Wheel_Simulation/Wheel_Simulation/Main.cpp @@ -1,54 +1,81 @@ #include // OpenSiv3D v0.6.9 #include"Object.h" - +#include"Steer.h" +#include"PS5Controller.h" void Main() { Window::SetStyle(WindowStyle::Sizable); + Window::SetTitle(U"Wheel_Simulation"); Color color = Palette::White; Object robot; - + Steer steer; + PS5Controller ps; Vec2 LeftAxis{ 0,0 }; double InputTurn = 0; - + double robotSpeed = 100; size_t controllerIndex = 0; - bool stopped = true; - for (const auto& info : System::EnumerateGamepads()) - { - Print << U"[{}] {} ({:#x} {:#x})"_fmt(info.playerIndex, info.name, info.vendorID, info.productID); - } + size_t robotIndex = 0; while (System::Update()) { - SimpleGUI::RadioButtons(controllerIndex, { U"keybord",U"PS4 orPS5" },{10,100}); ClearPrint(); - if (const auto gamepad = Gamepad(0)) { - /*ps5だったらGamepad(0)*/ - /*ps4だったらGamepad(1)*/ - //const auto& info = gamepad.getInfo(); - if (controllerIndex) { - LeftAxis.x = (int)(gamepad.axes.at(0) * 255); - LeftAxis.y = -(int)(gamepad.axes.at(1) * 255); - InputTurn = (int)(gamepad.axes.at(2) * 255); - /*ps5だったらat(2)*/ - /*ps4だったらat(3)*/ - } + + // "Licenses" ボタンが押されたら + if (SimpleGUI::Button(U"Licenses", Vec2{ 10, 250 })) + { + // ライセンス情報を表示 + LicenseManager::ShowInBrowser(); } - if(controllerIndex == 0){ - LeftAxis.y = KeyW.pressed() * 40 - KeyS.pressed() * 40; - LeftAxis.x = KeyD.pressed() * 40 - KeyA.pressed() * 40; - InputTurn = KeyRight.pressed() * 40 - KeyLeft.pressed() * 40; + switch (controllerIndex) { + case 0: + LeftAxis.y = KeyW.pressed() * 255 - KeyS.pressed() * 255; + LeftAxis.x = KeyD.pressed() * 255 - KeyA.pressed() * 255; + InputTurn = KeyRight.pressed() * 255 - KeyLeft.pressed() * 255; + break; + case 1: + ps.Update(0); + LeftAxis = ps.GetLeftAxis(); + InputTurn = ps.GetRightAxis().x; + break; } - Print << Scene::DeltaTime() * 1000; - + //Print << Scene::DeltaTime() * 1000; - robot.SetPower({ 0,0,0,0 }, {0,0,0,0}).draw(color); - //std::arrayWheel; - //Wheel[0] = LeftAxis.x + LeftAxis.y + InputTurn; - //Wheel[1] = LeftAxis.x - LeftAxis.y + InputTurn; - //Wheel[2] = -LeftAxis.x - LeftAxis.y + InputTurn; - //Wheel[3] = -LeftAxis.x + LeftAxis.y + InputTurn; - ///*オムニ用角度*/ - // robot.SetPower(Wheel, { 45,135,225,315 }).draw(color); + if (robotIndex) { + steer.Update(LeftAxis, InputTurn, 0); + steer.Allow(); + std::arrayangle = { steer.GetPower(place::LF).x + ,steer.GetPower(place::RF).x + ,steer.GetPower(place::RB).x + ,steer.GetPower(place::LB).x }; + std::arraylength = { steer.GetPower(place::LF).y + ,steer.GetPower(place::RF).y + ,steer.GetPower(place::RB).y + ,steer.GetPower(place::LB).y + }; + robot.SetPower(length, angle).draw(color); + } + else { + std::arrayWheel; + Wheel[0] = LeftAxis.x + LeftAxis.y + InputTurn; + Wheel[1] = LeftAxis.x - LeftAxis.y + InputTurn; + Wheel[2] = -LeftAxis.x - LeftAxis.y + InputTurn; + Wheel[3] = -LeftAxis.x + LeftAxis.y + InputTurn; + const auto max = std::max(abs(*std::max_element(Wheel.begin(), Wheel.end())) + , abs(*std::min_element(Wheel.begin(), Wheel.end()))); + double MAX = robotSpeed; + if (max > MAX) { + double maximum = MAX / max; + for (int i = 0; i < 4; i++) { + Wheel[i] *= maximum; + } + } + /*オムニ用角度*/ + robot.SetPower(Wheel, { 45,135,225,315 }).draw(color); + } + SimpleGUI::RadioButtons(controllerIndex, { U"keybord",U"PS5" }, { 10,90 }); + SimpleGUI::RadioButtons(robotIndex, { U"Omni",U"Steering" }, { 10,170 }); + SimpleGUI::Slider(U"Speed{:.2f}"_fmt(robotSpeed), robotSpeed, 0, 255, Vec2{10, 50},130.0,150.0); + steer.SetPower(robotSpeed); } } diff --git a/Wheel_Simulation/Wheel_Simulation/Object.cpp b/Wheel_Simulation/Wheel_Simulation/Object.cpp index ce2e417..1555071 100644 --- a/Wheel_Simulation/Wheel_Simulation/Object.cpp +++ b/Wheel_Simulation/Wheel_Simulation/Object.cpp @@ -28,7 +28,7 @@ Object& Object::draw(const Color& Framecolor=Palette::White) { , 6 , Vec2{ 8,8 }).draw(); } - SimpleGUI::CheckBox(moveMode,U"ロボットの固定",Vec2{10,60}); + SimpleGUI::CheckBox(moveMode,U"ロボットの固定",Vec2{10,10}); return *this; } diff --git a/Wheel_Simulation/Wheel_Simulation/PS5Controller.cpp b/Wheel_Simulation/Wheel_Simulation/PS5Controller.cpp new file mode 100644 index 0000000..6e87813 --- /dev/null +++ b/Wheel_Simulation/Wheel_Simulation/PS5Controller.cpp @@ -0,0 +1,75 @@ +#include "stdafx.h" +#include "PS5Controller.h" + +void PS5Controller::Update(size_t playerIndex) { + if (playerIndex == 0) { + if (const auto gamepad = Gamepad(playerIndex)) { + /*ps5だったらGamepad(0)*/ + /*ps4だったらGamepad(1)*/ + //const auto& info = gamepad.getInfo(); + leftAxis.x = gamepad.axes.at(0) * 255.00; + leftAxis.y = -gamepad.axes.at(1) * 255.00; + rightAxis.x = gamepad.axes.at(2) * 255.00; + /*ps5だったらat(2)*/ + /*ps4だったらat(3)*/ + } + } + else if (playerIndex == 1) { + if (const auto gamepad = Gamepad(playerIndex)) { + leftAxis.x = gamepad.axes.at(0) * 255.00; + leftAxis.y = -gamepad.axes.at(1) * 255.00; + rightAxis.x = gamepad.axes.at(3) * 255.00; + } + } + + if (leftAxis.x > cutValue) { + leftAxis.x = map(leftAxis.x, cutValue, 255, 0, 255); + }else if (leftAxis.x < -cutValue) { + leftAxis.x = map(leftAxis.x, -cutValue, -255, 0, -255); + } + else { + leftAxis.x = 0; + } + + if (leftAxis.y > cutValue) { + leftAxis.y = map(leftAxis.y, cutValue, 255, 0, 255); + } + else if (leftAxis.y < -cutValue) { + leftAxis.y = map(leftAxis.y, -cutValue, -255, 0, -255); + } + else { + leftAxis.y = 0; + } + + + if (rightAxis.x > cutValue) { + rightAxis.x = map(rightAxis.x, cutValue, 255, 0, 255); + } + else if (rightAxis.x < -cutValue) { + rightAxis.x = map(rightAxis.x, -cutValue, -255, 0, -255); + } + else { + rightAxis.x = 0; + } + + if (rightAxis.y > cutValue) { + rightAxis.y = map(rightAxis.y, cutValue, 255, 0, 255); + } + else if (rightAxis.y < -cutValue) { + rightAxis.y = map(rightAxis.y, -cutValue, -255, 0, -255); + } + else { + rightAxis.y = 0; + } +} + + + +Vec2 PS5Controller::GetLeftAxis() { + return leftAxis; +} + +Vec2 PS5Controller::GetRightAxis() { + return rightAxis; +} + diff --git a/Wheel_Simulation/Wheel_Simulation/PS5Controller.h b/Wheel_Simulation/Wheel_Simulation/PS5Controller.h new file mode 100644 index 0000000..20d3b4e --- /dev/null +++ b/Wheel_Simulation/Wheel_Simulation/PS5Controller.h @@ -0,0 +1,23 @@ +#pragma once +class PS5Controller +{ +private: + int cutValue=20; + Vec2 leftAxis = {}; + Vec2 rightAxis = {}; + +public: + PS5Controller() {} + + + void Update(size_t playerIndex); + + Vec2 GetLeftAxis(); + + Vec2 GetRightAxis(); + + double map(double x, double in_min, double in_max, double out_min, double out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } +}; + diff --git a/Wheel_Simulation/Wheel_Simulation/Steer.cpp b/Wheel_Simulation/Wheel_Simulation/Steer.cpp new file mode 100644 index 0000000..d94e5ff --- /dev/null +++ b/Wheel_Simulation/Wheel_Simulation/Steer.cpp @@ -0,0 +1,68 @@ +#include "stdafx.h" +#include "Steer.h" + +void Steer::Update(Vec2 Axis, double z, double gyro) { + /*とりあえずジャイロのことは加味せずに計算していく*/ + /*z(旋回のベクトル)を(0,z)とすることでy軸と同じ方向になるベクトルが作れる*/ + //LFから時計回りにタイヤの計算 + /* + [0]LF  ↑ [1]RF + | ̄ ̄ ̄ ̄ ̄ ̄| + | | + | | + | | + | | + |____________| + [3]LB [2]RB + */ + std::array data{};//計算結果一時保存場所 + + for (int i = 0; i < 4; i++) {//タイヤの出力計算 + data[i] = Axis + Vec2{ 0,z }.rotate((i * -0.5 - 0.25) * Math::Pi);/*Vec2*/ + //Tire[i].Update(ToDegrees(data[i].yx().rotated(-0.5 * Math::Pi).getAngle()), + //(double)data[i].length()); + + Tire[i].UpdateRev(ToDegrees(data[i].yx().rotated(-0.5 * Math::Pi).getAngle()), + (double)data[i].length()); + } + + /*PWMの最大値をとる*/ + const auto max = (*std::max_element(Tire.begin(), Tire.end(), [](const Wheel& lhs, const Wheel& rhs) { + return abs(lhs.length) < abs(rhs.length);})).length; + if (max > MAX) { + double maximum = MAX / max; + for (int i = 0; i < 4; i++) { + Tire[i].length *= maximum; + } + } +} + +void Steer::Show() { + //for(int i=0;i<4;++i) + //Print << U"angle :"<< Tire[i].GetAngle(); + //for(int i=0;i<4;++i) + //Print << U"length :"<< Tire[i].GetLength(); +} + +void Steer::Stop() { + for (int i = 0; i < 4;++i) { + Tire[i].Stop(); + } +} +void Steer::Allow() { + for (int i = 0; i < 4; ++i) { + Tire[i].Allow(); + } +} + +Vec2 Steer::GetPower(place unit) { + return { Tire[static_cast(unit)].GetAngle(),Tire[static_cast(unit)].GetLength() }; +} + +void Steer::Culibration() { + +} + +void Steer::SetPower(double power) { + MAX = power; +} diff --git a/Wheel_Simulation/Wheel_Simulation/Steer.h b/Wheel_Simulation/Wheel_Simulation/Steer.h new file mode 100644 index 0000000..2b7acfe --- /dev/null +++ b/Wheel_Simulation/Wheel_Simulation/Steer.h @@ -0,0 +1,155 @@ +#pragma once + +enum class place { + LF, + RF, + RB, + LB +}; + +class Steer +{ +private: + /*ベクトルの計算後のデータ保存場所*/ + struct Wheel { + double length, angle, oldAngle, newOffset,oldOffset; + int newRevLength = 1, oldRevLength = 1;/*モーターの値を反転するかの変数*/ + bool stopFlag = false; + + /// @brief -double--+doubleの範囲に変更する関数 + void Update(double newAngle, double newLength) { + angle = newAngle; + length = newLength; + /*nanのままいったん計算*/ + /*無限増加を防ぐために違う変数を作る*/ + double overAngle = angle + 180.00; + double oldOverAngle = oldAngle + 180.00; + + /*nanとの比較演算子は常にfalseが出るらしい*/ + /*nan判定の前にこれを行うことでバグはなくなるのでは?*/ + if (oldOverAngle - overAngle > 330 && oldOverAngle - overAngle <= 360) + /*変化量がほぼ一周分になるときは一周したと判定する*/ + newOffset += 360; + if (oldOverAngle - overAngle < -330 && oldOverAngle - overAngle >= -360) + /*変化量がほぼ一周分になるときは一周したと判定する*/ + newOffset -= 360; + + /*nanをなくす*/ + if (IsNaN(angle))/*角度がnanのときは前周期の角度のままにする*/ + angle = oldAngle; + + /*最初はタイヤの回転角度は0から359までの範囲にする*/ + /*最大の許容値を400としておいて、回転量が少ないときは小さく回転し、回転量が多いときにタイヤを反転して許容値から離す*/ + if (GetAngle() >= 350) { + if (GetAngle() > 400)/*絶対反転角---個の角度になったときは必ず反転をする*/ + newOffset -= 360.00; + if (angle - oldAngle > 20)/*指定量より動く回転の量が多いときタイヤをあるときのみ360回転させる*/ + newOffset -= 360.00; + } + if (GetAngle() <= 10) { + if (GetAngle() < -40) /*絶対反転角---個の角度になったときは必ず反転をする*/ + newOffset += 360.00; + if (angle - oldAngle < -20)/*指定量より動く回転の量が多いときタイヤをあるときのみ360回転させる*/ + newOffset += 360.00; + } + } + void UpdateRev(double newAngle,double newLength) {/*ステアの最適化制御*/ + oldAngle = angle; + oldOffset = newOffset; + oldRevLength = newRevLength; + if (stopFlag) { + angle = newAngle; + length = newLength; + double overAngle = angle + 180.00; + double oldOverAngle = oldAngle + 180.00; + + /*nanとの比較演算子は常にfalseが出るらしい*/ + /*nan判定の前にこれを行うことでバグはなくなるのでは?*/ + if (oldOverAngle - overAngle > 330.00 && oldOverAngle - overAngle <= 360.00) + /*変化量がほぼ一周分になるときは一周したと判定する*/ + newOffset += 360.00; + if (oldOverAngle - overAngle < -330.00 && oldOverAngle - overAngle >= -360.00) + /*変化量がほぼ一周分になるときは一周したと判定する*/ + newOffset -= 360.00; + + /*nanをなくす*/ + if (IsNaN(angle))/*角度がnanのときは前周期の角度のままにする*/ + angle = oldAngle; + + /*回転量を最小にするための計算 +  どっち回転が速いかの判定後計算する + */ + if (GetAngle() - GetOldAngle() > GetOldAngle() - GetAngle()) { + if (GetAngle() - GetOldAngle() > 90.00 && GetAngle() - GetOldAngle() <= 270.00) { + newOffset -= 180.00; + newRevLength *= -1; + } + else if (GetAngle() - GetOldAngle() > 270.00 && GetAngle() - GetOldAngle() <= 360.00) { + newOffset -= 360; + } + } + else { + if (GetAngle() - GetOldAngle() < -90.00 && GetAngle() - GetOldAngle() >= -270.00) { + newOffset += 180.00; + newRevLength *= -1; + } + else if (GetAngle() - GetOldAngle() < -270.00 && GetAngle() - GetOldAngle() >= -360.00) { + newOffset += 360; + } + } + } + else { + angle = oldAngle; + newOffset =oldOffset; + length = 0; + newRevLength =oldRevLength; + } + } + /// @brief タイヤの現在角度を返す関数 + /// @return タイヤの現在角度 + double GetAngle() { + return angle + newOffset; + } + /// @brief タイヤの全周期の角度を返す関数 + /// @return 前周期の角度 + double GetOldAngle() { + return oldAngle + oldOffset; + } + /// @brief モータのPWMを返す関数 + /// @return 回転モーターの出力値 + double GetLength() { + return length * newRevLength; + } + void Stop() { + stopFlag = false; + } + void Allow() { + stopFlag = true; + } + + /*タイヤの角度はゼロ点を取った瞬間は50度*/ + void SetAngle(double sets) { + newOffset = sets-angle; + } + }; + + std::array Tire{}; + double MAX = 0; +public: + Steer() = default; + + void Update(Vec2 Axis, double z, double gyro); + + void Stop(); + + void Allow(); + + void Show(); + + Vec2 GetPower(place unit); + + void Culibration(); + + void SetPower(double power); +}; + diff --git a/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj b/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj index 29121c9..e910518 100644 --- a/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj +++ b/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj @@ -57,8 +57,8 @@ $(SolutionDir)Intermediate\$(ProjectName)\Release\ $(SolutionDir)Intermediate\$(ProjectName)\Release\Intermediate\ $(ProjectDir)App - $(SIV3D_0_6_8)\include;$(SIV3D_0_6_8)\include\ThirdParty;$(IncludePath) - $(SIV3D_0_6_8)\lib\Windows;$(LibraryPath) + $(SIV3D_0_6_9)\include;$(SIV3D_0_6_9)\include\ThirdParty;$(IncludePath) + $(SIV3D_0_6_9)\lib\Windows;$(LibraryPath) @@ -101,6 +101,7 @@ /Zc:__cplusplus %(AdditionalOptions) Use stdafx.h;%(ForcedIncludeFiles) + false true @@ -117,10 +118,12 @@ + Create Create + @@ -318,7 +321,9 @@ + + diff --git a/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj.filters b/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj.filters index 7d61aeb..c5277fb 100644 --- a/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj.filters +++ b/Wheel_Simulation/Wheel_Simulation/Wheel_Simulation.vcxproj.filters @@ -150,6 +150,12 @@ Source Files + + Source Files + + + Source Files + @@ -788,5 +794,11 @@ Header Files + + Header Files + + + Header Files + \ No newline at end of file