Merge branch 'modulus-plus' of https://git.myitr.org/adeck/rib-test-code into modulus-plus

Co-authored-by: Amelia Deck <cdeck@hawk.iit.edu>

lib/Sabertooth/changes.txt

@@ -0,0 +1,37 @@
+Arduino Libraries for SyRen/Sabertooth
+Copyright (c) 2012-2013 Dimension Engineering LLC
+http://www.dimensionengineering.com/arduino
+
+1 July 2013, Version 1.5
+- USB Sabertooth Packet Serial Library
+  - Initial release.
+
+1 April 2013, Version 1.4
+- Added documentation.
+- Improved support for Arduino Leonardo.
+- Packet Serial Library
+  - Simplified the code to save space and increase readability.
+
+3 May 2012, Version 1.3
+- Improved compatibility with earlier versions of the Arduino software.
+  Previous versions required Arduino 1.0 or newer, but this version
+  should work back to Arduino 0018 (still in use by Debian Linux 6.0)
+  and possibly earlier.
+  
+  If you are using a version of Arduino older than 1.0, you will need
+  to rename the examples from .ino to .pde to see them in the Examples
+  menu.
+  
+20 April 2012, Version 1.2
+- Packet Serial Library
+  - Added a stop() function to match the command set of the Simplified
+    Serial libraries.
+
+19 April 2012, Version 1.1
+- Packet Serial Library
+  - Added a single-argument motor() function as a convenience for SyRen.
+
+18 April 2012, Version 1.0
+- Initial release.
+
+Please let us know if you run into any bugs! Thanks and enjoy the library!

lib/Sabertooth/examples/1.Basics/Jolty/Jolty.ino

@@ -0,0 +1,46 @@
+// Jolty Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <Sabertooth.h>
+
+Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
+                    // If you've set up your Sabertooth on a different address, of course change
+                    // that here. For how to configure address, etc. see the DIP Switch Wizard for
+                    //   Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
+                    //   SyRen      - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
+                    // Be sure to select Packetized Serial Mode for use with this library.
+                    //
+                    // On that note, you can use this library for SyRen just as easily.
+                    // The diff-drive commands (drive, turn) do not work on a SyRen, of course, but it will respond correctly
+                    // if you command motor 1 to do something (ST.motor(1, ...)), just like a Sabertooth.
+                    //
+                    // In this sample, hardware serial TX connects to S1.
+                    // See the SoftwareSerial example in 3.Advanced for how to use other pins.
+                                        
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
+  ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
+                 // NOTE: *Not all* Sabertooth controllers need this command.
+                 //       It doesn't hurt anything, but V2 controllers use an
+                 //       EEPROM setting (changeable with the function setBaudRate) to set
+                 //       the baud rate instead of detecting with autobaud.
+                 //
+                 //       If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
+                 //       the autobaud line and save yourself two seconds of startup delay.
+}
+
+
+void loop()
+{
+  ST.motor(1, 127);  // Go forward at full power.
+  delay(2000);       // Wait 2 seconds.
+  ST.motor(1, 0);    // Stop.
+  delay(2000);       // Wait 2 seconds.
+  ST.motor(1, -127); // Reverse at full power.
+  delay(2000);       // Wait 2 seconds.
+  ST.motor(1, 0);    // Stop.
+  delay(2000);
+}
+

lib/Sabertooth/examples/1.Basics/Sweep/Sweep.ino

@@ -0,0 +1,53 @@
+// Sweep Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <Sabertooth.h>
+
+Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
+                    // If you've set up your Sabertooth on a different address, of course change
+                    // that here. For how to configure address, etc. see the DIP Switch Wizard for
+                    //   Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
+                    //   SyRen      - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
+                    // Be sure to select Packetized Serial Mode for use with this library.
+                    //
+                    // On that note, you can use this library for SyRen just as easily.
+                    // The diff-drive commands (drive, turn) do not work on a SyRen, of course, but it will respond correctly
+                    // if you command motor 1 to do something (ST.motor(1, ...)), just like a Sabertooth.
+                    //
+                    // In this sample, hardware serial TX connects to S1.
+                    // See the SoftwareSerial example in 3.Advanced for how to use other pins.
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
+  ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
+                 // NOTE: *Not all* Sabertooth controllers need this command.
+                 //       It doesn't hurt anything, but V2 controllers use an
+                 //       EEPROM setting (changeable with the function setBaudRate) to set
+                 //       the baud rate instead of detecting with autobaud.
+                 //
+                 //       If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
+                 //       the autobaud line and save yourself two seconds of startup delay.
+}
+
+void loop()
+{
+  int power;
+  
+  // Ramp motor 1 from -127 to 127 (full reverse to full forward),
+  // waiting 20 ms (1/50th of a second) per value.
+  for (power = -127; power <= 127; power ++)
+  {
+    ST.motor(1, power);
+    delay(20);
+  }
+  
+  // Now go back the way we came.
+  for (power = 127; power >= -127; power --)
+  {
+    ST.motor(1, power); // Tip for SyRen users: Typing ST.motor(power) does the same thing as ST.motor(1, power).
+    delay(20);          //                      Since SyRen doesn't have a motor 2, this alternative can save you typing.
+  }
+}
+

lib/Sabertooth/examples/1.Basics/TankStyleSweep/TankStyleSweep.ino

@@ -0,0 +1,69 @@
+// Tank-Style Sweep Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <Sabertooth.h>
+
+// Mixed mode is for tank-style diff-drive robots.
+
+Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
+                    // If you've set up your Sabertooth on a different address, of course change
+                    // that here. For how to configure address, etc. see the DIP Switch Wizard for
+                    //   Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
+                    //   SyRen      - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
+                    // Be sure to select Packetized Serial Mode for use with this library.
+                    //
+                    // This sample uses the mixed mode (diff-drive) commands, which require two motors
+                    // and hence do not work on a SyRen.
+                    //
+                    // In this sample, hardware serial TX connects to S1.
+                    // See the SoftwareSerial example in 3.Advanced for how to use other pins.
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
+  ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
+                 // NOTE: *Not all* Sabertooth controllers need this command.
+                 //       It doesn't hurt anything, but V2 controllers use an
+                 //       EEPROM setting (changeable with the function setBaudRate) to set
+                 //       the baud rate instead of detecting with autobaud.
+                 //
+                 //       If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
+                 //       the autobaud line and save yourself two seconds of startup delay.
+             
+  ST.drive(0); // The Sabertooth won't act on mixed mode packet serial commands until
+  ST.turn(0);  // it has received power levels for BOTH throttle and turning, since it
+               // mixes the two together to get diff-drive power levels for both motors.
+}
+
+// The SLOW ramp here is turning, and the FAST ramp is throttle.
+// If that's the opposite of what you're seeing, swap M2A and M2B.
+void loop()
+{
+  int power;
+  
+  // Don't turn. Ramp from going backwards to going forwards, waiting 20 ms (1/50th of a second) per value.
+  for (power = -127; power <= 127; power ++)
+  {
+    ST.drive(power);
+    delay(20);
+  }
+  
+  // Now, let's use a power level of 20 (out of 127) forward.
+  // This way, our turning will have a radius.
+  ST.drive(20);
+  
+  // Ramp turning from full left to full right SLOWLY by waiting 50 ms (1/20th of a second) per value.
+  for (power = -127; power <= 127; power ++)
+  {
+    ST.turn(power);
+    delay(50);
+  }
+  
+  // Now stop turning, and stop driving.
+  ST.turn(0);
+  ST.drive(0);
+  
+  // Wait a bit. This is so you can catch your robot if you want to. :-)
+  delay(5000);
+}

lib/Sabertooth/examples/2.Settings/MinVoltage/MinVoltage.ino

@@ -0,0 +1,34 @@
+// Set Minimum Voltage Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+// The values in this sample are specifically for the Sabertooth 2x25, and may
+// not have the same effect on other models.
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  ST.autobaud();
+
+  // This setting does not persist between power cycles.
+  // See the Packet Serial section of the documentation for what values to use
+  // for the minimum voltage command. It may vary between Sabertooth models
+  // (2x25, 2x60, etc.).
+  //
+  // On a Sabertooth 2x25, the value is (Desired Volts - 6) X 5.
+  // So, in this sample, we'll make the low battery cutoff 12V: (12 - 6) X 5 = 30.
+  ST.setMinVoltage(30);
+}
+
+void loop()
+{
+  ST.motor(1, 50);
+  delay(5000);
+  
+  ST.motor(1, -50);
+  delay(5000);
+}
+

lib/Sabertooth/examples/2.Settings/Persistent/BaudRate/BaudRate.ino

@@ -0,0 +1,48 @@
+// Set Baud Rate Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+// WARNING: This sample makes changes that will persist between restarts.
+// NOTE: The setBaudRate function will only have an effect on V2 controllers (2x12, 2x25 V2, 2x60, SyRen 50).
+//       Earlier controllers automatically detect the baud rate you choose in Serial.begin
+//       when you call the autobaud function. Autobaud was replaced in V2 controllers for reliability
+//       in the event that the Sabertooth lost power.
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  // This sample will tell the Sabertooth *at 9600 baud* to *switch to 2400 baud*.
+  // Keep in mind you must send the command to change the baud rate *at the baud rate
+  // the Sabertooth is listening at* (factory default is 9600). After that, if it works,
+  // you will be able to communicate using the new baud rate.
+  //
+  // Options are:
+  //   2400
+  //   9600
+  //   19200
+  //   38400
+  //   115200 (only supported by some devices such as 2X60 -- check the device's datasheet)
+  //
+  // WARNING: The Sabertooth remembers this command between restarts.
+  // To change your Sabertooth back to its default, you must *be at the baud rate you've
+  // set the Sabertooth to*, and then call ST.setBaudRate(9600)
+  SabertoothTXPinSerial.begin(9600);
+  ST.setBaudRate(2400);
+  SabertoothTXPinSerial.end();
+
+  // OK, we're at 2400. Let's talk to the Sabertooth at that speed.
+  SabertoothTXPinSerial.begin(2400);  
+}
+
+void loop()
+{
+  ST.drive(0);
+  ST.turn(20);
+  delay(2000);
+  
+  ST.turn(-20);
+  delay(2000);
+}
+

lib/Sabertooth/examples/2.Settings/Persistent/Deadband/Deadband.ino

@@ -0,0 +1,33 @@
+// Set Deadband Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+// WARNING: This sample makes changes that will persist between restarts AND in all modes.
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  ST.autobaud();
+  
+  // This makes the deadband from -20 to 20 (of 127).
+  // If your commands for a motor stay entirely within the deadband for more than
+  // a second, the motor driver will stop the motor.
+  // WARNING: The Sabertooth remembers this command between restarts AND in all modes.
+  // To change your Sabertooth back to its default, call ST.setDeadband(0)
+  ST.setDeadband(20);
+}
+
+void loop()
+{
+  // 50 is greater than 20, so the motor moves.
+  ST.motor(1, 50);
+  delay(5000);
+  
+  // 10 is NOT, so the motor does not move.
+  ST.motor(1, 10);
+  delay(5000);
+}
+

lib/Sabertooth/examples/2.Settings/Persistent/MaxVoltage/MaxVoltage.ino

@@ -0,0 +1,41 @@
+// Set Maximum Voltage Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+// WARNING: This sample makes changes that will persist between restarts AND in all modes.
+//          The values in this sample are specifically for the Sabertooth 2x25, and may
+//          not have the same effect on other models.
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  ST.autobaud();
+
+  // See the Packet Serial section of the documentation for what values to use
+  // for the maximum voltage command. It may vary between Sabertooth models
+  // (2x25, 2x60, etc.).
+  //
+  // On a Sabertooth 2x25, the value is (Desired Volts) X 5.12.
+  // In this sample, we'll cap the max voltage before the motor driver does
+  // a hard brake at 14V. For a 12V ATX power supply this might be reasonable --
+  // at 16V they tend to shut off. Here, if the voltage climbs above
+  // 14V due to regenerative braking, the Sabertooth will go into hard brake instead.
+  // While this is occuring, the red Error LED will turn on.
+  //
+  // 14 X 5.12 = 71.68, so we'll go with 71, cutting off slightly below 14V.
+  //
+  // WARNING: This setting persists between power cycles.
+  ST.setMaxVoltage(71);
+}
+
+void loop()
+{
+  ST.motor(1, 50);
+  delay(5000);
+  
+  ST.motor(1, -50);
+  delay(5000);
+}

