void initializePlatform(){
/* Initialize the PWMs that will be used to driver the motors*/
PWM_dreapta_SetRatio16(0xFFFF); // 0xFFFF = duty cycle 0%
PWM_stanga_SetRatio16(0xFFFF);
PWM_dreapta_Enable();
PWM_stanga_Enable();
}
void motor(int left, int right)
{
/*#############################################################################################################################################################################
** Functia motor este functia de control al motoarelor.
**Aceasta functie ia ca si argument vitezele celor doua motoare(-65535 -> 65535), iar in cazul in care viteza este negativa robotul va merge inapoi.
**Pasii pe care ii urmeaza:
** --seteaza pinii corespunzatori motoarelor ca pini de output
** --daca viteza motorului din stanga/dreapta este negativa sa inverseaza directia
** --scade din viteza maxima viteza data deoarece 65535 este valoarea pentru viteza 0, iar 0 este valoarea pentru viteza 65535, astfel inversand valorile pentru viteza
**#############################################################################################################################################################################*/
const int maxspd=65535;
sens_dreapta_SetOutput();
sens_stanga_SetOutput();
if(left>0)
{
sens_stanga_PutVal(1);
}
else
{
sens_stanga_PutVal(0);
left=-left;
}
if(right>0)
{
sens_dreapta_PutVal(1);
}
else
{
sens_dreapta_PutVal(0);
right=-right;
}
PWM_stanga_SetRatio16(maxspd - left);
PWM_dreapta_SetRatio16(maxspd - right);
PWM_stanga_Enable();
PWM_dreapta_Enable();
}
/*lint -save -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
/* Write your local variable definition here */
bool button=1;
struct sensor s[6];
int64_t min_avg[6]={23, 14, 15, 15, 18, 23};
int64_t max_avg[6]={1000, 193, 172, 170, 160, 550};
int i;
int64_t error;
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
//Initializeaza timer-ul
CountTimer_Init((LDD_TUserData *)NULL);
//Opreste motoarele
motor(0, 0);
//Activeaza PWM-ul pentru motoare
PWM_stanga_Enable();
PWM_dreapta_Enable();
IR_LED_PutVal(0);
while(button)
button=Button_GetVal();
WAIT1_Waitms(500);
IR_LED_PutVal(1);
while(1){
readSensors(min_avg, max_avg, s);
WAIT1_Waitms(1);
#if DEBUG==TRUE
for(i=0; i<6; i++){
Term1_SendNum(s[i].value);
Term1_SendStr(" ");
}
for(i=0; i<6; i++){
Term1_SendNum(s[i].seen);
Term1_SendStr(" ");
}
#endif
error=propder(s);
#if DEBUG==TRUE
Term1_SendNum(error);
Term1_SendChar('\n');
Term1_SendChar('\r');
#endif
drive(error);
}
/*** Don't write any code pass this line, or it will be deleted during code generation. ***/
/*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
#ifdef PEX_RTOS_START
PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/*** End of RTOS startup code. ***/
/*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
for(;;){}
/*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/
