From 3b53f84c8fb5901c67dbfb25aa3a5f98df5adacc Mon Sep 17 00:00:00 2001 From: Alex Henning Date: Thu, 6 Nov 2014 18:54:30 -0500 Subject: [PATCH] Fixes for 0 based joysticks and joystick axes in simulation. Change-Id: I67608450d0818b38f6d5b5c709190433f2dc5cf5 --- .../wpi/first/wpilibj/simulation/ds/DS.java | 2 +- .../wpi/first/wpilibj/simulation/ds/Main.java | 2 +- simulation/debs/Makefile | 1 + wpilibc/wpilibC++Sim/include/DriverStation.h | 6 +- wpilibc/wpilibC++Sim/src/DriverStation.cpp | 62 ++++++++++++------- wpilibc/wpilibC++Sim/src/Joystick.cpp | 6 +- .../edu/wpi/first/wpilibj/DriverStation.java | 10 +-- 7 files changed, 54 insertions(+), 35 deletions(-) diff --git a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/DS.java b/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/DS.java index 07286cd0e1..bd12c4119b 100644 --- a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/DS.java +++ b/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/DS.java @@ -105,7 +105,7 @@ public class DS { public void scanForJoysticks() { joysticks.removeAll(); List sticks = joystickProvider.scanForJoysticks(); - while (sticks.size() < 4) { + while (sticks.size() < 6) { sticks.add(new FakeJoystick()); } joysticks.setListData(sticks); diff --git a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java b/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java index 50f8e7651c..2f5188ad03 100644 --- a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java +++ b/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java @@ -42,7 +42,7 @@ public class Main { final double start = simTime; for (int i = 0; i < provider.getJoysticks().size(); i++) { ISimJoystick joystick = provider.getJoysticks().get(i); - joystick.advertise(node, i+1); + joystick.advertise(node, i); joystick.publish(); } ds.publish(); diff --git a/simulation/debs/Makefile b/simulation/debs/Makefile index 5c294d828a..3b737513da 100644 --- a/simulation/debs/Makefile +++ b/simulation/debs/Makefile @@ -66,6 +66,7 @@ clean: -rm frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin_$(eclipse-toolchain-package-version)_all.deb -rm frcsim-libwpilibsim-cpp/frcsim-libwpilibsim-cpp_$(libwpilibsim-package-version)_amd64.deb -rm frcsim-libwpilibsim-cpp/frcsim-libwpilibsim-cpp_$(libwpilibsim-package-version)_i386.deb + -rm -r frcsim-libwpilibsim-cpp/frcsim-libwpilibsim-cpp/allwpilib -rm frcsim/frcsim_$(frcsim-package-version)_all.deb install: diff --git a/wpilibc/wpilibC++Sim/include/DriverStation.h b/wpilibc/wpilibC++Sim/include/DriverStation.h index dbac051bdf..9816f025dc 100644 --- a/wpilibc/wpilibC++Sim/include/DriverStation.h +++ b/wpilibc/wpilibC++Sim/include/DriverStation.h @@ -109,10 +109,12 @@ private: void stateCallback(const msgs::ConstDriverStationPtr &msg); void joystickCallback(const msgs::ConstJoystickPtr &msg, int i); + void joystickCallback0(const msgs::ConstJoystickPtr &msg); void joystickCallback1(const msgs::ConstJoystickPtr &msg); void joystickCallback2(const msgs::ConstJoystickPtr &msg); void joystickCallback3(const msgs::ConstJoystickPtr &msg); void joystickCallback4(const msgs::ConstJoystickPtr &msg); + void joystickCallback5(const msgs::ConstJoystickPtr &msg); uint8_t m_digitalOut; MULTIWAIT_ID m_waitForDataSem; @@ -125,7 +127,7 @@ private: bool m_userInTest; transport::SubscriberPtr stateSub; - transport::SubscriberPtr joysticksSub[4]; + transport::SubscriberPtr joysticksSub[6]; msgs::DriverStationPtr state; - msgs::JoystickPtr joysticks[4]; + msgs::JoystickPtr joysticks[6]; }; diff --git a/wpilibc/wpilibC++Sim/src/DriverStation.cpp b/wpilibc/wpilibC++Sim/src/DriverStation.cpp index fef168ba5a..f78fa1119c 100644 --- a/wpilibc/wpilibC++Sim/src/DriverStation.cpp +++ b/wpilibc/wpilibC++Sim/src/DriverStation.cpp @@ -53,17 +53,23 @@ DriverStation::DriverStation() &DriverStation::stateCallback, this); // TODO: for loop + boost bind joysticks[0] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/1", - &DriverStation::joystickCallback1, this); + joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0", + &DriverStation::joystickCallback0, this); joysticks[1] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/2", - &DriverStation::joystickCallback2, this); + joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1", + &DriverStation::joystickCallback1, this); joysticks[2] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/3", - &DriverStation::joystickCallback3, this); + joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2", + &DriverStation::joystickCallback2, this); joysticks[3] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/4", + joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5", + &DriverStation::joystickCallback3, this); + joysticks[4] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4", &DriverStation::joystickCallback4, this); + joysticks[5] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5", + &DriverStation::joystickCallback5, this); AddToSingletonList(); } @@ -107,22 +113,22 @@ float DriverStation::GetBatteryVoltage() */ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) { - if (axis < 1 || axis > kJoystickAxes) + if (axis < 0 || axis > (kJoystickAxes - 1)) { wpi_setWPIError(BadJoystickAxis); return 0.0; } - if (stick < 1 || stick > 4) + if (stick < 0 || stick > 5) { wpi_setWPIError(BadJoystickIndex); return 0.0; } CRITICAL_REGION(m_joystickSemaphore) - if (joysticks[stick-1] == NULL || axis >= joysticks[stick-1]->axes().size()) + if (joysticks[stick] == NULL || axis >= joysticks[stick]->axes().size()) { return 0.0; } - return joysticks[stick-1]->axes(axis-1); + return joysticks[stick]->axes(axis); END_REGION } @@ -136,17 +142,17 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) */ bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) { - if (stick < 1 || stick > 4) + if (stick < 0 || stick >= 6) { - wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 1 and 4"); + wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5"); return false; } CRITICAL_REGION(m_joystickSemaphore) - if (joysticks[stick-1] == NULL || button >= joysticks[stick-1]->buttons().size()) + if (joysticks[stick] == NULL || button >= joysticks[stick]->buttons().size()) { return false; } - return joysticks[stick-1]->buttons(button-1); + return joysticks[stick]->buttons(button-1); END_REGION } @@ -159,17 +165,17 @@ bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) */ short DriverStation::GetStickButtons(uint32_t stick) { - if (stick < 1 || stick > 4) + if (stick < 0 || stick >= 6) { - wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 1 and 4"); + wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5"); return false; } short btns = 0, btnid; CRITICAL_REGION(m_joystickSemaphore) - msgs::JoystickPtr joy = joysticks[stick-1]; + msgs::JoystickPtr joy = joysticks[stick]; for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) { - if (joysticks[stick-1]->buttons(btnid)) + if (joysticks[stick]->buttons(btnid)) { btns |= (1 << btnid); } @@ -359,22 +365,32 @@ void DriverStation::joystickCallback(const msgs::ConstJoystickPtr &msg, END_REGION; } -void DriverStation::joystickCallback1(const msgs::ConstJoystickPtr &msg) +void DriverStation::joystickCallback0(const msgs::ConstJoystickPtr &msg) { joystickCallback(msg, 0); } -void DriverStation::joystickCallback2(const msgs::ConstJoystickPtr &msg) +void DriverStation::joystickCallback1(const msgs::ConstJoystickPtr &msg) { joystickCallback(msg, 1); } -void DriverStation::joystickCallback3(const msgs::ConstJoystickPtr &msg) +void DriverStation::joystickCallback2(const msgs::ConstJoystickPtr &msg) { joystickCallback(msg, 2); } -void DriverStation::joystickCallback4(const msgs::ConstJoystickPtr &msg) +void DriverStation::joystickCallback3(const msgs::ConstJoystickPtr &msg) { joystickCallback(msg, 3); } + +void DriverStation::joystickCallback4(const msgs::ConstJoystickPtr &msg) +{ + joystickCallback(msg, 4); +} + +void DriverStation::joystickCallback5(const msgs::ConstJoystickPtr &msg) +{ + joystickCallback(msg, 5); +} diff --git a/wpilibc/wpilibC++Sim/src/Joystick.cpp b/wpilibc/wpilibC++Sim/src/Joystick.cpp index 7de12d78a0..5cb60be4e4 100644 --- a/wpilibc/wpilibC++Sim/src/Joystick.cpp +++ b/wpilibc/wpilibC++Sim/src/Joystick.cpp @@ -70,7 +70,7 @@ void Joystick::InitJoystick(uint32_t numAxisTypes, uint32_t numButtonTypes) joysticks[i] = NULL; joySticksInitialized = true; } - joysticks[m_port - 1] = this; + joysticks[m_port] = this; m_ds = DriverStation::GetInstance(); m_axes = new uint32_t[numAxisTypes]; @@ -79,11 +79,11 @@ void Joystick::InitJoystick(uint32_t numAxisTypes, uint32_t numButtonTypes) Joystick * Joystick::GetStickForPort(uint32_t port) { - Joystick *stick = joysticks[port - 1]; + Joystick *stick = joysticks[port]; if (stick == NULL) { stick = new Joystick(port); - joysticks[port - 1] = stick; + joysticks[port] = stick; } return stick; } diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index addc39c4a9..66c5ccb175 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -28,7 +28,7 @@ public class DriverStation implements RobotState.Interface { /** * Number of Joystick Ports */ - public static final int kJoystickPorts = 4; + public static final int kJoystickPorts = 6; /** * Number of Joystick Axes */ @@ -72,7 +72,7 @@ public class DriverStation implements RobotState.Interface { private boolean m_userInTest = false; private boolean m_newControlData; private GzDriverStation.DriverStation state; - private Joystick joysticks[] = new Joystick[4]; + private Joystick joysticks[] = new Joystick[6]; /** * Gets an instance of the DriverStation @@ -104,7 +104,7 @@ public class DriverStation implements RobotState.Interface { } ); - for (int i = 1; i <= 4; i++) { + for (int i = 0; i < 6; i++) { final int j = i; MainNode.subscribe("ds/joysticks/"+i, Joystick.getDefaultInstance(), new SubscriberCallback() { @@ -161,10 +161,10 @@ public class DriverStation implements RobotState.Interface { if (stick < 0 || stick >= joysticks.length || joysticks[stick] == null) { return 0.0; } - if (axis < 1 || axis > kJoystickAxes || axis > joysticks[stick].getAxesCount()) { + if (axis < 0 || axis >= kJoystickAxes || axis >= joysticks[stick].getAxesCount()) { return 0.0; } - return joysticks[stick].getAxes(axis - 1); + return joysticks[stick].getAxes(axis); } /**