diff --git a/main.c b/main.c
index 5db8358..a42a1d9 100644
--- a/main.c
+++ b/main.c
@@ -18,6 +18,7 @@
 
 #define SCS (*((volatile unsigned int *) 0xe01fc1a0))
 
+#define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000))
 
 void init_pins(void)
 {
@@ -180,6 +181,53 @@ void timer_event_handler(void)
 
 void menu_handler(void);
 
+void wait_for_button_pressed(bool target)
+{
+	bool pressed;
+
+	led_set(!target);
+
+	/* Very crude debouncing */
+	timer_delay_ms(100);
+
+	target = target ? TRUE:FALSE;
+
+	do {
+		pressed = BUTTON_PRESSED;
+	} while (pressed != target);
+	led_set(pressed);
+}
+
+void calibrate_escs()
+{
+	wait_for_button_pressed(0);
+
+	putstr("Calibration mode\r\n");
+
+	wait_for_button_pressed(1);
+	wait_for_button_pressed(0);
+
+	set_thrust(0, 1.0);
+	set_thrust(1, 1.0);
+	set_thrust(2, 1.0);
+	set_thrust(3, 1.0);
+	putstr("Max throttle set\r\n");
+
+	wait_for_button_pressed(1);
+	wait_for_button_pressed(0);
+
+	set_thrust(0, 0.0);
+	set_thrust(1, 0.0);
+	set_thrust(2, 0.0);
+	set_thrust(3, 0.0);
+	putstr("Zero throttle set\r\n");
+
+	wait_for_button_pressed(1);
+	wait_for_button_pressed(0);
+
+	putstr("Exit calibration mode\r\n");
+}
+
 int main(void) {
 	init_interrupt();
 	init_uart();
@@ -196,6 +244,9 @@ int main(void) {
 
 	putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
 
+	if (BUTTON_PRESSED)
+		calibrate_escs();
+
 	putstr("prompt> ");
 
 	timer_delay_ms(1000);