Beispiel #1
0
void testMotor(void)
{
char c;
Serialflush();
Serialprint("Motor Test\n");
while(1)
{
if(uartNewLineFlag == 1)
{
	Serialprint("LF received\n");
c = uartReadBuffer[0];
Serialflush();
switch(c)
{
case 'a': motorForward();
Serialprint("Motor FW\n");
          break;
case 'b': motorBackward();
Serialprint("Motor BW\n");
          break;
case 'c': motorLeft();
Serialprint("Motor Left\n");
          break;
case 'd': motorRight();
Serialprint("Motor Right\n");
          break;
case 'e': motorStop();
Serialprint("Motor Stop\n");
          break;		  
}
}
}
}
Beispiel #2
0
void main() {
    initPIC16F88();

    STATUS_LED = 1;
    __delay_ms(500); //Required delay before startup

    motorForward();

    while(1){

        if(checkIfBalanced() == TRUE)
        {
            //Stop processing
            while(1){
                __delay_ms(250);
                STATUS_LED = 1;
                __delay_ms(250);
                STATUS_LED = 0;
            }
        }
        
        /*Check if we are still on the ramp*/
        if(!onRamp(LEFT_IR_SENSOR)){
            motorRight();
        }else if(!onRamp(RIGHT_IR_SENSOR))
        {
            motorLeft();
        }
        else{
            motorForward();
        }

        
    }

    return;
}
/**********************************************************
 * @brief  robotGo
 * @param  
   coordonner X and coordonner Y
 * @retval None
**********************************************************/
bool robotGo(int Maxspeed, int coord_X, int coord_Y)
{
float posA, posX, posY;
float consigneDistance=0, consigneOrientation=0;
float erreurDistance=0, erreurOrientation=0;
int cmdMoteurRight=0, cmdMoteurLeft=0;	
bool robotGo_state = false;
int speedRight, speedLeft;
const unsigned int MAX_TIMEOUT = 100;

		posX = robotPositionX();
		posY = robotPositionY();
		posA = robotPositionOrientation();
		
		// calcul de la distance à parcourir
		consigneDistance = sqrt( pow(coord_X-posX,2)+pow(coord_Y-posY,2) );
		
		// calcul de l'orientation de consigne du robot
		consigneOrientation = atan2( coord_Y-posY, coord_X-posX);
		
		// calcul erreur en distance
		erreurDistance = consigneDistance;
		if(erreurDistance > ERR_DIS_SAT) erreurDistance = ERR_DIS_SAT;
		
		// calcul erreur en orientation
		erreurOrientation = consigneOrientation - posA;
		if(erreurOrientation > M_PI) erreurOrientation -= M_2PI;
		if(erreurOrientation < -M_PI) erreurOrientation += M_2PI;
		
		if(erreurDistance > DIST_MINI)
		{
			// calcul des commandes
			cmdMoteurRight = erreurDistance*K_PID_R + erreurOrientation*KO_PID_R;
			cmdMoteurLeft = erreurDistance*K_PID_L - erreurOrientation*KO_PID_L;
			
			// saturation de la commande
			cmdMoteurRight = saturationCmd(cmdMoteurRight, Maxspeed);
			cmdMoteurLeft = saturationCmd(cmdMoteurLeft, Maxspeed);
			
			// gestion du sens de rotation des moteurs
			if(cmdMoteurRight < 0 ) motorRight(abs(cmdMoteurRight), 1);
			else motorRight(abs(cmdMoteurRight), 0);  
			
			if(cmdMoteurLeft < 0 ) motorLeft(abs(cmdMoteurLeft), 1);
			else motorLeft(abs(cmdMoteurLeft), 0);  
			
			
			speedRight = motorRightSpeed();
			speedLeft = motorLeftSpeed();
			
			if((speedRight == 0)and(speedLeft == 0)and(timeout > 2))
			{
				stop();
				robotGo_state = true;
				timeout = 0;
			}
			
			timeout++;

		} 
		else
		{
			stop();
			robotGo_state = true;
			timeout = 0;
			
		}
		
		delay(50);
		
		return(robotGo_state);
	  
  
}