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

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

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

// 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;

}

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