Exemplo n.º 1
0
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;
		}
	}
}
Exemplo n.º 2
0
// ---------------------------------------------------------------------------------
// @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;
//		}
	}
}
Exemplo n.º 3
0
// @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;
		}
	}
}
Exemplo n.º 4
0
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");
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
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");
}
Exemplo n.º 7
0
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);
	}
}