main.c
来自「一个机器人开发的相关嵌入式开发源码」· C语言 代码 · 共 1,936 行 · 第 1/4 页
C
1,936 行
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* main.c - main control loop for SRV-1 robot
* Copyright (C) 2005-2006 Surveyor Corporation
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details (www.gnu.org/licenses)
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include "general.h"
#include <lpc2xxx.h>
#include <unistd.h>
#include <setjmp.h>
#include <math.h>
#include <ctype.h>
#include "uart.h"
#include "utils.h"
#include "iap.h"
#include "camera.h"
#include "startup/config.h"
#include "startup/framework.h"
#define _BASIC_
#define COLLISION_THRESHOLD 0x18
#define LEFT_ENCODER 0x00040000
#define RIGHT_ENCODER 0x00020000
#define CHECK_LEFT_ENCODER (IOPIN0 & 0x00040000)
#define CHECK_RIGHT_ENCODER (IOPIN0 & 0x00020000)
#define FLASH_START 0x0001C000 // sector 14 on LPC2106 - 8kb sector
#define MAX_BLOBS 16
#define MAX_COLORS 16
#define MIN_BLOB_SIZE 5
#define index(x,y) (((63-y)*80) + x) // hardwired for 80 x 64 resolution - IMJ3
extern void set_motor(unsigned char, int, int*, int*);
extern void vgrab(unsigned int, unsigned int, unsigned int, unsigned int, unsigned int);
extern void vscan(unsigned int);
extern void vblob(unsigned int);
#ifdef _BASIC_
extern int basic(char *);
extern struct for_stack fpop();
extern char *find_label(char *), *gpop();
extern char get_token();
extern void print(), printx(), time(), motors(), delay(), ir();
extern void imgcap(), colorcap(), colorset(), colorget(), scan(), blob();
extern void bitdir(), bitset(), bitget(), comment();
extern void scan_labels(), find_eol(), exec_goto();
extern void exec_if(), exec_for(), next(), fpush(struct for_stack), input();
extern void gosub(), greturn(), gpush(char *), label_init();
extern void serror(int), get_exp(int *), putback();
extern void level2(int *), level3(int *), level4(int *), level5(int *), level6(int *), primitive(int *);
extern void unary(char, int*), arith(char, int*, int*), assignment();
extern int find_var(char *), iswhite(char), isdelim(char), look_up(char *), get_next_label(char *);
#endif _BASIC_
unsigned char *c328_buf;
unsigned char *decode_buf;
unsigned int ymax[MAX_COLORS], ymin[MAX_COLORS], umax[MAX_COLORS], umin[MAX_COLORS], vmax[MAX_COLORS], vmin[MAX_COLORS];
unsigned int blobx1[MAX_BLOBS], blobx2[MAX_BLOBS], bloby1[MAX_BLOBS], bloby2[MAX_BLOBS], blobcnt[MAX_BLOBS];
unsigned int tvect[80];
/*****************************************************************************
*
* main loop
*
****************************************************************************/
int
main(void) {
const unsigned char version_string[] = __DATE__ " - " __TIME__ ;
unsigned char svfr[] = { "IMJ3" };
unsigned int decode_jpeg = FALSE;
unsigned int failsafe = FALSE;
unsigned int xmit_beacon = 0;
unsigned int locate_beacon = 0;
unsigned int nleft, nright, nforward, nback;
int base_speed = 50; int lspeed = 0;
int rspeed = 0;
unsigned int delay = 0;
unsigned int ix, iy, jx, i1, i2, i3, i4;
unsigned int itmp, t0, t1, retry_count;
unsigned int check_id;
unsigned int id = 0;
int jpegsize = 0;
unsigned int framecount = 0;
unsigned char rxChar, ch, *cp;
unsigned char flash_buf[256], *flash_start, ch1, ch2;
unsigned int flash_sector;
unsigned int tmax[10], tmean[10], tzeros[10];
unsigned int tflag = 0;
unsigned int init_wander = 1;
unsigned int wander_motor = 1;
unsigned char motor_cmd[128] = {
// assign motor command to each 7-bit tflag pattern
// high bits represent clear areas for movement
// 0000000, 0000001, 0000010, 0000011, 0000100, 0000101, 0000110, 0000111,
'.', '.', '.', '.', '6', '6', '6', '6',
// 0001000, 0001001, 0001010, 0001011, 0001100, 0001101, 0001110, 0001111,
'8', '8', '9', '9', '8', '7', '6', '6',
// 0010000, 0010001, 0010010, 0010011, 0010100, 0010101, 0010110, 0010111,
'4', '4', '4', '.', '8', '6', '6', '6',
// 0011000, 0011001, 0011010, 0011011, 0011100, 0011101, 0011110, 0011111,
'4', '4', '4', '8', '8', '8', '8', '9',
// 0100000, 0100001, 0100010, 0100011, 0100100, 0100101, 0100110, 0100111,
'0', '.', '0', '.', '6', '6', '6', '6',
// 0101000, 0101001, 0101010, 0101011, 0101100, 0101101, 0101110, 0101111,
'4', '4', '4', '8', '8', '8', '9', '9',
// 0110000, 0110001, 0110010, 0110011, 0110100, 0110101, 0110110, 0110111,
'4', '4', '4', '4', '4', '4', '2', '6',
// 0111000, 0111001, 0111010, 0111011, 0111100, 0111101, 0111110, 0111111,
'4', '4', '4', '4', '4', '4', '8', '9',
// 1000000, 1000001, 1000010, 1000011, 1000100, 1000101, 1000110, 1000111,
'0', '0', '0', '.', '0', '6', '6', '6',
// 1001000, 1001001, 1001010, 1001011, 1001100, 1001101, 1001110, 1001111,
'7', '8', '8', '8', '8', '8', '6', '6',
// 1010000, 1010001, 1010010, 1010011, 1010100, 1010101, 1010110, 1010111,
'4', '4', '4', '4', '4', '2', '6', '6',
// 1011000, 1011001, 1011010, 1011011, 1011100, 1011101, 1011110, 1011111,
'7', '7', '7', '7', '8', '8', '8', '8',
// 1100000, 1100001, 1100010, 1100011, 1100100, 1100101, 1100110, 1100111,
'0', '0', '0', '2', '0', '6', '6', '6',
// 1101000, 1101001, 1101010, 1101011, 1101100, 1101101, 1101110, 1101111,
'4', '4', '4', '4', '7', '7', '6', '6',
// 1110000, 1110001, 1110010, 1110011, 1110100, 1110101, 1110110, 1110111,
'4', '4', '4', '4', '4', '4', '7', '8',
// 1111000, 1111001, 1111010, 1111011, 1111100, 1111101, 1111110, 1111111,
'7', '7', '7', '7', '8', '8', '8', '8'
};
// initialize real time clock (reads in milliseconds)
initRTC();
//initialize uart #0: 115.2 kbps, 8N1, no FIFO
initUart0(B115200((CRYSTAL_FREQUENCY * PLL_FACTOR) / VPBDIV_FACTOR), UART_8N1, UART_FIFO_16);
//initialize uart #1:
initUart1(B115200((CRYSTAL_FREQUENCY * PLL_FACTOR) / VPBDIV_FACTOR), UART_8N1, UART_FIFO_16);
c328_buf = pHeapStart;
for (ix=0; ix<C328_BUFSIZE; ix++)
c328_buf[ix] = 0;
decode_buf = pHeapStart + C328_BUFSIZE;
for (ix=0; ix<DECODE_BUFSIZE; ix++)
decode_buf[ix] = 0;
flash_start = FLASH_START;
flash_sector = 0;
uart0SendString((unsigned char *)"##Version - ");
uart0SendString((unsigned char *)version_string);
uart0SendChar('\n');
initIR();
//initialize PWM for left and right motors to 1000 Hz update rate
initPwm((CRYSTAL_FREQUENCY * PLL_FACTOR)/ (VPBDIV_FACTOR * 1000));
/*
initServos();
ix = 60;
iy = 50;
while (1) {
setServos(ix, iy);
if (uart0GetChar(&rxChar)) {
ix = ((unsigned int)rxChar & 0x0F) * 10;
iy = ((unsigned int)uart0GetCh() & 0x0F) * 10;
}
}
*/
decode_jpeg = FALSE;
wander_motor = 0;
for (ix=0; ix<MAX_COLORS; ix++) { // initialize stored colors
ymax[ix] = umax[ix] = vmax[ix] = 0;
ymin[ix] = umin[ix] = vmin[ix] = 255;
}
restart:
setPwmDutyPercent(0, 0);
camera_reset(svfr);
retry_count = 0;
t0 = t1 = TIMER0_TC;
while (1) {
rxChar = 0;
if (uart0GetChar(&rxChar)) { // check for command from host
t1 = TIMER0_TC; // reset failsafe timer when any character is received
/* if ID has been assigned, we're in swarm mode. all swarm mode commands start with '@',
followed by a 2 character (ASCII '0' - 'F') ID, followed by the command. an ID of
"00" is invalid, and "FF" applies to all robots. the ID is internally stored as an
8-bit bit binary value, but commands are ASCII encoded, so a conversion has to take
place
*/
if (id) {
if (rxChar != '@') // command must start with '@'
goto breakout;
check_id = atoh(uart0GetCh()) << 4;
check_id += atoh(uart0GetCh());
if ((check_id != id) && (check_id != 0xFF))
goto breakout;
rxChar = uart0GetCh(); // grab the command, and fall through to process the command
}
switch (rxChar) {
case '#': // someone else's ACK - flush all incoming characters
while (uart0GetChar(&rxChar))
delayUs(1000);
break;
#ifdef _BASIC_
case 'Q':
basic(flash_start);
break;
#endif _BASIC_
case 'C': // send cummulative count of captured frames
uart0SendString((unsigned char *)"##Count Frames - ");
printNumber(10, 8, FALSE, ' ', framecount);
uart0SendChar('\n');
break;
case 'f': // enable failsafe
uart0SendString((unsigned char *)"#f");
failsafe = TRUE;
break;
case 'F': // disable failsafe
uart0SendString((unsigned char *)"#F");
failsafe = FALSE;
break;
case 'B': // Bounce IR
uart0SendString((unsigned char *)"##BounceIR - ");
ix = bounceIR(FORWARD, 0xFF);
printNumber(16, 8, FALSE, ' ', ix);
ix = bounceIR(LEFT, 0xFF);
printNumber(16, 8, FALSE, ' ', ix);
ix = bounceIR(BACK, 0xFF);
printNumber(16, 8, FALSE, ' ', ix);
ix = bounceIR(RIGHT, 0xFF);
printNumber(16, 8, FALSE, ' ', ix);
uart0SendChar('\n');
break;
case 'S': // dump scan results if in Wander Mode
if (decode_jpeg) {
uart0SendString((unsigned char *)"##Scan - ");
for (ix=0; ix<80; ix++)
printNumber(16, 2, FALSE, ' ', tvect[ix]);
uart0SendChar('\n');
}
break;
case 'V': // send version string
uart0SendString((unsigned char *)"##Version - ");
uart0SendString((unsigned char *)version_string);
uart0SendChar('\n');
break;
case 'R': // pick another ID
id = 0; // clear the current ID, and fall through ...
case '@':
case 'r': // generate a new robot ID for swarm operation
if (id == 0)
id = read_id_pins();
while ((id == 0) || (id == 0xFF))
id = TIMER0_TC & 0xFF;
delayUs(id * 100); // delay sending ACK so that all robots don't respond simultaneously
uart0SendString((unsigned char *)"##r");
printNumber(16, 2, FALSE, '0', id);
uart0SendChar('\n');
break;
case 'x': // turn on "xmit beacon" mode
uart0SendString((unsigned char *)"#x");
xmit_beacon = 1;
break;
case 'X': // turn on "xmit beacon" mode
uart0SendString((unsigned char *)"#X");
xmit_beacon = 0;
break;
case 'd': // program direction, read or write data bits E9 - E13
ch = uart0GetCh();
switch (ch) {
case 'd': // set direction
ch1 = uart0GetCh();
ch2 = uart0GetCh();
ix = atoh(ch1) * 16 + atoh(ch2);
bits_dir((unsigned char)ix);
uart0SendString((unsigned char *)"##dd\n");
break;
case 'w': // write bits
ch1 = uart0GetCh();
ch2 = uart0GetCh();
ix = atoh(ch1) * 16 + atoh(ch2);
bits_write((unsigned char)ix);
uart0SendString((unsigned char *)"##dw\n");
break;
case 'r': // read bits
ch1 = bits_read();
uart0SendString((unsigned char *)"##dr");
printNumber(16, 2, FALSE, '0', ch1);
uart0SendChar('\n');
break;
}
break;
case 'v': // vision processing commands
// vc = set colors
// vg = grab color sample
// vs = "Scan" function
// vb = find blobs
// vr = recall colors
decode_jpeg = TRUE; // any 'v' commands enable vision processing
rxChar = uart0GetCh();
switch (rxChar) {
case 'c': // vc = set colors
ix = (unsigned int)atoh(uart0GetCh()) & 0x0F;
ch1 = uart0GetCh();
ch2 = uart0GetCh();
ymin[ix] = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
ymax[ix] = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
umin[ix] = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
umax[ix] = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
vmin[ix] = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
vmax[ix] = atoh(ch1) * 16 + atoh(ch2);
uart0SendString((unsigned char *)"##vc");
printNumber(16, 1, FALSE, '0', ix);
uart0SendChar('\n');
break;
case 'g': // vg = grab color sample (color index, x1,x2, y1, y2)
ix = (unsigned int)atoh(uart0GetCh()) & 0x0F;
ch1 = uart0GetCh();
ch2 = uart0GetCh();
i1 = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
i2 = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
i3 = atoh(ch1) * 16 + atoh(ch2);
ch1 = uart0GetCh();
ch2 = uart0GetCh();
i4 = atoh(ch1) * 16 + atoh(ch2);
vgrab(ix, i1, i2, i3, i4);
uart0SendString((unsigned char *)"##vg");
printNumber(16, 1, FALSE, '0', ix);
printNumber(16, 2, FALSE, '0', ymin[ix]);
printNumber(16, 2, FALSE, '0', ymax[ix]);
printNumber(16, 2, FALSE, '0', umin[ix]);
printNumber(16, 2, FALSE, '0', umax[ix]);
printNumber(16, 2, FALSE, '0', vmin[ix]);
printNumber(16, 2, FALSE, '0', vmax[ix]);
uart0SendChar('\n');
break;
case 's': // vs = "Scan" function
ch1 = uart0GetCh();
vscan(atoh(ch1));
uart0SendString((unsigned char *)"##vs");
uart0SendChar(ch1);
for (ix=0; ix<80; ix++)
printNumber(16, 2, FALSE, ' ', tvect[ix]);
uart0SendChar('\n');
break;
case 'b': // vb = find blobs for a given color
ch1 = uart0GetCh();
vblob(atoh(ch1));
uart0SendString((unsigned char *)"##vb");
uart0SendChar(ch1);
for (ix=0; ix<MAX_BLOBS; ix++)
if (blobcnt[ix] < MIN_BLOB_SIZE)
break;
for (iy=0; iy<ix; iy++) {
printNumber(16, 2, FALSE, '0', blobx1[iy]);
printNumber(16, 2, FALSE, '0', blobx2[iy]);
printNumber(16, 2, FALSE, '0', bloby1[iy]);
printNumber(16, 2, FALSE, '0', bloby2[iy]);
printNumber(16, 4, FALSE, '0', blobcnt[iy]);
}
uart0SendChar('\n');
break;
case 'r': // vr = recall colors
ix = (unsigned int)atoh(uart0GetCh()) & 0x0F;
uart0SendString((unsigned char *)"##vr");
printNumber(16, 1, FALSE, '0', ix);
printNumber(16, 2, FALSE, '0', ymin[ix]);
printNumber(16, 2, FALSE, '0', ymax[ix]);
printNumber(16, 2, FALSE, '0', umin[ix]);
printNumber(16, 2, FALSE, '0', umax[ix]);
printNumber(16, 2, FALSE, '0', vmin[ix]);
printNumber(16, 2, FALSE, '0', vmax[ix]);
uart0SendChar('\n');
break;
}
break;
case 'z': // flash memory read / write
rxChar = uart0GetCh();
switch (rxChar) {
case 'r': // read sector
ch1 = uart0GetCh(); // ch1 *16 + ch2 = number of chars to read
ch2 = uart0GetCh();
ix = (atoh(ch1) * 16) + atoh(ch2);
uart0SendString((unsigned char *)"##zr");
uart0SendChar(ch1);
uart0SendChar(ch2);
cp = (unsigned char *)FLASH_START;
for (jx=0; jx<ix; jx++)
uart0SendChar(*cp++);
uart0SendChar('\n');
break;
case 'w': // write sector
for (jx=0; jx<256; jx++) // clear flash buffer
flash_buf[jx] = 0;
ch1 = uart0GetCh(); // ch1 *16 + ch2 = number of chars to write
ch2 = uart0GetCh();
ix = (atoh(ch1) * 16) + atoh(ch2);
for (jx=0; jx<ix; jx++)
flash_buf[jx] = uart0GetCh();
printNumber(10, 4, FALSE, ' ', iapSelectSector(14,14));
printNumber(10, 4, FALSE, ' ', iapEraseSector(14,14));
printNumber(10, 4, FALSE, ' ', iapSelectSector(14,14));
printNumber(10, 4, FALSE, ' ', iapRamToFlash(FLASH_START, flash_buf, 512));
uart0SendString((unsigned char *)"##zw");
uart0SendChar(ch1);
uart0SendChar(ch2);
uart0SendChar('\n');
break;
case 'g': // get sector number
uart0SendString((unsigned char *)"##zg");
printNumber(16, 2, FALSE, '0', flash_sector);
uart0SendChar('\n');
break;
case 's': // set sector number
ch1 = uart0GetCh(); // ch1 *16 + ch2 = number of chars to write
ch2 = uart0GetCh();
//flash_sector = (atoh(ch1) * 16) + atoh(ch2);
flash_sector = 0; // at the moment, this command is ignored
uart0SendString((unsigned char *)"##zs\n");
break;
}
break;
case 'l': // turn on "locate beacon" mode
locate_beacon = atoh(uart0GetCh()) << 4;
locate_beacon += atoh(uart0GetCh());
uart0SendString((unsigned char *)"##l");
printNumber(16, 2, FALSE, '0', locate_beacon);
uart0SendChar('\n');
break;
case 'L': // turn off "locate beacon" mode
uart0SendString((unsigned char *)"#L");
locate_beacon = 0;
break;
case 'I': // send jpeg frame
if (jpegsize < 1) // don't send if frame isn't good
break;
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?