las lineas
lcd_putc("\f");
lcd_gotoxy(6,1);
printf(lcd_putc,"Piso");
la puedes colocar antes del Do, puesto que no cambiará mas en el programa y no se borrará mas en la pantalla (a menos que quites energía o quieras mostrar otra cosa en la misma línea)