Меню для ардуино и китайского экрана 20х4

Добрый день! Вот уже 3-й день ковыряюсь с меню для Ардуино. Конечно понимаю что существует множество библиотек для этого и проще было бы их использовать, но я только учусь, поэтому не вижу смысла в упрощении .
Выкладываю код. В нем уже реализована структура дерева меню. Все хорошо. Но вопрос заключается только в том , как передвигать курсор на экране по нажатию кнопки. Я просто в руках держу Ардуинку неделю , а Си изучаю 3-й месяц. Код полностью мой , поэтому не обессудьте.

Код (Text):

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <LiquidCrystal.h>;


//Переменный для кнопок
LiquidCrystal lcd(12, 11, 5,  4,  3,  2);
const int buttonPin_up = 21;
const int buttonPin_down = 20;
const int buttonPin_next= 19;
const int buttonPin_prev = 18;
const int buttonPin_ok = 17;
const int contrast = 6;

int buttonState_up = 0;
int buttonState_down = 0;
int buttonState_next = 0;
int buttonState_prev = 0;
int buttonState_ok = 0;
int contrastState = 0;
int znach=0;
//Переменные для структуры и переменный функций.
void menu_new(int,char *,char *,char *,char *,int (*myFunc4)(int, int),int (*myFunc5)(int, int),void(*Arduino12)(void));//Основная функция создания одного дерева меню
// Название_функции(Порядковый_номер,Название_1-го_основного_эл-та_меню,Название_первого_подменю,Название_второго подменю,Название_третьего_подменю,
//Функция котороая прикреплена к первому подменю,-//-вторму,-//-третьему)
int summ3(int,int);//Первая функция
int summ4(int,int);//Вторая функция
int summ5(int,int);//Третья функция
void Arduino(void);//Четвертая функция
int hj=0;

typedef struct M {
struct M *down;//указатель на нижний элемент
struct M *up;//указатель на верхний элемент
struct M *child_head;//указатель на начало подменю(в подменю оба указателя указывают на функцию)
struct M *child_end;//указатель на конец подменю(в подменю оба указателя указывают на функцию)
int id;
char name[20];
} menu;
static menu *pos1;//указатель на структуры основного меню
static menu *pos2;//указатель на структуру подменю
static menu *current;//указатель на конец и начало дерева
static menu *current2;//Указатель для передвижения по меню
static menu base;

void setup(){
//Обьявления для дисплея
lcd.begin(20, 4);
lcd.print("        Menu");
pinMode(buttonPin_down, INPUT);
pinMode(buttonPin_up, INPUT);
pinMode(buttonPin_next, INPUT);
pinMode(buttonPin_prev, INPUT);
pinMode(buttonPin_ok, INPUT);
pinMode(contrast,OUTPUT);
current=&base;
base.down=base.up=base.child_end=base.child_head=&base;
//Создаем указатели на функции.
int (*myFunc3)(int, int)=&summ3;
int (*myFunc4)(int, int)=&summ4;
int (*myFunc5)(int, int)=&summ5;
void (*Arduino12)(void)=&Arduino;
//Вызываем основную функцию создания дерева.
menu_new(1,"Maxim","Ves","Rost","IQ",(*myFunc3),(*myFunc4),(*Arduino12));
menu_new(2,"Anino","Ves_Anino","Rost_Anino","IQ_Anino",(*myFunc3),(*myFunc4),(*Arduino12));
menu_new(3,"Luka","Ves_Luka","Rost_Luka","IQ_Luka",(*myFunc3),(*myFunc4),(*Arduino12));


pos1=current->up;
current2=pos1;
}

