7.5.4. Структура программы и блок-схема алгоритма

Схема подключения на рис. 7.23 показывает связи компонентов парящего робота с контроллером. На рис. 7.24 схематично показана конструкция робота со всеми компонентами. Обратите внимание, что ШИМ-сигналы регулирующие скорость вращения подаются на ИС силового коммутатора, которая обеспечивают достаточный ток для двигателей постоянного тока. Чтобы обеспечить питание системы используется несколько батарей. Сигналы поступают в МК от датчиков, а выходные сигналы МК управляют ИС коммутаторов двигателей.

Рис. 7.23. Схема подключения управляющих компонентов парящего робота

Рис. 7.24. Диаграмма движения робота со всеми бортовыми компонентами

На рис. 7.25 представлена структура программы для парящего робота, в которой показаны направления потоков данных между программными модулями. На рис. 7.26 показана блок-схема алгоритма программы для управления роботом.

Рис. 7.25. Структура программы парящего робота

Рис. 7.26. Блок-схема алгоритма программы парящего робота

7.5.5. Программный код

В этом разделе, мы представляем программу для управления парящим роботом на языке Си. Программа была первоначально написана Джоулом Перлином, одним из наших студентов, и скорректирована авторами настоящей книги.

//********************************************************************

// filename: flying.с

// Описание программы: Эта программа запускает четыре двигателя и затем

// управляет скоростью вращения каждого из них. После взлета робота

// программа проверяет каждый из датчиков для определения положения

// робота. При обнаружении преграды или крыши, скорости вращения двигателей

// корректируются, чтобы обеспечить правильное направление движения.

//

// Авторы: Джоэль Перлин, Даниэль Пак, Стив Барретт

// Дата создания: 27 июля 2004

// используемая память: программа - 0x1000, данные - 0x3000 и

// стек - 0x4000

//********************************************************************

#include <stdio.h>

#include "hcs12dp256.h"

#pragma abs_address 0x3000

unsigned int count1; //счетчики переменных

unsigned int count2;

unsigned int sensor;

char sensoravg; //сохраняют данные датчиков в виде 8-разрядных чисел

volatile unsigned p; // текущий счетчик

#pragma end_abs_address

//********************************************************************

void main(void) {

 //Инициализация робота

 PWME = 0x00; //запрет ШИМ

 DDRA = 0xFF; //конфигурирование портов A и В как выходных

 DDRB = 0xFF;

 PORTA = 0xAA; //подача питания на датчики

 PORTB = 0xFF; //индикация режима программы на линейке светодиодов

               //Инициализация модуля ATD

 PORTAD1 = 0x00; //конфигурирование портов как входных

 ATD1CTL2 = 0xC2; //инициализация ATD с установкой флагов

                  //преобразования в каналах

 ATD1CTL3 = 0x00; // функция установки ATD

 ATD1CTL4 = 0x80; //установить 8-разрядный режим

 PORTB = 0xFE; //показать состояние программы на линейке

               // светодиодов

 // Инициализация режима входного захвата

 TSCR1 = 0x80; //включение таймера (нормальный режим)

 TSCR2 = 0x80; //установить период переполнения счетчика временной

               // базы 8.192 мс, коэффициент деления = 1

 TIOS = 0x00; //установить каналы таймера в режим входного захвата

 TMSK1 = 0xE0;//биты разрешения прерывания по событиям на линиях [7:5]

 TFLG1 = 0xE0; //очистить флаги прерываний TFLG1

 PORTB = 0xFC; // показать состояние программы на линейке светодиодов

 // инициализация ШИМ

 PWMCTL = 0x00; //установить 8-разрядный режим

 PWMCAE = 0x10; //установить фронтовую ШИМ

 PWMPOL = 0x5F; //выбрать активным высокий логический уровень

                //назначить режим ШИМ для каналов 0,1,2,3,4 и 6

 PWMCLK = 0x50; //каналы 0,1,4 тактируются CLOCK_A и каналы

                //2,3,6 - CLOCK_В

 PWMSCLA = 0x20; //период A 0x20 = 4.1 мс

 PWMSCLB = 0x04; //период B 0x02 = 255 мкс, выбрать

                 // наибольший коэффициент заполнения для всех каналов

 PWMPER0 = 255;

 PWMPER1 = 255;

 PWMPER2 = 255;

 PWMPER3 = 255;

 //Запуск двигателей

 PWME = PWME | 0x5F; // разрешения режима ШИМ для каналов 0,1,2,3,4 и 5

 PWMDTY0 = 80; // установить коэффициенты заполнения

               // для каналов 0,1,2 и 3

 PWMDTY1 = 80;

 PWMDTY2 = 80;

 PWMDTY3 = 80;

 // Взлет

 while (PWMTY0 < 200) {

  PWMDTY0 = PWMDTY0 + 1;

  PWMDTY1 =PWMDTY1 + 1;

  PWMDTY2 = PWMDTY2 + 1;

  PWMDTY3 = PWMDTY3 + 1;

 }

 P = 20;

 // установить коэффициенты заполнения для двигателей на режим полета

 while (battery == 1) //проверить включение батареи

 {

  // двигатель 1

  ATD0CLT5 = 0x04; //режим оцифровки многоканальный

  while ((ATD0STAT0 & 0x80) == 0);

  sensoravg = ATD0DR4H; //установить PAD00 для датчика 1

  if ((int)sensoravg > 80) {

   PWMDTY0 = PWMDTY0 + 20;

   delay2();

   for (i=0; i<p; i++) PWMDTY0--;

  }

  // двигатель 2

  ATD0CLT5 = 0x04;

  while ((ATD0STAT0 & 0x80) == 0);

  sensoravg = ATD0DR5H; //установить PAD00 для датчика 2

  if ((int)sensoravg > 80) {

   PWMDTY1 = PWMDTY1 + 20;

   delay2();

   for (i=0; i<p; i++) PWMDTY1--;

  }

  // двигатель 3

  ATD0CLT5 = 0x04;

  while ((ATD0STAT0 & 0x80) == 0);

  sensoravg = ATD0DR6H; //установить PAD00 для датчика 3

  if ((int)sensoravg > 80) {

   PWMDTY2 = PWMDTY2 + 0;

   delay2();

   for (i=0; i<p; i++) PWMDTY2--;

  }

  // двигатель 4

  ATD0CLT5 = 0x04;

  while ((ATD0STAT0 & 0x80) == 0);

  sensoravg = ATD0DR7H; //установить PAD00 для датчика 4

  if ((int)sensoravg > 80) {

   PWMDTY7 = PWMDTY7 + 20;

   delay2();

   for (i=0; i<p; i++) PWMDTY7--;

  }

 } //конец while

 // снизить скорость двигателей для посадки

 while (PWMDTY0 > 80) {

  PWMDTY0--;

  PWMDTY1--;

  PWMDTY2--;

  PWMDTY3--;

  delay1();

 }

 // остановить двигатели

 PWME = 0x00;

}

//********************************************************************