lib/Sabertooth/examples/2.Settings/Persistent/Ramping/Ramping.ino

@@ -0,0 +1,39 @@
+// Set Ramping Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+// WARNING: This sample makes changes that will persist between restarts AND in all modes.
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  ST.autobaud();
+  
+  // See the Sabertooth 2x60 documentation for information on ramping values.
+  // There are three ranges: 1-10 (Fast), 11-20 (Slow), and 21-80 (Intermediate).
+  // The ramping value 14 used here sets a ramp time of 4 seconds for full
+  // forward-to-full reverse.
+  //
+  // 0 turns off ramping. Turning off ramping requires a power cycle.
+  //
+  // WARNING: The Sabertooth remembers this command between restarts AND in all modes.
+  // To change your Sabertooth back to its default, call ST.setRamping(0)
+  ST.setRamping(14);
+}
+
+void loop()
+{
+  // Full forward, both motors.
+  ST.motor(1, 127);
+  ST.motor(2, 127);
+  delay(5000);
+  
+  // Full reverse
+  ST.motor(1, -127);
+  ST.motor(2, -127);
+  delay(5000);
+}
+

lib/Sabertooth/examples/2.Settings/SerialTimeout/SerialTimeout.ino

@@ -0,0 +1,33 @@
+// Set Serial Timeout Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <Sabertooth.h>
+
+Sabertooth ST(128);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  ST.autobaud();
+  
+  // setTimeout rounds up to the nearest 100 milliseconds, so this 950 will actually be 1 second.
+  // A value of 0 disables the serial timeout.
+  ST.setTimeout(950);
+}
+
+void loop()
+{
+  // Set motor 1 to reverse 20 (out of 127), and sleep for 5 seconds.
+  // Notice how it cuts out after 1 second -- this is the serial timeout in action.
+  // Since we configured it in setup() for 1 second, 1 second without any new
+  // commands will cause the motors to stop.
+  ST.motor(1, -20);
+  delay(5000);
+  
+  // Why do this?
+  // If the S1 wire gets cut for some reason, or if your program crashes,
+  // the Sabertooth will stop receiving commands from the Arduino.
+  // With a timeout, your robot will stop. So, it's a safety feature mostly.
+}
+

lib/Sabertooth/examples/3.Advanced/SharedLine/SharedLine.ino

@@ -0,0 +1,43 @@
+// Shared Line Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <Sabertooth.h>
+
+// Up to 8 Sabertooth/SyRen motor drivers can share the same S1 line.
+// This sample uses three: address 128 and 129 on ST1[0] and ST1[2],
+// and address 130 on ST2.
+Sabertooth ST1[2] = { Sabertooth(128), Sabertooth(129) };
+Sabertooth ST2(130);
+
+void setup()
+{
+  SabertoothTXPinSerial.begin(9600);
+  Sabertooth::autobaud(SabertoothTXPinSerial); // Autobaud is for the whole serial line -- you don't need to do
+                                               // it for each individual motor driver. This is the version of
+                                               // the autobaud command that is not tied to a particular
+                                               // Sabertooth object.
+                                               // See the examples in 1.Basics for information on whether you
+                                               // need this line at all.
+}
+
+void loop()
+{
+  // ST1[0] (address 128) has power 50 (of 127 max) on M1,
+  // ST1[1] (address 129) has power 60 (of 127 max) on M2, and
+  // ST2    (address 130) we'll do tank-style and have it drive 20 and turn right 50.
+  // Do this for 5 seconds.
+  ST1[0].motor(1, 50);
+  ST1[1].motor(2, 60);
+  ST2.drive(20);
+  ST2.turn(50);
+  delay(5000);
+  
+  // And now let's stop for 5 seconds, except address 130 -- we'll let it stop and turn left...
+  ST1[0].motor(1, 0);
+  ST1[1].motor(2, 0);
+  ST2.drive(0);
+  ST2.turn(-40);  
+  delay(5000);
+}
+

lib/Sabertooth/examples/3.Advanced/SoftwareSerial/SoftwareSerial.ino

@@ -0,0 +1,35 @@
+// Software Serial Sample for Packet Serial
+// Copyright (c) 2012 Dimension Engineering LLC
+// See license.txt for license details.
+
+#include <SoftwareSerial.h>
+#include <Sabertooth.h>
+
+SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
+Sabertooth ST(128, SWSerial); // Address 128, and use SWSerial as the serial port.
+
+void setup()
+{
+  SWSerial.begin(9600);
+  ST.autobaud();
+}
+
+void loop()
+{
+  int power;
+  
+  // Ramp from -127 to 127 (full reverse to full forward), waiting 20 ms (1/50th of a second) per value.
+  for (power = -127; power <= 127; power ++)
+  {
+    ST.motor(1, power);
+    delay(20);
+  }
+  
+  // Now go back the way we came.
+  for (power = 127; power >= -127; power --)
+  {
+    ST.motor(1, power);
+    delay(20);
+  }
+}
+

lib/Sabertooth/keywords.txt

@@ -0,0 +1,22 @@
+# Syntax Coloring for the Sabertooth Library
+
+# Classes
+Sabertooth	KEYWORD1
+
+# Sabertooth methods
+address	KEYWORD2
+port	KEYWORD2
+autobaud	KEYWORD2
+
+# Common methods
+command	KEYWORD2
+motor	KEYWORD2
+drive	KEYWORD2
+turn	KEYWORD2
+setMinVoltage	KEYWORD2
+setMaxVoltage	KEYWORD2
+setBaudRate	KEYWORD2
+setDeadband	KEYWORD2
+setRamping	KEYWORD2
+setTimeout	KEYWORD2
+stop	KEYWORD2

lib/Sabertooth/license.txt

@@ -0,0 +1,7 @@
+Arduino Libraries for SyRen/Sabertooth
+Copyright (c) 2012-2013 Dimension Engineering LLC
+http://www.dimensionengineering.com/arduino
+
+Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above copyright notice and this permission notice appear in all copies.
+
+THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

lib/Sabertooth/src/Sabertooth.cpp

@@ -0,0 +1,142 @@
+/*
+Arduino Library for SyRen/Sabertooth Packet Serial
+Copyright (c) 2012-2013 Dimension Engineering LLC
+http://www.dimensionengineering.com/arduino
+
+Permission to use, copy, modify, and/or distribute this software for any
+purpose with or without fee is hereby granted, provided that the above
+copyright notice and this permission notice appear in all copies.
+
+THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
+RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
+USE OR PERFORMANCE OF THIS SOFTWARE.
+*/
+
+#include "Sabertooth.h"
+
+Sabertooth::Sabertooth(byte address)
+  : _address(address), _port(SabertoothTXPinSerial)
+{
+  
+}
+
+Sabertooth::Sabertooth(byte address, SabertoothStream& port)
+  : _address(address), _port(port)
+{
+
+}
+
+void Sabertooth::autobaud(boolean dontWait) const
+{
+  autobaud(port(), dontWait);
+}
+
+void Sabertooth::autobaud(SabertoothStream& port, boolean dontWait)
+{
+  if (!dontWait) { delay(1500); }
+  port.write(0xAA);
+#if defined(ARDUINO) && ARDUINO >= 100
+  port.flush();
+#endif
+  if (!dontWait) { delay(500); }
+}
+
+void Sabertooth::command(byte command, byte value) const
+{
+  port().write(address());
+  port().write(command);
+  port().write(value);
+  port().write((address() + command + value) & 0b01111111);
+}
+
+void Sabertooth::throttleCommand(byte command, int power) const
+{
+  power = constrain(power, -126, 126);
+  this->command(command, (byte)abs(power));
+}
+
+void Sabertooth::motor(int power) const
+{
+  motor(1, power);
+}
+
+void Sabertooth::motor(byte motor, int power) const
+{
+  if (motor < 1 || motor > 2) { return; }
+  throttleCommand((motor == 2 ? 4 : 0) + (power < 0 ? 1 : 0), power);
+}
+
+void Sabertooth::drive(int power) const
+{
+  throttleCommand(power < 0 ? 9 : 8, power);
+}
+
+void Sabertooth::turn(int power) const
+{
+  throttleCommand(power < 0 ? 11 : 10, power);
+}
+
+void Sabertooth::stop() const
+{
+  motor(1, 0);
+  motor(2, 0);
+}
+
+void Sabertooth::setMinVoltage(byte value) const
+{
+  command(2, (byte)min(value, 120));
+}
+
+void Sabertooth::setMaxVoltage(byte value) const
+{
+  command(3, (byte)min(value, 127));
+}
+
+void Sabertooth::setBaudRate(long baudRate) const
+{
+#if defined(ARDUINO) && ARDUINO >= 100
+  port().flush();
+#endif
+
+  byte value;
+  switch (baudRate)
+  {
+  case 2400:           value = 1; break;
+  case 9600: default: value = 2; break;
+  case 19200:          value = 3; break;
+  case 38400:          value = 4; break;
+  case 115200:         value = 5; break;
+  }
+  command(15, value);
+  
+#if defined(ARDUINO) && ARDUINO >= 100
+  port().flush();
+#endif
+  
+  // (1) flush() does not seem to wait until transmission is complete.
+  //     As a result, a Serial.end() directly after this appears to
+  //     not always transmit completely. So, we manually add a delay.
+  // (2) Sabertooth takes about 200 ms after setting the baud rate to
+  //     respond to commands again (it restarts).
+  // So, this 500 ms delay should deal with this.
+  delay(500);
+}
+
+void Sabertooth::setDeadband(byte value) const
+{
+  command(17, (byte)min(value, 127));
+}
+
+void Sabertooth::setRamping(byte value) const
+{
+  command(16, (byte)constrain(value, 0, 80));
+}
+
+void Sabertooth::setTimeout(int milliseconds) const
+{
+  command(14, (byte)((constrain(milliseconds, 0, 12700) + 99) / 100));
+}

lib/Sabertooth/src/Sabertooth.h

