diff --git a/config.h b/config.h
index aa427e7..9c768cf 100644
--- a/config.h
+++ b/config.h
@@ -30,69 +30,18 @@
 // Serial baud rate
 #define BAUD_RATE 9600
 
-// Define pin-assignments
-// NOTE: All step bit and direction pins must be on the same port.
-#define STEPPING_DDR       DDRD
-#define STEPPING_PORT      PORTD
-#define X_STEP_BIT         2  // Uno Digital Pin 2
-#define Y_STEP_BIT         3  // Uno Digital Pin 3
-#define Z_STEP_BIT         4  // Uno Digital Pin 4
-#define X_DIRECTION_BIT    5  // Uno Digital Pin 5
-#define Y_DIRECTION_BIT    6  // Uno Digital Pin 6
-#define Z_DIRECTION_BIT    7  // Uno Digital Pin 7
-#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
-#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
-#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
+// Pin mappings, pick one and comment others
+#define DEFAULT_328P
+//#define DEFAULT_2560
+//#define CUSTOM_PINS
 
-#define STEPPERS_DISABLE_DDR    DDRB
-#define STEPPERS_DISABLE_PORT   PORTB
-#define STEPPERS_DISABLE_BIT    0  // Uno Digital Pin 8
-#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
-
-// NOTE: All limit bit pins must be on the same port
-#define LIMIT_DDR       DDRB
-#define LIMIT_PIN       PINB
-#define LIMIT_PORT      PORTB
-#define X_LIMIT_BIT     1  // Uno Digital Pin 9
-#define Y_LIMIT_BIT     2  // Uno Digital Pin 10
-#define Z_LIMIT_BIT     3  // Uno Digital Pin 11
-#define LIMIT_INT       PCIE0  // Pin change interrupt enable pin
-#define LIMIT_INT_vect  PCINT0_vect 
-#define LIMIT_PCMSK     PCMSK0 // Pin change interrupt register
-#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
-
-#define SPINDLE_ENABLE_DDR   DDRB
-#define SPINDLE_ENABLE_PORT  PORTB
-#define SPINDLE_ENABLE_BIT   4  // Uno Digital Pin 12
-
-#define SPINDLE_DIRECTION_DDR   DDRB
-#define SPINDLE_DIRECTION_PORT  PORTB
-#define SPINDLE_DIRECTION_BIT   5  // Uno Digital Pin 13 (NOTE: D13 can't be pulled-high input due to LED.)
-
-#define COOLANT_FLOOD_DDR   DDRC
-#define COOLANT_FLOOD_PORT  PORTC
-#define COOLANT_FLOOD_BIT   3  // Uno Analog Pin 3
-
-// NOTE: Uno analog pins 4 and 5 are reserved for an i2c interface, and may be installed at
-// a later date if flash and memory space allows.
-// #define ENABLE_M7  // Mist coolant disabled by default. Uncomment to enable.
-#ifdef ENABLE_M7
-  #define COOLANT_MIST_DDR   DDRC
-  #define COOLANT_MIST_PORT  PORTC
-  #define COOLANT_MIST_BIT   4 // Uno Analog Pin 4
-#endif  
-
-// NOTE: All pinouts pins must be on the same port
-#define PINOUT_DDR       DDRC
-#define PINOUT_PIN       PINC
-#define PINOUT_PORT      PORTC
-#define PIN_RESET        0  // Uno Analog Pin 0
-#define PIN_FEED_HOLD    1  // Uno Analog Pin 1
-#define PIN_CYCLE_START  2  // Uno Analog Pin 2
-#define PINOUT_INT       PCIE1  // Pin change interrupt enable pin
-#define PINOUT_INT_vect  PCINT1_vect
-#define PINOUT_PCMSK     PCMSK1 // Pin change interrupt register
-#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
+if defined(DEFAULT_328P)
+  #include config_default_328p.h
+elif defined(DEFAULT_2560)
+  #include config_default_2560.h
+elif defined(CUSTOM_PINS)
+  #include config_custom.h // Create from the closest pre-existing config_default_*.h
+#endif
 
 // Define runtime command special characters. These characters are 'picked-off' directly from the
 // serial read data stream and are not passed to the grbl line execution parser. Select characters
