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