void LineFollow::doCalibration(int speedPct, int time){
motorsWritePct(speedPct, -speedPct);
unsigned long beginTime = millis();
while((millis()-beginTime)<time)
ajusta_niveles();
motorsStop();
}
void RobotControl::pointTo(int angle){
int target=angle;
uint8_t speed=80;
target=target%360;
if(target<0){
target+=360;
}
int direction=angle;
while(1){
int currentAngle=compassRead();
int diff=target-currentAngle;
direction=180-(diff+360)%360;
if(direction>0){
motorsWrite(speed,-speed);//right
delay(10);
}else{
motorsWrite(-speed,speed);//left
delay(10);
}
//if(diff<-180)
// diff += 360;
//else if(diff> 180)
// diff -= 360;
//direction=-diff;
if(abs(diff)<5){
motorsStop();
return;
}
}
}
void navigationCancel()
{
motorsStop(); //Make sure motors are stopped
currentNavigationLocation = -1; //Reset starting location
currentNavigationDestination = -1; //Reset travel destination
currentNavigationIntersection = -1; //Reset navigation intersection counter
navigationSounds->play( soundSad );
}
void GeekBot::timedDrive(unsigned int time, int speed) // time in mS, Speed -100 to 100
{
speed = constrain( speed, -100, 100 );
if ( speed < 0 ) motorsReverse( speed );
else motorsForward(speed);
delay(time);
motorsStop();
}
bool checkEStop()
{
if ( digitalRead(LCD_STOP_PIN) == LOW )
{
motorsStop(); //Stops both motors
navigationCancel();
return true;
}
return false;
}
void GeekBot::timedRotate(unsigned int time, int speed) // time in mS, rotation speed CCW -100 to CW +100
{
if (speed > 0)
{
motorsRotateRight(speed);
delay(time);
motorsStop();
}
if (speed < 0)
{
motorsRotateLeft(speed);
delay(time);
motorsStop();
}
if (speed == 0)
{
delay(time);
motorsStop();
}
}
void intersectionDetected()
{
motorsStop();
uint8_t * testIntersectionValue = navigationMap[ currentNavigationLocation ][ currentNavigationDestination ];
uint8_t currentIntersectionCommand = testIntersectionValue[ ++currentNavigationIntersection ];
//Serial.print( "Intersection command: " );
//Serial.println( currentIntersectionCommand );
switch( currentIntersectionCommand )
{
case NAV_FWD:
navigationSounds->play( soundWhistle );
intersectionForward();
break;
case NAV_UTURN:
navigationSounds->play( soundWhistle );
intersectionUTurn();
break;
case NAV_LEFT:
navigationSounds->play( soundWhistle );
intersectionLeft();
break;
case NAV_RIGHT:
navigationSounds->play( soundWhistle );
intersectionRight();
break;
case NAV_STOP:
navigationSounds->play( soundLaugh );
//no break, default desired
default:
motorsStop();
navigationSounds->play( soundBeeps );
currentNavigationLocation = currentNavigationDestination;
currentNavigationDestination = -1;
currentNavigationIntersection = -1;
}
}
void LineFollow::runLineFollow(){
for(int count=0; count<5; count++)
{
lectura_sensor[count]=map(IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
acu+=lectura_sensor[count];
}
//Serial.println(millis());
if (acu > NIVEL_PARA_LINEA)
{
acu/=5;
int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
error = constrain(error,-100,100);
//Calculamos la correcion de velocidad mediante un filtro PD
int vel = (error * KP)/10 + (error-last_error)*KD;
last_error = error;
//Corregimos la velocidad de avance con el error de salida del filtro PD
int motor_left = constrain((robotSpeed + vel),-100,100);
int motor_right =constrain((robotSpeed - vel),-100,100);
//Movemos el robot
//motorsWritePct(motor_left,motor_right);
motorsWritePct(motor_left,motor_right);
//Esperamos un poquito a que el robot reaccione
delay(intergrationTime);
}
else
{
//Hemos encontrado una linea negra
//perpendicular a nuestro camino
//paramos el robot
motorsStop();
//y detenemos la ejecución del programa
//while(true);
reportActionDone();
//setMode(MODE_SIMPLE);
}
}
bool lineFollowerUpdate()
{
if ( checkEStop() ) return; //Abort when requested
uint8_t nextState = lineFollowingState;
switch (lineFollowingState)
{
case IDLE_STATE:
motorsStop(); //Stops both motors
nextState = READ_LINE;
lastTurnDirection = IDLE_STATE;
break;
case GO_INTERSECTION:
intersectionDetected(); //This function will handle all drive and sensor commands until intersection is complete
nextState = IDLE_STATE;
break;
case READ_LINE:
if ( mySensorBar.getDensity() == 0 ) //Lost line completely?
{
switch ( lastTurnDirection )
{
case GO_LEFT:
recoverLineLeft();
nextState = READ_LINE;
break;
case GO_RIGHT:
recoverLineRight();
nextState = READ_LINE;
break;
default:
motorsStop();
lineFollowerSounds->play( soundDisconnection );
delay(1000); //Wait a second to prevent the sound from looping on itself
nextState = IDLE_STATE;
}
}
else if ( mySensorBar.getDensity() < 7 )
{
#ifdef USE_PID
Input = mySensorBar.getPosition();
turningPID.Compute();
#endif
nextState = GO_FORWARD;
if ( mySensorBar.getPosition() < -32 )
{
nextState = GO_LEFT;
lastTurnDirection = GO_LEFT;
}
if ( mySensorBar.getPosition() > 32 )
{
nextState = GO_RIGHT;
lastTurnDirection = GO_RIGHT;
}
}
else //all 8 on means we found an intersection
{
nextState = GO_INTERSECTION;
lastTurnDirection = GO_INTERSECTION;
}
break;
case GO_FORWARD:
motorsForward();
nextState = READ_LINE;
break;
case GO_LEFT:
motorsTurnLeft();
nextState = READ_LINE;
break;
case GO_RIGHT:
motorsTurnRight();
nextState = READ_LINE;
break;
default:
//Somehow something went wrong. This should never be experienced.
motorsStop(); //Stops both motors
while (1)
{
lineFollowerSounds->play( soundWhistle );
delay(1000);
}
}
lineFollowingState = nextState;
}
