/*
* Performs a context switch between two threads.
*/
                PUBLIC _port_switch
_port_switch:
                push    {r4, r5, r6, r7, lr}
                mov     r4, r8
                mov     r5, r9
                mov     r6, r10
                mov     r7, r11
                push    {r4, r5, r6, r7}
                mov     r3, sp
                str     r3, [r1, #CONTEXT_OFFSET]
                ldr     r3, [r0, #CONTEXT_OFFSET]
                mov     sp, r3
                pop     {r4, r5, r6, r7}
                mov     r8, r4
                mov     r9, r5
                mov     r10, r6
                mov     r11, r7
                pop     {r4, r5, r6, r7, pc}

typedef (func*)(void);
func led1On() {/*make*/ addDelayed(led1Off, 1000); return NULL; }
func led1Off() {/*make*/ addDelayed(led1On, 1000); return NULL; }
func led2On() {/*make*/ addDelayed(led2Off, 1000); return NULL; }
func led2Off() {/*make*/ addDelayed(led2On, 1000); return NULL; }

int main() {
  addDelayed(led1On, 1000);
  addDelayed(led1Off, 1000);
  main_loop();
}

// Управляющая структура машины состояний управляемой шаблоном
typedef struct
{
INT32U init_state;
INT32U counter;
INT32U *pattern_start_ptr; // Указатель на массив констант int являющийся цепочкой состояний (шаблоном)
// Если значение в массиве = 0xFFFF, то процесс обработки завершается
// Если значение в массиве = 0x0000, то вернуть указатель на начало цепочки
INT32U *pttn_ptr; // Текущая позиция в цепочке состояний

} T_solnd_ptrn;

// Шаблоны работы соленоидов
// ----------------------------------------------------------------------
// Шаблон состоит из массива пар слов.
// Первое слово - значение напряжения (в процентах от максимального) на текущем интервале времени
// Второе - длительность интервала времени в мс
// интервал равный 0 - означает возврат в начало шаблона
// интервал равный 65535 - означает застывание состояния

const INT32U SOLENOID1_ON[10] = { 100, 300, 25, 300, 100, 300, 25, 300, 25, 0xFFFF };
const INT32U SOLENOID2_ON[10] = { 25, 300, 100, 300, 25, 300, 100, 300, 25, 0xFFFF };
const INT32U SOLENOID_OFF[4] = { 0, 2000, 0, 0xFFFF };
// ----------------------------------------------------------------------

// Массив управляющих структур соленоидов .
T_solnd_ptrn solnd_cbl[2];


/*-------------------------------------------------------------------------------------------------------------
Инициализация машины состояний заданным шаблоном
*pttn - указатель на начало шаблона
n - индекс соленоида (0..1)
-------------------------------------------------------------------------------------------------------------*/
void Set_Solenoid_pattern(const INT32U *pttn, INT32U n)
{
if ( pttn != 0 )
{
solnd_cbl[n].pattern_start_ptr = (INT32U *)pttn;
solnd_cbl[n].pttn_ptr = (INT32U *)pttn;
Set_solenoids_voltage(*solnd_cbl[n].pttn_ptr, n);
solnd_cbl[n].pttn_ptr++;
solnd_cbl[n].counter = *solnd_cbl[n].pttn_ptr;
solnd_cbl[n].pttn_ptr++;
}
}

/*------------------------------------------------------------------------------
Вызывается из процедуры прерывания АЦП с периодичностью ADC_PERIOD
------------------------------------------------------------------------------*/
void Solenoid_automat(void)
{
static INT32U solnd_presc = 0;
INT32U duration;
INT32U voltage;
INT32U n;

solnd_presc++;

if ( solnd_presc >= (1000 / ADC_PERIOD) ) // Обработка ведется с периодом повторения - 1 мс
{
solnd_presc = 0;
for (n=0; n<2; n++)
{
// Управление состоянием сигнала соленоида
if ( solnd_cbl[n].counter )
{
solnd_cbl[n].counter--;
if ( solnd_cbl[n].counter == 0 ) // Меняем состояние сигнала при обнулении счетчика
{
if ( solnd_cbl[n].pattern_start_ptr != 0 )
{
voltage = *solnd_cbl[n].pttn_ptr;
solnd_cbl[n].pttn_ptr++;
duration = *solnd_cbl[n].pttn_ptr;
solnd_cbl[n].pttn_ptr++;
if ( duration != 0xFFFF )
{
if ( duration == 0 )
{
solnd_cbl[n].pttn_ptr = solnd_cbl[n].pattern_start_ptr;
voltage = *solnd_cbl[n].pttn_ptr;
solnd_cbl[n].pttn_ptr++;
solnd_cbl[n].counter = *solnd_cbl[n].pttn_ptr;
solnd_cbl[n].pttn_ptr++;
Set_solenoids_voltage(voltage, n);
}
else
{
solnd_cbl[n].counter = duration;
Set_solenoids_voltage(voltage, n);
}
}
else
{
// Обнуляем счетчик и таким образом выключаем обработку шаблона
Set_solenoids_voltage(voltage,n);
solnd_cbl[n].counter = 0;
}
}
else
{
Set_solenoids_voltage(0,n);
}
}
}
}

} // if ((prediv & 3)==0)
}

enum fsm_state { IDLE, PROCESS, ERROR };

struct fsm {
enum fsm_state state;
// data...
};

static struct fsm myfsm = {
.state = IDLE,
// data startup init...
};

void fsm_init(struct fsm* fsm){
fsm->state = IDLE;
// data (re)init...
}

void fsm_proc(struct fsm* fsm){
switch(fsm->state){
case IDLE:
// do startup init ...
fsm->state = PROCESS;
break;
case PROCESS:
// do process ...
fsm->state = IDLE;
// or fsm->state = ERROR;
break;
default:
fsm->state = ERROR;
case ERROR:
// error handler
break;
}
}