@@ -0,0 +1,179 @@
+/*
+Arduino Library for SyRen/Sabertooth Packet Serial
+Copyright (c) 2012-2013 Dimension Engineering LLC
+http://www.dimensionengineering.com/arduino
+
+Permission to use, copy, modify, and/or distribute this software for any
+purpose with or without fee is hereby granted, provided that the above
+copyright notice and this permission notice appear in all copies.
+
+THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
+RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
+USE OR PERFORMANCE OF THIS SOFTWARE.
+*/
+
+#ifndef Sabertooth_h
+#define Sabertooth_h   
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include <Arduino.h>
+typedef Stream SabertoothStream;
+#else
+#include <WProgram.h>
+typedef Print SabertoothStream;
+#endif
+
+#if defined(USBCON)
+#define SabertoothTXPinSerial Serial1 // Arduino Leonardo has TX->1 on Serial1, not Serial.
+#else
+#define SabertoothTXPinSerial Serial2
+#endif
+#define SyRenTXPinSerial SabertoothTXPinSerial
+
+/*!
+\class Sabertooth
+\brief Controls a %Sabertooth or %SyRen motor driver running in Packet Serial mode.
+*/
+class Sabertooth
+{
+public:
+  /*!
+  Initializes a new instance of the Sabertooth class.
+  The driver address is set to the value given, and the Arduino TX serial port is used.
+  \param address The driver address.
+  */
+  Sabertooth(byte address);
+  
+  /*!
+  Initializes a new instance of the Sabertooth class.
+  The driver address is set to the value given, and the specified serial port is used.
+  \param address The driver address.
+  \param port    The port to use.
+  */
+  Sabertooth(byte address, SabertoothStream& port);
+
+public:
+  /*!
+  Gets the driver address.
+  \return The driver address.
+  */
+  inline byte address() const { return _address; }
+  
+  /*!
+  Gets the serial port.
+  \return The serial port.
+  */
+  inline SabertoothStream& port() const { return _port; }
+
+  /*!
+  Sends the autobaud character.
+  \param dontWait If false, a delay is added to give the driver time to start up.
+  */
+  void autobaud(boolean dontWait = false) const;
+  
+  /*!
+  Sends the autobaud character.
+  \param port     The port to use.
+  \param dontWait If false, a delay is added to give the driver time to start up.
+  */
+  static void autobaud(SabertoothStream& port, boolean dontWait = false);
+  
+  /*!
+  Sends a packet serial command to the motor driver.
+  \param command The number of the command.
+  \param value   The command's value.
+  */
+  void command(byte command, byte value) const;
+  
+public:
+  /*!
+  Sets the power of motor 1.
+  \param power The power, between -127 and 127.
+  */
+  void motor(int power) const;
+  
+  /*!
+  Sets the power of the specified motor.
+  \param motor The motor number, 1 or 2.
+  \param power The power, between -127 and 127.
+  */
+  void motor(byte motor, int power) const;
+  
+  /*!
+  Sets the driving power.
+  \param power The power, between -127 and 127.
+  */
+  void drive(int power) const;
+  
+  /*!
+  Sets the turning power.
+  \param power The power, between -127 and 127.
+  */
+  void turn(int power) const;
+  
+  /*!
+  Stops.
+  */
+  void stop() const;
+  
+public:
+  /*!
+  Sets the minimum voltage.
+  \param value The voltage. The units of this value are driver-specific and are specified in the Packet Serial chapter of the driver's user manual.
+  */
+  void setMinVoltage(byte value) const;
+  
+  /*!
+  Sets the maximum voltage.
+  Maximum voltage is stored in EEPROM, so changes persist between power cycles.
+  \param value The voltage. The units of this value are driver-specific and are specified in the Packet Serial chapter of the driver's user manual.
+  */
+  void setMaxVoltage(byte value) const;
+  
+  /*!
+  Sets the baud rate.
+  Baud rate is stored in EEPROM, so changes persist between power cycles.
+  \param baudRate The baud rate. This can be 2400, 9600, 19200, 38400, or on some drivers 115200.
+  */
+  void setBaudRate(long baudRate) const;
+  
+  /*!
+  Sets the deadband.
+  Deadband is stored in EEPROM, so changes persist between power cycles.
+  \param value The deadband value.
+               Motor powers in the range [-deadband, deadband] will be considered in the deadband, and will
+               not prevent the driver from entering nor cause the driver to leave an idle brake state.
+               0 resets to the default, which is 3.
+  */
+  void setDeadband(byte value) const;
+  
+  /*!
+  Sets the ramping.
+  Ramping is stored in EEPROM, so changes persist between power cycles.
+  \param value The ramping value. Consult the user manual for possible values.
+  */
+  void setRamping(byte value) const;
+  
+  /*!
+  Sets the serial timeout.
+  \param milliseconds The maximum time in milliseconds between packets. If this time is exceeded,
+                      the driver will stop the motors. This value is rounded up to the nearest 100 milliseconds.
+                      This library assumes the command value is in units of 100 milliseconds. This is true for
+                      most drivers, but not all. Check the packet serial chapter of the driver's user manual
+                      to make sure.
+  */
+  void setTimeout(int milliseconds) const;
+  
+private:
+  void throttleCommand(byte command, int power) const;
+  
+private:
+  const byte        _address;
+  SabertoothStream& _port; 
+};
+
+#endif

platformio.ini

@@ -14,10 +14,15 @@ board = rpipicow
 framework = arduino
 platform_packages = 
 	framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master
-upload_port = /run/media/amelia/RPI-RP2/
+upload_port = /mnt/pico
 board_build.core = earlephilhower
 lib_deps = 
 	xreef/PCF8574 library@^2.3.6
 	gbr1/rp2040-encoder-library@^0.1.1
 	trevorwslee/DumbDisplay Arduino Library@^0.9.841
+	ftjuh/I2Cwrapper@^0.5.0
+	waspinator/AccelStepper@^1.64
+	olikraus/Ucglib@^1.5.2
+	107-systems/107-Arduino-Servo-RP2040@^0.1.0
 debug_tool = cmsis-dap
+build_flags = -O3

src/globals.h

@@ -1,5 +1,7 @@
 #pragma once
 #include "packet.h"
+#include <math.h>
+#define _USE_MATH_DEFINES // To access M_PI constant for trig functions
 
 extern packet_t pA, pB, safe;
 extern packet_t *astate, *incoming;
@@ -7,7 +9,93 @@ extern comm_state cs;
 extern char comm_ok;
 extern long last_p;
 
+/*
+#define CLOCKWISE 0
+#define ANTICLOCKWISE 1
+*/
 
+// Loop timing
+#define LOOP_DELAY_MS_CORE_0 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS / 1000.0f) // Previous number expressed in seconds
+
+// Math things
+#define DEGREES_PER_RADIAN (180.0 / M_PI)
+#define RADIANS_PER_DEGREE (M_PI / 180.0)
+#define max(x,y) ( (x) > (y) ? (x) : (y) )
+#define min(x,y) ( (x) < (y) ? (x) : (y) )
+
+#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
+
+// Drive modes
+#define DRIVE_STOP          0
+#define DRIVE_BASIC         1
+#define DRIVE_TRANSLATION   2
+#define DRIVE_ROTATION      3
+#define DRIVE_TANK          4
+
+// Default drive mode, currently default to stopping upon initialization
+#define DEFAULT_SWERVE_DRIVE_MODE DRIVE_STOP
+
+// Whether to enable swerve drive steering by default (set this to false if the robot is locked to tank drive)
+#define DEFAULT_SWERVE_ENABLE_STEERING true
+
+// Controller maximum inputs for joystick
+#define CONTROLLER_JOYSTICK_MAX 128.0
+
+// Basic mode conditions, specifies which direction and turning direction the robot is using
+#define DRIVE_BASIC_STOP        0
+#define DRIVE_BASIC_FORWARD     1
+#define DRIVE_BASIC_FRONTLEFT   2
+#define DRIVE_BASIC_FRONTRIGHT  3
+#define DRIVE_BASIC_BACKWARD    4
+#define DRIVE_BASIC_BACKLEFT    5
+#define DRIVE_BASIC_BACKRIGHT   6
+
+// Length of the buffer to monitor recent steering encoder positions to calculate speed
+// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the steering motors' current speeds
+// This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
+#define ENCODER_BUFFER_ENTRY_COUNT 5
+
+// Number of encoder ticks per degree of rotation for the swerve drive steering motors
+#define STEERING_ENCODER_TICKS_PER_DEGREE (1024.0 * 4.0) / 360.0 // TODO check as of 20230927
+
+// Maximum speed allowed for the steering motors (out of 127.0)
+#define STEERING_MOTOR_SPEED_LIMIT 127.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle
+
+// Start decelerating the steering motors linearly when they're within this many degrees of their target angle
+#define STEERING_SLOW_DELTA 30.0
+
+// Claw status
+#define CLAW_UNKNOWN        1   // Position unknown
+#define CLAW_STOPPED        2   // Stopped in current position (somewhere between open and closed)
+#define CLAW_CLOSED         3   // Claw is currently closed
+#define CLAW_OPEN           4   // Claw is currently open
+#define CLAW_MOVING         5   // The claw is currently moving
+
+// Claw commands
+#define CLAW_COMMAND_UNSET 0    // No command has been set yet, not doing anything
+// #define CLAW_COMMAND_STOP   1   // Stop immediately, no matter the location
+#define CLAW_COMMAND_CLOSE  2   // Close the claw
+#define CLAW_COMMAND_OPEN   3   // Open the claw
+
+// Claw things
+#define CLAW_OPEN_ANGLE 90.0f       // Open position of the claw
+#define CLAW_CLOSED_ANGLE -90.0f    // Closed position of the claw
+#define CLAW_DEFAULT_ANGLE 0.0f     // Default starting claw position
+
+// Tilt servo control parameters
+#define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
+#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
+#define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update
+#define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo
+#define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo
+#define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo
+
+// Tilt servo commands
+#define TILT_COMMAND_UNSET 0
+#define TILT_COMMAND_UP 1
+#define TILT_COMMAND_DOWN 2
 
 #define SerComm Serial1   //Serial port connected to Xbee
 #define DIAMOND_LEFT 0

src/main.cpp

@@ -1,22 +1,35 @@
 #include <Arduino.h>
 #include <WiFi.h>
 #include <Wire.h>
+#include <AccelStepperI2C.h>
 #include "globals.h"
 #include <SPI.h>
 #include "MCP3XXX.h"
 #include "PCF8574.h"
 #include "pio_encoder.h"
-#include "dumbdisplay.h"
+//#include "dumbdisplay.h"
-#include "wifidumbdisplay.h"
+//#include "wifidumbdisplay.h"
 #include "zserio.h"
 #include "SerialUART.h"
+#include <Sabertooth.h>
+#include <107-Arduino-Servo-RP2040.h>
+#include "swerve.h"
+#include "manipulator.h"
 
 const char* ssid = "TEST";
 const char* password = "pink4bubble";
-DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
+//DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
-LcdDDLayer *optionsdisplay = NULL;
+//LcdDDLayer *optionsdisplay = NULL;
-SevenSegmentRowDDLayer *sevenSeg; 
+//SevenSegmentRowDDLayer *sevenSeg; 
-JoystickDDLayer *appjoystick;
+//JoystickDDLayer *appjoystick;
+
+// Swerve Drive control
+swerve_drive swrv;
+// bool drive_type = DRIVE_TRANSLATION;
+int drive_mode = DRIVE_TRANSLATION; 
+
+// Manipulator control (arm and claw)
+manipulator_arm clawarm;
 
 packet_t pA, pB, safe;
 packet_t *astate, *incoming;
@@ -27,8 +40,66 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0
 
 PCF8574 ioex1(0x20, 20, 21);
 PCF8574 ioex2(0x21, 20, 21);
-PioEncoder enc1(18); // right
+
-PioEncoder enc2(14); // left
+// JANK: soldered to pico or headers
+PioEncoder enc1(18); // Front Left
+PioEncoder enc2(14); // Front Right
+PioEncoder enc3(26); // Back Left
+PioEncoder enc4(0);  // Back Right
+
+// pins for arm servos
+#define ARM_SERVO_PIN_1 2
+#define ARM_SERVO_PIN_2 3
+#define ARM_SERVO_PIN_3 8
+
+// stepper board slave pins
+#define MOTOR_X_ENABLE_PIN 8
+#define MOTOR_X_STEP_PIN   2
+#define MOTOR_X_DIR_PIN    5
+#define MOTOR_Y_ENABLE_PIN 8
+#define MOTOR_Y_STEP_PIN   3
+#define MOTOR_Y_DIR_PIN    6
+#define MOTOR_Z_ENABLE_PIN 8
+#define MOTOR_Z_STEP_PIN   4
+#define MOTOR_Z_DIR_PIN    7
+uint8_t i2cAddress = 0x08; // arduino address
+I2Cwrapper wrapper(i2cAddress);
+AccelStepperI2C stepperX(&wrapper); // Stepper Motor 1
+AccelStepperI2C stepperY(&wrapper); // Stepper Motor 2
+AccelStepperI2C stepperZ(&wrapper); // Stepper Motor 3
+int stepperX_pos = 0;
+int stepperY_pos = 0;
+int stepperZ_pos = 0;
+
+uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
+uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core
+uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
+
+#define defMaxSpeed     8000
+#define defAcceleration 8000
+
+Sabertooth swivel[2] = { Sabertooth(128, Serial2), Sabertooth(129, Serial2) };
+Sabertooth actuators(130, Serial2);
+
+#define MIN_MICROS        880
+#define MAX_MICROS        1985
+#define OC_OFFSET         0 // microseconds
+
+#define TALON_PIN_1       5
+#define TALON_PIN_2       6
+#define TALON_PIN_3       7
+#define TALON_PIN_4       9
+
+static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
+
+typedef struct
+{
+  int     servoIndex;
+  uint8_t servoPin;
+} ISR_servo_t;
+
+
+#define NUM_SERVOS            7
 
 MCP3008 adc;
 int count = 0;