void loop(){
  //Далее идут неудачные попытки сдвинуть метку на экране
  buttonState_up = digitalRead(buttonPin_up);
  buttonState_down = digitalRead(buttonPin_down);
  buttonState_next = digitalRead(buttonPin_next);
  buttonState_prev = digitalRead(buttonPin_prev);
  buttonState_ok = digitalRead(buttonPin_ok);
  digitalWrite(contrast,HIGH);

if(buttonState_up=LOW&current2==pos1){
  lcd.setCursor(15,(current2->id ));
lcd.print("    ");
}
if(buttonState_up=HIGH&&current2==pos1){
  pos1=pos1->down;
  current2=pos1;
  lcd.setCursor(15,(current2->id ));
lcd.print("<<<<<");
}
lcd.setCursor(0,1);
lcd.print(current->up->name);
lcd.setCursor(0, 2);
lcd.print(current->up->up->name);
lcd.setCursor(0, 3);
lcd.print(current->up->up->up->name);
lcd.setCursor(15,(current2->id ));
lcd.print("<<<<<");




}


void menu_new(int a,char *name_menu,char *name_podmenu1,char *name_podmenu2,char *name_podmenu3,int (*myFunc4)(int, int),int (*myFunc5)(int, int),void(*Arduino12)(void)){
//Выделение памяти для основного первого пункта меню
pos1=(menu*)malloc((sizeof(menu)));
pos1->down=current->down;
pos1->up=current;
current->down->up=pos1;
current->down=pos1;
strcpy(pos1->name,name_menu);
pos1->id=a;//Метка для позиционирования курсора
pos1->child_head=pos1->child_end=pos1;

//Выделение памяти для первого подменю
pos2=(menu*)malloc(sizeof(menu));
pos2->up=pos1;
pos2->down=pos1;
pos1->child_head=pos2;
pos1->child_end=pos2;
pos2->child_end=(menu*)myFunc4;
pos2->child_head=(menu*)myFunc4;
strcpy(pos2->name,name_podmenu1);
pos2->id=1;//Метка для позиционирования курсора

//Выделение памяти для второго подменю
pos2=(menu*)malloc(sizeof(menu));
pos2->up=pos1->child_end;
pos2->down=pos1;
pos1->child_end->down=pos2;
pos1->child_end=pos2;
pos2->child_end=(menu*)myFunc5;
pos2->child_head=(menu*)myFunc5;
strcpy(pos2->name,name_podmenu2);
pos2->id=2;//Метка для позиционирования курсора
}

//Выделение памяти для третьего подменю
pos2=(menu*)malloc(sizeof(menu));
pos2->up=pos1->child_end;
pos2->down=pos1;
pos1->child_end->down=pos2;
pos1->child_end=pos2;
pos2->child_end=(menu*)Arduino12;
pos2->child_head=(menu*)Arduino12;
strcpy(pos2->name,name_podmenu3);
pos2->id=3;//Метка для позиционирования курсора
}

int summ3(int a,int b){
int c;
c=a+b;
return c;
}

int summ4(int a,int b){
int c;
c=a+b;
return c;
}

int summ5(int a,int b){
int c;
c=a+b;
return c;
}

void Arduino(){
hj=hj+1;
}

 

Просто я рассуждаю так: Если кнопка зажата , то указатель должен сместится на следующую структуру. А на экран просто переместить курсор по метке от этой структуре. (список на экране должен быть статический, если мы не перешли в подструктуру)Все логично, но как это реализовать.

 

Пытаюсь по разному ..... вот последний способ.Типо если указатель стоит на pos1(основное меню) и мы нажимаем кнопку, то указатель двигается и с ним изменяется значение курсора .

Код (Text):

switch (current2->id  ) {
case 1:
lcd.setCursor(15,1);
lcd.print("<<<<<");
  break;
case 2:
  lcd.setCursor(15,2);
lcd.print("<<<<<");
  break;
case 3:
  lcd.setCursor(15,3);
lcd.print("<<<<<");
  break;
}

if(buttonState_down=LOW&&current2==pos1){
  pos1=pos1->down;
  current2=pos1;

}
lcd.setCursor(0,1);
lcd.print(current->up->name);
lcd.setCursor(0, 2);
lcd.print(current->up->up->name);
lcd.setCursor(0, 3);
lcd.print(current->up->up->up->name);