roboforum.ru

//структура с параметрами двигателя
struct motorstatus
   {
   unsigned int oc;//счётчик тиков датчика обратной связи
   char ocs;//состояние датчика
   char speedcur;//Текущая скорость мотора
   char speed;//Нужная скорость мотора
   char flag;//Запуск команды на выполнение
   char flagoc;//регулировка ШИМ при активном флаге
   unsigned char pause;//пауза перед скидыванием флага flagoc
   char dir;//Направление текущее мотора
   volatile unsigned long *reg;//Адрес регистра PWMMR
   volatile unsigned long bitoc;//вывод порта ОС
   volatile unsigned long *regoc;//Адрес порта ОС
   };

//Прерывание таймера 3кГц, считает время между тиками энкодера
//обрабатывает через функцию S=U*t и записывает в статус двигателя
__irq void T0_Isr( void )
{
char i;

for( i = 0 ; i < 2 ; i ++ ) {
if( motstat[ i ].oc < 255 )
   {
   motstat[ i ].oc ++;
   motstat[ i ].pause = 255;
   }
else
   {
   motstat[ i ].speedcur = 0;
   if( motstat[ i ].pause > 200 )
      {
      motstat[ i ].flagoc = 1;
      motstat[ i ].pause = 0;
      }
   else motstat[ i ].pause++;
   }
   //ОС2 анализируем состояние датчика и если было изменение состояния записываем тик
   if( ( *motstat[ i ].regoc & motstat[ i ].bitoc ) && !motstat[ i ].ocs )
      {
      if( motstat[ i ].oc > 250 ) motstat[ i ].speedcur = 1;
      else motstat[ i ].speedcur = 1100000 / ( motstat[ i ].oc * 833 );
      motstat[ i ].oc = 0;
      motstat[ i ].ocs = 1;
      motstat[ i ].flagoc = 1;
      }
   else
      if( !( *motstat[ i ].regoc & motstat[ i ].bitoc ) && motstat[ i ].ocs )
         {
         motstat[ i ].ocs = 0;
         }
}
}

//Этот код находится в функции main и выполняется при обновлении скорости
//Или, если не было обновление больше установленной паузы величиной
//с минимальную скорость
//Регулятор скорости по энкодерам
if( modemotor == 1 ) {
   for( i = 0 ; i < 2 ; i ++ )
   {
   if( motstat[ i ].flagoc ) {//регулируем ШИМ при обновлении датчика скорости
   motstat[ i ].flagoc = 0;

      r = *motstat[ i ].reg;//значение регистра задающий скважность ШИМ
      if( r == 0x64 ) motstat[ i ].flag = 0;
      if( motstat[ i ].speed == 0 && r < 0x64 )//Если заданная скорость равна нулю, а текущая скорость выше нуля и ШИМ включён, то уменьшаем мощность
         {
         if( motstat[ i ].speedcur == 0 )//Если двигатель не крутится выключаем ШИМ.
            {
            *motstat[ i ].reg = 0x64;
            PWMLER = 0x7F;
            motstat[ i ].flag = 0;//если двигатель остановился, то скидываем флаг запрета выполнения следующей команды
            }
         else
      //Если двигатель крутится уменьшаем мощность постепенно
            {
            if( r > 0x60 )
               *motstat[ i ].reg = r + 1;//инверсия ШИМ, при увеличении значения мощность падает
            else *motstat[ i ].reg = r + 4;
            PWMLER = 0x7F;
            }
         }
      else {
//      if( motstat[ i ].flag )
//         {
//         r = *motstat[ i ].reg;
         if( motstat[ i ].speedcur > motstat[ i ].speed && r < 0x64 )
            {
            if( ( motstat[ i ].speedcur - motstat[ i ].speed ) < 4 || r > 0x62 ) *motstat[ i ].reg = r + 1;
            else if( ( motstat[ i ].speedcur - motstat[ i ].speed ) < 15 || r > 0x60 ) *motstat[ i ].reg = r + 2;
               else *motstat[ i ].reg = r + 4;
//            *motstat[ i ].reg = r + 1;
            PWMLER = 0x7F;
            }
         else
            {
            if( motstat[ i ].speedcur < motstat[ i ].speed && r > 3 )//при максимальном ШИМ должна оставатся скважность, а то ключи сгорят
               {
               if( ( motstat[ i ].speed - motstat[ i ].speedcur ) < 4 || r < 5 ) *motstat[ i ].reg = r - 1;
               else if( ( motstat[ i ].speed - motstat[ i ].speedcur ) < 15 || r < 7 ) *motstat[ i ].reg = r - 2;
                  else *motstat[ i ].reg = r - 4;
//               *motstat[ i ].reg = r - 1;
               PWMLER = 0x7F;
               }
//            else motstat[ i ].flag = 0;
            }
//            }
         }
      }
   }
}

            if( ( motstat[ i ].speedcur - motstat[ i ].speed ) < 4 || r > 0x62 ) *motstat[ i ].reg = r + 1;
            else if( ( motstat[ i ].speedcur - motstat[ i ].speed ) < 15 || r > 0x60 ) *motstat[ i ].reg = r + 2;
               else *motstat[ i ].reg = r + 4;

if( motstat[ i ].oc < 255 )
   {
   motstat[ i ].oc ++;
   motstat[ i ].pause = 255;
   }
else
   {
//   motstat[ i ].oc = 230;
   motstat[ i ].speedcur = 0;//текущая скорость
   if( motstat[ i ].pause > 200 )
      {
      motstat[ i ].flagoc = 1;
      motstat[ i ].pause = 0;
      }
   else motstat[ i ].pause++;
   }