Skip to content

Commit b59733b

Browse files
ryo-0723ryo-0723
committed
v1.0.0_completed
1 parent 15aca67 commit b59733b

File tree

9 files changed

+402
-37
lines changed

9 files changed

+402
-37
lines changed

‎Wheel_Simulation/Wheel_Simulation/App/Wheel_Simulation.exe‎

47.2 MB
Binary file not shown.

‎Wheel_Simulation/Wheel_Simulation/Main.cpp‎

Lines changed: 61 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,54 +1,81 @@
11
#include <Siv3D.hpp> // OpenSiv3D v0.6.9
22
#include"Object.h"
3-
3+
#include"Steer.h"
4+
#include"PS5Controller.h"
45
void Main()
56
{
67
Window::SetStyle(WindowStyle::Sizable);
8+
Window::SetTitle(U"Wheel_Simulation");
79
Color color = Palette::White;
810
Object robot;
9-
11+
Steer steer;
12+
PS5Controller ps;
1013
Vec2 LeftAxis{ 0,0 };
1114
double InputTurn = 0;
12-
15+
double robotSpeed = 100;
1316
size_t controllerIndex = 0;
14-
bool stopped = true;
15-
for (const auto& info : System::EnumerateGamepads())
16-
{
17-
Print << U"[{}] {} ({:#x} {:#x})"_fmt(info.playerIndex, info.name, info.vendorID, info.productID);
18-
}
17+
size_t robotIndex = 0;
1918
while (System::Update())
2019
{
21-
SimpleGUI::RadioButtons(controllerIndex, { U"keybord",U"PS4 orPS5" },{10,100});
2220
ClearPrint();
23-
if (const auto gamepad = Gamepad(0)) {
24-
/*ps5だったらGamepad(0)*/
25-
/*ps4だったらGamepad(1)*/
26-
//const auto& info = gamepad.getInfo();
27-
if (controllerIndex) {
28-
LeftAxis.x = (int)(gamepad.axes.at(0) * 255);
29-
LeftAxis.y = -(int)(gamepad.axes.at(1) * 255);
30-
InputTurn = (int)(gamepad.axes.at(2) * 255);
31-
/*ps5だったらat(2)*/
32-
/*ps4だったらat(3)*/
33-
}
21+
22+
// "Licenses" ボタンが押されたら
23+
if (SimpleGUI::Button(U"Licenses", Vec2{ 10, 250 }))
24+
{
25+
// ライセンス情報を表示
26+
LicenseManager::ShowInBrowser();
3427
}
35-
if(controllerIndex == 0){
36-
LeftAxis.y = KeyW.pressed() * 40 - KeyS.pressed() * 40;
37-
LeftAxis.x = KeyD.pressed() * 40 - KeyA.pressed() * 40;
38-
InputTurn = KeyRight.pressed() * 40 - KeyLeft.pressed() * 40;
28+
switch (controllerIndex) {
29+
case 0:
30+
LeftAxis.y = KeyW.pressed() * 255 - KeyS.pressed() * 255;
31+
LeftAxis.x = KeyD.pressed() * 255 - KeyA.pressed() * 255;
32+
InputTurn = KeyRight.pressed() * 255 - KeyLeft.pressed() * 255;
33+
break;
34+
case 1:
35+
ps.Update(0);
36+
LeftAxis = ps.GetLeftAxis();
37+
InputTurn = ps.GetRightAxis().x;
38+
break;
3939
}
4040

41-
Print << Scene::DeltaTime() * 1000;
42-
41+
//Print << Scene::DeltaTime() * 1000;
4342

44-
robot.SetPower({ 0,0,0,0 }, {0,0,0,0}).draw(color);
4543

46-
//std::array<double, 4>Wheel;
47-
//Wheel[0] = LeftAxis.x + LeftAxis.y + InputTurn;
48-
//Wheel[1] = LeftAxis.x - LeftAxis.y + InputTurn;
49-
//Wheel[2] = -LeftAxis.x - LeftAxis.y + InputTurn;
50-
//Wheel[3] = -LeftAxis.x + LeftAxis.y + InputTurn;
51-
///*オムニ用角度*/
52-
// robot.SetPower(Wheel, { 45,135,225,315 }).draw(color);
44+
if (robotIndex) {
45+
steer.Update(LeftAxis, InputTurn, 0);
46+
steer.Allow();
47+
std::array<double, 4>angle = { steer.GetPower(place::LF).x
48+
,steer.GetPower(place::RF).x
49+
,steer.GetPower(place::RB).x
50+
,steer.GetPower(place::LB).x };
51+
std::array<double, 4>length = { steer.GetPower(place::LF).y
52+
,steer.GetPower(place::RF).y
53+
,steer.GetPower(place::RB).y
54+
,steer.GetPower(place::LB).y
55+
};
56+
robot.SetPower(length, angle).draw(color);
57+
}
58+
else {
59+
std::array<double, 4>Wheel;
60+
Wheel[0] = LeftAxis.x + LeftAxis.y + InputTurn;
61+
Wheel[1] = LeftAxis.x - LeftAxis.y + InputTurn;
62+
Wheel[2] = -LeftAxis.x - LeftAxis.y + InputTurn;
63+
Wheel[3] = -LeftAxis.x + LeftAxis.y + InputTurn;
64+
const auto max = std::max(abs(*std::max_element(Wheel.begin(), Wheel.end()))
65+
, abs(*std::min_element(Wheel.begin(), Wheel.end())));
66+
double MAX = robotSpeed;
67+
if (max > MAX) {
68+
double maximum = MAX / max;
69+
for (int i = 0; i < 4; i++) {
70+
Wheel[i] *= maximum;
71+
}
72+
}
73+
/*オムニ用角度*/
74+
robot.SetPower(Wheel, { 45,135,225,315 }).draw(color);
75+
}
76+
SimpleGUI::RadioButtons(controllerIndex, { U"keybord",U"PS5" }, { 10,90 });
77+
SimpleGUI::RadioButtons(robotIndex, { U"Omni",U"Steering" }, { 10,170 });
78+
SimpleGUI::Slider(U"Speed{:.2f}"_fmt(robotSpeed), robotSpeed, 0, 255, Vec2{10, 50},130.0,150.0);
79+
steer.SetPower(robotSpeed);
5380
}
5481
}

‎Wheel_Simulation/Wheel_Simulation/Object.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ Object& Object::draw(const Color& Framecolor=Palette::White) {
2828
, 6
2929
, Vec2{ 8,8 }).draw();
3030
}
31-
SimpleGUI::CheckBox(moveMode,U"ロボットの固定",Vec2{10,60});
31+
SimpleGUI::CheckBox(moveMode,U"ロボットの固定",Vec2{10,10});
3232
return *this;
3333
}
3434

