ORIGINALLY THERE WAS AN ARROW ON THE SERVO NOW THERE IS A CAMERA PLUGGED INTO COM3. CAN…
- User
ORIGINALLY THERE WAS AN ARROW ON THE SERVO NOW THERE IS A CAMERA PLUGGED INTO COM3. CAN YOU ENABLE THE SERIAL PORT IN THE CODE AND DO SOMETHING USEFUL WITH CV2 AND PYSERIAL LIB WITH MY EXISTING CODE PLS FRIEND?
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 ----- - Assistant
To enable the serial port in the code, you can add the following code snippet:
// Enable Serial Port #define BAUDRATE 9600 #define UBRR_VALUE ((F_CPU / (BAUDRATE * 16UL)) - 1) void initialize_serial(void) { // Set baud rate UBRR0H = (UBRR_VALUE >> 8); UBRR0L = UBRR_VALUE; // Enable receiver and transmitter UCSR0B = (1 << RXEN0) | (1 << TXEN0); // Set frame format: 8 data bits, 1 stop bit UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); } // Call initialize_serial() function in the main function before initializing TRTTo use the cv2 and pyserial libraries with the existing code, you need to install the libraries and import them in your Python code.
First, install the libraries using pip:
pip install opencv-python pip install pyserialThen, in your Python code, you can import the libraries as follows:
import cv2 import serialYou can now use the cv2 library for computer vision tasks and the pyserial library to communicate with the serial port.
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.