diff --git a/common/cpu.c b/common/cpu.c
index f8ae136..cdc8787 100644
--- a/common/cpu.c
+++ b/common/cpu.c
@@ -19,4 +19,34 @@ uint32_t __attribute__((naked)) CPU_cpsie(void)
     // naked attribute).
     //
     return(ret);
-}
\ No newline at end of file
+}
+
+uint32_t __attribute__((naked)) CPU_cpsid(void)
+{
+    uint32_t ret;
+
+    //
+    // Read PRIMASK and disable interrupts.
+    //
+    __asm("    mrs     r0, PRIMASK\n"
+            "    cpsid   i\n"
+            "    bx      lr\n"
+            : "=r" (ret));
+
+    //
+    // The return is handled in the inline assembly, but the compiler will
+    // still complain if there is not an explicit return here (despite the fact
+    // that this does not result in any code being produced because of the
+    // naked attribute).
+    //
+    return(ret);
+}
+
+void __attribute__((naked)) CPU_wfi(void)
+{
+    //
+    // Wait for the next interrupt.
+    //
+    __asm("    wfi\n"
+            "    bx      lr\n");
+}
diff --git a/common/cpu.h b/common/cpu.h
index c45349d..eddb426 100644
--- a/common/cpu.h
+++ b/common/cpu.h
@@ -4,5 +4,7 @@
 #include <stdint.h>
 
 uint32_t CPU_cpsie(void);
+uint32_t CPU_cpsid(void);
+void CPU_wfi(void);
 
 #endif /* CPU_H */
