diff --git a/src/commands.c b/src/commands.c
index 0608257..a1053ad 100644
--- a/src/commands.c
+++ b/src/commands.c
@@ -3,7 +3,8 @@
 #include <string.h>
 #include <avr/io.h>
 
-#include "include/commands.h"
+#include "commands.h"
+#include "pins.h"
 
 cmd_t command = {
 	.buf = {0},
@@ -48,8 +49,8 @@ void cmd_handle(cmd_t *command)
 	if (0 == strcmp(tok1, "set"))
 		cmd_handle_set(tok2, tok3);
 
-	if (0 == strcmp(tok1, "switch"))
-		cmd_handle_switch();
+	if (0 == strcmp(tok1, "switch") || 0 == strcmp(tok1, "sw"))
+		cmd_handle_switch(tok2, tok3);
 }
 
 void cmd_handle_set(char *op1, char *op2) 
@@ -93,12 +94,50 @@ void cmd_handle_set(char *op1, char *op2)
 	}
 }
 
-void cmd_handle_switch(void)
+void cmd_handle_switch(char *op1, char *op2)
 {
-	int state = 0;
+	if (0 == strcmp(op1, "pd6")) {
+		if (0 == strcmp(op2, "on"))
+			setPin(&PORTD, PD6);
+		else if (0 == strcmp(op2, "off"))
+			unsetPin(&PORTD, PD6);
+		else
+			togglePin(&PORTD, PD6);
+	}
 
-	PORTD ^= (1<<PD6);
+	if (0 == strcmp(op1, "pb6")) {
+		if (0 == strcmp(op2, "on"))
+			setPin(&PORTB, PB6);
+		else if (0 == strcmp(op2, "off"))
+			unsetPin(&PORTB, PB6);
+		else
+			togglePin(&PORTB, PB6);
+	}
 
-	state = PORTD & (1 << PD6);
-	printf("Pin state is %i\r\n", state);
+	if (0 == strcmp(op1, "pc7")) {
+		if (0 == strcmp(op2, "on"))
+			setPin(&PORTC, PC7);
+		else if (0 == strcmp(op2, "off"))
+			unsetPin(&PORTC, PC7);
+		else
+			togglePin(&PORTC, PC7);
+	}
+
+	if (0 == strcmp(op1, "pd1")) {
+		if (0 == strcmp(op2, "on"))
+			setPin(&PORTD, PD1);
+		else if (0 == strcmp(op2, "off"))
+			unsetPin(&PORTD, PD1);
+		else
+			togglePin(&PORTD, PD1);
+	}
+
+		if (0 == strcmp(op1, "pd0")) {
+		if (0 == strcmp(op2, "on"))
+			setPin(&PORTD, PD0);
+		else if (0 == strcmp(op2, "off"))
+			unsetPin(&PORTD, PD0);
+		else
+			togglePin(&PORTD, PD0);
+	}
 }
\ No newline at end of file
diff --git a/src/include/commands.h b/src/include/commands.h
index afd3573..2ba8201 100644
--- a/src/include/commands.h
+++ b/src/include/commands.h
@@ -7,4 +7,4 @@ void cmd_mirror_char(char c);
 void cmd_collect_char(char c);
 void cmd_handle(cmd_t *command);
 void cmd_handle_set(char *op1, char *op2);
-void cmd_handle_switch(void);
\ No newline at end of file
+void cmd_handle_switch(char *op1, char *op2);
\ No newline at end of file
diff --git a/src/inverter.c b/src/inverter.c
index dc7280a..1343470 100644
--- a/src/inverter.c
+++ b/src/inverter.c
@@ -13,17 +13,23 @@ int main()
 
 	printf("Starting up!\r\n");
 
-	/* Make PB2 an output pin */
 	DDRB |= (1 << DDB2);
+	DDRB |= (1 << DDB6);
+
+	DDRC |= (1 << DDC7);
+
+	DDRD |= (1 << DDD0);
+	DDRD |= (1 << DDD1);
 	DDRD |= (1 << DDD6);
 
+
 	initCtcTimer0();
 
 	sei();
 
 	while (1) {
 		if ((PORTB & (1<<PB2))>1)  {
-			_delay_us(1);
+			_delay_ms(5);
 			PORTB &= ~(1<<PB2);
 		}	
 	}