diff --git a/common/motor.c b/common/motor.c
index ee79d7c..98c2499 100644
--- a/common/motor.c
+++ b/common/motor.c
@@ -3,21 +3,72 @@
 
 #include "inc/msp432p401r.h"
 
-// Initialize GPIO pins for output, which will be
-// used to control the direction of the motors and
-// to enable or disable the drivers.
-// The motors are initially stopped, the drivers
-// are initially powered down, and the PWM speed
-// control is uninitialized.
+#undef BIT0
+#undef BIT1
+#undef BIT2
+#undef BIT3
+#undef BIT4
+#undef BIT5
+#undef BIT6
+#undef BIT7
+
+#define BIT0 (uint8_t)(0x01)
+#define BIT1 (uint8_t)(0x02)
+#define BIT2 (uint8_t)(0x04)
+#define BIT3 (uint8_t)(0x08)
+#define BIT4 (uint8_t)(0x10)
+#define BIT5 (uint8_t)(0x20)
+#define BIT6 (uint8_t)(0x40)
+#define BIT7 (uint8_t)(0x80)
+
+// duty := H/(H+L) = H/10000
+// H+L is always 10 000 microsecond
+
+// Left motor (PH) direction connected to P5.4 (J3.29)
+#define LEFT_DIR_PIN     BIT4  // P5.4
+// Right motor (PH) direction connected to P5.5 (J3.30)
+#define RIGHT_DIR_PIN    BIT5  // P5.5
+
+// Left motor (EN) PWM connected to P2.7/TA0CCP4 (J4.40)
+#define LEFT_PWM_PIN     BIT7  // P2.7
+// Right motor (EN) PWM connected to P2.6/TA0CCP3 (J4.39)
+#define RIGHT_PWM_PIN    BIT6  // P2.6
+
+// Left motor (nSLEEP) enable connected to P3.7 (J4.31)
+#define LEFT_SLEEP_PIN   BIT7  // P3.7
+// Right motor (nSLEEP) enable connected to P3.6 (J2.11)
+#define RIGHT_SLEEP_PIN  BIT6  // P3.6
+
+
+// Initializes the 6 GPIO output lines and puts driver to sleep
 void MotorInit(void) {
-    // write this code
+    // 1. Configure all motor control pins as outputs
+    // P5.4 (Left DIR), P5.5 (Right DIR)
+    P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);         // GPIO function
+    P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
+    P5->DIR  |=  (LEFT_DIR_PIN | RIGHT_DIR_PIN);           // Output direction
+
+    // P3.6 (Right SLEEP), P3.7 (Left SLEEP)
+    P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);         // GPIO function
+    P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
+    P3->DIR  |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);           // Output direction
+    
+    // P2.6 (Right EN), P2.7 (Left EN)
+    P2->SEL0 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);         // GPIO function
+    P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
+    P2->DIR  |=  (LEFT_PWM_PIN | RIGHT_PWM_PIN);           // Output direction
+
+    // 2. Initial state: disable motors
+    P3->OUT  &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);  // SLEEP pins low - DRV8838 disabled
+    P2->OUT  &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);  // EN pins low
 }
 
 // Stop the motors, power down the drivers, and
 // set the PWM speed control to 0% duty cycle.
