void reactToButton(void) {
if (!Button_GetVal(Button_DeviceData ) && !pressed) {
pressed = TRUE;
LED_Neg();
CDC1_SendString((unsigned char*) "press\r\n");
} else if (Button_GetVal(Button_DeviceData ) && pressed) {
pressed = FALSE;
CDC1_SendString((unsigned char*) "release\r\n");
}
}
void reactToButton(void) { // TODO signal cdc not connected
if (!Button_GetVal(Button_DeviceData ) && !pressed) {
pressed = TRUE;
LED_Neg();
SendString((unsigned char*) "press\r\n", &deviceData);
if(cdc_connected) {
CDC1_SendString((unsigned char*) "press\r\n");
}
} else if (Button_GetVal(Button_DeviceData ) && pressed) {
pressed = FALSE;
SendString((unsigned char*) "release\r\n", &deviceData);
if(cdc_connected) {
CDC1_SendString((unsigned char*) "release\r\n");
}
}
}
void write_all_letters_with_button(char start_letter, char end_letter) {
char caracter = start_letter;
int char_count = 0;
while (caracter <= end_letter) {
if (char_count < 16)
send_data(caracter);
caracter++;
char_count++;
if (char_count > 15) {
send_cmd(0x01, 26);
write_all_letters(caracter - 16, caracter - 1);
}
while (!Button_GetVal());
while (Button_GetVal());
}
}
/*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 */
uint16_t sensorsVector[NUMBER_OF_SENSORS];
/* variables needed for deciding which direction it should turn more */
uint16_t robotTurnsRightMotor;
uint16_t robotTurnsLeftMotor;
/* in case of a white detection - it may be that simply the robot got completely
* out of the track - but it should get back to the truck in case it was
* just loosing a bit the track
*/
uint16_t flagStopWhite;
/* correction algorith will relay on counting how bad
* the curve (error), figuring out how many times the
* sensor is activated
*/
uint16_t correctionS1;
uint16_t correctionS2;
uint16_t i; // for serial debug only
flagStopWhite=0;
/* initialize the correction counters
* they follow how often in a loop cycle the error appears
*/
correctionS1 = 0;
correctionS2 = 0;
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
initializePlatform();
/* Wait for a button press before doing anything*/
while(Button_GetVal()) {}
while (1)
{
/* Read the input sensors */
readSensors(sensorsVector);
robotTurnsLeftMotor = sensorsVector[0] + sensorsVector[1] + sensorsVector[2];
robotTurnsRightMotor = sensorsVector[3] + sensorsVector [4] + sensorsVector[5];
#if 1
for (i = 0; i < NUMBER_OF_SENSORS; i++)
{
Term2_SendNum(sensorsVector[i]);
Term2_SendChar(' ');
}
Term2_SendChar('R');
Term2_SendChar('=');
Term2_SendNum(robotTurnsRightMotor);
Term2_SendChar(' ');
Term2_SendChar('L');
Term2_SendChar('=');
Term2_SendNum(robotTurnsLeftMotor);
Term2_CRLF();
//setRightMotorSpeed(FULL_SPEED);
//setLeftMotorSpeed(FULL_SPEED);
/* motor right turns faster case robot turns left as issue is on the left side */
if (robotTurnsRightMotor > robotTurnsLeftMotor){
/* if last sensor is seeing the black line
* is pretty bad so speed has to be pretty high
* also if situation repeats correction must be more dramatic
* */
if (sensorsVector[0]==0 && sensorsVector[1]!= 0){
if(correctionS1 < TOLERANCE_S1)
correctionS1= correctionS1 + CORRECTION_STEP_S1;
setRightMotorSpeed(FULL_SPEED);
setRightMotorSpeed(SPEED_MOTOR_S1_HIGH_REFERENCE + correctionS1);
setLeftMotorSpeed(SPEED_MOTOR_S1_LOW_REFERENCE - correctionS1);
/* clean up correction flag for RS2*/
WAIT1_Waitus(WAIT_SENSORS_1);
correctionS2 = 0;
}
/* the case in which second sensor sees black line
* less critical but also with potential of becoming an
* issue
*/
else if( sensorsVector[1]==0){
if(correctionS2 < TOLERANCE_S2)
correctionS2 = correctionS2 + CORRECTION_STEP_S2;
setRightMotorSpeed(SPEED_MOTOR_S2_HIGH_REFERENCE + correctionS2);
setLeftMotorSpeed(SPEED_MOTOR_S2_LOW_REFERENCE - correctionS2);
/* clean up the correction flag for RS1 */
correctionS1 = 0;
WAIT1_Waitus(WAIT_SENSORS_2);
}
flagStopWhite=0;
}
/* motor left turns robot goes to right as issue is on the right side */
if ( robotTurnsLeftMotor > robotTurnsRightMotor){
/* if last sensor is seeing the black line
* is pretty bad so speed has to be pretty high
* also if situation repeats correction must be more dramatic
*/
if (sensorsVector[5]==0 && sensorsVector[4]!=0){
if(correctionS1 < TOLERANCE_S1)
correctionS1= correctionS1 + CORRECTION_STEP_S1;
setRightMotorSpeed(SPEED_MOTOR_S1_LOW_REFERENCE - correctionS1);
setLeftMotorSpeed(SPEED_MOTOR_S1_HIGH_REFERENCE + correctionS1);
/* clean up correction flag for S2*/
WAIT1_Waitus(WAIT_SENSORS_1);
correctionS2 = 0;
}
/* the case in which second sensor sees black line
* less critical but also with potential of becoming an
* issue
*/
else if (sensorsVector[4]==0){
if(correctionS2 < TOLERANCE_S2)
correctionS2 = correctionS2 + CORRECTION_STEP_S2;
setRightMotorSpeed(SPEED_MOTOR_S2_LOW_REFERENCE - correctionS2);
setLeftMotorSpeed(SPEED_MOTOR_S2_HIGH_REFERENCE + correctionS2);
/* clean up the correction flag for RS1 */
correctionS1 = 0;
WAIT1_Waitus(WAIT_SENSORS_2);
}
flagStopWhite=0;
}
//case line is correctly read and it should move full speed
if(robotTurnsRightMotor!=0 && robotTurnsLeftMotor != 0 && sensorsVector[2]==0 && sensorsVector[3]==0){
if(robotTurnsRightMotor==robotTurnsLeftMotor && robotTurnsRightMotor== 2){
setRightMotorSpeed(FULL_SPEED);
setLeftMotorSpeed (FULL_SPEED);
WAIT1_Waitus(WAIT_NORMAL);
correctionS1 = 0;
correctionS2 = 0;
flagStopWhite=0;
}
}
//case when it is on the black it also stops
if (robotTurnsRightMotor==0 && robotTurnsLeftMotor == 0 ){
setRightMotorSpeed(0);
setLeftMotorSpeed (0);
correctionS1 = 0;
correctionS2 = 0;
flagStopWhite=0;
}
// case when it is on white board not sensor read black it stops
if (robotTurnsRightMotor==3 && robotTurnsLeftMotor == 3 ){
if(flagStopWhite==TIME_TO_STOP_ON_WHITE){
setRightMotorSpeed(0);
setLeftMotorSpeed (0);
}
else{
flagStopWhite++;
WAIT1_Waitus(WAIT_NORMAL);
}
}
#endif
}
/*** 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!!! ***/
/*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!!! ***/
int propder(struct sensor s[6]){
/*#############################################################################################################################################################################
** In aceasta functie am implementat algoritmul PD propriu-zis.
**Functia ia ca argument un vector de tip structura de date sensor.
**Pasii pe care ii urmeaza:
** --determina pozitia liniei:
** ++in functie sunt setate distantele fata de centru(proportionale cu distantele reale fata de centru)
** ++apeleaza functia see si atribuie valoare variabilei seen
** ++in functie de variabila seen avem 3 cai de executie:
** >>seen==-1 -> niciun senzor nu vede linia:
** ->daca cel mai din dreapta senzor a vazut ultimul linia(eroarea <= -450) robotul vireaza la maxim spre dreapta
** ->daca cel mai din stanga senzor a vazut ultimul linia(eorarea >= 450) robotul vireaza la maxim spre stanga
** >>seen==0 -> doar unii senzori vad linia -> calculeaza pozitia cu ajutorul unei medii aritmetice ponderate:
** (inmulteste valoarea senzorului cu distanta si aduaga rezultatul la eroare, apoi imparte eroarea la suma valorilor,
** daca senzorul respectiv a vazut linia - nu se afla pe alb)
** >>seen==1 ->toti senzorii se afla pe negru -> oprirea motoarelor -> asteapta apasarea butonului pentru a porni din nou
** --calculeaza eroarea rezultata din pozitia fata de centru inmultind-o cu constanta kp
** --calculeaza eroarea in functie de cat de mult s-a modificat fata de ultima eroare inmultind diferenta cu kd
** --aduna cele doua erori si returneaza eroarea finala
###############################################################################################################################################################################*/
/*** i - contor folosi la for-uri ***/
/*** d - suma valorilo senzorilor senzorilor care au vazut linia ***/
/*** button - valoarea intrarii la care este conectat butonul ***/
int i,d=0,button=1;
int seen;
/*** diff - diferenta dintre eroarea curenta si eroarea anterioara ***/
int diff;
/*** last_error - eroarea anterioara ***/
static int last_error=0;
/*** error - eroarea ***/
int64_t error=0;
/*** kd - cat participa componenta derivativa la eroare ***/
/*** kp - cat participa componenta proportionala la eroare ***/
const float kd=5,kp=3;
/*** s[i].dist - distantele fata de centru ***/
s[0].dist=-500;
s[1].dist=-270;
s[2].dist=-80;
s[3].dist=80;
s[4].dist=270;
s[5].dist=500;
seen=see(s);
if(seen==-1){
if(last_error<-450)
error=-1000;
else if (last_error>450)
error=1000;
}
else if (seen==0){
for(i=0; i<6; i++){
if(s[i].seen>=0){
error+=s[i].value*s[i].dist;
d+=s[i].value;
}
}
error/=d;
diff=error-last_error;
}
else if (seen==1){
/*** Oprirea motoarelor. ***/
motor(0,0);
while(button)
button=Button_GetVal();
WAIT1_Waitms(500);
}
last_error=error;
error*=kp;
diff*=kd;
error+=diff;
return error;
}
