diff --git a/src/ClawMachineOverhaul.ino b/src/ClawMachineOverhaul.ino
index 9d008a8..c970372 100644
--- a/src/ClawMachineOverhaul.ino
+++ b/src/ClawMachineOverhaul.ino
@@ -6,21 +6,28 @@
 #include "ZCodeParser.h"
 #include "Shared/MotorMoveParams.h"
 
+void anotherFunc(void * params){
+  MotorMoveParams *pam = (MotorMoveParams *)malloc(sizeof(MotorMoveParams));
+  pam->motorChar = 'x';
+  pam->moveTimeMs = 3000;
+  xTaskCreate(MoveMotor, "Motor", 1024, (void *)pam, 10,  NULL);
+
+  vTaskDelay(pdMS_TO_TICKS(50)); // Allow time for other task to accept params
+  free(pam);
+  vTaskDelete(NULL);
+}
+
 void setup() {
   Serial.begin(115200);
 
-  // setupMotor();
+  setupMotor();
 
   // Create tasks
-  // xTaskCreate(MotorControlTask, "Motor", 200, NULL, 10,  NULL);
+  xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5,  NULL);
 }
 
 void loop() {
   // The RTOS scheduler manages the tasks. No code is needed here.
 
   // ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
-  MotorMoveParams pam;
-  pam.motorChar = 'x';
-  xTaskCreate(MoveMotor, "Motor", 200, NULL, 10,  NULL);
-  delay(5000);
 }
diff --git a/src/MotorControlPart.h b/src/MotorControlPart.h
index 3edddd9..b18537e 100644
--- a/src/MotorControlPart.h
+++ b/src/MotorControlPart.h
@@ -14,13 +14,13 @@ struct CharMapping {
     int value;
 };
 
-// [X, Y, Head]
+// [X, Y, Head] TODO: 
 #define NUM_MOTORS 3
 static const uint8_t STEP_PINS[NUM_MOTORS] = {54, 60, 36};
 static const uint8_t DIR_PINS[NUM_MOTORS] = {55, 61, 34};
 static const uint8_t EN_PINS[NUM_MOTORS] = {38, 56, 30};
 bool motorsEnabled[NUM_MOTORS] = {false, false, false};
-CharMapping mappings[] = {{'a', 0}, {'b', 1}, {'c', 2}};
+CharMapping mappings[] = {{'x', 0}, {'y', 1}, {'e', 2}};
 uint8_t selectedIndex = 1;
 
 // Critical range for detecting joystick movement
@@ -62,17 +62,17 @@ void IRAM_ATTR timer3ISR()
 #else
 
 // Timers for stepping the motors
-ISR(TIMER1_COMPA_vect) {
+ISR(TIMER3_COMPA_vect) {
   if (motorsEnabled[0]) {
     digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
   }
 }
-ISR(TIMER3_COMPA_vect) {
+ISR(TIMER4_COMPA_vect) {
   if (motorsEnabled[1]) {
     digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
   }
 }
-ISR(TIMER4_COMPA_vect) {
+ISR(TIMER5_COMPA_vect) {
   if (motorsEnabled[2]) {
     digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
   }
@@ -110,19 +110,28 @@ void ExecuteCommand(void *params){
   ZCommand receivedValue = *((ZCommand *)params);
 }
 
-void MoveMotor(void *params)
-{
+void MoveMotor(void *params) {
   MotorMoveParams *motorParams = ((MotorMoveParams *) params);
   char motorChar = motorParams->motorChar;
-  float moveTime = motorParams->moveTime;
+  float moveTimeMs = motorParams->moveTimeMs;
 
   int index = charToMotorIndex(motorChar);
-  int isBackwards = signbit(moveTime); // Negative, then move backwards. Positive, then move forward.
+  int isBackwards = signbit(moveTimeMs); // Negative means move backwards, positive means move forward.
 
+  // Enable motor and set direction
+  motorsEnabled[index] = true;
   digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
   digitalWrite(DIR_PINS[index], isBackwards);
+  
+  vTaskDelay(pdMS_TO_TICKS(moveTimeMs));
+
+  // Stop motor
+  motorsEnabled[index] = false;
+  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
+  vTaskDelete(NULL);
 }
 
+
 void MotorControlTask(void *params) {
   while (true) {
     updateInput();
@@ -178,23 +187,23 @@ void setupMotor() {
     timerAlarmEnable(timer3);
     #else
     cli();
-    // Timer 1 (16-bit)
-    TCCR1A = 0;
-    TCCR1B = (1 << WGM12) | (1 << CS10); // CTC mode, no prescaler
-    OCR1A = 499; // Compare match value
-    TIMSK1 |= (1 << OCIE1A); // Enable Timer1 Compare A Match Interrupt
-
     // Timer 3 (16-bit)
     TCCR3A = 0;
     TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler
     OCR3A = 499; // Compare match value
-    TIMSK3 |= (1 << OCIE3A); // Enable Timer3 Compare A Match Interrupt
+    TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt
 
     // Timer 4 (16-bit)
     TCCR4A = 0;
     TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler
     OCR4A = 499; // Compare match value
-    TIMSK4 |= (1 << OCIE4A); // Enable Timer1 Compare A Match Interrupt
+    TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt
+
+    // Timer 5 (16-bit)
+    TCCR5A = 0;
+    TCCR5B = (1 << WGM42) | (1 << CS50); // CTC mode, no prescaler
+    OCR5A = 499; // Compare match value
+    TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt
     sei();
     #endif
 }
\ No newline at end of file
diff --git a/src/Shared/MotorMoveParams.h b/src/Shared/MotorMoveParams.h
index 737b566..8941af0 100644
--- a/src/Shared/MotorMoveParams.h
+++ b/src/Shared/MotorMoveParams.h
@@ -3,7 +3,7 @@
 
 typedef struct {
     char motorChar;
-    float moveTime;
+    float moveTimeMs;
 } MotorMoveParams;
 
 #endif
\ No newline at end of file