From 4e55f62501e6de590f7324cd58c12142d6c641cc Mon Sep 17 00:00:00 2001
From: AR2100 <andrewroachpublic@gmail.com>
Date: Wed, 29 May 2024 02:13:20 -0400
Subject: [PATCH] added speed cap

---
 lib/SoloCAN/include/SoloCAN.hpp  | 18 +++++++++---------
 platformio.ini                   |  2 +-
 src/SoloComputerControl/main.cpp |  6 ++++++
 3 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/lib/SoloCAN/include/SoloCAN.hpp b/lib/SoloCAN/include/SoloCAN.hpp
index ee03103..8805739 100644
--- a/lib/SoloCAN/include/SoloCAN.hpp
+++ b/lib/SoloCAN/include/SoloCAN.hpp
@@ -139,15 +139,15 @@ namespace solo_can {
             // can.write(createMessage(data));
             // delayMicroseconds(200);
 
-            // // set control mode
-            // data = (struct CanOpenData) {
-            //     .id = (uint16_t)(0x0600 + soloID),
-            //     .type = SDO_WRITE_COMMAND,
-            //     .code = CONTROL_MODE_CODE,
-            //     .payload = MODE_SPEED
-            // };
-            // can.write(createMessage(data));
-            // delayMicroseconds(200);
+            // set control mode
+            data = (struct CanOpenData) {
+                .id = (uint16_t)(0x0600 + soloID),
+                .type = SDO_WRITE_COMMAND,
+                .code = CONTROL_MODE_CODE,
+                .payload = MODE_SPEED
+            };
+            can.write(createMessage(data));
+            delayMicroseconds(200);
 
             // set speed
             data = (struct CanOpenData) {
diff --git a/platformio.ini b/platformio.ini
index 03f55de..00b1653 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -1,5 +1,5 @@
 [platformio]
-default_envs = StatusLight
+default_envs = SoloComputerControl
 
 [env:main]
 platform = teensy
diff --git a/src/SoloComputerControl/main.cpp b/src/SoloComputerControl/main.cpp
index c2d2d2a..2c7d3f9 100644
--- a/src/SoloComputerControl/main.cpp
+++ b/src/SoloComputerControl/main.cpp
@@ -68,6 +68,12 @@ int main()  {
             // protobuf::Messages::decodeRequest(requestBuffer, requestLength, requestMessage);
             protobuf::Messages::decodeRequest(requestBuffer, requestLength, requestMessage);
         
+            if (requestMessage.leftSpeed > 4000) requestMessage.leftSpeed = 4000;
+            else if (requestMessage.leftSpeed < -4000) requestMessage.leftSpeed = -4000;
+
+            if (requestMessage.rightSpeed > 4000) requestMessage.rightSpeed = 4000;
+            else if (requestMessage.rightSpeed < -4000) requestMessage.rightSpeed = -4000;
+
             Serial.print("left: ");
             Serial.print(requestMessage.leftSpeed);
             Serial.print(", right: ");