@@ -46,7 +117,26 @@ int acceleration = 1;
 bool turbo = false;
 int left_cooldown = 0;
 int right_cooldown = 0;
-int olddisplay = 99999;
+int olddisplay = 99999; // guarantee a change when first value comes in
+
+// motor indeces for set_motor() function
+#define FLSTEER 1
+#define FRSTEER 2
+#define BLSTEER 3
+#define BRSTEER 4
+#define FLDRIVE 5
+#define FRDRIVE 6
+#define BLDRIVE 7
+#define BRDRIVE 8
+#define EXTEND1 9
+#define EXTEND2 10
+#define LIFT1 11
+#define LIFT2 12
+#define LIFT3 13
+#define LIFTALL 14
+#define ARMSERVO1 15
+#define ARMSERVO2 16
+#define ARMSERVO3 17
 
 unsigned int getButton(unsigned int num) {
   if (num <= 7) {
@@ -63,7 +153,7 @@ unsigned int getDPad() {
 }
 
 
-void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
+/*void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
     if (pLayer == optionsdisplay) {
         // clicked the "clear" button
         if(turbo) {
@@ -76,7 +166,7 @@ void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
         }
         //delay(100);
     } 
-}
+}*/
 
 void osmc_init() {
   digitalWrite(ALI1, LOW);
@@ -105,7 +195,7 @@ void osmc_init() {
 
   digitalWrite(22, LOW);
   pinMode(22, OUTPUT);
-  analogWriteFreq(4000); // set PWM frequency to 16kHz
+  analogWriteFreq(4000); // set PWM frequency to 4kHz
 }
 
 char try_enable_osmc(char enabled, char enablepin, float vbatt,
@@ -238,29 +328,33 @@ void set_mosfet(bool pin, bool value) {
 
 void set_digit(byte digit, byte value) 
 {
-  Wire.beginTransmission(0x38);
+  /*Wire.beginTransmission(0x38);
   Wire.write(0x20 + digit);
   Wire.write(d[value]);
-  Wire.endTransmission();
+  Wire.endTransmission();*/
+  
   //Serial.print("Set digit ");
   //Serial.print(digit);
   //Serial.print(" to ");
   //Serial.println(value);
   //delay(5000);
+  return;
 }
 
 void set_raw(byte digit, byte value) {
-  Wire.beginTransmission(0x38);
+  /*Wire.beginTransmission(0x38);
   Wire.write(0x20 + digit);
   Wire.write(value);
-  Wire.endTransmission();
+  Wire.endTransmission();*/
+  return;
 }
 void set_blank() {
-  Wire.beginTransmission(0x38);
+  /*Wire.beginTransmission(0x38);
   Wire.write(0x20);
   Wire.write((byte)0);
   Wire.write((byte)0);
-  Wire.endTransmission();
+  Wire.endTransmission();*/
+  return;
 }
 void set_hex(byte num) {
   byte digit1 = num; 
@@ -274,9 +368,8 @@ void set_hex(byte num) {
   }
   set_digit(0, digit1);
   set_digit(1, digit2);
-  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
+  if(num != olddisplay && millis() % 10 < 1) {
     olddisplay = num;
-    sevenSeg->showHexNumber(num);
   }
   set_raw(4, 0x00); // clear second dot
 }
@@ -292,13 +385,102 @@ void set_dec(byte num) {
   }
   set_digit(0, digit1);
   set_digit(1, digit2);
-  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
+  if(num != olddisplay && millis() % 10 < 1) {
     olddisplay = num;
-    sevenSeg->showNumber(num);
   }
   set_raw(4, 0x02); // set second dot
 }
 
+void set_motor(byte motor, int speed) {
+  // 1 - 4   : swivel motors on Sabertooth 1-2
+  // 5 - 8   : drive motors on Talon SRX
+  // 9 - 10  : actuators on Sabertooth 3
+  // 11 - 13 : Steppers on slave board
+  // 14      : drive 11-13 with identical position & speed
+  // 15 - 17 : arm servos
+  // speed is -127 to 127
+  Serial.print("Driving motor ");
+  Serial.print(motor);
+  Serial.print(" with speed ");
+  Serial.println(speed);
+  if (motor <= 4) {
+    // swerve controls
+    swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
+  }
+  else if (motor == 5)
+    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 6)
+    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 7)
+    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 8)
+    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor <= 10) {
+    // actuator controls
+    actuators.motor((motor - 9 + 1), speed);
+  }
+  // set servos
+  // speed needs to be a high number to be reasonable
+  else if (motor == 11) {
+    //stepperX.setSpeed((float)speed);
+    if (abs(speed) > 0) 
+      ioex1.digitalWrite(2, LOW); // enable
+      
+    
+    stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+    stepperX_pos = speed * 96 + stepperX.currentPosition();
+    stepperX.moveTo(stepperX_pos);
+    stepperX.runState();
+    //stepperX.runSpeedState();
+  }
+  else if (motor == 12) {
+    //stepperY.setSpeed((float)speed);
+    if (abs(speed) > 0) 
+      ioex1.digitalWrite(2, LOW); // enable
+      
+
+    stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+    stepperY_pos = speed * 96 + stepperY.currentPosition();
+    stepperY.moveTo(stepperY_pos);
+    stepperY.runState();
+    //stepperY.runSpeedState();
+  }
+  else if (motor == 13) {
+    //stepperY.setSpeed((float)speed);
+    if (abs(speed) > 0) 
+      ioex1.digitalWrite(2, LOW); // enable
+      
+    stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+    stepperZ_pos = speed * 96 + stepperZ.currentPosition();
+    stepperZ.moveTo(stepperZ_pos);
+    stepperZ.runState();
+    //stepperY.runSpeedState();
+  }
+  else if (motor == 14) { // all steppers together
+    if (abs(speed) > 0) {
+      ioex1.digitalWrite(2, LOW); // enable
+      stepperX_pos = speed * 96 + stepperX.currentPosition();
+
+      stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+      stepperX.moveTo(stepperX_pos);
+      stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+      stepperY.moveTo(stepperX_pos);
+      stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+      stepperZ.moveTo(stepperX_pos);
+      stepperX.runState();
+      stepperY.runState();
+      stepperZ.runState();
+    } else {
+      ioex1.digitalWrite(2, HIGH); // disable
+    }
+  }
+  else if (motor == 15)
+    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 16)
+    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 17)
+    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+}
 
 void setup() {
   WiFi.noLowPowerMode();
@@ -310,7 +492,7 @@ void setup() {
   digitalWrite(LED_BUILTIN, HIGH);
   Serial.begin(115200);
   //Serial.println("hello!");
-  delay(2000);
+  //delay(2000);
   Serial.println("Initializing RIB subsystems..");
 
 
@@ -329,69 +511,181 @@ void setup() {
   Wire.write(0x07); // display mode register
   Wire.write(0x01); // display test mode
   Wire.endTransmission();
-  delay(100);
 
   Wire.beginTransmission(0x38);
   Wire.write(0x07);
   Wire.write(0x00); // disable display test mode
   Wire.endTransmission();
   Serial.println(" done");
-  
-
-  Serial.print("Initializing ADC..");
-  set_hex(0x1);
-  SPI1.setRX(12);
-  SPI1.setCS(13);
-  SPI1.setTX(11);
-  SPI1.setSCK(10);
-  adc.begin(13);
-  //pinMode(13, OUTPUT);
-  //pinMode(11, OUTPUT);
-  //pinMode(10, OUTPUT);
-  //digitalWrite(13, HIGH);
-  //digitalWrite(11, HIGH);
-  //digitalWrite(10, HIGH);
-  //adc.begin(13,11,12,10);
-  Serial.println(" done");
-  delay(20);
-
-  Serial.print("Initializing OSMCs..");
-  set_hex(0x2);
-  osmc_init();
-  Serial.println(" done");
-  delay(20);
-  //delay(2000);
-  
 
   bool ioex1p, ioex2p = false;
   Serial.print("Initializing I/O expanders..");
-  set_hex(0x3);
+  set_hex(0x1);
+  
   if(ioex1.begin()) {
-    delay(200);
+    for (int i = 0; i < 8; i++) {
+      ioex1.pinMode(i,OUTPUT);
+      ioex1.digitalWrite(i, LOW);
+    }
+    delay(20);
     Serial.print(" 1");
     ioex1p = true;
   } else {
     delay(20);
   }
-  set_hex(0x4);
+  set_hex(0x2);
+
   if(ioex2.begin()) {
+    for (int i = 0; i < 8; i++) {
+      ioex2.pinMode(i,OUTPUT);
+      ioex2.digitalWrite(i, LOW);
+    }
     delay(20);
     Serial.print(" 2");
     ioex2p = true;
   } else {
     delay(20);
   }
+  
+
+  Serial.print("Initializing ADC..");
+  set_hex(0x3);
+  SPI1.setRX(12);
+  SPI1.setCS(13);
+  SPI1.setTX(11);
+  SPI1.setSCK(10);
+  adc.begin(13);
+  Serial.println(" done");
+  delay(20);
+
+  //Serial.print("Initializing OSMCs..");
+  //set_hex(0x2);
+  //osmc_init();
+  //Serial.println(" done");
+  //delay(20);
+  ////delay(2000);
+
+  
+
+  
+  Serial.println(" done");
+  delay(20);
+
+  Serial.print("Initializing motor controllers..");
+  set_hex(0x4);
+
+  ioex1.pinMode(3, OUTPUT); // reset arduino
+  ioex1.digitalWrite(3, LOW);
+  delay(20);
+  ioex1.digitalWrite(3, HIGH);
+  delay(2000);
+
+  digitalWrite(ALI1, LOW);
+  digitalWrite(BLI1, LOW);
+  digitalWrite(AHI1, LOW);
+  digitalWrite(BHI1, LOW);
+  digitalWrite(ALI2, LOW);
+  digitalWrite(BLI2, LOW);
+  digitalWrite(AHI2, LOW);
+  digitalWrite(BHI2, LOW);
+  
+  pinMode(ALI1, OUTPUT);
+  pinMode(AHI1, OUTPUT);
+  pinMode(BLI1, OUTPUT);
+  pinMode(BHI1, OUTPUT);
+  pinMode(ALI2, OUTPUT);
+  pinMode(AHI2, OUTPUT);
+  pinMode(BLI2, OUTPUT);
+  pinMode(BHI2, OUTPUT);
+
+
+  digitalWrite(22, LOW); // finally, enable output buffers
+  pinMode(22, OUTPUT);
+
+  
+
+  // enable stepper slave board
+  if (!wrapper.ping()) {
+    Serial.println("Arduino stepper driver not found!");
+    while (true) {}
+  }
+  wrapper.reset(); // reset the target device
+                   // note: this doesn't fucking work
+                   // we reset by flipping the reset pin above
+
+  wrapper.setI2Cdelay(20); // Set small delay for I2C bus communication
+
+  stepperX.attach(AccelStepper::DRIVER, MOTOR_X_STEP_PIN, MOTOR_X_DIR_PIN); // Stepper Motor 1 - X
+  stepperY.attach(AccelStepper::DRIVER, MOTOR_Y_STEP_PIN, MOTOR_Y_DIR_PIN); // Stepper Motor 2 - Y
+  stepperZ.attach(AccelStepper::DRIVER, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN); // Stepper Motor 3 - Z
+
+  Serial.print("stepperX.myNum=");
+  Serial.println(stepperX.myNum);
+  Serial.print("stepperY.myNum=");
+  Serial.println(stepperY.myNum);
+  Serial.print("stepperZ.myNum=");
+  Serial.println(stepperZ.myNum);
+  if (stepperX.myNum < 0) { // Should not happen after a reset
+    Serial.println("Error: stepperX could not be allocated");
+    while (true) {}
+  }
+  if (stepperY.myNum < 0) { // Should not happen after a reset
+    Serial.println("Error: stepperY could not be allocated");
+    while (true) {}
+  }
+  if (stepperZ.myNum < 0) { // Should not happen after a reset
+    Serial.println("Error: stepperZ could not be allocated");
+    while (true) {}
+  }
+  stepperX.setMaxSpeed(defMaxSpeed);
+  stepperX.setAcceleration(defAcceleration);
+  stepperY.setMaxSpeed(defMaxSpeed);
+  stepperY.setAcceleration(defAcceleration);
+  stepperZ.setMaxSpeed(defMaxSpeed);
+  stepperZ.setAcceleration(defAcceleration);
+  ioex1.digitalWrite(2, HIGH); // setup output pin
+  ioex1.pinMode(2, OUTPUT);
+  ioex1.digitalWrite(2, HIGH);
+
+
+  // Sabertooth init
+  Serial2.setTX(4);
+  Serial2.setRX(5);
+  Serial2.begin(2400); // Sabertooths on modulus have been reprogrammed to 2400 baud! ST1.setBaudrate(2400); from another pico
+  Sabertooth::autobaud(Serial2);
+  swivel[0].setTimeout(100);
+  swivel[1].setTimeout(100);
+  actuators.setTimeout(100);
+  
+
+  // Servo & Talon SRX init
+  // talon0, talon1, talon2, talon3, arm1, arm2, arm3
+  talon1.attach(TALON_PIN_1);
+  talon2.attach(TALON_PIN_2);
+  talon3.attach(TALON_PIN_3);
+  talon4.attach(TALON_PIN_4);
+
+  arm1.attach(ARM_SERVO_PIN_1);
+  arm2.attach(ARM_SERVO_PIN_2);
+  arm3.attach(ARM_SERVO_PIN_3);
+  
 
   Serial.println(" done");
   delay(20);
 
+
+  
+  // Initialize encoders
   Serial.print("Initializing encoders..");
   set_hex(0x5);
-  enc1.begin();
+  //enc1.begin();
-  enc2.begin();
+  //enc2.begin();
+  //enc3.begin();
+  //enc4.begin();
   Serial.println(" done");
   delay(20);
 
+  // Radio initialization
   Serial.print("Initializing xBee radio..");
   set_hex(0x6);
   SerComm.setRX(17);
@@ -400,6 +694,8 @@ void setup() {
   comm_init(); //Initialize the communication FSM
   safe.stickX = 127;
   safe.stickY = 127;
+  safe.stickRX = 127;
+  safe.stickRY = 127;
   safe.btnhi = 0;
   safe.btnlo = 0;
   safe.cksum = 0b1000000010001011;
@@ -450,34 +746,34 @@ void setup() {
   set_offset();
   Serial.println("done");
 
-  Serial.print("Checking OSMC 1..");
+  //Serial.print("Checking OSMC 1..");
-  set_hex(0x9);
+  //set_hex(0x9);
-  if (get_voltage(0) > 13) {
+  //if (get_voltage(0) > 13) {
-    Serial.println(" done");
+  //  Serial.println(" done");
-    delay(20);
+  //  delay(20);
-    pass++;
+  //  pass++;
-  } else {
+  //} else {
-    Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
+  //  Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
-    set_hex(0xF9);
+  //  set_hex(0xF9);
-    delay(500);
+  //  delay(500);
-  }
+  //}
-  set_hex(0xA);
+  //set_hex(0xA);
-  Serial.print("Checking OSMC 2..");
+  //Serial.print("Checking OSMC 2..");
-  if (get_voltage(1) > 13) {
+  //if (get_voltage(1) > 13) {
-    Serial.println(" done");
+  //  Serial.println(" done");
-    delay(20);
+  //  delay(20);
-    pass++;
+  //  pass++;
-  } else {
+  //} else {
-    Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
+  //  Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
-    set_hex(0xFA);
+  //  set_hex(0xFA);
-    delay(500);
+  //  delay(500);
-  }
+  //}
 
   set_hex(0xB);
   Serial.print("Checking I/O expander 1..");
-  for (int i = 0; i < 8; i++) {
+  // for (int i = 0; i < 8; i++) {
-    ioex1.pinMode(i,OUTPUT, LOW);
+  //   ioex1.pinMode(i,OUTPUT, LOW);
-  }
+  // }
   if(ioex1p == false) {
     Serial.println(" WARNING: I/O expander not detected");
     set_hex(0xFB);
@@ -489,9 +785,9 @@ void setup() {
   }
   set_hex(0xC);
   Serial.print("Checking I/O expander 2..");
-  for (int i = 0; i < 8; i++) {
+  // for (int i = 0; i < 8; i++) {
-    ioex2.pinMode(i,OUTPUT, LOW);
+  //   ioex2.pinMode(i,OUTPUT, LOW);
-  }
+  // }
   if(!ioex2p == false) {
     Serial.println(" WARNING: I/O expander not detected");
     set_hex(0xFC);
@@ -504,7 +800,7 @@ void setup() {
 
   Serial.print("Self tests complete: ");
   Serial.print(pass);
-  Serial.println("/6 tests passed.");
+  Serial.println("/4 tests passed.");
   Serial.println("RIB is ready to go. Starting program.");
   set_blank();
   /*dumbdisplay.recordLayerSetupCommands();
@@ -523,7 +819,21 @@ void setup() {
                                     optionsdisplay->getLayerId())));
   dumbdisplay.playbackLayerSetupCommands("basic");*/
   //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
+
+
+  // Initialize swerve_drive struct, load with initial encoder values
+  int front_left_encoder = enc1.getCount(), front_right_encoder = enc2.getCount(), back_left_encoder = enc3.getCount(), back_right_encoder = enc4.getCount();
+  swrv = initializeSwerveDrive(front_left_encoder, front_right_encoder, back_left_encoder, back_right_encoder);
+
+  // Initialize manipulator
+  clawarm = initializeManipulatorArm(clawarm);
+  clawarm = setArmSpeedLimit(clawarm, 1.0f);
+  clawarm = setArmSpeedCoefficient(clawarm, 1.0f);
+
+  // Setup 
   setup_complete = true;
+
+  
   rp2040.wdt_begin(500); // start watchdog with 500ms limit. Safety feature; reset during crash to disable motors!
 }
 
@@ -551,15 +861,30 @@ void print_status() {
 
 }
 
+void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \
+               float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude) { // Print encoder positions for steering motors
+  
+  char buffer[300] = "\0";
+
+  // Create a new line to show the start of this telemetry cycle
+  //Serial.println(buffer);
+  //SerComm.println(buffer);
+
+  // Encoder data and loop number
+  sprintf(buffer, "Loop = %llu  E1 = %i  E2 = %i  E3 = %i  E4 = %i  z_lx = %f  z_ly = %f  z_rx = %f  z_ry = %f  mode = %i  mag-L = %f  ang-L = %f mag-R = %f  ang-R = %f", \
+  loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
+  zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \
+  left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle);
+  Serial.println(buffer);
+  SerComm.println(buffer);
+}
 
 void loop() {
+  previous_loop_start_time_core_0 = millis(); // Track the start time of this loop to determine how long to sleep before the next loop
+  
   rp2040.wdt_reset();
   
   comm_parse();
-  if (getButton(SHOULDER_TOP_RIGHT))
-    turbo = true;
-  else 
-    turbo = false;
   //const DDFeedback*  fb;
   /*if (!dumbdisplay.connected() || !WiFi.isConnected()) {
     target_left_power = 0;
@@ -569,7 +894,115 @@ void loop() {
   } else
     fb = appjoystick->getFeedback();*/
   
-  int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
+  int zeroed_lx =    ((int)(astate->stickX) - 127);
+  int zeroed_ly =    -1 * ((int)(astate->stickY) - 127);
+  int zeroed_rx =     ((int)(astate->stickRX) - 127);
+  int zeroed_ry =    -1 * ((int)(astate->stickRY) - 127);
+
+  // Modulus swerve drive
+
+  // Modify controller joystick inputs to be within a range of -1.0 to 1.0
+  float zeroed_lx_float = ((float) zeroed_lx) / CONTROLLER_JOYSTICK_MAX;
+  float zeroed_ly_float = ((float) zeroed_ly) / CONTROLLER_JOYSTICK_MAX;
+  float zeroed_rx_float = ((float) zeroed_rx) / CONTROLLER_JOYSTICK_MAX;
+  float zeroed_ry_float = ((float) zeroed_ry) / CONTROLLER_JOYSTICK_MAX;
+
+  // Joystick calculations
+  float left_joystick_magnitude = sqrt(pow(zeroed_lx_float,2) + pow(zeroed_ly_float,2));
+  float left_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ly_float, zeroed_lx_float)) + 270.0, 360.0) , 360.0);
+  float right_joystick_magnitude = sqrt(pow(zeroed_rx_float,2) + pow(zeroed_ry_float,2));
+  float right_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ry_float, zeroed_rx_float)) + 270.0, 360.0) , 360.0);
+  //   add 270, fmod 360, 360 - n, fmod 360
+
+  // Drive mode calculation
+  int loop_drive_mode = DRIVE_STOP; // Temporary drive mode specific to this loop, is DRIVE_STOP (0) until it is changed
+
+  if (getButton(SHOULDER_TOP_RIGHT))
+    drive_mode = DRIVE_TRANSLATION;
+  else if(getButton(SHOULDER_TOP_LEFT))
+    drive_mode = DRIVE_BASIC;
+  
+  if (fabs(left_joystick_magnitude) > 0.1 ) { // if left stick is touched
+    loop_drive_mode = drive_mode;
+  }
+  if (fabs(right_joystick_magnitude) > 0.1 ) { // if right stick is touched - override translation
+    loop_drive_mode = DRIVE_ROTATION;
+  }
+
+  // Select operation based on current drive mode
+  switch(loop_drive_mode) {
+    case DRIVE_STOP:
+      swrv = stopSwerve(swrv);
+      swrv.enable_steering = false;
+      break;
+    case DRIVE_BASIC:
+      swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
+      swrv.enable_steering = true;
+      break;
+    case DRIVE_TRANSLATION:
+      swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
+      swrv.enable_steering = true;
+      break;
+    case DRIVE_ROTATION:
+      swrv = rotationDrive(swrv, zeroed_rx_float);
+      swrv.enable_steering = true;
+      break;
+    case DRIVE_TANK:
+      swrv = tankDrive(swrv, left_joystick_angle, left_joystick_magnitude);
+      swrv.enable_steering = false;
+      break;
+  }
+  swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
+  swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
+  
+  // Arm motor control (stepper motors)
+  float arm_speed = 0.0f; // TODO as of 20230928: PLACEHOLDER for input for arm speed 
+  clawarm = setArmSpeed(clawarm, arm_speed);
+
+  // Claw servo control
+  int new_claw_command = CLAW_COMMAND_UNSET;
+  /*
+    // TODO select action for claw
+  */
+  clawarm = updateClawCommand(clawarm, new_claw_command);
+
+  // Tilt servo control
+  int new_tilt_command = TILT_COMMAND_UNSET;
+  /*
+    // TODO select action for the tilt servo
+  */
+  clawarm = updateTiltCommand(clawarm, new_tilt_command);
+
+  telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude); // DEBUG ONLY, print steering motor encoder positions
+
+  // update motors after calculation
+  set_motor(FLSTEER, swrv.front_left_spin_power);
+  set_motor(BRSTEER, swrv.back_right_spin_power);
+  set_motor(FRSTEER, swrv.front_right_spin_power);
+  set_motor(BLSTEER, swrv.back_left_spin_power);
+  set_motor(FLDRIVE, swrv.front_left_power);
+  set_motor(BRDRIVE, swrv.back_right_power);
+  set_motor(FRDRIVE, swrv.front_right_power);
+  set_motor(BLDRIVE, swrv.back_left_power);
+
+  // update stepper motors
+  set_motor(LIFTALL, clawarm.arm_set_motor_int);
+
+  // update servos
+  /*
+  // TODO: Figure out servo mapping
+  set_motor(SERVOTILT, clawarm.tilt_set_motor_int);
+  set_motor(SERVOCLAW, clawarm.claw_set_motor_int);
+
+  */
+
+
+  /////////////////////////////////////////////////////////////
+  // END OF MODULUS PLUS CODE UNTIL THE END OF THIS FUNCTION //
+  /////////////////////////////////////////////////////////////
+
+  // Goliath / 2 side arcade tank drive code below
+  /*int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
   int zeroed_turn =      ((int)(astate->stickY) - 127);
   
   
@@ -644,10 +1077,10 @@ void loop() {
   //SerComm.println();
   set_hex(avg_speed);
 
-  drive_right(right_enabled, right_power);
+  //drive_right(right_enabled, right_power);
-  drive_left(left_enabled, -left_power);
+  //drive_left(left_enabled, -left_power);
   SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));
-
+  */
   //if(left_power != target_left_power || right_power != target_right_power)
 
   