\ No newline at end of file
diff --git a/common/input_capture.c b/common/input_capture.c
new file mode 100644
index 0000000..d9c4b51
--- /dev/null
+++ b/common/input_capture.c
@@ -0,0 +1,56 @@
+#include "input_capture.h"
+
+#include "inc/msp432p401r.h"
+
+void (*capture_task)(uint16_t time);
+
+// Initialize Timer A0 in edge time mode to request interrupts on
+// the rising edge of P7.3 (TA0CCP0). The interrupt service routine
+// acknowledges the interrupt and calls a user function.
+void TimerA0CaptureInit(void (*task)(uint16_t time)) {
+    capture_task = task;
+    P7->SEL0 |= 0x08;
+    P7->SEL1 &= ~0x08; // configure P7.3 as TA0CCP0
+    P7->DIR &= ~0x08;
+    TIMER_A0->CTL &= ~0x30; // halt Timer A0
+    TIMER_A0->CTL = 0x200; // SMCLK
+    TIMER_A0->CCTL[0] = 0x4910; // capture on rising edge, synchronous capture, capture mode, interrupt enabled
+    TIMER_A0->EX0 &= ~0x7; // input clock divider /1
+    NVIC->IP[2] = (NVIC->IP[2] & 0xFFFFFF00) | 0x00000002; // priority 2
+    // interrupts enabled in the main program after all devices initialized
+    NVIC->ISER[0] = 0x100; // enable interrupt 8 in NVIC
+    TIMER_A0->CTL |= 0x24;        // reset and start Timer A0 in continuous up mode
+}
+
+void TA0_0_IRQHandler(void) {
+    TIMER_A0->CCTL[0] &= ~0x1;      // acknowledge capture/compare interrupt
+    capture_task(TIMER_A0->CCR[0]); // execute user task
+}
+
+
+// Left Encoder A connected to P10.5 (J5)
+// Right Encoder A connected to P10.4 (J5)
+
+// user function, time is up-counting timer value when edge occurred in units of 0.083 usec
+// called when P10.4/.5 (TA3CCP0/1) edge occurs
+void (*capture_task0)(uint16_t time); 
+void (*capture_task1)(uint16_t time);
+
+// Initialize Timer A3 in edge time mode to request interrupts on
+// the rising edges of P10.4 (TA3CCP0) and P10.5 (TA3CCP1).
+// Interrupt service routines acknowledge the interrupt and call a user function.
+// Assumes: low-speed subsystem master clock is 12 MHz
+void TimerA3CaptureInit(void (*task0)(uint16_t time), 
+                        void (*task1)(uint16_t time)) {
+    // write this code
+}
+
+// The rising edge of P10.4 will cause an interrupt
+void TA3_0_IRQHandler(void) {
+    // write this code
+}
+
+// The rising edge of P10.5 will cause an interrupt
+void TA3_N_IRQHandler(void) {
+    // write this code
+}
diff --git a/common/input_capture.h b/common/input_capture.h
new file mode 100644
index 0000000..e365f77
--- /dev/null
+++ b/common/input_capture.h
@@ -0,0 +1,9 @@
+#ifndef INPUT_CAPTURE_H
+#define INPUT_CAPTURE_H
+
+#include <stdint.h>
+
+void TimerA3CaptureInit(void (*task0)(uint16_t time), 
+                        void (*task1)(uint16_t time));
+
+#endif /* INPUT_CAPTURE */
\ No newline at end of file
diff --git a/common/tachometer.c b/common/tachometer.c
new file mode 100644
index 0000000..274f86c
--- /dev/null
+++ b/common/tachometer.c
@@ -0,0 +1,41 @@
+#include "tachometer.h"
+
+#include "input_capture.h"
+
+#include "inc/msp432p401r.h"
+
+#define P5_0_IN (*((volatile uint8_t *)(0x42098800)))
+#define P5_2_IN (*((volatile uint8_t *)(0x42098808)))
+
+uint16_t tach_r_time1, tach_r_time2, tach_l_time1, tach_l_time2;
+
+// incremented with every step forward, decremented with every step backward
+int tach_r_steps, tach_l_steps;
+
+TachDirection_t tach_r_dir, tach_l_dir;
+
+// Right Encoder B connected to P5.0 (J2.13)
+// When high, increase r_steps and set dir forward
+// When low, decrease r_steps and set dir reverse
+void TachometerRightInt(uint16_t current_time) {
+    tach_r_time1 = tach_r_time2;
+    tach_r_time2 = current_time;
+    // write this code
+}
+
+// Left Encoder B connected to P5.2 (J2.12)
+// When high, increase l_steps and set dir forward
+// When low, decrease l_steps and set dir reverse
+void TachometerLeftInt(uint16_t current_time) {
+    tach_l_time1 = tach_l_time2;
+    tach_l_time2 = current_time;
+    // write this code
+}
+
+// Initialize P5.0 and P5.2 and make them GPIO inputs, 
+// which will be used to determine the direction of rotation.
+// Initialize the input capture interface, which
+// will be used to measure the speed of rotation.
+void TachometerInit(void) {
+    // write this code
+}
diff --git a/common/tachometer.h b/common/tachometer.h
new file mode 100644
index 0000000..5c391b4
--- /dev/null
+++ b/common/tachometer.h
@@ -0,0 +1,15 @@
+#ifndef TACHOMETER_H
+#define TACHOMETER_H
+
+#include <stdint.h>
+
+// Specifies the direction of the motor rotation, relative to the front of the robot
+typedef enum TachDirection {
+    Reverse = -1,
+    Stopped,
+    Forward,
+} TachDirection_t;
+
+void TachometerInit(void);
+
+#endif /* TACHOMETER_H */
diff --git a/lab6/Makefile b/lab6/Makefile
new file mode 100644
index 0000000..ad7dadc
--- /dev/null
+++ b/lab6/Makefile
@@ -0,0 +1,77 @@
+CC = arm-none-eabi-gcc
+
+# The location of the C compiler
+# ARMGCC_ROOT is used by some makefiles that need to know where the compiler
+# is installed.
+ARMGCC_BIN := ${shell which ${CC}}
+ifeq (${shell test -h ${ARMGCC_BIN} && echo "true" || echo "false"}, true)
+    ARMGCC_ROOT := ${shell dirname ${shell readlink ${ARMGCC_BIN}}}/..
+else
+    ARMGCC_ROOT := ${shell dirname ${ARMGCC_BIN}}/..
+endif
+
+# Search for source files that are not in this directory
+VPATH = ../common/
+
+OBJECTS = main.o timer_a1.o motor.o pwm.o input_capture.o tachometer.o cpu.o clock.o system.o startup.o syscalls.o
+
+NAME = lab
+
+CFLAGS = -I.. -I../inc -I../common \
+    -D__MSP432P401R__ \
+    -DDeviceFamily_MSP432P401x \
+    -mcpu=cortex-m4 \
+    -march=armv7e-m \
+    -mthumb \
+    -std=c99 \
+    -mfloat-abi=hard \
+    -mfpu=fpv4-sp-d16 \
+    -ffunction-sections \
+    -fdata-sections \
+    -gstrict-dwarf \
+    -Wall \
+    -I$(ARMGCC_ROOT)/arm-none-eabi/include/newlib-nano \
+    -I$(ARMGCC_ROOT)/arm-none-eabi/include
+
+LFLAGS = -Wl,-T,../config.lds \
+    -Wl,-Map,$(NAME).map \
+    -march=armv7e-m \
+    -mthumb \
+    -mfloat-abi=hard \
+    -mfpu=fpv4-sp-d16 \
+    -static \
+    -Wl,--gc-sections \
+	-lgcc \
+    -lc \
+    -lm \
+    -lnosys \
+    --specs=nano.specs
+
+all: $(NAME).out
+	@echo "SUCCESS: Compilation completed successfully."
+
+%.o: %.c
+	@$(CC) $(CFLAGS) -g $< -c -o $@
+
+cpu.o: ../common/cpu.c
+	@$(CC) $(CFLAGS) $< -c -o $@
+
+clock.o: ../common/clock.c
+	@$(CC) $(CFLAGS) $< -c -o $@
+
+system.o: ../system.c
+	@$(CC) $(CFLAGS) $< -c -o $@
+
+startup.o: ../startup.c
+	@$(CC) $(CFLAGS) $< -c -o $@
+
+syscalls.o: ../syscalls.c
+	@$(CC) $(CFLAGS) $< -c -o $@
+
+$(NAME).out: $(OBJECTS)
+	@$(CC) $(OBJECTS) $(LFLAGS) -o $(NAME).out
+
+clean: # Redirecting both the stderr and stdout to /dev/null 
+	@rm -f $(OBJECTS) > /dev/null 2>&1
+	@rm -f $(NAME).out > /dev/null 2>&1
+	@rm -f $(NAME).map > /dev/null 2>&1
diff --git a/lab6/main.c b/lab6/main.c
new file mode 100644
index 0000000..7f2174b
--- /dev/null
+++ b/lab6/main.c
@@ -0,0 +1,140 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "cpu.h"
+#include "motor.h"
+#include "pwm.h"
+#include "clock.h"
+#include "input_capture.h"
+#include "timer_a1.h"
+#include "tachometer.h"
+
+#include "inc/msp432p401r.h"
+
+void RedLEDInit(void) {
+    P1->SEL0 &= ~0x01;
+    P1->SEL1 &= ~0x01;   // 1) configure P1.0 as GPIO
+    P1->DIR |= 0x01;     // 2) make P1.0 out
+}
+
+// bit-banded address
+#define REDLED (*((volatile uint8_t *)(0x42098040)))
+
+void ColorLEDInit(void) {
+    P2->SEL0 &= ~0x07;
+    P2->SEL1 &= ~0x07;    // 1) configure P2.2-P2.0 as GPIO
+    P2->DIR |= 0x07;      // 2) make P2.2-P2.0 out
+    P2->DS |= 0x07;       // 3) activate increased drive strength
+    P2->OUT &= ~0x07;     //    all LEDs off
+}
+
+// bit-banded addresses
+#define BLUEOUT  (*((volatile uint8_t *)(0x42098068)))
+#define GREENOUT (*((volatile uint8_t *)(0x42098064)))
+#define REDOUT   (*((volatile uint8_t *)(0x42098060)))
+
+uint16_t period0, period1;  // (1/SMCLK) units = 83.3 ns units
+uint16_t first0, first1;   // Timer A3 first edge, P10.4/.5
+bool done0, done1;        // set each rising
+
+// max period is (2^16-1)*83.3 ns = 5.4612 ms
+// min period determined by time to run ISR, which is about 1 us
+void PeriodMeasure0(uint16_t time) {
+    REDOUT ^= 1;
+    period0 = time - first0;  // 83.3 ns resolution
+    first0 = time; // setup for next
+    done0 = true;
+}
+
+// max period is (2^16-1)*83.3 ns = 5.4612 ms
+// min period determined by time to run ISR, which is about 1 us
+void PeriodMeasure1(uint16_t time) {
+    REDLED ^= 1;
+    period1 = time - first1;  // 83.3 ns resolution
+    first1 = time; // setup for next
+    done1 = true;
+}
+
+int main_period_measure(void) {
+    CPU_cpsid(); // Disable interrupts
+    ClockInit48MHz();  // 48 MHz clock; 12 MHz Timer A clock
+    RedLEDInit();
+    ColorLEDInit();
+    PWMInitMotor(?);
+    MotorInit();
+    TimerA3CaptureInit(&PeriodMeasure0, &PeriodMeasure1);
+    MotorForward(3751, 3751);  // 50%
+    CPU_cpsie(); // Enable interrupts
+    CPU_wfi(); // Wait for interrupt
+    return 0;
+}
+
+uint16_t speed_buf[500];       // RPM, radius 0.035m wheels
+uint32_t period_buf[500];      // 1/12MHz = 0.083 usec
+uint32_t duty;                 // 0 to 15000
+uint32_t time;                 // in 0.01 sec
+
+void Collect(void) {
+    GREENOUT ^= 1;
+    if (!done0) period0 = 0xFFFF - 1;  // stopped
+    if (!done1) period1 = 0xFFFF - 1;
+    done0 = done1 = false; // set on subsequent
+    switch (time) {
+        case 0:
+            duty = 1875; // 25%
+            MotorForward(duty, duty);
+            break;
+        case 100:
+            duty = 3751; // 50%
+            MotorForward(duty, duty);
+            break;
+        case 200:
+            duty = 5626; // 75%
+            MotorForward(duty, duty);
+            break;
+        case 300:
+            duty = 3751; // 50%
+            MotorForward(duty, duty);
+            break;
+        case 400:
+            duty = 1875; // 25%
+            MotorForward(duty, duty);
+            break;
+        case 500:
+            MotorStop();
+            TIMER_A1->CTL &= ~0x30; // Halt timer A1
+            break;
+        default:
+            speed_buf[time] = 2000000 / period0;
+            period_buf[time] = period0;
+            break;
+    }
+    ++time;
+}
+
+int main_collect(void) {
+    CPU_cpsid(); // Disable interrupts
+    ClockInit48MHz();  // 48 MHz clock; 12 MHz Timer A clock
+    RedLEDInit();
+    ColorLEDInit();
+    PWMInitMotor(?);
+    MotorInit();
+    TimerA3CaptureInit(&PeriodMeasure0, &PeriodMeasure1);
+    TimerA1Init(&Collect, 5000);  // 100 Hz
+    CPU_cpsie(); // Enable interrupts
+    CPU_wfi(); // Wait for interrupt
+    return 0;
+}
+
+int main_tach(void) {
+    CPU_cpsid(); // Disable interrupts
+    ClockInit48MHz();
+    PWMInitMotor(?);
+    MotorInit();
+    TachometerInit();
+    MotorForward(2000, 2000); // confirm if rotating forward
+    //MotorBackward(2000, 2000); // confirm if rotating backward
+    CPU_cpsie(); // Enable interrupts
+    CPU_wfi(); // Wait for interrupt
+    return 0;
+}