Копаться в чужой программе не зная алгоритма работы это бездарно потраченное время, так как видимые ошибки могут быть фичами, а фичи ошибками. Полезно только для самообучения приемов программирования.
Правильно вам тут говорят, опишите агоритм работы, укажите оборудование и вам напишут программу на ST даже не смотря в вашу бумажку с LCONM