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;

}

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