@@ -712,27 +1145,104 @@ void loop() {
     }
   }*/
   //delay(200);
-  delay(50);
+
+  previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
+  int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time
+  if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
+    delay(delay_time_ms);
+  }
+  loop_counter_core_0++;
   //DDYield();
 
 }
-int loopcount = 0;
+
 void loop1() {
+  previous_loop_start_time_core_1 = millis();
   rp2040.wdt_reset();
   //drive_left(left_enabled, 255);
   //digitalWrite(LED_BUILTIN, HIGH);  
-  if(loopcount == 20) {
+  if(loop_counter_core_1 == 20) {
     //print_status();
-    loopcount = 0;
+    loop_counter_core_1 = 0;
     delay(25);
   }
   else {
     delay(25);
-    loopcount++;
+    //loop_counter_core_1++;
   }
+
   //SerComm.println("update");
-  left_enabled = try_enable_left(left_enabled, get_voltage(1));
+  //left_enabled = try_enable_left(left_enabled, get_voltage(1));
-  right_enabled = try_enable_right(right_enabled, get_voltage(0));
+  //right_enabled = try_enable_right(right_enabled, get_voltage(0));
   //digitalWrite(LED_BUILTIN, LOW);
-  delay(25);
+
-}
+  /*if (stepperX.distanceToGo() == 0) { // Give stepper something to do...
+    if (stepperXdir) {
+      Serial.println("Driving stepper");
+      stepperX.moveTo(stepsToGo);
+    } else {
+      Serial.println("Driving stepper");
+      stepperX.moveTo(-stepsToGo);
+    }
+    stepperX.runState();  
+    stepperXdir = !stepperXdir;
+  }
+  if (stepperY.distanceToGo() == 0) { // Give stepper something to do...
+    if (stepperYdir)
+      stepperY.moveTo(stepsToGo);
+    else
+      stepperY.moveTo(-stepsToGo);    
+    stepperY.runState();  
+    stepperYdir = !stepperYdir;
+  }
+
+  if (stepperZ.distanceToGo() == 0) { // Give stepper something to do...
+    if (stepperZdir)
+      stepperZ.moveTo(stepsToGo);
+    else
+      stepperZ.moveTo(-stepsToGo);
+    stepperZ.runState(); 
+    stepperZdir = !stepperZdir;
+  }*/
+  if (mode == 1) {
+    //set_hex(count);
+    if (count < 125) {
+      count += 1;
+    } else {
+      //count = 0;
+
+      mode = 2;
+    }
+  }
+  if (mode == 2) {
+
+    //set_hex(count);
+    if (count > -125) {
+      count -= 1;
+    } else {
+      //count = 0;
+      
+      mode = 1;
+    }
+  }
+  /*for (int x = 5; x < 9; x++) {
+    set_motor(x, count);  
+  }
+  swivel[0].motor(0, 127);
+  swivel[0].motor(1, 127);
+  swivel[1].motor(0, 127);
+  swivel[1].motor(1, 127);
+  set_motor(14, count); // drive all steppers in sync
+  digitalWrite(0, HIGH);
+  digitalWrite(1, HIGH);
+  digitalWrite(2, HIGH);
+  digitalWrite(3, HIGH);*/
+
+  
+  previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1;
+  int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time
+  if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
+    delay(delay_time_ms);
+  }
+  loop_counter_core_1++;
+}