diff --git a/config_default_2560.h b/config_default_2560.h
new file mode 100644
index 0000000..5ddb52e
--- /dev/null
+++ b/config_default_2560.h
@@ -0,0 +1,70 @@
+#ifndef config_default_2560_h
+#define config_default_2560_h
+
+// Define pin-assignments
+// NOTE: All step bit and direction pins must be on the same port.
+#define STEPPING_DDR      DDRA
+#define STEPPING_PORT     PORTA
+#define STEPPING_PIN      PINA
+#define X_STEP_BIT        0 // MEGA2560 Digital Pin 22
+#define Y_STEP_BIT        2 // MEGA2560 Digital Pin 24
+#define Z_STEP_BIT        4 // MEGA2560 Digital Pin 26
+#define C_STEP_BIT        6 // MEGA2560 Digital Pin 28
+#define X_DIRECTION_BIT   1 // MEGA2560 Digital Pin 23
+#define Y_DIRECTION_BIT   3 // MEGA2560 Digital Pin 25
+#define Z_DIRECTION_BIT   5 // MEGA2560 Digital Pin 27
+#define C_DIRECTION_BIT   7 // MEGA2560 Digital Pin 29
+#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
+#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
+#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
+
+#define STEPPERS_DISABLE_DDR   DDRC
+#define STEPPERS_DISABLE_PORT  PORTC
+#define STEPPERS_DISABLE_BIT   7 // MEGA2560 Digital Pin 30
+#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
+
+// NOTE: All limit bit pins must be on the same port
+#define LIMIT_DDR       DDRC
+#define LIMIT_PORT      PORTC
+#define LIMIT_PIN       PINC
+#define X_LIMIT_BIT     6 // MEGA2560 Digital Pin 31
+#define Y_LIMIT_BIT     5 // MEGA2560 Digital Pin 32
+#define Z_LIMIT_BIT     4 // MEGA2560 Digital Pin 33
+#define C_LIMIT_BIT     3 // MEGA2560 Digital Pin 34
+#define LIMIT_INT       PCIE0  // Pin change interrupt enable pin
+#define LIMIT_INT_vect  PCINT0_vect 
+#define LIMIT_PCMSK     PCMSK0 // Pin change interrupt register
+#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
+
+#define SPINDLE_ENABLE_DDR   DDRC
+#define SPINDLE_ENABLE_PORT  PORTC
+#define SPINDLE_ENABLE_BIT   2 // MEGA2560 Digital Pin 35
+
+#define SPINDLE_DIRECTION_DDR   DDRC
+#define SPINDLE_DIRECTION_PORT  PORTC
+#define SPINDLE_DIRECTION_BIT   1 // MEGA2560 Digital Pin 36
+
+#define COOLANT_FLOOD_DDR   DDRC
+#define COOLANT_FLOOD_PORT  PORTC
+#define COOLANT_FLOOD_BIT   0 // MEGA2560 Digital Pin 37
+
+// NOTE: Uno analog pins 4 and 5 are reserved for an i2c interface, and may be installed at
+// a later date if flash and memory space allows.
+// #define ENABLE_M7  // Mist coolant disabled by default. Uncomment to enable.
+#ifdef ENABLE_M7
+  #define COOLANT_MIST_DDR   DDRC
+  #define COOLANT_MIST_PORT  PORTC
+  #define COOLANT_MIST_BIT   0 // MEGA2560 Digital Pin 37
+#endif  
+
+// NOTE: All pinouts pins must be on the same port
+#define PINOUT_DDR       DDRC
+#define PINOUT_PIN       PINC
+#define PINOUT_PORT      PORTC
+#define PIN_RESET        0  // Uno Analog Pin 0
+#define PIN_FEED_HOLD    1  // Uno Analog Pin 1
+#define PIN_CYCLE_START  2  // Uno Analog Pin 2
+#define PINOUT_INT       PCIE1  // Pin change interrupt enable pin
+#define PINOUT_INT_vect  PCINT1_vect
+#define PINOUT_PCMSK     PCMSK1 // Pin change interrupt register
+#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
diff --git a/config_default_328p.h b/config_default_328p.h
new file mode 100644
index 0000000..4497f23
--- /dev/null
+++ b/config_default_328p.h
@@ -0,0 +1,66 @@
+#ifndef config_default_2560_h
+#define config_default_2560_h
+
+// Define pin-assignments
+// NOTE: All step bit and direction pins must be on the same port.
+#define STEPPING_DDR       DDRD
+#define STEPPING_PORT      PORTD
+#define X_STEP_BIT         2  // Uno Digital Pin 2
+#define Y_STEP_BIT         3  // Uno Digital Pin 3
+#define Z_STEP_BIT         4  // Uno Digital Pin 4
+#define X_DIRECTION_BIT    5  // Uno Digital Pin 5
+#define Y_DIRECTION_BIT    6  // Uno Digital Pin 6
+#define Z_DIRECTION_BIT    7  // Uno Digital Pin 7
+#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
+#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
+#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
+
+#define STEPPERS_DISABLE_DDR    DDRB
+#define STEPPERS_DISABLE_PORT   PORTB
+#define STEPPERS_DISABLE_BIT    0  // Uno Digital Pin 8
+#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
+
+// NOTE: All limit bit pins must be on the same port
+#define LIMIT_DDR       DDRB
+#define LIMIT_PIN       PINB
+#define LIMIT_PORT      PORTB
+#define X_LIMIT_BIT     1  // Uno Digital Pin 9
+#define Y_LIMIT_BIT     2  // Uno Digital Pin 10
+#define Z_LIMIT_BIT     3  // Uno Digital Pin 11
+#define LIMIT_INT       PCIE0  // Pin change interrupt enable pin
+#define LIMIT_INT_vect  PCINT0_vect 
+#define LIMIT_PCMSK     PCMSK0 // Pin change interrupt register
+#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
+
+#define SPINDLE_ENABLE_DDR   DDRB
+#define SPINDLE_ENABLE_PORT  PORTB
+#define SPINDLE_ENABLE_BIT   4  // Uno Digital Pin 12
+
+#define SPINDLE_DIRECTION_DDR   DDRB
+#define SPINDLE_DIRECTION_PORT  PORTB
+#define SPINDLE_DIRECTION_BIT   5  // Uno Digital Pin 13 (NOTE: D13 can't be pulled-high input due to LED.)
+
+#define COOLANT_FLOOD_DDR   DDRC
+#define COOLANT_FLOOD_PORT  PORTC
+#define COOLANT_FLOOD_BIT   3  // Uno Analog Pin 3
+
+// NOTE: Uno analog pins 4 and 5 are reserved for an i2c interface, and may be installed at
+// a later date if flash and memory space allows.
+// #define ENABLE_M7  // Mist coolant disabled by default. Uncomment to enable.
+#ifdef ENABLE_M7
+  #define COOLANT_MIST_DDR   DDRC
+  #define COOLANT_MIST_PORT  PORTC
+  #define COOLANT_MIST_BIT   4 // Uno Analog Pin 4
+#endif  
+
+// NOTE: All pinouts pins must be on the same port
+#define PINOUT_DDR       DDRC
+#define PINOUT_PIN       PINC
+#define PINOUT_PORT      PORTC
+#define PIN_RESET        0  // Uno Analog Pin 0
+#define PIN_FEED_HOLD    1  // Uno Analog Pin 1
+#define PIN_CYCLE_START  2  // Uno Analog Pin 2
+#define PINOUT_INT       PCIE1  // Pin change interrupt enable pin
+#define PINOUT_INT_vect  PCINT1_vect
+#define PINOUT_PCMSK     PCMSK1 // Pin change interrupt register
+#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
diff --git a/doc/pinmapping.txt b/doc/pinmapping.txt
new file mode 100644
index 0000000..021b2e9
--- /dev/null
+++ b/doc/pinmapping.txt
@@ -0,0 +1,97 @@
+Mega328P Arduino Pin Mapping
+============================
+
+Digital 0     PD0 (RX)
+Digital 1     PD1 (TX)
+Digital 2     PD2
+Digital 3     PD3
+Digital 4     PD4
+Digital 5     PD5
+Digital 6     PD6
+Digital 7     PD7
+Digital 8     PB0
+Digital 9     PB1
+Digital 10    PB2
+Digital 11    PB3 (MOSI)
+Digital 12    PB4 (MISO)
+Digital 13    PB5 (SCK)
+
+Analog 0      PC0
+Analog 1      PC1
+Analog 2      PC2
+Analog 3      PC3
+Analog 4      PC4
+
+
+Mega2560 Arduino Pin Mapping
+============================
+Digital pin 22                     PA0 ( AD0 )
+Digital pin 23                     PA1 ( AD1 )
+Digital pin 24                     PA2 ( AD2 )
+Digital pin 25                     PA3 ( AD3 )
+Digital pin 26                     PA4 ( AD4 )
+Digital pin 27                     PA5 ( AD5 )
+Digital pin 28                     PA6 ( AD6 )
+Digital pin 29                     PA7 ( AD7 )
+Digital pin 53 (PWM)(RX1)          PB0 ( SS/PCINT0 )
+Digital pin 52 (PWM)(SDA)          PB1 ( SCK/PCINT1 )
+Digital pin 51 (PWM)(SCL)          PB2 ( MOSI/PCINT2 )
+Digital pin 50                     PB3 ( MISO/PCINT3 )
+Digital pin 10 (PWM)               PB4 ( OC2A/PCINT4 )
+Digital pin 11 (PWM)               PB5 ( OC1A/PCINT5 )
+Digital pin 12 (PWM)               PB6 ( OC1B/PCINT6 )
+Digital pin 13 (PWM)               PB7 ( OC0A/OC1C/PCINT7 )
+Digital pin 37                     PC0 ( A8 )
+Digital pin 36                     PC1 ( A9 )
+Digital pin 35                     PC2 ( A10 )
+Digital pin 34                     PC3 ( A11 )
+Digital pin 33                     PC4 ( A12 )
+Digital pin 32                     PC5 ( A13 )
+Digital pin 31                     PC6 ( A14 )
+Digital pin 30                     PC7 ( A15 )
+Digital pin 21 (SCL)               PD0 ( SCL/INT0 )
+Digital pin 20 (SDA)               PD1 ( SDA/INT1 )
+Digital pin 19                     PD2 ( RXDI/INT2 )
+Digital pin 18                     PD3 ( TXD1/INT3 )
+Digital pin 38                     PD7 ( T0 )
+Digital pin 0 (PWM) (RX0)          PE0 ( RXD0/PCINT8 )
+Digital pin 1 (PWM) (TX0)          PE1 ( TXD0 )
+Digital pin 5 (PWM)                PE3 ( OC3A/AIN1 )
+Digital pin 2 (PWM)                PE4 ( OC3B/INT4 )
+Digital pin 3 (PWM)                PE5 ( OC3C/INT5 )
+Analog pin 0                       PF0 ( ADC0 )
+Analog pin 1                       PF1 ( ADC1 )
+Analog pin 2                       PF2 ( ADC2 )
+Analog pin 3                       PF3 ( ADC3 )
+Analog pin 4                       PF4 ( ADC4/TMK )
+Analog pin 5                       PF5 ( ADC5/TMS )
+Analog pin 6                       PF6 ( ADC6/PCINT14 )
+Analog pin 7                       PF7 ( ADC7/PCINT15 )
+Digital pin 41                     PG0 ( WR )
+Digital pin 40                     PG1 ( RD )
+Digital pin 39                     PG2 ( ALE )
+Digital pin 4 (PWM)                PG5 ( OC0B )
+Digital pin 17 (PWM)               PH0 ( RXD2 )
+Digital pin 16 (PWM)               PH1 ( TXD2 )
+Digital pin 6 (PWM)(RX3 )          PH3 ( OC4A )
+Digital pin 7 (PWM)(TX2)           PH4 ( OC4B )
+Digital pin 8 (PWM)(RX2 )          PH5 ( OC4C )
+Digital pin 9 (PWM)(TX1)           PH6 ( OC2B )
+Digital pin 15                     PJ0 ( RXD3/PCINT9 )
+Digital pin 14                     PJ1 ( TXD3/PCINT10 )
+Analog pin 8                       PK0 ( ADC8/PCINT16 )
+Analog pin 9                       PK1 ( ADC9/PCINT17 )
+Analog pin 10                      PK2 ( ADC10/PCINT18 )
+Analog pin 11                      PK3 ( ADC11/PCINT19 )
+Analog pin 12                      PK4 ( ADC12/PCINT20 )
+Analog pin 13                      PK5 ( ADC13/PCINT21 )
+Analog pin 14                      PK6 ( ADC14/PCINT22 )
+Analog pin 15                      PK7 ( ADC15/PCINT23 )
+Digital pin 49                     PL0 ( ICP4 )
+Digital pin 48                     PL1 ( ICP5 )
+Digital pin 47                     PL2 ( T5 )
+Digital pin 46 (PWM)               PL3 ( OC5A )
+Digital pin 45 (PWM)               PL4 ( OC5B )
+Digital pin 44 (PWM)               PL5 ( OC5C )
+Digital pin 43                     PL6
+Digital pin 42                     PL7
diff --git a/serial.c b/serial.c
index 69fa717..01a1f3a 100644
--- a/serial.c
+++ b/serial.c
@@ -91,11 +91,7 @@ void serial_write(uint8_t data) {
 }
 
 // Data Register Empty Interrupt handler
