NIU Competition code final 20230930

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,16 @@ 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
+; upload_protocol = cmsis-dap

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,8 +9,114 @@ 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    20 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS_CORE_0 / 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
+#define DRIVE_MOTOR_MAX_POWER   64.0    // Maximum power for drive motors 
+
+// 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 3
+
+// Steering parameters
+#define STEERING_ENCODER_TICKS_PER_ROTATION     (1024.0 * 8.0)                                  // Number of encoder ticks per full rotation of each swerve drive steering motor
+#define STEERING_ENCODER_TICKS_PER_DEGREE       (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)   // Number of encoder ticks per degree of rotation for the swerve drive steering motors
+#define STEERING_MOTOR_SPEED_LIMIT 80.0                                                         // Maximum speed allowed for the steering motors (out of 127.0)
+// Steering PID parameters
+#define STEERING_SLOW_DELTA 35.0                                                                // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
+#define STEERING_ACCEL_SLOW_DELAY 0.20                                                          // Estimated acceleration delay of steering motors at low speeds (seconds)
+#define STEERING_TOLERANCE 1.0                                                                  // Steering tolerance in degrees
+#define STEERING_STALL_DETECT_ANGULAR_SPEED 5.0                                                 // Detect steering motor stall if measured angular speed is below this
+#define STEERING_SLOW_APPROACH_SPEED (0.16 * (MOTOR_MAX_POWER / STEERING_MOTOR_SPEED_LIMIT))    // Slow approach speed for steering motors
+#define STEERING_TOLERANCE_DISABLE_DRIVE 30.0                                                    // Disable the drive motors if any steering motor is off-target by more than this many degrees
+#define STEERING_HOVER_RANGE 10.0                                                               // Angular range where steering motors tend to hover around their targets
+
+// 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
+#define CLAW_COMMAND_STAY   4   // Maintain the current position
+
+// 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_DEGREES_PER_SECOND         50.0f // Speed in degrees per second that the tilt motor will move up or down (except when resetting to a flat position)
+#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      (TILT_DEGREES_PER_SECOND * TILT_ANGLE_MIN_UPDATE_INTERVAL) // 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   // Command not yet set, go to default (flat) position
+#define TILT_COMMAND_RESET  1   // Reset the tilt servo to the flat position
+#define TILT_COMMAND_UP     2   // Raise the tilt servo
+#define TILT_COMMAND_DOWN   3   // Lower the silt servo
+
+// Control parameters, specify which buttons control new_angle actions
+#define ARM_UP_BUTTON       DPAD_UP
+#define ARM_DOWN_BUTTON     DPAD_DOWN
+#define CLAW_OPEN_BUTTON    DPAD_LEFT
+#define CLAW_CLOSE_BUTTON   DPAD_RIGHT
+#define TILT_UP_BUTTON      DIAMOND_UP
+#define TILT_DOWN_BUTTON    DIAMOND_DOWN
+#define TILT_RESET_BUTTON   DIAMOND_LEFT
 
 
+
+// Button definitions
 #define SerComm Serial1   //Serial port connected to Xbee
 #define DIAMOND_LEFT 0
 #define DIAMOND_DOWN 1

src/manipulator.cpp

@@ -0,0 +1,135 @@
+#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 != out.tilt_command || out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Make sure that it's been long enough since the last update to not spam the servo command
+        float new_angle = out.tilt_target_angle; // Maintain the existing angle by default
+        switch(new_tilt_command) {
+            case TILT_COMMAND_UNSET:
+                new_angle = out.tilt_target_angle; // Maintain the existing angle
+            case TILT_COMMAND_RESET:
+                new_angle = TILT_FLAT_ANGLE;
+            case TILT_COMMAND_UP:
+                new_angle = out.tilt_target_angle + TILT_ANGLE_UPDATE_DISTANCE;
+                break;
+            case TILT_COMMAND_DOWN:
+                new_angle = out.tilt_target_angle - TILT_ANGLE_UPDATE_DISTANCE;
+                break;
+        }
+        out = setTiltAngle(out, new_angle);
+        out.tilt_angle_loops_since_update = 0; // Reset the counter tracking loops since the last update, since an update was just performed
+    } 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++;
+    }
+
+    out.tilt_command = new_tilt_command; // Save the new command to check whether it has changed in the next loop
+
+    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;
+        case CLAW_COMMAND_STAY:
+            new_claw_angle = out.claw_position;
+            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, must be between -1.0 and 1.0
+{
+    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))) * 2;
+    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, must be between -1.0 and 1.0
+
+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/spinlock.cpp