‎Wheel_Simulation/Wheel_Simulation/PS5Controller.cpp‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
#include "stdafx.h"
2+
#include "PS5Controller.h"
3+
4+
void PS5Controller::Update(size_t playerIndex) {
5+
if (playerIndex == 0) {
6+
if (const auto gamepad = Gamepad(playerIndex)) {
7+
/*ps5だったらGamepad(0)*/
8+
/*ps4だったらGamepad(1)*/
9+
//const auto& info = gamepad.getInfo();
10+
leftAxis.x = gamepad.axes.at(0) * 255.00;
11+
leftAxis.y = -gamepad.axes.at(1) * 255.00;
12+
rightAxis.x = gamepad.axes.at(2) * 255.00;
13+
/*ps5だったらat(2)*/
14+
/*ps4だったらat(3)*/
15+
}
16+
}
17+
else if (playerIndex == 1) {
18+
if (const auto gamepad = Gamepad(playerIndex)) {
19+
leftAxis.x = gamepad.axes.at(0) * 255.00;
20+
leftAxis.y = -gamepad.axes.at(1) * 255.00;
21+
rightAxis.x = gamepad.axes.at(3) * 255.00;
22+
}
23+
}
24+
25+
if (leftAxis.x > cutValue) {
26+
leftAxis.x = map(leftAxis.x, cutValue, 255, 0, 255);
27+
}else if (leftAxis.x < -cutValue) {
28+
leftAxis.x = map(leftAxis.x, -cutValue, -255, 0, -255);
29+
}
30+
else {
31+
leftAxis.x = 0;
32+
}
33+
34+
if (leftAxis.y > cutValue) {
35+
leftAxis.y = map(leftAxis.y, cutValue, 255, 0, 255);
36+
}
37+
else if (leftAxis.y < -cutValue) {
38+
leftAxis.y = map(leftAxis.y, -cutValue, -255, 0, -255);
39+
}
40+
else {
41+
leftAxis.y = 0;
42+
}
43+
44+
45+
if (rightAxis.x > cutValue) {
46+
rightAxis.x = map(rightAxis.x, cutValue, 255, 0, 255);
47+
}
48+
else if (rightAxis.x < -cutValue) {
49+
rightAxis.x = map(rightAxis.x, -cutValue, -255, 0, -255);
50+
}
51+
else {
52+
rightAxis.x = 0;
53+
}
54+
55+
if (rightAxis.y > cutValue) {
56+
rightAxis.y = map(rightAxis.y, cutValue, 255, 0, 255);
57+
}
58+
else if (rightAxis.y < -cutValue) {
59+
rightAxis.y = map(rightAxis.y, -cutValue, -255, 0, -255);
60+
}
61+
else {
62+
rightAxis.y = 0;
63+
}
64+
}
65+
66+
67+
68+
Vec2 PS5Controller::GetLeftAxis() {
69+
return leftAxis;
70+
}
71+
72+
Vec2 PS5Controller::GetRightAxis() {
73+
return rightAxis;
74+
}
75+