-#ifdef __AVR_ATmega644P__
-ISR(USART0_UDRE_vect)
-#else
-ISR(USART_UDRE_vect)
-#endif
+ISR(Serial_UDRE_vect)
 {
   // Temporary tx_buffer_tail (to optimize for volatile)
   uint8_t tail = tx_buffer_tail;
@@ -144,11 +140,7 @@ uint8_t serial_read()
   }
 }
 
-#ifdef __AVR_ATmega644P__
-ISR(USART0_RX_vect)
-#else
-ISR(USART_RX_vect)
-#endif
+ISR(Serial_RX_vect)
 {
   uint8_t data = UDR0;
   uint8_t next_head;
diff --git a/serial.h b/serial.h
index 723d48f..79ffb7e 100644
--- a/serial.h
+++ b/serial.h
@@ -27,6 +27,20 @@
 
 #include "nuts_bolts.h"
 
+#if defined(USART_RX_vect) // For atmega328p (Arduino Uno) etc.
+#define Serial_RX_vect USART_RX_vect
+#define Serial_UDRE_vect USART_UDRE_vect
+#elif defined(USART0_RX_vect) // For atmega644p, atmega1280 and atmega2560 (ArduinoMEGA) etc.
+#define Serial_RX_vect USART0_RX_vect
+#define Serial_UDRE_vect USART0_UDRE_vect
+#elif defined(USART_RXC_vect) // For some odd chips
+#define Serial_RX_vect USART_RXC_vect
+#define Serial_UDRE_vect USART_UDRE_vect
+#elif defined(USART1_RX_vect)// Some AVRs have 1 USB and a single USART (USART1), though the USB is often preffered for serial comms (like Arduino Leonardo etc.)
+#define Serial1_RX_vect USART1_RX_vect
+#define Serial1_UDRE_vect USART1_UDRE_vect
+#endif
+
 #ifndef RX_BUFFER_SIZE
   #define RX_BUFFER_SIZE 128
 #endif