@@ -0,0 +1,34 @@
+#include <Arduino.h>
+#include "globals.h"
+#include "spinlock.h"
+
+void spinlock_lock_core_0(byte* spinlock_flag) // Lock a spinlock from core 0
+{
+    while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
+    *spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
+    // Wait to see if the memory was overwritten by the other core
+    delay(1);
+    if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
+        spinlock_lock_core_0(spinlock_flag);
+        return;
+    }
+    return;
+}
+
+void spinlock_lock_core_1(byte* spinlock_flag) // Lock a spinlock from core 1
+{
+    while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
+    *spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
+    // Wait to see if the memory was overwritten by the other core
+    delay(1);
+    if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
+        spinlock_lock_core_0(spinlock_flag);
+        return;
+    }
+    return;
+}
+
+void spinlock_release(byte* spinlock_flag) // Release a spinlock
+{
+    *spinlock_flag = SPINLOCK_OPEN;
+}

src/spinlock.h

@@ -0,0 +1,14 @@
+#include "globals.h"
+
+
+// Spinlock flags
+#define SPINLOCK_UNSET          0
+#define SPINLOCK_OPEN           1
+#define SPINLOCK_LOCK_CORE_0    2
+#define SPINLOCK_LOCK_CORE_1    3
+
+void spinlock_lock_core_0(byte* spinlock_flag); // Lock a spinlock from core 0
+
+void spinlock_lock_core_1(byte* spinlock_flag); // Lock a spinlock from core 1
+
+void spinlock_release(byte* spinlock_flag); // Release a spinlock

src/swerve.cpp

@@ -0,0 +1,516 @@
+#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;
+    float new_drive_coefficient = out.target_drive_power;
+    // 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);
+        // Use the delta and speed of each steering motor to calculate the necessary speed
+        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
+        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
+        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
+        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);
+
+        float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
+        if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
+            new_drive_coefficient = 0;
+        }
+        Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);
+
+        // TESTING DEBUG print 20230929
+        Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
+        Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
+        Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
+        Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
+        
+
+    } 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.current_drive_power = new_drive_coefficient;
+    out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
+    
+    return out;
+}
+
+float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
+{
+    float abs_steering_delta = fabs(steering_delta);
+    if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
+        return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
+    } else { // Slow down the speed of the steering motor since it's close to its target angle
+
+        float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
+        float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
+        float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
+        float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
+        //return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+        if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
+            steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
+            
+            if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
+                steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
+            } else if(abs_steering_delta < STEERING_HOVER_RANGE) {
+                steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
+            }
+        }
+        if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
+            return 0.0f;
+        }
+
+        return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
+    }
+}
+
+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;
+}
+
+int normalizeEncoderData(int encoder_data) // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
+{
+    encoder_data %= (int) STEERING_ENCODER_TICKS_PER_ROTATION;
+    encoder_data += (STEERING_ENCODER_TICKS_PER_ROTATION * (encoder_data < 0));
+    return encoder_data;
+}
+
+float relativeAngleFromEncoder(int encoder_data_relative ) // Calculate the relative angle from the difference between 2 encoder data points
+{
+    return ((float) normalizeEncoderData(encoder_data_relative)) / ((float) STEERING_ENCODER_TICKS_PER_DEGREE);
+}
+
+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);
+    angle += (360 * (angle < 0.0));
+    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, 315.0, 225.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,115 @@
+#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, float current_angular_speed); // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
+
+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
+
+int normalizeEncoderData(int encoder_data); // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
+
+float relativeAngleFromEncoder(int encoder_data_relative ); // Calculate the relative angle from the difference between 2 encoder data points
+
+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;