-void MotorStop(void) {
-    P2->OUT &= ~0xC0;   // off
-    P3->OUT &= ~0xC0;   // low current sleep mode
+void MotorStop(void) {    
+    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH low
+    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
+
     SetPWMDutyLeftMotor(0);
     SetPWMDutyRightMotor(0);
 }
@@ -25,7 +76,11 @@ void MotorStop(void) {
 // Left and right wheels forward with the given duty cycles. 
 // Duty cycle is 0 to 14,998.
 void MotorForward(uint16_t left_duty, uint16_t right_duty) { 
-    // write this code
+    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH low
+    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
+
+    SetPWMDutyRightMotor(right_duty);
+    SetPWMDutyLeftMotor(left_duty);
 }
 
 // Left and right wheels backward with the given duty cycles.
diff --git a/common/pwm.c b/common/pwm.c
index 97fd15a..8c15762 100644
--- a/common/pwm.c
+++ b/common/pwm.c
@@ -38,13 +38,32 @@ void SetPWMDutyP2_5(uint16_t duty) {
 // P2.6/.7 = 1 when timer equals TA0CCR3/4 on way down, 
 // P2.6/.7 = 0 when timer equals TA0CCR3/4 on way up
 void PWMInitMotor(uint16_t period) {
-    // write this code
+    P2->DIR  |=   BIT6 | BIT7;
+    P2->SEL0 |=   BIT6 | BIT7;
+    P2->SEL1 &= ~(BIT6 | BIT7);
+
+    TIMER_A0->CCR[0] = period;
+    TIMER_A0->EX0 = 0x00;
+
+    TIMER_A0->CCTL[3] = 0x40;
+    TIMER_A0->CCTL[4] = 0x40;
+
+    TIMER_A0->CCR[3] = 0;
+    TIMER_A0->CCR[4] = 0;
+
+    TIMER_A0->CTL = 0x02F0;
+
+    // ZKURVENA MRDKA
 }
 
+// right motor on P2.6 -> CCR3
 void SetPWMDutyRightMotor(uint16_t duty) {
-    // write this code
+    if (duty >= TIMER_A0->CCR[0]) return;
+    TIMER_A0->CCR[3] = duty;
 }
 
+// left motor on P2.7 -> CCR4
 void SetPWMDutyLeftMotor(uint16_t duty) {
-    // write this code
-}
+    if (duty >= TIMER_A0->CCR[0]) return;
+    TIMER_A0->CCR[4] = duty;
+}
\ No newline at end of file
diff --git a/common/timer_a1.c b/common/timer_a1.c
index 80ea00c..b6096b0 100644
--- a/common/timer_a1.c
+++ b/common/timer_a1.c
@@ -7,9 +7,30 @@ void (*timer_a1_task)(void); // user function
 // Activate Timer A1 interrupts to run user task periodically
 // 16 bits period in units (24/SMCLK, with SMCLK 12 MHz -> 2us)
 void TimerA1Init(void (*task)(void), uint16_t period) {
-    // write this code
+    timer_a1_task = task;
+
+    // bits9-8=10, clock source to SMCLK
+    // bits7-6=10, input clock divider
+    // bits5-4=00, stop mode
+    // bit2=0, set this bit to clear
+    // bit1=0, no interrupt on
+    TIMER_A1->CTL = 0x0280;
+
+    // bits15-14=00, no capture mode
+    // bit8=0, compare mode
+    // bit4=1, enable capture/compare interrupt on CCIFG
+    // bit2=0, output this value in output mode 0
+    TIMER_A1->CCTL[0] &= ~0x0001;
+
+    TIMER_A1->CCR[0] = (period - 1);
+    TIMER_A1->EX0 = 0x0005;
+    NVIC->IP[3] = (NVIC->IP[3]&0xFFFFFF00)|0x00000040; // priority 2
+    NVIC->ISER[0] = 0x00001000; // enable interrupt 12 in NVIC
+    TIMER_A1->CTL |= 0x0014; // reset and start Timer A in up mode
+    TIMER_A1->CTL |= 0x0020; // Enable Timer A1 interrupts (TAIE)
 }
 
 void TA1_0_IRQHandler(void) {
-    // write this code
+    TIMER_A1->CCTL[0] &= ~0x0001;
+    timer_a1_task();
 }
diff --git a/lab4/main.c b/lab4/main.c
index f6f4843..14d3356 100644
--- a/lab4/main.c
+++ b/lab4/main.c
@@ -95,8 +95,8 @@ int main(void) {
     BumpInit();
     MotorInit(); // your function
 
-    MotorForward(1500, 100);
-    return 0;
+    // MotorForward(1500, 100);
+    // return 0;
 
     while (true) {
         WaitTouchRelease();
diff --git a/lab5/main.c b/lab5/main.c
index 11f41d0..da2a722 100644
--- a/lab5/main.c
+++ b/lab5/main.c
@@ -44,7 +44,7 @@ void Task(void) {
     REDLED ^= 1;
 }
 
-int main_periodic_test(void) {
+int main_f(void) {
     ClockInit48MHz();
     RedLEDInit();
     TimerA1Init(&Task, 500);  // 1000 Hz
@@ -52,7 +52,7 @@ int main_periodic_test(void) {
     while (true) {}
 }
 
-int main_motor_test(void) {
+int main(void) {
     ClockInit48MHz();
     SwitchInit();
     PWMInitMotor(7502);     // your function
@@ -72,7 +72,7 @@ int main_motor_test(void) {
 // Write a program that uses PWM to move the robot
 // but uses TimerA1 to periodically check the bump switches, 
 // stopping the robot on collision
-int main(void) {
+int main_final(void) {
     ClockInit48MHz();
     // write code here
     CPU_cpsie(); // Enable interrupts