void IR2(void *p)
{
while(1)
{
UpdateLeftPWM(400); //Velocity Setting
UpdateRightPWM(400); //Velocity Setting
if (ADC_Data[0]>160) //IR 2
{
Stop();
vTaskSuspend(xforward);
if( sen_dat[5]>180)
{
//DelaymSec(150);
Soft_Left2();
DelaymSec(50);
}
else if(ADC_Data[2]<180)
{
Soft_Right();
DelaymSec(50);
}
else
{
Back();
DelaymSec(50);
}
if(ADC_Data[0]<180)
{
vTaskResume(xforward);
}
}
}
}
void IR6 (void *p)
{
while(1)
{
UpdateLeftPWM(400); //Velocity Setting
UpdateRightPWM(400); //Velocity Setting
if(sen_dat[4]<210) //IR 6
{
Stop();
vTaskSuspend(xforward);
if( ADC_Data[2]<180)
{
//DelaymSec(200);
Soft_Right();
DelaymSec(50);
}
else if(ADC_Data[0]<150)
{
Soft_Left();
DelaymSec(50);
}
else
{
Stop();
DelaymSec(100);
}
if(sen_dat[4]>200)
{
vTaskResume(xforward);
}
}
}
}
void WaitSW(void) {
while(SWstate != SW_LOW); // while USER_SW is not released (LOW)
DelaymSec(10);
while(SWstate != SW_HIGH); // while USER_SW is not pressed (HIGH)
DelaymSec(10);
bSWFlag = FALSE;
}
void BeepOK() {
int i;
for (i=0; i<4; i++) {
Bird();
DelaymSec(20);
}
}
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 IR3(void *p)
{
while(1)
{
UpdateLeftPWM(400); //Velocity Setting
UpdateRightPWM(400); //Velocity Setting
if(sen_dat[1]<210) //IR 3
{
Stop();
vTaskSuspend(xforward);
if( sen_dat[2]>170)
{
//DelaymSec(200);
Right();
DelaymSec(50);
Stop();
DelaymSec(50);
}
else if (sen_dat[0]>190)
{
Left();
DelaymSec(50);
}
else if(sen_dat[2]>170)
{
Right();
DelaymSec(50);
}
else
{
Back();
DelaymSec(50);
}
if(sen_dat[1]>200)
{
vTaskResume(xforward);
}
}
}
}
void IR3_IR4 (void *p)
{
while(1)
{
UpdateLeftPWM(400); //Velocity Setting
UpdateRightPWM(400); //Velocity Setting
if(( ADC_Data[2]>60) && (sen_dat[1]<230)) //IR 3 and IR 4
{
Stop();
vTaskSuspend(xforward);
Back();
DelaymSec(50);
Soft_Left2();
DelaymSec(50);
if(( ADC_Data[2]<180)&&(sen_dat[1]>180))
{
vTaskResume(xforward);
}
}
}
}
//This function sends commands to LCD
void LCD_Command(unsigned int data){
unsigned int temp=0;
EN_LOW();
COMMAND_PORT();
WRITE_DATA();
temp=data;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp & 0xF0) << 18;
EN_HI();
DelaymSec(1);
EN_LOW();
temp=data & 0x0F;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp) << 22;
EN_HI();
DelaymSec(1);
EN_LOW();
}
void forward(void *p)
{
while(1)
{
UpdateLeftPWM(300);
UpdateRightPWM(300);
Forward();
if((sen_dat[0]<190) || (ADC_Data[0]>160) || (sen_dat[1]<210) || ( ADC_Data[2]>170) || ( sen_dat[2]<170) || (sen_dat[4]<200) || (sen_dat[5]<180) || (sen_dat[5]<180) || (( ADC_Data[2]>60) && (sen_dat[1]<230)) || ((ADC_Data[0]>60) &&(sen_dat[1]<230))||(( ADC_Data[2]>150) && (sen_dat[1]<190) && (ADC_Data[0]>120) ) )
{
Stop();
DelaymSec(30);
}
}
}
int main()
{
PINSEL0 = 0x00000000;
PINSEL1 = 0x00000000;
//PINSEL2 = 0x00000000;
Init_Peripherals();
adc_data();
DelaymSec(40);
Init_Peripherals();
LCD_4Bit_Mode();
LCD_Init();
LCD_Command(0x01);
DelaymSec(15);
while(1)
{
xTaskCreate(IR1,"IR1",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR1);
xTaskCreate(IR2,"IR2",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR2);
xTaskCreate(IR3,"IR3",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR3);
xTaskCreate(IR4,"IR4",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR4);
xTaskCreate(IR5,"IR5",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR5);
xTaskCreate(IR6,"IR6",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR6);
xTaskCreate(IR7,"IR7",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR7);
xTaskCreate(IR8,"IR8",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR8);
xTaskCreate(IR2_IR3,"IR2_IR3",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR2_IR3);
xTaskCreate(IR3_IR4,"IR3_IR4",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR3_IR4);
xTaskCreate(forward,"forward",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xforward);
xTaskCreate(IR2_IR3_IR4,"IR2_IR3_IR4",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xIR2_IR3_IR4);
xTaskCreate(lcdprint,"Display",configMINIMAL_STACK_SIZE,NULL,tskIDLE_PRIORITY+1,&xdisplay);
vTaskStartScheduler();
}
}
//This Functions switches LCD panel in 4-bit interfacing mode
void LCD_4Bit_Mode(void)
{
unsigned int temp=0;
EN_LOW();
COMMAND_PORT();
WRITE_DATA();
temp=0x30;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp & 0xF0) << 18;
EN_HI();
DelaymSec(5);
EN_LOW();
DelaymSec(5);
temp=0x30;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp & 0xF0) << 18;
EN_HI();
DelaymSec(5);
EN_LOW();
DelaymSec(5);
temp=0x30;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp & 0xF0) << 18;
EN_HI();
DelaymSec(5);
EN_LOW();
DelaymSec(5);
temp=0x20;
IO1PIN&=0xFC3FFFFF;
IO1PIN|=(temp & 0xF0) << 18;
EN_HI();
DelaymSec(5);
EN_LOW();
DelaymSec(5);
}
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 Bird2() {
DelaymSec(50);
}