src/manipulator.cpp

@@ -0,0 +1,126 @@
+#include <Arduino.h>
+#include "globals.h"
+#include "manipulator.h"
+//#include <math.h>
+
+// Utilities
+
+manipulator_arm initializeManipulatorArm(manipulator_arm input) // Initialize an existing manipulator_arm struct
+{
+    manipulator_arm out = input;
+
+    // Do initialization of values here
+    
+    return out;
+}
+
+int servoDegreesToInt(float servo_degrees) // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function
+{
+    return (int)(servo_degrees * (90.0 / 127.0));
+}
+
+float clampServoAngle(float servo_angle) // Clamp an angle for a servo between -90.0 and 90.0
+{
+    return max(-90.0f, min(90.0f, servo_angle));
+}
+
+
+// Tilt servo functions
+
+manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Internal function which sets the target angle of the tilt servo
+{
+    manipulator_arm out = input;
+    new_tilt_angle = clampServoAngle(new_tilt_angle); // Clamp the new tilt angle
+    out.tilt_target_angle = new_tilt_angle;
+    out.tilt_set_motor_int = max(TILT_MIN_ANGLE, min(TILT_MAX_ANGLE, new_tilt_angle));
+    return out;
+}
+
+manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
+{
+    manipulator_arm out = input;
+    if(new_tilt_command != TILT_COMMAND_UNSET && out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Ignore value of 0, make sure that its been long enough since the last update
+        float tilt_angle_offset_direction = 0.0f;
+        switch(new_tilt_command) {
+            case TILT_COMMAND_UP:
+                tilt_angle_offset_direction = 1.0f;
+                break;
+            case TILT_COMMAND_DOWN:
+                tilt_angle_offset_direction = -1.0f;
+                break;
+        }
+        float angle_offset = TILT_ANGLE_UPDATE_DISTANCE * tilt_angle_offset_direction; // Degrees that the target angle is being changed by
+        out = setTiltAngle(out, out.tilt_target_angle + angle_offset);
+    } else { // Increment the number of loops since the previous update, since an update was not performed during this loop
+        out.tilt_angle_loops_since_update++;
+    }
+    
+    return out;
+}
+
+// Claw servo functions
+
+manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle) // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)
+{
+    manipulator_arm out = input;
+    out.claw_target_angle = clampServoAngle(new_claw_angle);
+    return out;
+}
+
+manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) // Update the command and position for the claw servo
+{
+    manipulator_arm out = input;
+    float new_claw_angle;
+    switch(new_claw_command) {
+        case CLAW_COMMAND_UNSET:
+            new_claw_angle = CLAW_DEFAULT_ANGLE;
+            break;
+        case CLAW_COMMAND_OPEN:
+            new_claw_angle = CLAW_OPEN_ANGLE;
+            break;
+        case CLAW_COMMAND_CLOSE:
+            new_claw_angle = CLAW_CLOSED_ANGLE;
+            break;
+        default:
+            new_claw_angle = CLAW_DEFAULT_ANGLE;
+    }
+    out.claw_position = new_claw_angle;
+    out.claw_set_motor_int = servoDegreesToInt(new_claw_angle);
+    return out;
+}
+
+
+// Arm functions (stepper motors)
+
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed
+{
+    manipulator_arm out = input;
+    arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));
+    out.arm_speed = arm_speed;
+    out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed)));
+    return out;
+}
+manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit) // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
+{
+    manipulator_arm out = input;
+    out.arm_speed_limit = min(1.0f, max(0.0f, arm_speed_limit));
+    return out;
+}
+manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient) // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0
+{
+    manipulator_arm out = input;
+    out.arm_speed_coefficient = min(1.0f, max(0.0f, arm_speed_coefficient));;
+    return out;
+}
+
+// Template function
+
+manipulator_arm manipulatorTemplate(manipulator_arm input) // manipulator_arm template function
+{
+    manipulator_arm out = input;
+    
+    // Do stuff
+    
+    return out;
+}
+

src/manipulator.h

@@ -0,0 +1,60 @@
+#include "globals.h"
+
+// Manipulator arm
+
+// Stepper motors for lift
+// Servo to open and close claw
+// Servo to tilt claw
+
+typedef struct {
+    // Claw servo variables
+    int claw_command = CLAW_COMMAND_UNSET;          // COMMAND for claw servo
+    int claw_set_motor_int = 0;                     // COMMAND for claw servo, formatted to integer used in set_motor()
+    int claw_status = CLAW_UNKNOWN;                 // CURRENT claw position, initially unknown
+    float claw_position = CLAW_DEFAULT_ANGLE;       // CURRENT angle of claw servo
+    float claw_target_angle = CLAW_DEFAULT_ANGLE;   // TARGET position of claw servo, -90.0 to 90.0 degrees
+    float claw_power_limit = 0.0f;                  // LIMIT power for the claw servo
+
+    // Tilt servo variables
+    int tilt_command = TILT_COMMAND_UNSET;          // COMMAND for the tilt servo
+    float tilt_target_angle = TILT_FLAT_ANGLE;      // CURRENT angle in degrees for the claw tilt servo, can be between -90.0 and 90.0 degrees
+    int tilt_set_motor_int = 0;                     // Tilt motor target angle value as an integer used for set_motor()
+    int tilt_angle_loops_since_update = 0;          // Number of control loops since the tilt angle was modified, don't modify the tilt angle if this is less than TILT_ANGLE_MIN_UPDATE_LOOPS
+
+    // Arm motor variables (stepper motors)
+    float arm_speed = 0.0f;                         // Arm command, initialize as stopped, between -1.0 and 1.0
+    float arm_speed_limit = 1.0f;                   // Limit applied to the speed of the arm (before the coefficient), must be between 0.0 and 1.0
+    float arm_speed_coefficient = 1.0f;             // Coefficient applied to the arm speed, has no constraints, default to 1.0 (unmodified)
+    int arm_set_motor_int = 0;                      // Arm speed used for the set_motor() function
+
+} manipulator_arm;
+
+
+// Utilities
+
+manipulator_arm initializeManipulatorArm(manipulator_arm input); // Initialize a manipulator_arm struct
+
+int servoDegreesToInt(float servo_degrees); // Convert a servo position in degrees to an integer between -127 and 127 to be used by the set_motor() function
+
+float clampServoAngle(float servo_angle); // Clamp an angle for a servo between -90.0 and 90.0
+
+// Tilt servo functions
+manipulator_arm setTiltAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the tilt servo
+
+manipulator_arm updateTiltCommand(manipulator_arm input, int tilt_command); // Update the command for the tilt motor
+
+// Claw servo functions
+
+manipulator_arm setClawTargetAngle(manipulator_arm input, float new_claw_angle); // Internal function which sets the target angle of the claw servo (NOT THE ACTUAL COMMAND ANGLE)
+
+manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command); // Update the command and position for the claw servo
+
+// Arm functions (stepper motors)
+
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed
+
+manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit); // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
+
+manipulator_arm setArmSpeedCoefficient(manipulator_arm input, float arm_speed_coefficient); // Set the arm's speed coefficient, coefficient must be between 0.0 and 1.0
+
+

