void forward(USHORT lspeed,USHORT rspeed)
{
lmotor.setReverse(true);
rmotor.setReverse(false);
lmotor.setAcceleration(normal_acc);
rmotor.setAcceleration(normal_acc);
lmotor.setTargetSpeed(lspeed);
rmotor.setTargetSpeed(rspeed);
}
//Actions
void stop (USHORT acc)
{
int motorcounter =0;
lmotor.setAcceleration(acc);
rmotor.setAcceleration(acc);
lmotor.setTargetSpeed(0); //stop
rmotor.setTargetSpeed(0);
while ( rmotor.getCurrentSpeed()>1 && motorcounter<stop_timeout)
{
motorcounter++;
}
}
void reverse(USHORT speed) //straight back
{
lmotor.setReverse(false);
rmotor.setReverse(true);
lmotor.setAcceleration(normal_acc);
rmotor.setAcceleration(normal_acc);
lmotor.setTargetSpeed(speed);
rmotor.setTargetSpeed(speed);
for(int del =0;del<20000;del++)
{ for(int eel =0;eel<reverse_delay;eel++)
{}
}
}
void turn_left(bool inplace, USHORT lspeed, USHORT rspeed)
{
lmotor.setAcceleration(normal_acc);
rmotor.setAcceleration(normal_acc);
lmotor.setTargetSpeed(lspeed); //stop
rmotor.setTargetSpeed(rspeed);
lmotor.setReverse(!inplace);
rmotor.setReverse(false);
for(int del =0;del<20000;del++)
{ for(int eel =0;eel<turn_delay;eel++)
{}
}
}
int main()
{
state = state_search;
//USHORT speed =search_speed;
Setup_Sumo_Ports();
line_triggered=false;
ioinit ();
// enable interrupts
sei();
// create timer that executes refreshDotMatrix() at 4000 hertz
Timer::instance(0)->setFrequency(1000);
Timer::instance(0)->setFunctionPtr(refreshDotMatrix);
Timer::instance(0)->start();
lmotor.setPhaseRegisters(&LEFT_PHASE_A, &LEFT_PHASE_B);
rmotor.setPhaseRegisters(&RIGHT_PHASE_A, &RIGHT_PHASE_B);
lmotor.setTimerNumber(1);
rmotor.setTimerNumber(5);
lmotor.setTargetSpeed(0);
rmotor.setTargetSpeed(0);
lmotor.setTorque(255);
rmotor.setTorque(255);
lmotor.setAcceleration(normal_acc);
rmotor.setAcceleration(normal_acc);
lmotor.setReverse(true);
rmotor.setReverse(false);
while(countdownrunning)//wait for countdown
{}
Timer::instance(1)->setFrequency(200);
Timer::instance(1)->setFunctionPtr(leftMotorUpdate);
Timer::instance(1)->start();
Timer::instance(5)->setFrequency(200);
Timer::instance(5)->setFunctionPtr(rightMotorUpdate);
Timer::instance(5)->start();
// enable the adc
A2DConverter::enable();
IRSensor IRL(UBYTE(14)); //left
IRSensor IRB(UBYTE(13)); //back
IRSensor IRR(UBYTE(12)); //right
IRSensor IRFR(UBYTE(11)); //front right
IRSensor IRFL(UBYTE(0)); //frong left
IRSensor * rangeSensors[5];
rangeSensors[0] = &IRFL;
rangeSensors[1] = &IRFR;
rangeSensors[2] = &IRR;
rangeSensors[3] = &IRB;
rangeSensors[4] = &IRL;
lines.SetFunctionPtr(checkLines);
if(enable_lines)
lines.EnableFunction();
USHORT counter =0;
//displayInt(speed);
while (true)
{
if(counter == 2000)
{
double d = volts.GetVoltageBatt();
displayFloat(d,2);
counter = 0;
}
counter++;
switch (state)
{
case state_search:
{
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[1],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
forward(search_speed,search_speed);
USHORT fl =0;
USHORT fr=0;
USHORT r=0;
USHORT b=0;
USHORT l=0; //read IR sensors x avg times
SHORT avg = 10;
for(SHORT c = 0;c<avg;c++)
{
fl += IRFL.ReadSensorDist();
fr += IRFR.ReadSensorDist();
r += IRR.ReadSensorDist();
b += IRB.ReadSensorDist();
l += IRL.ReadSensorDist();
}
fl = fl/avg;
fr = fr/avg;
r = r/avg;
b = b/avg;
l = l/avg;
if(fl<IR_front_threshold||fr<IR_front_threshold) //obstacle in front
{
state = state_obstacle_front;
}
else if(r < IR_side_threshold)
{
state = state_obstacle_right;
}
else if(l < IR_side_threshold)
{
state = state_obstacle_left;
}
else if(b < IR_back_threshold)
{
state = state_obstacle_back;
}
if(lines.GetFunctionEnabled())
{
lines.CheckLines();
checkLines();
}
break;
}
case state_obstacle_front:
{
USHORT dot = 1;
DotMatrix::instance()->loadSprite(&dot,1,1,0,0);
DotMatrix::instance()->loadSprite(&dot,1,1,7,0);
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[2],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
USHORT fl,fr; //read IR sensors
fl = IRFL.ReadSensorDist();
fr = IRFR.ReadSensorDist();
static SHORT rmotor_diff = 0;
if(fl>IR_front_threshold&&fr>IR_front_threshold) //obstacle in not front
{
state = state_search; //go back to looking
rmotor_diff = 0;
//clear dots
dot =0;
DotMatrix::instance()->loadSprite(&dot,1,1,0,0);
DotMatrix::instance()->loadSprite(&dot,1,1,7,0);
}
if(fl < fr) //veer left
{
if(rmotor_diff<100)
rmotor_diff += 12;
dot =0; //dot on left, clear right
DotMatrix::instance()->loadSprite(&dot,1,1,0,0);
dot =1;
DotMatrix::instance()->loadSprite(&dot,1,1,7,0);
}
else //veer right
{
if(rmotor_diff > -100)
rmotor_diff -= 12;
dot =1; //dot on left, clear right
DotMatrix::instance()->loadSprite(&dot,1,1,0,0);
dot =0;
DotMatrix::instance()->loadSprite(&dot,1,1,7,0);
}
forward(search_speed,search_speed + rmotor_diff);
break;
}
case state_obstacle_right:
{
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[3],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
turn_right(true,search_speed,search_speed);
state = state_search;
break;
}
case state_obstacle_left:
{
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[4],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
turn_left(true,search_speed,search_speed);
state = state_search;
break;
}
case state_obstacle_back:
{
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[5],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
turn_right(true,search_speed,search_speed);
state = state_search;
break;
}
case state_line_detected:
{
/*
line1 = front right
line2 = back right
line3 = back left
line4 = front left
*/
int timeout = 0;
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[6],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
if(lines.GetLineDetected(2) || lines.GetLineDetected(3)) //line at rear, go forward
{
state = state_search;
}
else //reverse and turn
{
stop(3000);
bool line_left = true;
if(lines.GetLineDetected(1)) //which side is the line on?
line_left = false;
reverse(search_speed);
if(!line_left) //turn away from line
turn_left(true,search_speed,search_speed);
else
turn_right(true,search_speed,search_speed);
stop(3000);
state = state_search;
}
break;
}
case state_rush:
{
state = state_search;
break;
}
case state_push:
{
state = state_search;
break;
}
case state_search_left:
{
state = state_search;
break;
}
case state_search_right:
{
state = state_search;
break;
}
}
}
/* while (true)
{
//USHORT adcValue = A2DConverter::getSample(14);
//adcValue++;
UBYTE numavg = 30;
UINT adcValue = 0;
for(UBYTE i=0;i< numavg;i++)
{
adcValue += IRB.ReadSensorAD();
}
adcValue = adcValue/numavg; //average numavg points
UBYTE adcIndex = UBYTE(adcValue % 10); //1s
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[adcIndex],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
12
);
adcIndex = UBYTE((adcValue/10) % 10); //10s
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[adcIndex],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
8
);
adcIndex = UBYTE((adcValue/100) % 10); //100s
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[adcIndex],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
4
);
adcIndex = UBYTE((adcValue/1000) % 10); //1000s
DotMatrix::instance()->loadSprite(
sc_hexDigits_mini[adcIndex],
SPRITE_ALPHANUMERIC_MINI_WIDTH,
SPRITE_ALPHANUMERIC_MINI_HEIGHT,
1,
0
);
for(int del =0;del<10000;del++)
{ for(int eel =0;eel<70;eel++)
{}
}
} */
return int(0);
}