‎Wheel_Simulation/Wheel_Simulation/PS5Controller.h‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
#pragma once
2+
class PS5Controller
3+
{
4+
private:
5+
int cutValue=20;
6+
Vec2 leftAxis = {};
7+
Vec2 rightAxis = {};
8+
9+
public:
10+
PS5Controller() {}
11+
12+
13+
void Update(size_t playerIndex);
14+
15+
Vec2 GetLeftAxis();
16+
17+
Vec2 GetRightAxis();
18+
19+
double map(double x, double in_min, double in_max, double out_min, double out_max) {
20+
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
21+
}
22+
};
23+

‎Wheel_Simulation/Wheel_Simulation/Steer.cpp‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
#include "stdafx.h"
2+
#include "Steer.h"
3+
4+
void Steer::Update(Vec2 Axis, double z, double gyro) {
5+
/*とりあえずジャイロのことは加味せずに計算していく*/
6+
/*z（旋回のベクトル）を(0,z)とすることでy軸と同じ方向になるベクトルが作れる*/
7+
//LFから時計回りにタイヤの計算
8+
/*
9+
[0]LF　 ↑ [1]RF
10+
|￣￣￣￣￣￣|
11+
| |
12+
| |
13+
| |
14+
| |
15+
|____________|
16+
[3]LB [2]RB
17+
*/
18+
std::array<Vec2, 4> data{};//計算結果一時保存場所
19+
20+
for (int i = 0; i < 4; i++) {//タイヤの出力計算
21+
data[i] = Axis + Vec2{ 0,z }.rotate((i * -0.5 - 0.25) * Math::Pi);/*Vec2*/
22+
//Tire[i].Update(ToDegrees(data[i].yx().rotated(-0.5 * Math::Pi).getAngle()),
23+
//(double)data[i].length());
24+
25+
Tire[i].UpdateRev(ToDegrees(data[i].yx().rotated(-0.5 * Math::Pi).getAngle()),
26+
(double)data[i].length());
27+
}
28+
29+
/*PWMの最大値をとる*/
30+
const auto max = (*std::max_element(Tire.begin(), Tire.end(), [](const Wheel& lhs, const Wheel& rhs) {
31+
return abs(lhs.length) < abs(rhs.length);})).length;
32+
if (max > MAX) {
33+
double maximum = MAX / max;
34+
for (int i = 0; i < 4; i++) {
35+
Tire[i].length *= maximum;
36+
}
37+
}
38+
}
39+
40+
void Steer::Show() {
41+
//for(int i=0;i<4;++i)
42+
//Print << U"angle :"<< Tire[i].GetAngle();
43+
//for(int i=0;i<4;++i)
44+
//Print << U"length :"<< Tire[i].GetLength();
45+
}
46+
47+
void Steer::Stop() {
48+
for (int i = 0; i < 4;++i) {
49+
Tire[i].Stop();
50+
}
51+
}
52+
void Steer::Allow() {
53+
for (int i = 0; i < 4; ++i) {
54+
Tire[i].Allow();
55+
}
56+
}
57+
58+
Vec2 Steer::GetPower(place unit) {
59+
return { Tire[static_cast<int>(unit)].GetAngle(),Tire[static_cast<int>(unit)].GetLength() };
60+
}
61+
62+
void Steer::Culibration() {
63+
64+
}
65+
66+
void Steer::SetPower(double power) {
67+
MAX = power;
68+
}

0 commit comments

Comments
 (0)