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; } } }
// --------------------------------------------------------------------------------- // @brief : To move Robot a certain distance. Distance can be translational (in mm) or // rotational(in degree) // @param see SetMoveCommand() void MoveRobot(int16_t speedType, float dist, float brakeDist, float topSpeed, float endSpeed, float acc) { SetMoveCommand(speedType, dist, brakeDist, topSpeed, endSpeed, acc); while(!EndOfMove(speedType)) { // Do other stuff here!!! // like checking for sensors to detect object etc //TODO: Break The robot // if (bSWFlag) { // user switch break! // break; // } } }
// @brief : To move robot a certain distance. Distance can be translational (in mm) or // : rotational (in deg.) // @param : see SetMoveCommand() void MoveRobot(int16_t speedType, int32_t dist, int16_t brakeDist, int16_t topSpeed, int16_t endSpeed, int16_t acc) { SetMoveCommand(speedType, dist, brakeDist, topSpeed, endSpeed, acc); while (!EndOfMove(speedType)) { // Do other stuff here!!! // like checking for sensors to detect object etc if (curSpeed[XSPEED] == 0) { break; } if (bSWFlag) { // user switch break! bSWFlag = FALSE; break; } } }
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"); }
bool MoveRobotFastRun(int16_t speedType, int32_t dist, int16_t brakeDist, int16_t topSpeed, int16_t endSpeed, int16_t acc) { int markerCnt_old = 0; int32_t actualMaxSpeed = 0; int32_t actualMinSpeed = DIST_mm_oc(MAX_STRAIGHT_SPEED); bool bBreakSegFlag = FALSE; bool bSyncFlag = (*segPtr).bSyncFlag[segmentIndex]; if ( (bSyncFlag) || (segmentIndex == (*segPtr).cnt-1) ) { dist += SYNC_DISTANCE; brakeDist += SYNC_DISTANCE; } SetMoveCommand(speedType, dist, brakeDist, topSpeed, endSpeed, acc); while (!EndOfMove(speedType)) { if (bSyncFlag) { if (!bBreakSegFlag) { if ((dist - (curPos[XSPEED]/DIST_mm_oc(1))) < (SYNC_DISTANCE+50)) { // 400mm markerCnt_old = markerCnt[LEFT_SENSOR]; bBreakSegFlag = TRUE; } } else { if (markerCnt[LEFT_SENSOR] > markerCnt_old) { if (!(*segPtr).bMarkerBreakFlag[segmentIndex]) { (*segPtr).length[segmentIndex+1] -= (*segPtr).syncLength[segmentIndex]; } (*segPtr).bActBreakFlag[segmentIndex] = TRUE; (*segPtr).minDispSync = 0; // StartBeep(); break; } } } if (bStopFlag) break; if (bSWFlag) { bSWFlag = FALSE; return TRUE; } if (actualMaxSpeed < curSpeed[XSPEED]) actualMaxSpeed = curSpeed[XSPEED]; if (actualMinSpeed > curSpeed[XSPEED]) actualMinSpeed = curSpeed[XSPEED]; // gotoxy(0,1); // printf("curSpeed[XSPEED] = %4d m/s\n", (int16_t)(curSpeed[XSPEED]/SPEED_mm_oc(1))); // printf("curAcc[XSPEED] = %4d m/s/s\n", (int16_t)(curAcc[XSPEED]/ACC_mm_oc(1))); // printf("curDist[XSPEED] = %4d mm\n", (int32_t)(curPos[XSPEED]/DIST_mm_oc(1))); // printf("finalPos[XSPEED] = %4d mm\n", (int32_t)(finalPos[XSPEED]/DIST_mm_oc(1))); // printf("align_Kp = %4d align_Kd = %4d\n", align_Kp, align_Kd); // printf("gainSchedule = %4d\n", gainSchedule); // printf("Left PWM: %4d Right PWM: %4d\n", wheelPWM[0], wheelPWM[1]); // printf("Alignment speed: %6d\n", alignmentSpeed); // printf("posErr[XSPEED]: %6d posErr[WSPEED]: %6d\n", posErr[XSPEED], posErr[WSPEED]); // printf("posPWM[XSPEED]: %6d posPWM[WSPEED]: %6d\n", posPWM[XSPEED], posPWM[WSPEED]); } (*segPtr).actMaxSpeed[segmentIndex] = actualMaxSpeed / SPEED_mm_oc(1); (*segPtr).actMinSpeed[segmentIndex] = actualMinSpeed / SPEED_mm_oc(1); (*segPtr).actDisp[segmentIndex] = curPos[XSPEED] / DIST_mm_oc(1); (*segPtr).dist[segmentIndex] = dist; return FALSE; }
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"); }
static void mainTask(void* pvParameters) { (void)pvParameters; ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); ledsbsp_toogleOutputLed(LED1); vTaskDelay(500); controller_Init(); EnWheelMotor(); float maxLinearSpeed = 800; float endLinearSpeed = 0; float maxAngularSpeed = 500; float accX = 2200; float accW = 8000; MoveRobot(XSPEED, 5000, 0, maxLinearSpeed, 0, accX); #if 0 for(int i = 0; i < 8; i++) { SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); } SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, endLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); #endif #if 0 for(int i = 0; i < 4; i++) { SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, 90, 0, maxAngularSpeed, 0, accW); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW); } SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, endLinearSpeed, accX); WaitDist(XSPEED, 150); buzzerbsp_beep(ON); vTaskDelay(10); buzzerbsp_beep(OFF); while(!EndOfMove(XSPEED)); #endif for(;;) { ledsbsp_toogleOutputLed(LED1); vTaskDelay(300); } }