void initialize(void); void servo(float degree); // UART file descriptor c // putchar…
- User
void initialize(void); void servo(float degree);
// UART file descriptor c // putchar and getchar are in uart.c FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
// semaphore to protect shared variable #define SEM_SHARED 3
int args[3] ;
volatile char timer0_overflows; volatile char mic_state; volatile char mic_order [3]; volatile unsigned int sound_time1; volatile unsigned int sound_time2; volatile char vector_update_flag; char servo_update_flag; float sound_vector; int servo_min_ticks; int servo_max_ticks;
#define servo_min 0.535 //period in ms #define servo_max 2.425 //period in ms #define speed_sound 34029.0 //speed in cm/sec #define mic_distance 14.0 //distance in cm
int cur_deg; int avg_deg; int num_samples; int next_deg;
//MICROPHONE #1 - B0 ISR (PCINT1_vect) { if (!(PINB & 0x01)) { switch (mic_state) { case 0: TCNT2= 0; //restart timer mic_state= 1; //update state mic_order[0]= 1; //update mic order break;
case 1: sound_time1= TCNT2; //save timer value1 mic_state= 2; //update state mic_order[1]= 1; //update mic order break; case 2: sound_time2= TCNT2; //save timer value2 mic_state= 3; mic_order[2]= 1; //update mic order vector_update_flag=1; //UPDATE VECTOR!! PORTD &= ~(1<<PIND2); break; default: mic_state= -1; break; } } else { //Mic 1 off => reset PORTD |= (1<<PIND2); mic_state= 0; }}
//MICROPHONE #2 - C0 ISR (PCINT2_vect) { if (!(PINC & 0x01)) {
switch (mic_state) { case 0: TCNT2= 0; //restart timer mic_state= 1; //update state mic_order[0]= 2; //update mic order break;case 1: sound_time1= TCNT2; //save timer value1 mic_state= 2; //update state mic_order[1]= 2; //update mic order break; case 2: sound_time2= TCNT2; //save timer value2 mic_state= 3; mic_order[2]= 2; //update mic order vector_update_flag=1; //UPDATE VECTOR!! PORTD &= ~(1<<PIND2); break; default: mic_state= -1; break; } } else { //Mic 2 off => reset PORTD |= (1<<PIND2); mic_state= 0; }}
//MICROPHONE #3 - D0 ISR (PCINT3_vect) { if (!(PIND & 0x01)) {
switch (mic_state) { case 0: TCNT2= 0; //restart timer mic_state= 1; //update state mic_order[0]= 3; //update mic order break;case 1: sound_time1= TCNT2; //save timer value1 mic_state= 2; //update state mic_order[1]= 3; //update mic order; break; case 2: sound_time2= TCNT2; //save timer value2 mic_state= 3; mic_order[2]= 3; //update mic order vector_update_flag=1; //UPDATE VECTOR!! PORTD &= ~(1<<PIND2); break; default: mic_state= -1; break; } } else { //Mic 3 off => reset PORTD |= (1<<PIND2); mic_state= 0; }}
//MIC TIMER2 OVERFLOW ISR (TIMER2_OVF_vect) { mic_state= 0; }
//SERVO CONTROL TIMER0 ISR (TIMER0_COMPA_vect) { PORTB &= ~(1<<PINB3); //Set pin B.3 LOW }
ISR (TIMER0_OVF_vect) { timer0_overflows++; if (timer0_overflows ==4) { //wait ~16ms PORTB |= (1<<PIN3); //Set pin B.3 HIGH timer0_overflows=0; } }
// --- TASK1: Calculate Vector ---------------------------------------- void sound_calculation(void* args) {
uint32_t rel, dead ; int max_delay = floor(mic_distance * 1000000 / speed_sound); float theta; char cur_mic_order [3];while (1) { if (vector_update_flag==1) { //flag to calculate new vector vector_update_flag=0; cur_mic_order[0]= mic_order[0]; cur_mic_order[1]= mic_order[1]; cur_mic_order[2]= mic_order[2]; float dt1= 4*sound_time1; //in uSec float dt2= 4*sound_time2; //in uSec if (dt2 < max_delay) { //final check for invalid sound if ((sound_time2 - sound_time1) > sound_time1){ //sound came from back, compare 1 and 2 theta= ( (float) asin ( (double) (( dt1 * (float) speed_sound ) / ( (float) mic_distance * (float) 1000000)) ) * ( (float) 180 / (float) M_PI) ); switch (cur_mic_order[0]){ case 1: if (cur_mic_order[1] == 2) { theta = 360.0 - theta; } else { //mic_order[1]==3 theta = 240.0 + theta; } break; case 2: if (cur_mic_order[1] == 1) { theta = theta; } else { //mic_order[1]==3 theta = 120.0 - theta; } break; case 3: if (cur_mic_order[1] == 1) { theta = 240.0 - theta; } else{ //mic_order[1]==2 theta = 120.0 + theta; } break; default:// fprintf(stdout,"error"); break; } }
else{ //sound came from front, compare 2 and 3 theta= ( (float) asin ( (double) (( (dt2-dt1) * (float) speed_sound ) / ( (float) mic_distance * (float) 1000000)) ) * ( (float) 180 / (float) M_PI) ); switch (cur_mic_order[1]){ case 1: if (cur_mic_order[2] == 2){ theta = 180.0 + theta; } else{ //mic_order[2]=3 theta = 60.0 - theta; } break; case 2: if (cur_mic_order[2] == 1){ theta = 180.0 - theta; } else{ //mic_order[2]=3 theta = 300.0 + theta; } break; case 3: if (cur_mic_order[2] == 1){ theta = 60.0 + theta; } else{ //mic_order[2]=2 theta = 300.0 - theta; } break; default:// fprintf(stdout,"error"); break; } }
trtWait(SEM_SHARED); sound_vector = theta;// servo_update_flag = 1; servo(sound_vector); trtSignal(SEM_SHARED);
// fprintf(stdout, "state= %i\n\r", mic_state); // fprintf(stdout, "%i ", cur_mic_order[0]); // fprintf(stdout, "%i ", cur_mic_order[1]); // fprintf(stdout, "%i \n\r", cur_mic_order[2]); // fprintf(stdout, "%i uSec\n\r", (int)dt); // fprintf(stdout, "%i degrees\n\r", sound_vector); } } rel = trtCurrentTime() + SECONDS2TICKS(0.15); dead = trtCurrentTime() + SECONDS2TICKS(0.25); trtSleepUntil(rel, dead); }}
// --- TASK2: Servo Control ---------------------------------------- void servo_control(void* args) { uint32_t rel, dead ; int deg_window = 15; int update_window = 7; while (1) { trtWait(SEM_SHARED); if (servo_update_flag == 1){ //flag to update servo position servo_update_flag = 0;
if ( fabs( (float) (sound_vector - avg_deg) ) < deg_window ){ next_deg = sound_vector; avg_deg = (avg_deg * num_samples + sound_vector ) / (num_samples + 1); num_samples++;// fprintf(stdout, "Avg = %i\n\r\n\r", avg_deg); if ( fabs( (float) (cur_deg - avg_deg) ) > update_window ){ cur_deg = avg_deg; if (num_samples > 5){ num_samples = 5; } // if (cur_deg > 180 || cur_deg < 0) {fprintf(stdout,"Error 1");}
// fprintf(stdout, "Update\n\r\n\r"); servo(cur_deg); } }
else if (fabs( (float) (sound_vector - next_deg) ) < deg_window ){ avg_deg = (sound_vector + next_deg)/2; next_deg = sound_vector; num_samples = 2;// fprintf(stdout, "\n\rNew location: %i\n\r\n\r", avg_deg); cur_deg = avg_deg; // if (cur_deg > 180 || cur_deg < 0) {fprintf(stdout,"Error 2");} servo(cur_deg); }
else{ next_deg = sound_vector;// fprintf(stdout,"\n\routlier\n\r\n\r" ); } } trtSignal(SEM_SHARED);
//sleep, run at 50Hz rel = trtCurrentTime() + SECONDS2TICKS(0.02); dead = trtCurrentTime() + SECONDS2TICKS(0.02); trtSleepUntil(rel, dead); }}
// --- MAIN ----------------------------------------------------------- int main(void) { initialize();
//init the UART -- trt_uart_init() is in trtUart.c trt_uart_init(); stdout = stdin = stderr = &uart_str; fprintf(stdout,"\n\r TRT 9feb2009\n\r\n\r"); // start TRT trtInitKernel(80); // 80 bytes for the idle task stack // --- create semaphores ---------- // You must creat the first two semaphores if you use the uart trtCreateSemaphore(SEM_RX_ISR_SIGNAL, 0) ; // uart receive ISR semaphore trtCreateSemaphore(SEM_STRING_DONE,0) ; // user typed <enter> // variable protection trtCreateSemaphore(SEM_SHARED, 1) ; // protect shared variables // --- creat tasks ---------------- trtCreateTask(sound_calculation, 400, SECONDS2TICKS(0.1), SECONDS2TICKS(0.25), &(args[0])); trtCreateTask(servo_control, 200, SECONDS2TICKS(0.1), SECONDS2TICKS(0.1), &(args[1])); // Idle task // just sleeps the cpu to save power // every time it executes set_sleep_mode(SLEEP_MODE_IDLE); sleep_enable(); while (1) { sleep_cpu(); }}
// --- SERVO CONTROL ------------------------------------------- void servo(float degree) { int cmpMatch; degree= degree/2.0; // fprintf(stdout,"/n/r %i \n\r",(int)degree);
cmpMatch= (int)(round( ( (float) (degree * (float) (servo_max - servo_min) / (float) 180.0) + servo_min) * 62.5) -1); if (cmpMatch <= servo_max_ticks && cmpMatch >= servo_min_ticks) { //Saftey Checking OCR0A= cmpMatch; } //var "period" in miliseconds //OCR0A= (int)(round(period*62.5) -1); //0 => 0.016ms //62 => 1.000ms //124 => 2.000ms //255 => 4.096ms}
// --- INITIALIZE ------------------------------------------------- void initialize(void) {
//RESET VARIABLES timer0_overflows = 0; mic_state = 0; vector_update_flag = 0; servo_update_flag = 0; cur_deg = 180; avg_deg = 180; num_samples = 0; next_deg = 180; //SERVO TIMER0 SETUP TCCR0B = 4; //timer0 divide by 256 TIMSK0= (1<<OCIE0A) | (1<<TOIE0); //turn on timer0 match & ovrflow ISR DDRB |= (1<<PINB3); //make B.3 an output servo_min_ticks= ceil(servo_min/0.016); servo_max_ticks= floor(servo_max/0.016); servo(180); //Set servo to neutral //MIC EXTERNAL INTERRUPT SETUP PCICR= 14; //enable external interrupts PCMSK1= (1<<PCINT8); //choose pins B0, C0, D0 PCMSK2= (1<<PCINT16); PCMSK3= (1<<PCINT24); //SOUND TIMER2 SETUP TCCR2B= 4; //timer2 divide by 64 TIMSK2= (1<<TOIE2); //enable overflow ISR //Onboard LED enable DDRD |= (1<<PIND2); PORTD |= (1<<PIND2); sei();}
- Assistant
This code appears to be for a microcontroller using the Tiny Real Time (TRT) kernel. It sets up and controls a servo motor and a set of microphones, likely for some kind of sound localization system. Here's an overview of its major parts:
-
Variable Definitions: Various flags, constants, and variables are defined to control the system's behavior. These include settings for the microphones and servo motor, as well as variables to track the state of the system.
-
Interrupt Service Routines (ISRs): These routines run when certain events occur, such as a microphone detecting sound or a timer expiring. They are responsible for changing the state of the system in response to these events.
-
Tasks: Two tasks are defined,
sound_calculationandservo_control. These tasks are scheduled and managed by the TRT kernel. The first task calculates the direction of incoming sound based on the time it takes to reach each microphone. The second task controls the position of the servo motor based on these calculations. -
Main Function: The main function initializes the system, creates semaphores for task synchronization, and starts the TRT kernel.
-
Servo Control Function: This function is responsible for controlling the servo motor. It calculates the required servo position based on the calculated sound direction and updates the servo's position accordingly.
-
Initialize Function: This function sets up the system's hardware and initial state.
In general, this code represents a real-time embedded system that locates the source of a sound and points a servo motor in that direction.
-
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.