src/packet.h

@@ -15,4 +15,6 @@ uint8_t stickY;
 uint8_t btnhi;
 uint8_t btnlo;
 uint16_t cksum;
+uint8_t stickRX; // add right stick, without changieng existing var names or ordering
+uint8_t stickRY; 
 } packet_t;

src/swerve.cpp

@@ -0,0 +1,467 @@
+#include <Arduino.h>
+#include "globals.h"
+#include "swerve.h"
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+
+// 3-mode drive control system based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html
+
+
+template <typename T> int signum(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
+// Utility functions
+
+float closestAngle(float current, float target) // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
+{
+        // get direction
+        float dir = normalizeAngle(target) - normalizeAngle(current);
+
+        // convert from -360 to 360 to -180 to 180
+        if (abs(dir) > 180.0)
+        {
+                dir = -(signum(dir) * 360.0) + dir;
+        }
+        return dir;
+}
+
+swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Initialize a swerve_drive struct, use the current time and encoder positions to fill the arrays
+{
+    swerve_drive out;
+
+    for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++) // Iterate over the elements in the arrays
+    {
+        out.encoder_buffer_times_ms[i] = 0; // Initialize the contents of this array to 0, since no ongoing encoder samples have been taken yet
+        out.encoder_buffer_front_left[i] = front_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet 
+        out.encoder_buffer_front_right[i] = front_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
+        out.encoder_buffer_back_left[i] = back_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
+        out.encoder_buffer_back_right[i] = back_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
+    }
+    out.encoder_buffer_times_ms[0] = millis(); // Record an initial encoder state sample
+
+    // Store the initial positions of the steering motor encoders
+    out.front_left_initial_spin_encoder = front_left_encoder;
+    out.front_right_initial_spin_encoder = front_right_encoder;
+    out.back_left_initial_spin_encoder = back_left_encoder;
+    out.back_right_initial_spin_encoder = back_right_encoder;
+
+    return out;
+}
+
+// This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
+swerve_drive updateSwerveCommand(swerve_drive input)
+{
+    swerve_drive out = input;
+
+    // Set the new speed of the steering motors
+    if((out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) && out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
+        // Calculate the distance and direction each motor needs to steer from where it is now
+        float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
+        float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
+        float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
+        float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
+        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
+        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
+        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
+        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
+    } else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
+        out.front_left_spin_power = 0.0f;
+        out.front_right_spin_power = 0.0f;
+        out.back_left_spin_power = 0.0f;
+        out.back_right_spin_power = 0.0f;
+    }
+
+    // Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
+    out.current_drive_power = out.target_drive_power;  
+
+    // Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
+    out.front_left_power = out.current_drive_power * out.front_left_coefficient * MOTOR_MAX_POWER;
+    out.front_right_power = out.current_drive_power * out.front_right_coefficient * MOTOR_MAX_POWER;
+    out.back_left_power = out.current_drive_power * out.back_left_coefficient * MOTOR_MAX_POWER;
+    out.back_right_power = out.current_drive_power * out.back_right_coefficient * MOTOR_MAX_POWER;
+    
+    return out;
+}
+
+float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle
+{
+    float abs_steering_delta = fabs(steering_delta);
+    if(abs_steering_delta > STEERING_SLOW_DELTA) { // In full speed range, still far enough away from the target angle
+        return STEERING_MOTOR_SPEED_LIMIT;
+    } else { // Slow down the speed of the steering motor since it's close to its target angle
+        return STEERING_MOTOR_SPEED_LIMIT * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+    }
+}
+
+swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Process new encoder data, calculate the speed and angle of the steering motors
+{
+    swerve_drive out = in;
+    // Move values over 1 slot in the encoder data buffer to make room for new data, discard data at the end of the buffer
+    for(int i = 0; i < ENCODER_BUFFER_ENTRY_COUNT - 1; i++)
+    {
+        out.encoder_buffer_times_ms[i + 1] = out.encoder_buffer_times_ms[i];
+        out.encoder_buffer_front_left[i + 1] = out.encoder_buffer_front_left[i];
+        out.encoder_buffer_front_right[i + 1] = out.encoder_buffer_front_right[i];
+        out.encoder_buffer_back_left[i + 1] = out.encoder_buffer_back_left[i];
+        out.encoder_buffer_back_right[i + 1] = out.encoder_buffer_back_right[i];
+    }
+
+    uint64_t current_time_ms = millis(); // Milliseconds since pico startup, using 64 bit value for safety
+    out.encoder_buffer_times_ms[0] = current_time_ms;
+    out.encoder_buffer_front_left[0] = front_left_encoder;
+    out.encoder_buffer_front_right[0] = front_right_encoder;
+    out.encoder_buffer_back_left[0] = back_left_encoder;
+    out.encoder_buffer_back_right[0] = back_right_encoder;
+
+    if(current_time_ms == 0) { // Set current_time_ms to a nonzero value if it is zero to prevent a division by zero later in the function
+        current_time_ms = 1;
+    }
+    
+    // Calculate the speed in degrees per second of each of the steering motors
+    out.front_left_measured_spin_speed = 0.0f;
+    out.front_right_measured_spin_speed = 0.0f;
+    out.back_left_measured_spin_speed = 0.0f;
+    out.back_right_measured_spin_speed = 0.0f;
+    for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++)
+    {
+        out.front_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_left[0] - out.encoder_buffer_front_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
+        out.front_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_right[0] - out.encoder_buffer_front_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
+        out.back_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_left[0] - out.encoder_buffer_back_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
+        out.back_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_right[0] - out.encoder_buffer_back_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
+    }
+    
+    out.front_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
+    out.front_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
+    out.back_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
+    out.back_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
+
+    // Calculate the current orientation of each drive wheel in degrees
+    out.front_left_spin_angle = ((float) (front_left_encoder - out.front_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.front_right_spin_angle = ((float) (front_right_encoder - out.front_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.back_left_spin_angle = ((float) (back_left_encoder - out.back_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.back_right_spin_angle = ((float) (back_right_encoder - out.back_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+
+    return out;
+}
+
+float normalizeAngle(float angle) { // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
+    angle = fmod(angle, 360);
+    if(angle < 0.0) {
+        angle += 360;
+    }
+    return angle;
+}
+
+swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the motor speed coefficients for each motor
+    swerve_drive out = input;
+    out.front_left_coefficient = front_left;
+    out.front_right_coefficient = front_right;
+    out.back_left_coefficient = back_left;
+    out.back_right_coefficient = back_right;
+    return out;
+}
+
+swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the target spin for each wheel
+    swerve_drive out = input;
+    out.front_left_target_spin = front_left + input.spin_offset;
+    out.front_right_target_spin = front_right + input.spin_offset;
+    out.back_left_target_spin = back_left + input.spin_offset;
+    out.back_right_target_spin = back_right + input.spin_offset;
+    return out;
+}
+
+swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) { // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
+    swerve_drive out = input;
+    
+    float delta_spin_offset = new_spin_offset - input.spin_offset;
+    
+    out.front_left_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.front_right_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.back_left_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
+    return out;
+}
+
+swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power
+    swerve_drive out = input;
+    out.target_drive_power = target_drive_power;
+    return out;
+}
+
+// Drive mode functions
+
+swerve_drive stopSwerve(swerve_drive input) // Stop all motors
+{
+    swerve_drive out = input;
+
+    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
+    out = setDriveTargetPower(out, 0.0); // Set the power
+    return out;
+}
+
+swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
+{
+    swerve_drive out = input;
+    
+    float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
+
+    out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); // Set the target angle for each rotation motor
+    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
+    out = setDriveTargetPower(out, target_speed); // Set the power
+    return out;
+}
+
+swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise
+{
+    swerve_drive out = input;
+
+    //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
+
+    out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.0); // Set the target angle for each rotation motor
+    out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
+    out = setDriveTargetPower(out, target_speed); // Set the power
+
+    return out;
+}
+
+swerve_drive basicDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for basic drive mode
+{
+    swerve_drive out = input;
+
+    float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
+    byte drive_condition = identifyBasicDriveCondition(target_speed, normalized_target_angle); // Identify the drive condition
+
+    // Find inner_difference_angle
+    // Measure from the target angle to the nearest section of the y-axis (like on a unit circle), this is for the inner wheels in the turn (wheels closer to the center of the turning circle)
+    float inner_difference_angle;
+    float semicircle_normalized_target_angle = fmod(normalized_target_angle, 180.0);
+    if(semicircle_normalized_target_angle < 90) { // inner_difference_angle is less than 90
+        inner_difference_angle = semicircle_normalized_target_angle;
+    } else { // inner_difference_angle is greater than 90
+        inner_difference_angle = 180 - semicircle_normalized_target_angle;
+    }
+
+    // Find the turning radius for the inner wheels, multiply by the distance between the robot's wheels to get the actual distance
+    float inner_wheel_turning_radius = tan(RADIANS_PER_DEGREE * (90.0 - inner_difference_angle)) / 2.0;
+
+    // Find the turning radius for the outer wheels, multiply by the distance between the robot's wheels to get the actual distance
+    float outer_wheel_turning_radius = inner_wheel_turning_radius + 1.0;
+    
+    // Find the inner drive motor coefficient (to slow down the inner motors to compensate for a smaller turning radius), this is the ratio bewteen the inner and outer turning radii
+    float inner_drive_coefficient = outer_wheel_turning_radius / inner_wheel_turning_radius;
+
+    // Find outer_difference_angle
+    // Measure from the target angle to the nearest y-axis, this is for the outer wheels in the turn (wheels further from the center of the turning circle)
+    float outer_difference_angle = 90.0 - (DEGREES_PER_RADIAN * atan(2.0 * outer_wheel_turning_radius));
+
+    // Calculate the target spin angle for the outer wheels in the turn from the outer_difference_angle
+    float outer_target_angle;
+    int normalized_target_quadrant = ((int) normalized_target_angle ) / 90;
+    if (normalized_target_quadrant % 2 == 0) { // Quadrants 0 or 2
+        outer_target_angle = outer_difference_angle + (float) (90 * normalized_target_quadrant);
+    } else { // Quadrants 1 or 3
+        outer_target_angle = -outer_difference_angle + (float) (90 * (normalized_target_quadrant + 1));
+    }
+
+    // Prepare these variables to be written to in the switch statement below
+    // Set these to 0.0, so the robot will stop if none of the cases are hit for some reason
+    float front_left_coefficient = 0.0;
+    float front_right_coefficient = 0.0;
+
+    // Determine the speed coefficient for each motor (how fast motors are spinning relative to each other)
+    switch(drive_condition) {
+        case DRIVE_BASIC_STOP: // Motors are stopping, set target_speed to 0
+            front_left_coefficient = 0.0;
+            front_right_coefficient = 0.0;
+            target_speed = 0.0;
+            break;
+        case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // Motors are all spinning at the same speed since the robot is going in a straight line
+            front_left_coefficient = 1.0;
+            front_right_coefficient = 1.0;
+            break;
+        case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Left wheels are the inner wheels in the turn, so they will go slower
+            front_left_coefficient = inner_drive_coefficient;
+            front_right_coefficient = 1.0;
+            break;
+        case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Right wheels are the inner wheels in the turn, so they will go slower
+            front_left_coefficient = 1.0;
+            front_right_coefficient = inner_drive_coefficient;
+            break;
+    }
+    // Set the coefficients for the remaining 2 motors
+    float back_left_coefficient = front_left_coefficient; // Front and back coefficients are the same for each side (left and right)
+    float back_right_coefficient = front_right_coefficient; // Front and back coefficients are the same for each side (left and right)
+
+    // Set target wheel spin angles
+    switch(drive_condition) {
+        case DRIVE_BASIC_STOP: // Do not modify wheel target angles, robot is stopping
+            break;
+        case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // All wheels facing in the same direction since the robot is going in a straight line
+            out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); 
+            break;
+        case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Vary the angles of the wheels since the robot is turning, left wheels are the inner wheels in the turn
+            out = setTargetSpin(out, normalized_target_angle, outer_difference_angle, normalized_target_angle, outer_difference_angle);
+            break;
+        case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Vary the angles of the wheels since the robot is turning, right wheels are the inner wheels in the turn
+            out = setTargetSpin(out, outer_difference_angle, normalized_target_angle, outer_difference_angle, normalized_target_angle);
+            break;
+    }
+
+    // Wrap up, set motor speed and speed coefficient values
+    out = setMotorCoefficients(out, front_left_coefficient, front_right_coefficient, back_left_coefficient, back_right_coefficient); // Set motor speed coefficients
+    out = setDriveTargetPower(out, target_speed); // Set the power
+
+    return out;
+}
+
+byte identifyBasicDriveCondition(float target_speed, float target_angle) // Identify the condition in which the basic drive mode will be operating
+{
+    if(target_speed <= 0.0 || target_speed >= 127.0) { // Speed is stopped, negative, or too high
+        return DRIVE_BASIC_STOP;
+    }
+    target_angle = normalizeAngle(target_angle); // Normalize the target angle
+    if(target_angle == 0) {
+        return DRIVE_BASIC_FORWARD;
+    }
+    if(target_angle == 180) {
+        return DRIVE_BASIC_BACKWARD;
+    }
+    int target_quadrant = ((int) target_angle) / 90; // Which quadrant is the target angle in (quadrant 0 is northeast, increases clockwise)
+    byte drive_condition = DRIVE_BASIC_STOP; // Keep the drive stopped if the normalized target angle somehow exceeded 360.
+    switch(target_quadrant) {
+        case 0:
+            drive_condition = DRIVE_BASIC_FRONTRIGHT;
+            break;
+        case 1:
+            drive_condition = DRIVE_BASIC_BACKRIGHT;
+            break;
+        case 2:
+            drive_condition = DRIVE_BASIC_BACKLEFT;
+            break;
+        case 3:
+            drive_condition = DRIVE_BASIC_FRONTLEFT;
+            break;
+        default: // Unreachable, assuming that the target angle is less than 360
+            drive_condition = DRIVE_BASIC_STOP;
+            break;
+    }
+    return drive_condition;
+}
+
+swerve_drive tankDrive(swerve_drive input, float target_speed, float turn_angle) // Implementation for tank drive mode, positive angle is left, negative angle is right
+{
+    swerve_drive out = input;
+
+    float normalized_turn_angle = normalizeAngle(turn_angle); // Normalize the turn angle
+
+    // Calculate the speed of each motor
+    float angle_distance_from_y_axis = 90.0f - fabs(fmod(normalized_turn_angle, 180.0f) - 90.0f); // Distance in degrees that the joystick is from the y-axis
+    float slow_side_coefficient = angle_distance_from_y_axis / 90.0f; // Coefficient applied to whichever motor is not going full speed in the tank drive
+    int turn_angle_quadrant = ((int) normalized_turn_angle) / 4;
+    float left_side_power, right_side_power;
+    
+    // Determine the motor speeds based on the quadrant that the joystick is in
+    switch(turn_angle_quadrant) {
+        case 0: // Going forward and turning right, Northeast quadrant
+            left_side_power = 1.0f;
+            right_side_power = slow_side_coefficient;
+            break;
+        case 1: // Going backward and turning left, Southeast quadrant 
+            left_side_power = -1.0f;
+            right_side_power = -slow_side_coefficient;
+            break;
+        case 2: // Going backward and turning right, Southwest quadrant
+            left_side_power = -slow_side_coefficient;
+            right_side_power = -1.0f;
+            break;
+        case 3: // Going forward and turning left, Northwest quadrant
+            left_side_power = slow_side_coefficient;
+            right_side_power = 1.0f;
+            break;
+    }
+
+    out = setMotorCoefficients(out, left_side_power, right_side_power, left_side_power, right_side_power); // Set the motor speed coefficients
+    out = setDriveTargetPower(out, target_speed); // Set the power
+
+    return out;
+}
+
+
+// Modulus Plus: Drive control planning: v1 on 20230922
+/*
+
+315   000   045
+       
+270    ^    090
+
+225   180   135
+
+If speed AND any wheel's target-current angle are BOTH above a certain threshold, decelerate before spinning the wheel
+
+Variables to monitor and control:
+    Drive modes:
+        Translation (Linear motion in any direction while preserving body orientation)
+        Basic (Forwards or backwards, may turn)
+        Rotation (rotation in place)
+    Rotation angle (in Translation or Basic drive mode)
+    Drive speed
+    Drive forward orientation relative to robot body
+
+Common Controls:
+    Left and Right shoulder buttons:
+        Switch between drive modes (Left for basic, right for translation)
+    D-Pad:
+        Reset drive orientation relative to body
+        Set direction to the corresponding d-pad button direction
+    Joystick B:
+        Left/Right:         Rotation speed, bypass existing drive mode and start rotating
+
+Translation Mode Controls:
+    Joystick A:
+        Vector Amplitude:   Drive speed
+        Vector Angle:       Drive angle
+
+Basic Mode Controls:
+    Joystick A:
+        Vector Amplitude:   Drive speed
+        Vector Angle:       Drive angle
+
+All Mode Controls:
+
+Rotation drive mode wheel orientation
+Turning in place
+//   \\
+
+\\   //
+
+
+Translation or Basic drive mode wheel orientation, with a rotation angle of 0
+Driving in a straight line
+||   ||
+
+
+||   ||
+
+Translation drive mode wheel orientation, with a rotation angle of 90 or 270
+Driving to the left or right
+_     _
+-     -
+
+_     _
+-     -
+
+Translation drive mode wheel orientation, with a rotation angle of 45
+Driving forward and to the right
+//   //
+
+//   //
+
+Basic drive mode wheel orientation with a rotation angle of 315
+Driving forwards while turning (to the left in this example)
+\\   \\
+
+//   //
+
+*/

