void ExplorationRun(void) {
int i;
if (pSegment.cnt != 0) {
switch (DispDotMatrixWait("There are recorded data. Continue?")) {
case SW_PRESS: return;
default: break;
}
}
// clear rSegment.
clrSegStruct = (char *) &rSegment;
for (i = 0; i < sizeof(rSegment); i++)
*clrSegStruct++ = 0;
// clear pSegment before recording data.
clrSegStruct = (char *) &pSegment;
for (i = 0; i < sizeof(pSegment); i++)
*clrSegStruct++ = 0;
StartBeep();
DispDotMatrix("GOGO");
DelaymSec(1500);
StartRun();
bExplorationFlag = TRUE;
// constant speed & acceleration
// SetRobotSpeedX(EXPLORATION_SPEED);
// SetRobotAccX(ACCELERATION);
SetMoveCommand(XSPEED, 100000, 0, EXPLORATION_SPEED, 0, ACCELERATION);
while (1) {
PrintRunStatus();
// prematurely terminate the run with switch press
if (bSWFlag) {
bExplorationFlag = FALSE;
bSWFlag = FALSE;
StopRun();
break;
}
if (bStopFlag) {
rSegment.totalDisp = curPos[XSPEED] / DIST_mm_oc(1);
StopRun();
ProcessData();
DispExplorationStats();
break;
}
}
}
void TestMotor(void) {
INT8U i;
StartTimeBase();
InitMotor();
for (i = 0;i < 3;i++) {
FrontRun(100);
Wait(1000);
StopRun();
Wait(1000);
BackRun(100);
Wait(1000);
}
StopRun();
}
void Dashboard::NextLap()
{
if (!run_in_progress) return;
current_lap++;
if (current_lap >= NUMBER_OF_LAPS)
{
// stop the run
start_button->setText("Start Run");
run_status->setText("<font color='red'>Stopped</font>");
run_in_progress = false;
emit StopRun();
}
// calculate the delta time for this lap
int delta_secs = expected_lap_secs[current_lap - 1] - current_secs;
int delta_m = delta_secs / 60;
int delta_s = delta_secs % 60;
QString lap_time_str;
if (delta_secs < 0)
{
// behind schedule!
delta_m *= -1;
delta_s *= -1;
lap_time_str = QString("<h2><font color='red'>+%1:%2</font></h2>").arg(delta_m).arg(delta_s, 2, 10, QChar('0'));
}
else
{
lap_time_str = QString("<h2><font color='green'>-%1:%2</font></h2>").arg(delta_m).arg(delta_s, 2, 10, QChar('0'));
}
lap_actual_time[current_lap - 1]->setText(lap_time_str);
}
void Dashboard::StartRunButtonClicked()
{
if (!run_in_progress)
{
// start the run
start_button->setText("Stop Run");
run_status->setText("<font color='green'>Running</font>");
for (int i = 0; i < NUMBER_OF_LAPS; i++)
{
lap_actual_time[i]->setText("<h2><font color='grey'>0:00</font></h2>");
}
run_in_progress = true;
current_lap = 0;
avgspeed_numerator = 0.0;
avgspeed_denominator = 0;
emit StartRun();
}
else
{
// stop the run
start_button->setText("Start Run");
run_status->setText("<font color='red'>Stopped</font>");
run_in_progress = false;
emit StopRun();
}
}
void Test_UseUpBattery(void) {
WaitEnable();
InitMotor();
for (;;) {
FrontRun(200);
Wait(60000);
StopRun();
Wait(10000);
}
}
void basicdaemon::Run()
{
// Write a file to check it's done...
const Configuration &c(GetConfiguration());
FILE *f = fopen(c.GetKeyValue("TestFile").c_str(), "w");
fclose(f);
while(!StopRun())
{
::sleep(10);
}
}
void AutoExperimentControl()
{
gScene->simulate(0);
gScene->checkResults(NX_ALL_FINISHED,true); // make sure time step is done
switch (gExperimentType)
{
case LAY_SUBSTRATE:
if (gScene->getNbDynamicShapes()<RUBBLE_SIZE) ThrowStone();
else
if (CountSleepers()==RUBBLE_SIZE) StopRun("everyone's asleep");
break;
}
return;
}
void SpeedTuning(void) {
int i, cnt = 0;
int16_t dist = 2500;
int16_t variable[4][2500];
DispDotMatrix(" ");
DelaymSec(1500);
StartRun();
SetRobotSpeedX(tuneSpeed);
SetRobotAccX(ACCELERATION);
while (markerCnt[RIGHT_SENSOR] != 1) {
if (bSWFlag) {
bSWFlag = FALSE;
StopRun();
return;
}
}
SetMoveCommand(XSPEED, dist, 0, tuneSpeed, tuneSpeed, ACCELERATION);
while (1) {
sprintf(s, "%04d", linePosition);
DispDotMatrix(s);
DelaymSec(1); // record every 1 msec
if (EndOfMove(XSPEED)) {
StartBeep();
break;
}
if (bStopFlag)
break;
if (bSWFlag) {
bSWFlag = FALSE;
break;
}
if (cnt <= 2500) {
variable[0][cnt] = linePosition;
variable[1][cnt] = alignmentSpeed;
variable[2][cnt] = posErr[WSPEED];
variable[3][cnt] = posPWM[WSPEED];
cnt++;
}
}
StartBeep();
StopRun();
sprintf(s, "%4d", cnt);
DispDotMatrixWait(s);
for (i = 0; i < cnt; i++) {
printf("%-4d %4d %4d %4d\n", variable[0][i], variable[1][i], variable[2][i], variable[3][i]);
}
StartBeep();
DispDotMatrixWait("Done");
}
void RecordSegment(void) {
int i, cnt = 0;
char s[5];
int16_t variable[2500];
// int16_t variable[2][2500];
// int16_t line_position_med[500], *med_sort;
// int a, b;
DispDotMatrix(" ");
DelaymSec(1500);
StartRun();
bRecordFlag = TRUE;
bSwapSide = TRUE;
SetRobotSpeedX(EXPLORATION_SPEED);
SetRobotAccX(ACCELERATION);
while (markerCnt[RIGHT_SENSOR] != 1) {
if (bSWFlag) {
bSWFlag = TRUE;
StopRun();
return;
}
}
while (1) {
sprintf(s, "%04d", linePosition);
DispDotMatrix(s);
DelaymSec(1); // record every 1 msec
if (cnt <= 2500) {
variable[cnt] = linePosition;
// variable[0][cnt] = gyroReading;
// variable[1][cnt] = PIDFeedback[WSPEED];
cnt++;
}
if (bStopFlag)
break;
if (bSWFlag) {
bSWFlag = FALSE;
break;
}
}
StartBeep();
StopRun();
bRecordFlag = FALSE;
bSwapSide = FALSE;
DispDotMatrixWait("Segment recorded successfully");
StartBeep();
clrscr();
sprintf(s, "%4d", cnt);
DispDotMatrixWait(s);
DispDotMatrix(" ");
// insertion sort (median)
// for (i = 0; i < cnt; i += 5) {
// med_sort = line_position+i;
//
// for (a = 1; a < 5; a++) {
// while (med_sort[a] < med_sort[a-1]) {
// b = med_sort[a];
// med_sort[a] = med_sort[a-1];
// med_sort[a-1] = b;
//
// a--;
//
// if (!a)
// break;
// }
// }
//
// line_position_med[i%5] = med_sort[2];
// }
for (i = 0; i < cnt; i++) {
printf("%-4d\n", variable[i]);
// printf("%-4d %-3d\n", variable[0][i], variable[1][i]);
// printf("%-3d\n", line_position_med[i]);
}
WaitSW();
}
void FastRun(int fastType) {
sSeg segment;
int index, *i = &segmentIndex;
char s[4];
if (pSegment.cnt == 0) {
DispDotMatrixWait("No data recorded");
return;
}
StartBeep();
DispDotMatrix("GOGO");
DelaymSec(1500);
bFastFlag = TRUE;
ComputeSpeed(fastType);
segment = pSegment;
segPtr = &segment;
segmentIndex = 0;
StartRun();
// SetRobotSpeedX(segment.topSpeed[*i]);
// SetRobotAccX(ACCELERATION);
SetMoveCommand(XSPEED, 1000, 0, segment.topSpeed[*i], segment.topSpeed[*i], ACCELERATION);
while (markerCnt[RIGHT_SENSOR] != 1) {
if (bSWFlag) {
bSWFlag = FALSE;
StopRun();
return;
}
}
StartRunTimer();
for (; *i < segment.cnt; (*i)++) {
sprintf(s, "%4d", *i);
DispDotMatrix(s);
WaitSyncFastRun();
if (MoveRobotFastRun(XSPEED, segment.length[*i], segment.brakeDist[*i], segment.topSpeed[*i],
segment.endSpeed[*i], ACCELERATION))
break; // switch break
// if (segment.bSyncFlag[*i]) {
// *i = markerCnt[LEFT_SENSOR] - 1;
// }
segment.curDisp += segment.length[*i];
if (bStopFlag) {
(*i)++;
break;
}
// StartBeep();
}
StopRunTimer();
StopRun();
align_Kp = 4;
align_Kd = 40;
sprintf(s, "%04d", runTime);
// DispDotMatrixWait(s);
WaitSW();
clrscr();
printf("segment cnt: %d i: %d\n", segment.cnt, *i);
printf("No. State Top Min Max Dist Length Actual Break Marker\n");
printf("--- ----- --- --- --- ---- ------ ------ ----- ------\n");
for (index = 0; index < *i; index++) {
printf("%3d %-9s %4dmm/s %4dmm/s %4dmm/s %4dmm %4dmm %4dmm %d %d\n", index, run_state[segment.state[index]],
segment.topSpeed[index], segment.actMinSpeed[index], segment.actMaxSpeed[index], segment.dist[index], segment.length[index],
segment.actDisp[index], segment.bActBreakFlag[index], segment.bMarkerBreakFlag[index]);
}
DispDotMatrixWait("Done");
}
// 核心进程
void CoreControl(void) {
INT16U i = 0, j;
INT8U tpls = 0;
// INT16U tmp;
// 串口发送的状态, 和串口字
//INT8U sciState = 0;//, sciChar;
StartSpeeder();
ClearDistanceCounter();
MotorControlInit();
crsN = 0;
nowLoop = 0;
FOREVER() {
//PORTB = ~PORTB;
// 判断小按键是否按下, 是则进入菜单
if (!PTIP_PTIP0) {
WaitEnable();
Wait(20);
if (!PTIP_PTIP0) {
StopRun();
StartMenu();
WaitSmallButtonPress();
Wait(1500);
}
}
// 采集红外值
for (i = 0;i < nIR / 2;i++) {
irSendDouble(irRevPair[i][0], irRevPair[i][1]);
for (j = 0;j < 100;j++);
ReadADCDouble(irRevPair[i][2],irRevPair[i][3], &wir[irRevPair[i][0]], &wir[irRevPair[i][1]]);
}
// 将各值归一化
for (i = 0;i < nIR;i++) {
if (whiteAvg[i] < wir[i]) {
irValue[i] = 100;
} else if (blackAvg[i] > wir[i]) {
irValue[i] = 0;
} else {
irValue[i] = (100 * (wir[i] - blackAvg[i])) / ( whiteAvg[i] - blackAvg[i]);
}
}
/** 红外值的手工修正 **/
//irValue[2] = irValue[2] * 100 / 94;
//irValue[3] = irValue[3] * 100 / 86;
//irValue[6] = irValue[6] * 100 / 94;
// 找最低
minIRv = 100;
maxIRv = 0;
ttotal = 0;
for (i = 0;i < nIR;i++) {
if ( minIRv > irValue[i] ) {
minIRn2 = minIRn;
minIRn = i;
minIRv2 = minIRv;
minIRv = irValue[i];
}
if ( maxIRv < irValue[i] ) {
maxIRv = irValue[i];
}
ttotal+= irValue[i];
}
total = ttotal;
// 求出精确位置
if ( minIRn <= (nIR - 1 - 1) && minIRn >= 1) {
position = ir_position[minIRn - 1] + irValue[minIRn - 1] * IR_SPACE_BETWEEN / (irValue[minIRn - 1] + irValue[minIRn + 1]);
} else if (minIRn == 0) {
if (irValue[1] >= 90) {
position = ir_position[0] - (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
} else {
position = ir_position[0] + (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
}
} else {
if (irValue[nIR - 1 - 1] >= 92/** TODO 这样合适么? **/) {
position = ir_position[nIR - 1] + (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
} else {
position = ir_position[nIR - 1] - (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
}
}
// 排错, 理论上前后两次之间的值相差不应超过15
if ( ((position > last_position)?(position - last_position):(last_position - position)) > 60) {
if (nowLoop == 1) {
if (GetDistance() - lastAllWhiteDist > 50)
position = last_position;
} else if (nowLoop == 2) {
if (GetDistance() - (StartLineDist[1] - StartLineDist[0]) - lastAllWhiteDist > 50)
position = last_position;
}
}
// 类模糊法判断各种路面情况
/** TODO 各各变量的权值还可以调节(通过数据), 变量数还可以增加 **/
//情况 minIRv minIRv2 maxIRv total
blStateArr[NORMAL] = (100 - minIRv) + (100 - minIRv2) + maxIRv + total / nIR;
blStateArr[LOST] = minIRv + minIRv2 + maxIRv + (100 - total / nIR);
blStateArr[CROSS] = (100 - minIRv) + (100 - minIRv2) + (100 - maxIRv) + (100 - total / nIR);
blStateArr[START] = (100 - minIRv) + (100 - minIRv2) + maxIRv + (100 - total / nIR);
// 找最有可能的情况
blState = NORMAL;
for (i = 1;i < 4;i++) {
if (blStateArr[blState] < blStateArr[i]) {
blState = i;
}
}
tBlState = blState;
// 根据判断结果作出相应
if ( blState != NORMAL) {
ProcessSpecialPoint();
if (GetDistance() - lastAllWhiteDist < 50) {
last_position = position;
} else {
position = last_position;
}
} else {
last_position = position;
}
// 得出舵机转角
#if nIR == 8
// 与以前的匹配
if (position > 125) {
position = 125 + (position - 125) * 220 / 175;
} else {
position = 125 - (125 - position) * 220 / 175;
}
#endif
/** TODO 使用一个函数发生器来为舵机提供转角 **/
servoTgtAngle = (INT8U)PosToAgl(position);
//累加舵机角度
if (PerDistSrvTotal < 0xFFFFFF && PerDistSrvN < 0xFFF0 && servoTgtAngle < 130 && servoTgtAngle > 50 ) {
PerDistSrvTotal += servoTgtAngle;
PerDistSrvN++;
}
// 简单转速控制
tgtSpeed = (INT16S)(minSpeed + (maxSpeed - minSpeed) * (speed_arr[position] ) / 40);
if (tgtSpeed < 50 || tgtSpeed > XSpeed) {
tgtSpeed = lastTgtSpeed;
} else {
lastTgtSpeed = tgtSpeed;
}
// 转动舵机
ServoControl();
// 转动马达, 有限制距离的
if (DistLimit != 0 && DistLimit * 25 < GetDistance() ) {
StopRun();
} else {
MotorControl();
}
tdist = GetDistance();
/*if(SCI0SR1 & 0x80) {
switch(sciState) {
case 0:
sciChar = 0xFE;
break;
case 1:
sciChar = (INT8U)position;
break;
case 2:
sciChar = (INT8U)servoTgtAngle;
break;
case 3:
sciChar = ((INT8U)((3927 * 4) / GetSpeed()) * _RTI_P )& 0xFF;
break;
case 4:
sciChar = (INT8U)(GetDistance());
default:
break;
}
sciState = (INT16U)(sciState + 1) % 5;
SCI0DRL = sciChar;
} */
///////////////////////////////////////////////////////////////////////
}
}
void
DataGloveLogger::Halt()
{
StopRun();
}
