 
 void calibIMUOffset(){
 	Slave1.requestPacket(SLAVE1_COMMANDS::CALIB_OFFSET);
-	// for (int i = 0; i < 50; i++){
-	// 	Slave1.requestPacket(SLAVE1_COMMANDS::REQUEST_STANDARD_PACKET);
-	// 	Slave1.receivePacket(inBuffer, 7, true);
-		
-	// 	f2b.b[0] = inBuffer[2];
-	// 	f2b.b[1] = inBuffer[3];
-	// 	f2b.b[2] = inBuffer[4];
-	// 	f2b.b[3] = inBuffer[5];
-	// 	bearing_offset += -f2b.f;
-	// 	delay(1);
-	// }
-	// bearing_offset /= 50;
+}
+
+
+void calibMag(){
+	tft.fillRect(0, 32, 320, 145, ILI9341_BLACK);
+
+	Slave1.requestPacket(SLAVE1_COMMANDS::CALIB_MAG);
+	// now we've entered calibration mode
+	tft.setCursor(0, 50);
+	tft.setTextSize(2);
+	tft.setTextColor(ILI9341_RED);
+	tft.println("entered calibration mode");
+	tft.println("tap anywhere on screen to finish calibration");
+
+	touched = false;
+	while(!touched){
+		getTouch();
+		Slave1.requestPacket(SLAVE1_COMMANDS::CALIB_DATA);
+		Slave1.receivePacket(inBuffer, 24, true);
+
+		tft.fillRect(0, 80, 320, 180, ILI9341_BLACK);
+		tft.setCursor(0, 80);
+
+		f2b.b[0] = inBuffer[0];
+		f2b.b[1] = inBuffer[1];
+		f2b.b[2] = inBuffer[2];
+		f2b.b[3] = inBuffer[3];
+
+		tft.print(f2b.f); tft.print(" ");
+
+		f2b.b[0] = inBuffer[4];
+		f2b.b[1] = inBuffer[5];
+		f2b.b[2] = inBuffer[6];
+		f2b.b[3] = inBuffer[7];
+
+		tft.print(f2b.f); tft.print("\n");
+
+		f2b.b[0] = inBuffer[8];
+		f2b.b[1] = inBuffer[9];
+		f2b.b[2] = inBuffer[10];
+		f2b.b[3] = inBuffer[11];
+
+		tft.print(f2b.f); tft.print(" ");
+
+		f2b.b[0] = inBuffer[12];
+		f2b.b[1] = inBuffer[13];
+		f2b.b[2] = inBuffer[14];
+		f2b.b[3] = inBuffer[15];
+
+		tft.print(f2b.f); tft.print("\n");
+
+		f2b.b[0] = inBuffer[16];
+		f2b.b[1] = inBuffer[17];
+		f2b.b[2] = inBuffer[18];
+		f2b.b[3] = inBuffer[19];
+
+		tft.print(f2b.f); tft.print(" ");
+
+		f2b.b[0] = inBuffer[20];
+		f2b.b[1] = inBuffer[21];
+		f2b.b[2] = inBuffer[22];
+		f2b.b[3] = inBuffer[23];
+
+		tft.print(f2b.f); tft.print("\n");
+
+		delay(20);
+	}
+	// exit when touched
+	Slave1.requestPacket(SLAVE1_COMMANDS::END_CALIB_MAG);
+	tft.println();
+	tft.println("Calibration complete!");
+
+	initDebugTFT();
+	drawButtons();
 }
 
 void getSlave1Data(){

	tft.println(targetDir_r_field);
	tft.println(targetVelocity);
	tft.println(bearing);
	tft.println(rotatationCorrection);
}

void serialDebug(){

	}
}

void getTouch(){
	touchedPoint = ctp.getPoint(); 
	touched = (touchedPoint.x != 0 || touchedPoint.y != 0);
	if (touched){
		int x, y;
		x = touchedPoint.y;
		y = 240 - touchedPoint.x;
		touchedPoint.x = x;
		touchedPoint.y = y;
	}
}

void drawPiLogo(){
	tft.setRotation(3);
	tft.fillScreen(ILI9341_BLACK);
	tft.setTextColor(ILI9341_WHITE);
	tft.setTextSize(4);
	tft.println("Team PI");
	tft.setTextSize(1);
}

void drawButtons(){
	magBtn.setText("MAG", 3, ILI9341_GREEN);
	ltBtn.setText("LT", 3, ILI9341_GREEN);
	kickBtn.setText("KICK", 3, ILI9341_GREEN);

	kickBtn.toggleBorderMode = true;

	tftEnableBtn.setBounds(320 - 60, 0, 320, 60);
	magBtn.setBounds(0, 180, 107, 240);
	ltBtn.setBounds(107, 180, 214, 240);
	kickBtn.setBounds(214, 180, 320, 240);


	tftEnableBtn.setColour(ILI9341_RED);
	magBtn.setColour(tft.color565(50,50,50));
	ltBtn.setColour(tft.color565(100,100,100));
	kickBtn.setColour(tft.color565(150,150,150));

	tftEnableBtn.draw();
	magBtn.draw();
	ltBtn.draw();
	kickBtn.draw();
}

void initDebugTFT(){
	// tft.color565(50,50,50)
	tft.setTextSize(1);

	tft.fillRect(0, 32, 320, 145, ILI9341_BLACK);
	//tft.fillScreen(ILI9341_BLACK);
	tft.setCursor(0, 50);
	tft.setTextColor(ILI9341_RED);
