NXC实现的NXT 8547 Color Sorter和Color Catapult通用程序

Programus · 发表于 2009-9-5 23:45:22

本帖最后由 Programus 于 2009-9-6 10:00 编辑

Color Sorter是NXT 8547软件里面自带的一个机器人。图纸在软件中。
Color Catapult是Color Sorter的改造，也是8547自带的。
8547自带的软件中，对如上两种机器人分别做了编程。
我将程序用下面的代码重写了一次。
在实现了基本功能和一些简单优化之外，提供了自动识别是Color sorter还是Color catapult的功能。

这个是我的第二个NXC程序，如果有写得拙劣的地方，还希望大家指出。

机器人实际效果请看如下视频(没有权限直接发嵌入视频，只有给出链接了)：
Color catapult，我翻译为颜色投石车——
http://www.tudou.com/programs/view/7ntn7GxLAyI/
Color sorter，我翻译为颜色甄选器——
http://www.tudou.com/programs/view/gIqjNcqD_Z8/

/*
Author: Programus
Wire setup:
Port 1 - Touch Sensor for dispenser
Port 2 - Touch Sensor for tray or catapult
Port 3 - Color sensor
Port 4 - UltraSonic Sensor
Output A - Catapult engine or nothing
Output B - dispenser engine
Output C - rotate engine
Function:
Robot can identify color balls and put same color into same tray or
throw to same direction
* Robot can detect whether there is a catapult
* Robot can dispense color balls automatically
* Robot can identify the color
* Robot can do the same thing for the same color
*/
#define DISPENSER_ENGINE OUT_B
#define ROTATION_ENGINE OUT_C
#define CATAPULT_ENGINE OUT_A
#define DISPENSER_SENSOR IN_1
#define ROTATION_SENSOR IN_2
#define COLOR_SENSOR IN_3
#define SONAR IN_4
#define CATAPULT_NEAR 15 // use to judge whether catapult is face to left
#define CATAPULT_2NEAR 9 // use to judge whether catapult is too near
#define TESTING_POWER 20 // power for testing
#define INIT_DISP_ANGLE -168 // rotate angle for initializing dispenser
#define TURN_DISP_PREANGLE 360 // rotate angle to release ball
#define TURN_DISP_ANGLE 288 // rotate angle to let a ball be under sensor
#define INIT_TRAY_ANGLE 80 // rotate angle for initializing color tray
#define INIT_CATA_ANGLE -13 // rotate angle for initializing catapult
#define THROW_TIME 500 // wait time when throw a ball
#define BALL_ROLL_TIME 500 // time to wait the ball roll out
#define DISPENSER_POWER 50
#define ROTATION_POWER 50
#define CATAPULT_POWER 100
#define COLOR_NUM 4 // how many colors the robot can identify
#define ERROR_FILE "Try Again.rso"
#define START_FILE "! Startup.rso"
/*************** Global Variable **********************************************/
int initCompletedCount = 0; // record how many init jobs done
int colors[] = { // all the colors, pay attention to the order
INPUT_REDCOLOR,
INPUT_YELLOWCOLOR,
INPUT_GREENCOLOR,
INPUT_BLUECOLOR};
int sortColorAngles[] = {0, 90, 180, -90}; // tray turn angles for each color
int cataColorAngles[] = {-50, -15, 15, 50}; // catapult angles for each color
string colorFiles[] = { // sound file for each color
"Red.rso",
"Yellow.rso",
"Green.rso",
"Blue.rso"};
int rotationAngle = 0; // the current angle of color tray
int cataIndex = -1; // record color index of last time
bool firstTime = true; // indicate whether this is the first time
// initiliazing work
sub init()
{
initCompletedCount = 0;
// set all sensors
SetSensorTouch(DISPENSER_SENSOR);
SetSensorTouch(ROTATION_SENSOR);
SetSensorColorFull(COLOR_SENSOR);
SetSensorLowspeed(SONAR);
}
// Detect whether there is an engine connected on the specified port.
bool isEnginePlugged(int port)
{
// rotate the motor a little
OnFwd(port, TESTING_POWER);
Wait(100);
// get the rotation angle
long angle = MotorRotationCount(port);
// set the return value
// if the angle is not 0, that means there is an engine
bool ret = !(angle == 0);
if (ret)
{
// if there is an engine,
// rotate it back because we rotate it before to do the test
RotateMotor(port, TESTING_POWER, -angle);
}
return ret;
}
// Tell robot whether it is a catapult.
bool isCatapult()
{
return isEnginePlugged(CATAPULT_ENGINE);
}
// initialize the dispenser
task initDispenser()
{
// rotate the dispenser engine until it pushed the touch sensor
OnFwd(DISPENSER_ENGINE, DISPENSER_POWER);
until(Sensor(DISPENSER_SENSOR));
// but if the engine pushed the touch sensor at the beginning,
// robot can not ensure the engine position after rotate a particular angle
// so we rotate the engine back,
// to find the position the sensor just be touched
OnRev(DISPENSER_ENGINE, DISPENSER_POWER);
until(!Sensor(DISPENSER_SENSOR));
// stop the engine, this might not necessary, do this just in case
Off(DISPENSER_ENGINE);
// rotate motor to ensure the dispenser is ready
RotateMotor(DISPENSER_ENGINE, DISPENSER_POWER, INIT_DISP_ANGLE);
initCompletedCount++;
}
// initialize the color tray
sub initColorTray()
{
// logic are very similar to the dispenser initialization
OnFwd(ROTATION_ENGINE, ROTATION_POWER);
until(Sensor(ROTATION_SENSOR));
until(!Sensor(ROTATION_SENSOR));
Off(ROTATION_ENGINE);
RotateMotor(ROTATION_ENGINE, TESTING_POWER, INIT_TRAY_ANGLE);
rotationAngle = 0;
}
// initialize the catapult
sub initCatapult()
{
OnRev(ROTATION_ENGINE, ROTATION_POWER);
int dist = SensorUS(SONAR);
while (true)
{
int currentDist = SensorUS(SONAR);
if ((currentDist < CATAPULT_NEAR && currentDist < dist)
|| currentDist < CATAPULT_2NEAR)
{
break;
}
}
Off(ROTATION_ENGINE);
long angle = MotorRotationCount(ROTATION_ENGINE);
OnFwd(ROTATION_ENGINE, ROTATION_POWER);
until(Sensor(ROTATION_SENSOR) ||
abs(MotorRotationCount(ROTATION_ENGINE) - angle) > 135);
if (abs(MotorRotationCount(ROTATION_ENGINE) - angle) > 135)
{
// This case means you installed catapult to a wrong angle,
// warning and stop program.
Off(ROTATION_ENGINE);
PlayFile("! Attention.rso");
Wait(500);
PlayFile("Error.rso");
Wait(1000);
StopAllTasks();
}
until(!Sensor(ROTATION_SENSOR));
Off(ROTATION_ENGINE);
RotateMotor(ROTATION_ENGINE, TESTING_POWER, INIT_CATA_ANGLE);
rotationAngle = 0;
}
// initialize rotation motor
task initRotation()
{
if (isCatapult())
{
PlayTone(440, 100);
initCatapult();
}
else
{
initColorTray();
}
initCompletedCount++;
}
// subrotine to release ball
sub releaseBall()
{
if (!firstTime)
{
RotateMotor(DISPENSER_ENGINE, DISPENSER_POWER, TURN_DISP_PREANGLE);
Wait(BALL_ROLL_TIME);
}
OnFwd(DISPENSER_ENGINE, DISPENSER_POWER);
until(Sensor(DISPENSER_SENSOR));
until(!Sensor(DISPENSER_SENSOR));
RotateMotor(DISPENSER_ENGINE, DISPENSER_POWER, TURN_DISP_ANGLE);
Wait(300);
}
// throw ball by catapult
sub throwBall()
{
OnFwd(CATAPULT_ENGINE, CATAPULT_POWER);
Wait(THROW_TIME);
int angle = MotorRotationCount(CATAPULT_ENGINE);
RotateMotor(CATAPULT_ENGINE, CATAPULT_POWER, -angle);
}
// robot works as a color sorter
sub colorSorter()
{
bool success = false;
int angle = rotationAngle;
for (int i = 0; i < COLOR_NUM; i++)
{
int color = Sensor(COLOR_SENSOR);
if (color == colors[i])
{
string sound = colorFiles[i];
PlayFile(sound);
int angle = sortColorAngles[i];
rotationAngle = angle;
success = true;
break;
}
}
if (!success)
{
PlayFile(ERROR_FILE);
}
int rotate = angle - rotationAngle;
if (rotate > 180)
{
rotate = (rotate % 360) - 360;
}
else if (rotate < -180)
{
rotate = 360 + (rotate % 360);
}
RotateMotor(ROTATION_ENGINE, ROTATION_POWER, rotate);
releaseBall();
}
// robot works as color catapult
sub colorCatapult()
{
releaseBall();
int angle = 0;
if (cataIndex >= 0)
{
string sound = colorFiles[cataIndex];
PlayFile(sound);
angle = cataColorAngles[cataIndex];
}
cataIndex = -1;
for (int i = 0; i < COLOR_NUM; i++)
{
int color = Sensor(COLOR_SENSOR);
if (color == colors[i])
{
cataIndex = i;
break;
}
}
if (angle != 0)
{
RotateMotor(ROTATION_ENGINE, ROTATION_POWER, angle);
Wait(1000);
throwBall();
RotateMotor(ROTATION_ENGINE, ROTATION_POWER, -angle);
}
else if (!firstTime)
{
PlayFile(ERROR_FILE);
}
}
task mainProcess()
{
until (initCompletedCount >= 2)
{
Wait(500);
}
PlayFile(START_FILE);
Wait(1000);
if (isCatapult())
{
while (true)
{
colorCatapult();
firstTime = false;
}
}
else
{
while (true)
{
colorSorter();
firstTime = false;
}
}
}
task main()
{
init();
Precedes(initDispenser, initRotation, mainProcess);
}