src/swerve.h

@@ -0,0 +1,111 @@
+#include "globals.h"
+#include <stdint.h>
+
+typedef struct { // swerve_drive struct, used to track and manage the state of the robot's swerve drive system
+    // byte spin_direction = CLOCKWISE; // Current state, 0 = clockwise, 1 = anticlockwise
+    // byte drive_direction = DRIVE_FORWARDS; // Current state, Directions: 0 = forwards, 1 = right, 2 = backwards, 3 = left
+    // byte target_spin_direction = CLOCKWISE; // Target state
+    // byte target_drive_direction = DRIVE_FORWARDS; // Target state
+    
+    int drive_mode = DEFAULT_SWERVE_DRIVE_MODE;
+    bool enable_steering = DEFAULT_SWERVE_ENABLE_STEERING;
+    
+    float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
+    float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
+    
+    float front_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float front_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float back_left_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+    float back_right_power = 0.0f; // -127.0 to 127.0 : CURRENT power for this specific drive motor
+
+    float spin_location = 0; // 0 - 360, CURRENT tracked body orientation relative to initial (or reset) orientation
+    float spin_offset = 0; // 0 - 360, offset applied to target spin when setting, this allows modification of the robot's forward orientation relative to its body
+    
+    // CURRENT wheel orientations relative to robot body in degrees [0.0, 360.0), read from encoders & convert
+    float front_left_spin_angle = 0.0f;
+    float front_right_spin_angle = 0.0f;
+    float back_left_spin_angle = 0.0f;
+    float back_right_spin_angle = 0.0f;
+
+    // COMMAND steering motor power, -127.0 to 127.0
+    float front_left_spin_power = 0.0f;
+    float front_right_spin_power = 0.0f;
+    float back_left_spin_power = 0.0f;
+    float back_right_spin_power = 0.0f;
+
+    // TARGET wheel orientations relative to robot body in degrees [0.0, 360.0), this is the state that the robot is trying to get to
+    float front_left_target_spin = 0.0f;
+    float front_right_target_spin = 0.0f;
+    float back_left_target_spin = 0.0f;
+    float back_right_target_spin = 0.0f;
+
+    // MEASURED steering motor speed in degrees per second, positive values are clockwise, calculated each time updateEncoderData() is called
+    float front_left_measured_spin_speed = 0.0f;
+    float front_right_measured_spin_speed = 0.0f;
+    float back_left_measured_spin_speed = 0.0f;
+    float back_right_measured_spin_speed = 0.0f;
+
+    // Initial encoder values of the steering motors, current orientation of the steering motors is calculated from this (assuming that they are all facing forward upon initialization)
+    int front_left_initial_spin_encoder = 0;
+    int front_right_initial_spin_encoder = 0;
+    int back_left_initial_spin_encoder = 0;
+    int back_right_initial_spin_encoder = 0;
+
+    // Motor power coefficients, this is used when motors must turn at different speeds. This is an input value, and is not directly affected by the current robot conditions
+    // Between 0 and 1
+    float front_left_coefficient = 0.0f;
+    float front_right_coefficient = 0.0f;
+    float back_left_coefficient = 0.0f;
+    float back_right_coefficient = 0.0f;
+
+    
+    // Encoder tracking, used to track speed of the steering motors
+    // The 0th entry in the buffer is the most recent, the highest index entry is the oldest
+    // The buffer is modified each time updateEncoderData() is called
+    uint64_t encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer tracks the times at which encoder states were measures, uses milliseconds (relative to pico start), using 64 bit value for safety
+    int encoder_buffer_front_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_front_right[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_right[ENCODER_BUFFER_ENTRY_COUNT];
+    
+} swerve_drive;
+
+// Utility functions
+
+float closestAngle(float current, float target); // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
+
+// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
+// swerve_drive setDirection(swerve_drive input, float setpoint);
+
+swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Initialize a swerve_drive struct, use the current time and encoder positions to fill the arrays
+
+swerve_drive updateSwerveCommand(swerve_drive input); // This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
+
+float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
+
+swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Process new encoder data, calculate the speed and angle of the steering motors
+
+float normalizeAngle(float angle); // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
+
+swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right); // Set the motor speed coefficients for each motor
+
+swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right); // Set the target spin for each wheel
+
+swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset); // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
+
+swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power
+
+
+// Drive mode functions
+
+swerve_drive stopSwerve(swerve_drive input); // Stop all motors
+
+swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode
+
+swerve_drive rotationDrive(swerve_drive input, float target_speed); // Implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise
+
+swerve_drive basicDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for basic drive mode
+
+byte identifyBasicDriveCondition(float target_speed, float target_angle); // Identify the condition in which the basic drive mode will be operating
+
+swerve_drive tankDrive(swerve_drive input, float target_speed, float turn_angle); // Implementation for tank drive mode, positive angle is left, negative angle is right

src/zserio.cpp

@@ -83,7 +83,8 @@ void comm_parse() {
   }
   
   if(millis()-ptime > FAILTIME){
-    digitalWrite(13,LOW);
+    //digitalWrite(13,LOW);
+    SerCommDbg.println("No input recieved, sending safe inputs");
     //Been too long, copy safe state over active one
     memcpy(astate,&safe,sizeof(packet_t));
     comm_ok=0;