From 55a0b6549f7aa6b0630f9f84512788c18532b4ea Mon Sep 17 00:00:00 2001
From: Cole Deck <cole@deck.sh>
Date: Fri, 29 Sep 2023 00:13:19 -0500
Subject: [PATCH] update vars

---
 platformio.ini |  1 +
 src/globals.h  |  2 +-
 src/main.cpp   | 39 ++++++++++++++++++---------------------
 3 files changed, 20 insertions(+), 22 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 0d45a6a..d531d0d 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -26,3 +26,4 @@ lib_deps =
 	107-systems/107-Arduino-Servo-RP2040@^0.1.0
 debug_tool = cmsis-dap
 build_flags = -O3
+; upload_protocol = cmsis-dap
\ No newline at end of file
diff --git a/src/globals.h b/src/globals.h
index 23fb8f2..dfb3052 100644
--- a/src/globals.h
+++ b/src/globals.h
@@ -17,7 +17,7 @@ extern long last_p;
 // 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
+#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)
diff --git a/src/main.cpp b/src/main.cpp
index 927bb76..dd5dd2e 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -44,13 +44,10 @@ PCF8574 ioex2(0x21, 20, 21);
 // JANK: soldered to pico or headers
 PioEncoder enc1(18); // Front Left
 PioEncoder enc2(14); // Front Right
-PioEncoder enc3(26); // Back Left
+PioEncoder enc3(27); // 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
@@ -89,18 +86,13 @@ Sabertooth actuators(130, Serial2);
 #define TALON_PIN_2       6
 #define TALON_PIN_3       7
 #define TALON_PIN_4       9
+// pins for arm servos
+#define ARM_SERVO_PIN_1   2
+#define ARM_SERVO_PIN_2   3
+#define ARM_SERVO_PIN_3   8
 
 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;
 int mode = 1;
@@ -410,11 +402,11 @@ void set_motor(byte motor, int 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));
+    talon2.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));
+    talon3.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));
+    talon4.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);
@@ -664,10 +656,14 @@ void setup() {
   talon2.attach(TALON_PIN_2);
   talon3.attach(TALON_PIN_3);
   talon4.attach(TALON_PIN_4);
+    for (int i = 5; i < 9; i++)
+    set_motor(i, 0);
 
   arm1.attach(ARM_SERVO_PIN_1);
   arm2.attach(ARM_SERVO_PIN_2);
   arm3.attach(ARM_SERVO_PIN_3);
+  for (int i = 15; i < 18; i++)
+    set_motor(i, 0);
   
 
   Serial.println(" done");
@@ -678,10 +674,11 @@ void setup() {
   // Initialize encoders
   Serial.print("Initializing encoders..");
   set_hex(0x5);
-  //enc1.begin();
-  //enc2.begin();
-  //enc3.begin();
-  //enc4.begin();
+  enc1.begin();
+  enc1.flip();
+  enc2.begin();
+  enc3.begin();
+  enc4.begin();
   Serial.println(" done");
   delay(20);