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;
}
}
}
}
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);
}
