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(); }