diff --git a/src/main.cpp b/src/main.cpp
index e25559c..6df290a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -36,10 +36,17 @@ 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
+PioEncoder enc1(18); // right1
+PioEncoder enc2(14); // left1
+PioEncoder enc3(26); // right2
+PioEncoder enc4(0); // left2 // JANK: soldered to pico or headers
 
-// stepper board slave config
+// 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
@@ -68,10 +75,10 @@ Sabertooth actuators(130, Serial2);
 #define MAX_MICROS        1985
 #define OC_OFFSET         0 // microseconds
 
-#define SERVO_PIN_1       5
-#define SERVO_PIN_2       6
-#define SERVO_PIN_3       7
-#define SERVO_PIN_4       9
+#define TALON_PIN_1       5
+#define TALON_PIN_2       6
+#define TALON_PIN_3       7
+#define TALON_PIN_4       9
 
 typedef struct
 {
@@ -80,11 +87,11 @@ typedef struct
 } ISR_servo_t;
 
 
-#define NUM_SERVOS            4
+#define NUM_SERVOS            7
 
 ISR_servo_t ISR_servo[NUM_SERVOS] =
 {
-  { -1, SERVO_PIN_1 }, { -1, SERVO_PIN_2 }, { -1, SERVO_PIN_3 }, { -1, SERVO_PIN_4 }
+  { -1, TALON_PIN_1 }, { -1, TALON_PIN_2 }, { -1, TALON_PIN_3 }, { -1, TALON_PIN_4 }, { -1, ARM_SERVO_PIN_1 }, { -1, ARM_SERVO_PIN_2 }, { -1, ARM_SERVO_PIN_3 }
 };
 
 MCP3008 adc;
@@ -120,6 +127,9 @@ int olddisplay = 99999; // guarantee a change when first value comes in
 #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) {
@@ -379,7 +389,8 @@ void set_motor(byte motor, int speed) {
   // 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
+  // 14      : drive 11-13 with identical position & speed
+  // 15 - 17 : arm servos
   // speed is -127 to 127
   Serial.print("Driving motor ");
   Serial.print(motor);
@@ -457,6 +468,10 @@ void set_motor(byte motor, int speed) {
       ioex1.digitalWrite(2, HIGH); // disable
     }
   }
+  else if (motor < 18) { // arm servos
+    uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
+    RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 15 + 4].servoIndex, newspeed);
+  }
 }
 
 void setup() {
@@ -863,6 +878,9 @@ void loop() {
   int zeroed_ly =      ((int)(astate->stickY) - 127);
   int zeroed_rx =    -1 * ((int)(astate->stickRX) - 127);
   int zeroed_ry =      ((int)(astate->stickRY) - 127);
+
+  // Modulus swerve drive
+
   
   if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched
     drive_mode = drive_type;
@@ -871,10 +889,7 @@ void loop() {
     drive_mode = DRIVE_ROTATION;
   }
 
-
-
-  // Modulus swerve drive
-
+  
   if (drive_mode == DRIVE_BASIC) {
     swrv = tempBasicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
   }
@@ -884,6 +899,19 @@ void loop() {
   if (drive_mode == DRIVE_ROTATION) {
     swrv = tempRotationDrive(swrv, zeroed_rx);
   }
+  swrv = updateSwerveCommand(swrv);
+
+  // update motors after calculation
+  set_motor(FLSTEER, swrv.front_left_command_spin);
+  set_motor(BRSTEER, swrv.back_right_command_spin);
+  set_motor(FRSTEER, swrv.front_right_command_spin);
+  set_motor(BLSTEER, swrv.back_left_command_spin);
+
+  set_motor(FLDRIVE, swrv.current_drive_power);
+  set_motor(BRDRIVE, swrv.current_drive_power);
+  set_motor(FRDRIVE, swrv.current_drive_power);
+  set_motor(BLDRIVE, swrv.current_drive_power);
+
 
   // Goliath / 2 side arcade tank drive code below
   /*int zeroed_power =    -1 * ((int)(astate->stickX) - 127);