//************************
// Segue Linha
//************************
void F_STATE_LINHA()
{
MTASK_SET_RUN(MT_STOP_BUTTON);
int Error = 0;
while(ESTADO_IS_CURRENT())
{
//=====================================
int Error = LLreadAverage(S1)-45;
if(Error ==-45){
Error = 0;
PlayTone(200,1);
}
nxtDisplayStringAt(20, 20, "%i",Error);
motor[motorA]=-20 - Error;
motor[motorC]=-20 + Error;
//Se detectou rampa com acceleracao, sobe a rampa
if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
//=====================================
}
}
void FollowSegment()
{
int steering;
T("enter FollowSegment");
ClearTimer(T1);
long lastTime = time1[T1];
while (true) {
int timeNow = time1[T1];
int elapsedTime = timeNow - lastTime;
lastTime = timeNow;
int llAvg = LLreadAverage(LINELEADER);
unsigned byte llResult = LLreadResult(LINELEADER);
if (llAvg != 0)
{
steering = DoPID(llAvg, 45, Kp, Ki, Kd, elapsedTime); // 45 is our goal, set point
//steering = llAvg;
motor[LEFT] = clip((basePower + steering), Min, Max);
motor[RIGHT] = clip((basePower - steering), Min, Max);
// data logging
int heading = HTMCreadHeading(COMPASS);
WriteShort(hFileHandle, nIoResult, timeNow);
WriteShort(hFileHandle, nIoResult, llAvg);
WriteShort(hFileHandle, nIoResult, steering);
WriteFloat(hFileHandle, nIoResult, integral);
WriteFloat(hFileHandle, nIoResult, derivative);
WriteShort(hFileHandle, nIoResult, elapsedTime);
WriteShort(hFileHandle, nIoResult, heading);
WriteByte(hFileHandle, nIoResult, llResult);
}
else
{
// stuff gone wrong, we lost the line
// this may be where we check result for an intersection
PlaySound(soundException);
}
if (TSreadState(BUMPER))
break;
lastTime = timeNow; // using the variable, not the actual clock
//wait1Msec(10); // some throttling
}
T("leave FollowSegment");
}
sub MTASK_DOTASK(int MTASK_ID){
switch (MTASK_ID)
{
//********
case MT_DEFAULT:
wait1Msec(1);
break;
//********
case MT_BEEP:
PlayTone(200, 12);
wait10Msec(120);
break;
//********
case MT_STOP_BUTTON:
if(nNxtButtonPressed==BT_ENTER)
{
int static TimeDif;
TimeDif=time10[T4];
while(nNxtButtonPressed==BT_ENTER){
if(time10[T4]-TimeDif>50)
{
MV_StopMotors();
ClearSounds();
PlaySound(soundBlip);
ESTADO_SET_TARGET(ST_WAIT);
break;
}
}
}
break;
//********
case MT_ACCEL:
static int LastTime;
if(time10[T3]-LastTime >= 30)
{
HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);
if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){
if (ACCEL_Filter++ > 2){
ACCEL_Rampa = true;
ACCEL_Filter = 3;
}
}else{
if (ACCEL_Filter-- < 2){
ACCEL_Filter = 0;
ACCEL_Rampa = false;
}
}
LastTime=time10[T3];
}
break;
//********
case MT_TOQUE:
wait1Msec(1);
break;
//********
case MT_LL:
LL_Avr = LLreadAverage(PORT_LL);
LL_IO = LLreadResult(PORT_LL);
break;
//********
case MT_US:
wait1Msec(30);
ReadAllUS_short(PORT_ARD,USwall);
break;
}
}
