Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Added Mecanum example code.

  • Loading branch information...
commit 5fc2d4bab76a36b072042c34735c28fd2cbbf0aa 1 parent d796a2a
@gmccoy gmccoy authored
Showing with 85 additions and 0 deletions.
  1. +85 −0 RobotOpen/examples/Mecanum/Mecanum.ino
View
85 RobotOpen/examples/Mecanum/Mecanum.ino
@@ -0,0 +1,85 @@
+#include <SPI.h>
+#include <Ethernet.h>
+#include <RobotOpen.h>
+
+
+/* I/O Setup */
+USBJoystick usb1('0'); // Assign the logitech USBJoystick object to bundle 0
+
+
+void setup()
+{
+ /* Initiate comms */
+ RobotOpen.begin();
+}
+
+
+/* This is your primary robot loop - all of your code
+ * should live here that allows the robot to operate
+ */
+void enabled() {
+
+ // get desired translation and rotation, scaled to [-127..128] (0 neutral)
+ int x = usb1.makePWM(ANALOG_LEFTX, NORMAL) - 127;
+ int y = usb1.makePWM(ANALOG_LEFTY, INVERT) - 127;
+ int rotate = usb1.makePWM(ANALOG_RIGHTX, NORMAL) - 127;
+
+ // calculate wheel throttles
+ int lf = x + y + rotate;
+ int rf = x - y + rotate;
+ int lr = -x + y + rotate;
+ int rr = -x - y + rotate;
+
+ // normalize wheel throttles
+ int maximum = max(max(abs(lf), abs(rf)), max(abs(lr), abs(rr)));
+ if (maximum > 127) {
+ lf = (lf / maximum) * 127;
+ rf = (rf / maximum) * 127;
+ lr = (lr / maximum) * 127;
+ rr = (rr / maximum) * 127;
+ }
+
+ // Set PWMs, shifted back to [0..255]
+ RobotOpen.setPWM(SIDECAR_PWM1, lf + 127);
+ RobotOpen.setPWM(SIDECAR_PWM2, rf + 127);
+ RobotOpen.setPWM(SIDECAR_PWM3, lr + 127);
+ RobotOpen.setPWM(SIDECAR_PWM4, rr + 127);
+}
+
+
+/* This is called while the robot is disabled
+ * You must make sure to set all of your outputs
+ * to safe/disable values here
+ */
+void disabled() {
+ // PWMs are automatically disabled
+}
+
+
+/* This loop ALWAYS runs - only place code here that can run during a disabled state
+ * This is also a good spot to put driver station publish code
+ * You can use either publishAnalog, publishDigital, publishByte, publishShort, or publishLong
+ * Specify a bundle ID with a single character (a-z, A-Z, 0-9) - Just make sure not to use the same twice!
+ */
+void timedtasks() {
+ RobotOpen.publishAnalog(ANALOG0, 'A'); // Bundle A
+ RobotOpen.publishAnalog(ANALOG1, 'B'); // Bundle B
+ RobotOpen.publishAnalog(ANALOG2, 'C'); // Bundle C
+ RobotOpen.publishAnalog(ANALOG3, 'D'); // Bundle D
+ RobotOpen.publishAnalog(ANALOG4, 'E'); // Bundle E
+ RobotOpen.publishAnalog(ANALOG5, 'F'); // Bundle F
+}
+
+
+/* This is the main program loop that keeps comms operational
+ * There's no need to touch anything here!!!
+ */
+void loop() {
+ RobotOpen.pollDS();
+ if (RobotOpen.enabled())
+ enabled();
+ else
+ disabled();
+ timedtasks();
+ RobotOpen.outgoingDS();
+}
Please sign in to comment.
Something went wrong with that request. Please try again.