复制代码

糖伯虎 · 发表于 2009-9-6 00:27:12

看楼主的程序还是蛮难受的，楼主好象没有用tab的习惯嘛？
还用了很多define，虽然修改容易了，可看的人其实蛮辛苦的，猛往上翻定义，
不过很佩服楼主啦，很厉害的程序

Programus · 发表于 2009-9-6 09:46:49

本帖最后由 Programus 于 2009-9-6 10:05 编辑

看楼主的程序还是蛮难受的，楼主好象没有用tab的习惯嘛？
还用了很多define，虽然修改容易了，可看的人其实蛮辛苦的，猛往上翻定义，
不过很佩服楼主啦，很厉害的程序
糖伯虎发表于 2009-9-6 00:27

用很多define是为了可升级性以及端口的可变性。另外，写的时候因为数字拿捏不准，总是要改，用define要方便很多。
而且，我倒觉得全是数字的程序看起来会比较累，因为不知道那个数字是什么意义。用define，就可以知道数字的意义，至于数字是多少，不同人的机器可能还会有一些微小的不同，反正也不重要。呵呵……

至于tab……其实用了，但是bricx默认把tab变成了一个空格，贴到这里竟然就消失了……

等我想想办法。呵呵……
没有tab的程序，我自己看着也会抓狂的。呵呵……

Programus · 发表于 2009-9-6 10:02:38

本帖最后由 Programus 于 2009-9-6 10:12 编辑

已经调整了，顺便问一下，在bricx里面怎么才能让大括号后的tab不是一个空格呢？
只要前一行有一个大括号，下一行顶头的tab总是默认一个空格。

补充：自己找到答案了，把设置里面的Quick Tab前面的勾去掉就行了。呵呵……

糖伯虎 · 发表于 2009-9-6 10:31:46

我之前写过篇如果设置bricxcc的小东西，有写过怎么设置tab的

xusongyan · 发表于 2009-10-21 19:34:20

不错！！

安步当车 · 发表于 2011-9-1 13:30:52

本帖最后由安步当车于 2011-9-1 13:32 编辑

实现了基本功能和一些简单优化之外，提供了自动识别是Color sorter还是Color catapult的功能？？？

请问是通过检测A电机有没有插么？
看出来了，是用超声波检测的。

账号		自动登录	找回密码
密码			马上注册