virtual Ice::Int activateLed(Ice::Int led, const Ice::Current&) { switch(led){ case 1: toggle_bit(api->led_state, CWIID_LED1_ON); set_led_state(wiimote, api->led_state); break; case 2: toggle_bit(api->led_state, CWIID_LED2_ON); set_led_state(wiimote, api->led_state); break; case 3: toggle_bit(api->led_state, CWIID_LED3_ON); set_led_state(wiimote, api->led_state); break; case 4: toggle_bit(api->led_state, CWIID_LED4_ON); set_led_state(wiimote, api->led_state); break; default: break; } return 1; }
int main(void) { // The following line sets bit 5 HIGH in register DDRD set_bit(DDRD,5); // Pin PD5 is now configured as an OUTPUT set_bit(DDRD,6); // Pin PD6 is now configured as an OUTPUT // The following line sets bit 0 LOW in register DDRF clear_bit(DDRF,0); // Pin PF0 is now configured as an INPUT set_bit(PORTF,0); // Turn on internal pullups for PF0 while(1) { // PINF is the register you have to read to check if a particular // pin on port F (PFx) is HIGH or LOW if(check_bit(PINF,0)) { // If PF0 is HIGH, toggle pin PD5's output status toggle_bit(PORTD,5); } else { // If PF0 is LOW, toggle pin PD6's output status toggle_bit(PORTD,6); } _delay_ms(1000); // Delay 1 second before checking PF0 again } return 0; }
int main(int argc, char *argv[]) { cwiid_wiimote_t *wiimote; /* wiimote handle */ //struct cwiid_state state; /* wiimote state */ bdaddr_t bdaddr; /* bluetooth device address */ //unsigned char mesg = 0; //unsigned char led_state = 0; unsigned char rpt_mode = 0; //unsigned char rumble = 0; /* Make stdout unbuffered, which is useful for piping the output of * this program into a timestamping utility, such as tai64n(1) */ setvbuf(stdout, NULL, _IOLBF, 0); cwiid_set_err(err); /* Connect to address given on command-line, if present */ if (argc > 1) { str2ba(argv[1], &bdaddr); } else { bdaddr = *BDADDR_ANY; } printf("Put Wiimote in discoverable mode now (press 1+2)...\n"); if (!(wiimote = cwiid_open(&bdaddr, 0))) { fprintf(stderr, "Unable to connect to wiimote\n"); exit(1); } if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) { fprintf(stderr, "Unable to set message callback\n"); exit(1); } toggle_bit(rpt_mode, CWIID_RPT_ACC); toggle_bit(rpt_mode, CWIID_RPT_BTN); set_rpt_mode(wiimote, rpt_mode); if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Unable to set message flag\n"); exit(1); } while(1) { sleep(1); } }
int main() { volatile int i; trace_append("%s: setup FreeRTOS...\n", __func__); MboxQueue = xQueueCreate( 32, sizeof( unsigned int* ) ); vSemaphoreCreateBinary(InitDoneSemaphore); xSemaphoreTake(InitDoneSemaphore, portMAX_DELAY); hw_init(); xTaskCreate(LedFlash, "led", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, NULL); xTaskCreate(IpcTask, "ipc", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL); xTaskCreate(RdaemonTask, "rdaemon", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL); trace_append("%s: start FreeRTOS...\n", __func__); vTaskStartScheduler(); /* Just sit and flash the LED quickly if we fail */ while( 1 ) { for( i = 0; i < 50000; i++ ); toggle_bit(GPIO_DATAOUT, 25); } }
/******************************************************************* ** OnMaster : ** ******************************************************************** ** In : - ** ** Out : - ** *******************************************************************/ void OnMaster(void){ _U8 ReturnCode = -1; // Test if a PING needs to be sent if(SendNextPacket){ // Copy the PING string to the buffer used to send the frame strcpy(RFbuffer, "PING"); // Sends the frame to the RF chip SendRfFrame(RFbuffer, strlen(RFbuffer), &ReturnCode); // Indicates that we want to wait for an answer SendNextPacket = false; } else{ // Receives the frame from the RF chip ReceiveRfFrame(RFbuffer, (_U8*)&RFbufferSize, &ReturnCode); // Tests if there is a TimeOut or if we have received a frame if(ReturnCode == OK){ // Sets the last byte of received buffer to 0. Needed by strcmp function RFbuffer[RFbufferSize] = '\0'; // Tests if the received buffer size is greater than 0 if(RFbufferSize > 0){ // Tests if the received buffer value is PONG if(strcmp(RFbuffer, "PONG") == 0){ // Indicates on a LED that the received frame is a PONG toggle_bit(RegPBOut, 0x01); } } // Indicates that we can send the next PING frame SendNextPacket = true; } else if(ReturnCode == RX_TIMEOUT){ // Indicates that we can send the next PING frame SendNextPacket = true; } } }
/** * Toggles the given pin output. * * @param pl Port descriptor for pin. * @param pinNum Pin number for pin. */ void togglePinOutput(portLetter pl, int pinNum) { switch (pl) { case A: toggle_bit(PORTA, pinNum); break; case B: toggle_bit(PORTB, pinNum); break; case C: toggle_bit(PORTC, pinNum); break; case D: toggle_bit(PORTD, pinNum); break; } }
static void LedFlash(void *Parameters) { portTickType LastWake; LastWake = xTaskGetTickCount(); while(1) { toggle_bit(GPIO_DATAOUT, 25); vTaskDelayUntil(&LastWake, 500); } }
// Creadora del device TPFC_device_wiimote::TPFC_device_wiimote(int ident, string bta):TPFC_device(ident){ // creamos el bufer de datos data = new TrackingPFC_data(TrackingPFC_data::TPFCDATA2D); // sobreescribimos el mensaje de error por defecto con el nuestro, para evitar spam cwiid_set_err(err); // comprobamos si tenemos una direccion asignada if (bta.compare("")==0){ // conectarse al primer wiimote que se encuentre bdaddr = *BDADDR_ANY;// bluetooth device address }else{ // conectarse al wiimote con direccion == bta str2ba(bta.c_str(), &bdaddr); } // Conectar los wiimotes printf("Pon el wiimote en modo discoverable (pulsa 1+2)...\n"); wiimote=NULL; while (!wiimote){ if ( !(wiimote = cwiid_open(&bdaddr, 0)) ){ printf("Esperando al wiimote (pulsa 1+2)...\n"); } } // registramos el wiimote para poder identificar despues los callbacks registerwiimote(wiimote, this); // registramos nuestro callback con el wiimote if (cwiid_set_mesg_callback(wiimote, callback)) { fprintf(stderr, "No se ha podido registrar el callback\n"); } // activamos el paso de mensajes desde el wiimote if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Error activando los mensajes\n"); } // configuramos el report mode unsigned char rpt_mode = 0; toggle_bit(rpt_mode, CWIID_RPT_IR); set_rpt_mode(wiimote, rpt_mode); printf("Wiimote conectado!\n"); }
int main(void) { clock_prescale_set(clock_div_1); /* CPU Clock: 8 MHz */ // Initialize serial UBRR0 = 12; /* (8 MHz / 16 / 38400) - 1 */ set_bit(UCSR0B, RXEN0); /* enable RX */ set_bit(UCSR0B, TXEN0); /* enable TX */ // Initialize input/output set_bit(BUTTON_PORT, BUTTON); /* set internal pullup resistor */ set_bit(LED_DDR, LED); /* set output mode for LED */ while (1) { // Poll to see if serial has a byte if (bit_is_set(UCSR0A, RXC0)){ if (UDR0 == 'L'){ /* if the received byte is an 'L' */ toggle_bit(LED_PORT, LED); /* blink LED */ } if (UDR0 == 'O'){ set_bit(LED_PORT, LED); /* on LED */ } if (UDR0 == 'F'){ clear_bit(LED_PORT, LED); /* off LED */ } } // Check to see if button pressed // button wired to ground, so a low voltage is a press if (bit_is_clear(BUTTON_PIN, BUTTON)){ /* wait for send buffer to clear */ loop_until_bit_is_set(UCSR0A, UDRE0); UDR0 = 'X'; /* load up data to be sent */ /* delay a second to keep from opening too many tabs */ _delay_ms(1000); } } /* End event loop */ return 0; }
/******************************************************************* ** OnSlave : ** ******************************************************************** ** In : - ** ** Out : - ** *******************************************************************/ void OnSlave(void){ _U8 ReturnCode = -1; // Receives the frame from the RF chip ReceiveRfFrame(RFbuffer, (_U8*)&RFbufferSize, &ReturnCode); // Tests if we have received a frame if (ReturnCode == OK){ // Tests if the received buffer size is greater than 0 if (RFbufferSize > 0){ // Sets the last byte of received buffer to 0. Needed by strcmp function RFbuffer[RFbufferSize] = '\0'; // Tests if the received buffer value is PING if(strcmp(RFbuffer, "PING") == 0){ // Indicates on a LED that the received frame is a PING toggle_bit(RegPBOut, 0x01); // Copy the PONG string to the buffer used to send the frame strcpy(RFbuffer, "PONG"); // Sends the frame to the RF chip SendRfFrame(RFbuffer, strlen(RFbuffer), &ReturnCode); } } } }
void WiiTrackExtension::cwiidConnect() { bdaddr_t bdaddr; /* bluetooth device address */ bdaddr = *BDADDR_ANY; // connect if (!(m_wiimote = cwiid_connect(&bdaddr, 0))) { qDebug() << "Unable to connect to wiimote"; return; } else { qDebug() << "Connected to wiimote"; m_actions.at(StartIndex)->setEnabled(false); m_actions.at(StopIndex)->setEnabled(true); } // Set the callback function if (cwiid_set_mesg_callback(m_wiimote, cwiid_callback)) { qDebug() << "Unable to set message callback"; cwiidDisconnect(); } // set the report mode unsigned char rpt_mode = 0; toggle_bit(rpt_mode, CWIID_RPT_IR); cwiidSetReportMode(m_wiimote, rpt_mode); // enable messaging if (cwiid_enable(m_wiimote, CWIID_FLAG_MESG_IFC)) { qDebug() << "Error enabling messages"; } m_lastDistance = 0.0; m_lastDot1x = 0.0; m_lastDot1y = 0.0; m_lastDot2x = 0.0; m_lastDot2y = 0.0; }
int main( int argc, char** argv ) { // Variables definition int wiimote_paired = 0; int x = -1; int y = -1; int batt = 0; char big_msg[256]; char small_msg[256]; unsigned char rumble = 0; unsigned char rpt_mode = 0; SDL_Surface* screen = NULL; SDL_Event event; SDL_Rect big_position, small_position; SDL_Surface* big_text = NULL; SDL_Surface* small_text = NULL; TTF_Font* big_font = NULL; TTF_Font* small_font = NULL; AppState appstate = PAIRING; cwiid_wiimote_t* wiimote; bdaddr_t bdaddr = *BDADDR_ANY; struct cwiid_state state; SDL_Color white = {0,0,0}; SDL_Rect dot; // Variables initialisation big_position.x = 180; big_position.y = 250; small_position.x = 300; small_position.y = 50; // SDL and SDL_TTF initialization if( SDL_Init( SDL_INIT_VIDEO | SDL_INIT_TIMER ) ) { fprintf( stderr, "Error while initializing SDL: %s\n", SDL_GetError() ); exit( EXIT_FAILURE ); } if( TTF_Init() ) { fprintf( stderr, "Error while initializing SDL_TTF: %s\n", TTF_GetError() ); exit( EXIT_FAILURE ); } // Resources loading big_font = TTF_OpenFont( "gratis.ttf", 35 ); if( big_font == NULL ) { fprintf( stderr, "Error while loading font: %s\n", TTF_GetError() ); exit( EXIT_FAILURE ); } small_font = TTF_OpenFont( "gratis.ttf", 15 ); if( small_font == NULL ) { fprintf( stderr, "Error while loading font: %s\n", TTF_GetError() ); exit( EXIT_FAILURE ); } // Main window creation screen = SDL_SetVideoMode( S_WIDTH, S_HEIGHT, S_BPP, S_FLAGS ); SDL_WM_SetCaption( "Wiimote pointer display", NULL ); // Pair with the wiimote and rumble to notify printf( "Trying to pair with the wiimote now.\nPut the wiimote in search mode (press 1+2) ...\n" ); if( wiimote = cwiid_open( &bdaddr, 0 ) ) { wiimote_paired = 1; appstate = DISPLAY; toggle_bit( rumble, 0x01 ); cwiid_set_rumble( wiimote, rumble ); usleep( 300000 ); toggle_bit( rumble, 0x01 ); cwiid_set_rumble( wiimote, rumble ); // toggle IR reporting ON toggle_bit( rpt_mode, 0x08 ); cwiid_set_rpt_mode( wiimote, rpt_mode ); } // Main loop while( appstate != EXIT ) { // Take care of events if there is any if( SDL_PollEvent( &event ) ) { switch( event.type ) { case SDL_QUIT: appstate = EXIT; break; case SDL_KEYDOWN: if( event.key.keysym.sym == SDLK_ESCAPE ) appstate = EXIT; break; } } // Query the wiimote if( appstate == DISPLAY ) { cwiid_get_state( wiimote, &state ); if( state.ir_src[0].valid ) { SDL_FillRect( screen, NULL, SDL_MapRGB( screen->format, 0, 255, 0 ) ); x = state.ir_src[0].pos[0]; y = state.ir_src[0].pos[1]; } else { SDL_FillRect( screen, NULL, SDL_MapRGB( screen->format, 255, 0, 0 ) ); } batt = (int)(100.0 * state.battery / 0xD0); // sprintf( big_msg, "IR source position: (%d,%d)", x, y ); dot.x = (int) (x * S_WIDTH) / X_MAX; dot.y = (int) (y * S_HEIGHT) / Y_MAX; dot.w = 10; dot.h = 10; SDL_FillRect( screen, &dot, SDL_MapRGB( screen->format, 255, 255, 255) ); sprintf( small_msg, "Battery remaining: %d%%", batt ); //big_text = TTF_RenderText_Solid( big_font, big_msg, white ); small_text = TTF_RenderText_Solid( small_font, small_msg, white ); //SDL_BlitSurface( big_text, NULL, screen, &big_position ); SDL_BlitSurface( small_text, NULL, screen, &small_position ); } // Render the screen SDL_Flip( screen ); } // Free resources and exits sub-systems TTF_CloseFont( big_font ); TTF_CloseFont( small_font ); TTF_Quit(); if( wiimote_paired ) cwiid_close( wiimote ); else fprintf( stderr, "No wiimotes could be found\n" ); SDL_Quit(); return EXIT_SUCCESS; }
int main() { /* toggle_bit */ ok(7 == toggle_bit(0b0101, 1)); ok(0b0100 == toggle_bit(0b0101, 0)); ok(0b0111 == toggle_bit(0b1111, 3)); /* split_url */ { char *name, *suffix; name = malloc(256*sizeof(char)); ok(0 == split_url(&name, &suffix, "github.com")); ok(0 == strcmp(name, "github")); ok(0 == strcmp(suffix, "com")); ok(0 != split_url(&name, &suffix, "www.github.com")); free(name); } /* is_valid_str */ ok(is_valid_str("github", 6)); ok(!is_valid_str("github.com", 10)); ok(is_valid_str("g1thub", 6)); /* bitsquat_char */ { char c = 'a'; ok('a' != bitsquat_char(c)); } /* random_loc */ { size_t loc = random_loc("github"); ok(loc > 0 && loc < 6); } /* create_url */ { struct Url m_url; m_url = create_url("github.com"); ok(0 == strcmp(m_url.name, "github")); ok(0 == strcmp(m_url.suffix, "com")); free_url(m_url); } /* generate_entries and bitsquat_entries */ { struct BSentries bs_entries; struct Url url = create_url("github.com"); bs_entries = generate_entries(url, 3); ok(3 == bs_entries.n_names); ok(0 == strcmp(bs_entries.suffix, "com")); ok(0 == strcmp(bs_entries.names[0], "github")); bitsquat_entries(bs_entries); ok(0 != strcmp(bs_entries.names[0], "github")); free_url(url); free_bs_entries(bs_entries); } return 0; }
void manual_mode(cwiid_wiimote_t *wiimote) { struct cwiid_state state; /* wiimote state */ int fd = init_maestro(); int target; /*Set wiimote to report accelerometer readings*/ rpt_mode=toggle_bit(rpt_mode, CWIID_RPT_ACC); if (cwiid_set_rpt_mode(wiimote, rpt_mode)) fprintf(stderr, "Error setting report mode\n"); int i = 0; struct acc_cal wm_cal; cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal); double a, a_x, a_y, a_z, pitch, roll; double deltaT_x, deltaT_y; double t_x_curr, t_x_past, t_y_curr, t_y_past; struct timeval tim; char man_mode_select = 0; //Initialise PID parameters for the x-axis and the wait or DELTA_T/2 pid_params x = {KC, TAU_I, TAU_D, TAU_F, 0, 0, x_cord/1000, 0, 0, 0, 0, 0}; //initialise PID parameters for x axis gettimeofday(&tim, NULL); t_x_past=tim.tv_sec+(tim.tv_usec/1000000.0); //initialise t_x_past with current time (in seconds) wait_for_deltat(&tim, &t_x_curr, &t_x_past, &deltaT_x, DELTA_T/2); //Wait until Delta_T/2 // Initialise PID parameters for the y-axis pid_params y = {KC, TAU_I, TAU_D, TAU_F, 0, 0, y_cord/1000, 0, 0, 0, 0, 0}; //initialise PID paramaters for y axis gettimeofday(&tim, NULL); t_y_past=tim.tv_sec+(tim.tv_usec/1000000.0); //initialise t_y_past with current time (in seconds) while(!next_mode) { if (cwiid_get_state(wiimote, &state)) { fprintf(stderr, "Error getting state\n"); } a_x = ((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) / (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X]); a_y = ((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) / (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y]); a_z = ((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) / (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z]); a = sqrt(pow(a_x,2)+pow(a_y,2)+pow(a_z,2)); roll = atan(a_x/a_z); if (a_z <= 0.0) { roll += PI * ((a_x > 0.0) ? 1 : -1); } pitch = atan(a_y/a_z*cos(roll)); roll *= (-180/PI); pitch *= (180/PI); roll *= 0.5; pitch *= 0.5; if (two_button_pressed) { if (man_mode_select ==0){ man_mode_select = 1; //mode 0 is accelerometer mode 1 is keypad playsound("/usr/share/sounds/ball_plate/key_input.wav");} else{ man_mode_select = 0; playsound("/usr/share/sounds/ball_plate/acc_input.wav");} two_button_pressed = 0; } if (left_button_pressed && man_mode_select == 1) { x.set_pt -= 0.005; printf("x= %f\n", x.set_pt); if (x.set_pt > 0.15) x.set_pt = 0.15; if (x.set_pt < -0.15) x.set_pt = -0.15; left_button_pressed = 0; } if (right_button_pressed && man_mode_select == 1) { x.set_pt += 0.005; printf("x= %f\n", x.set_pt); if (x.set_pt > 0.15) x.set_pt = 0.15; if (x.set_pt < -0.15) x.set_pt = -0.15; right_button_pressed = 0; } if (up_button_pressed && man_mode_select == 1) { y.set_pt += 0.005; printf("y= %f\n", y.set_pt); if (y.set_pt > 0.12) x.set_pt = 0.12; if (y.set_pt < -0.12) x.set_pt = -0.12; up_button_pressed = 0; } if (down_button_pressed && man_mode_select == 1) { y.set_pt -= 0.005; printf("y= %f\n", y.set_pt); if (y.set_pt > 0.12) x.set_pt = 0.12; if (y.set_pt < -0.12) x.set_pt = -0.12; down_button_pressed = 0; } if (man_mode_select == 0) { if ((int)pitch > 4 || (int)pitch < -4) { x.set_pt += -0.00005*pitch/10; if (x.set_pt > 0.15) x.set_pt = 0.15; if (x.set_pt < -0.15) x.set_pt = -0.15; } if ((int)roll > 4 || (int)roll < -4) { y.set_pt += -0.00005*roll/10; if (y.set_pt > 0.12) y.set_pt = 0.12; if (y.set_pt < -0.12) y.set_pt = -0.12; } } wait_for_deltat(&tim, &t_x_curr, &t_x_past, &deltaT_x, DELTA_T); //Wait until DELTA_T for x-axis x.pos_past = x.pos_curr; //store past ball position x.pos_curr = x_cord/1000; //current ball position is equal to the coordinates read from the touchscreen x.u_D_past = x.u_D; //store past derivative control signal x.u_act_past = x.u_act; //store past control signal x.error = (x.set_pt - x.pos_curr); //calculate error r(t) - y(t) t_x_past = t_x_curr; //Save new time //Calculate new derivative term x.u_D = (x.tauF/(x.tauF+deltaT_x/1000))*x.u_D_past + ((x.kc*x.tauD)/(x.tauF+deltaT_x/1000))*(x.pos_curr - x.pos_past); //Caluclate new control signal x.u_act = x.u_act_past + x.kc*(-x.pos_curr + x.pos_past) + ((x.kc * deltaT_x/1000)/x.tauI)*(x.error) - x.u_D + x.u_D_past; if (x.u_act > UMAX) x.u_act = UMAX; if (x.u_act < UMIN) x.u_act = UMIN; //Output Control Signal target=(int)((x.u_act*(2.4*180/PI))*40+(4*X_SERVO_CENTRE)); maestroSetTarget(fd, 0, target); //printf("Test Control Signal u_act_x = %f degrees\n", x.u_act*(180/PI)); wait_for_deltat(&tim, &t_y_curr, &t_y_past, &deltaT_y, DELTA_T); //Get Accurate timings y.pos_past = y.pos_curr; //store past ball position y.pos_curr = y_cord/1000; //current ball position is equal to the coordinates read from the touchscreen y.u_D_past = y.u_D; //store past derivative control signal y.u_act_past = y.u_act; y.error = (y.set_pt - y.pos_curr); t_y_past = t_y_curr; //Save new time //Calculate new derivative term y.u_D = (y.tauF/(y.tauF+deltaT_y/1000))*y.u_D_past + ((y.kc*y.tauD)/(y.tauF+deltaT_y/1000))*(y.pos_curr - y.pos_past); //Caluclate new control signal y.u_act = y.u_act_past + y.kc*(-y.pos_curr + y.pos_past) + ((y.kc * deltaT_y/1000)/y.tauI)*(y.error) - y.u_D + y.u_D_past; if (x.u_act > UMAX) x.u_act = UMAX; if (x.u_act < UMIN) x.u_act = UMIN; //Output control signal target=(int)(y.u_act*(2.4*180/PI)*40+(4*Y_SERVO_CENTRE)); maestroSetTarget(fd, 1, target); //printf("Test Control Signal u_act_y = %f degrees\n", y.u_act*(180/PI)); } printf("\n\n"); close_maestro(fd); /*Cancel accelerometer readings*/ rpt_mode=toggle_bit(rpt_mode, CWIID_RPT_ACC); if (cwiid_set_rpt_mode(wiimote, rpt_mode)) fprintf(stderr, "Error setting report mode\n"); }
void GPIO_output_toggle(volatile uint8 *port, uint8 pino) { toggle_bit(*port, pino); }
void usb_cdc_handle_tx() { uns8 cdc_tx_next; uns8 count; uns16 buffer_size; uns8 *buffer; buffer_descriptor *bd; bd = ep_in_bd_location[USB_CDC_DATA_ENDPOINT]; if (test_bit(bd->stat, UOWN)) { // if there's already something in play return; // give up } buffer_size = ep_in_buffer_size[USB_CDC_DATA_ENDPOINT]; buffer = ep_in_buffer_location[USB_CDC_DATA_ENDPOINT]; if (cdc_tx_end == cdc_tx_start) { // anything in the fifo? return; // nope } #ifdef CDC_DEBUG serial_putc('<'); #endif start_crit_sec(); count = 0; while ((cdc_tx_end != cdc_tx_start) && (count < buffer_size)) { cdc_tx_next = cdc_tx_start + 1; // get next position if (cdc_tx_next == USB_CDC_TX_BUFFER_SIZE) { // if we're at the end of the buffer cdc_tx_next = 0; // wrap to the beginning } buffer[count] = cdc_tx_buffer[cdc_tx_start]; // transmit the character #ifdef CDC_DEBUG serial_putc(buffer[count]); #endif count++; cdc_tx_start = cdc_tx_next; // move start position of fifo } if (count > 0) { bd->count = count; bd->addr = (uns16)buffer; toggle_bit(bd->stat, DTS); clear_bit(bd->stat, KEN); // clear the keep bit clear_bit(bd->stat, INCDIS); // clear the increment disable set_bit (bd->stat, DTSEN); clear_bit(bd->stat, BSTALL); // clear stall bit clear_bit(bd->stat, BC9); clear_bit(bd->stat, BC8); set_bit (bd->stat, UOWN); // SIE owns the buffer } end_crit_sec(); #ifdef CDC_DEBUG serial_putc('>'); serial_print_str("send="); serial_print_int(count); serial_putc(' '); #endif }
int main(int argc, char *argv[]) { if (argc != 2) { printf("Usage: %s 3, where 3 is the number of gestures to recognize\n", argv[0]); exit(1); } n_gestures = atoi(argv[1]); hmms = malloc(n_gestures * sizeof(HmmStateRef)); int n_states = n_gestures+3; int n_obs = 17; for(int i = 0; i < n_gestures; i++) { hmms[i] = hmm_new(n_states, n_obs); } cwiid_wiimote_t *wiimote; /* wiimote handle */ //struct cwiid_state state; /* wiimote state */ bdaddr_t bdaddr; /* bluetooth device address */ //unsigned char mesg = 0; //unsigned char led_state = 0; unsigned char rpt_mode = 0; //unsigned char rumble = 0; /* Make stdout unbuffered, which is useful for piping the output of * this program into a timestamping utility, such as tai64n(1) */ setvbuf(stdout, NULL, _IOLBF, 0); cwiid_set_err(err); #if WE_ACTUALLY_CARE_ABOUT_BLUETOOTH /* Connect to address given on command-line, if present */ if (argc > 1) { str2ba(argv[1], &bdaddr); } else { #endif bdaddr = *BDADDR_ANY; #if WE_ACTUALLY_CARE_ABOUT_BLUETOOTH } #endif printf("Put Wiimote in discoverable mode now (press 1+2)...\n"); if (!(wiimote = cwiid_open(&bdaddr, 0))) { fprintf(stderr, "Unable to connect to wiimote\n"); exit(1); } if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) { fprintf(stderr, "Unable to set message callback\n"); exit(1); } toggle_bit(rpt_mode, CWIID_RPT_ACC); toggle_bit(rpt_mode, CWIID_RPT_BTN); set_rpt_mode(wiimote, rpt_mode); if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Unable to set message flag\n"); exit(1); } while(1) { sleep(1); } }
// A quadratic sieve implementation for integers up to 100 bits. N must be composite. mpz_class quadratic_sieve(mpz_class &N) { std::vector<uint32_t> factor_base; mpz_class sqrt_N = sqrt(N); //const unsigned long sqrt_N_long = sqrt_N.get_ui(); // Set the smoothness bound. uint32_t B; { // Approximation of the natural logarithm of N. float log_N = mpz_sizeinbase(N.get_mpz_t(), 2) * log(2); // The optimal smoothness bound is exp((0.5 + o(1)) * sqrt(log(n)*log(log(n)))). B = (uint32_t)ceil(exp(0.56 * sqrt(log_N * log(log_N)))) + 300; } // Generate the factor base using a sieve. { char *sieve = new char[B + 1]; memset(sieve, 1, B + 1); for(unsigned long p = 2; p <= B; ++p) { if(!sieve[p]) continue; if(mpz_legendre(N.get_mpz_t(), mpz_class(p).get_mpz_t()) == 1) factor_base.push_back(p); for(unsigned long i = p; i <= B; i += p) sieve[i] = 0; } delete[] sieve; } std::vector<uint32_t> X; float *Y = new float[SIEVE_CHUNK]; std::vector<std::vector<uint32_t> > smooth; int fails = 0; // The sieve boundary. uint32_t min_x = 0; uint32_t max_x = SIEVE_CHUNK; // Calculate sieve index (where to start the sieve) for each factor base number. uint32_t **fb_indexes = new uint32_t*[2]; fb_indexes[0] = new uint32_t[factor_base.size()]; fb_indexes[1] = new uint32_t[factor_base.size()]; for(uint32_t p = 0; p < factor_base.size(); ++p) { // At what indexes do we start this sieve? Solve the congruence x^2 = n (mod p) to find out. // Results in two solutions, so we do two sieve iterations for each prime in the factor base. uint32_t idxs[2]; mpz_class temp = N % mpz_class(factor_base[p]); tonelli_shanks(temp.get_ui(), factor_base[p], idxs); temp = idxs[0] - sqrt_N; temp = ((temp % factor_base[p]) + factor_base[p]) % factor_base[p]; fb_indexes[0][p] = temp.get_ui(); temp = idxs[1] - sqrt_N; temp = ((temp % factor_base[p]) + factor_base[p]) % factor_base[p]; fb_indexes[1][p] = temp.get_ui(); } float last_estimate = 0; uint32_t next_estimate = 1; // Sieve new chunks until we have enough smooth numbers. while(smooth.size() < (factor_base.size() + 20)) { // Generate our Y vector for the sieve, containing log approximations that fit in machine words. for(uint32_t t = 1; t < SIEVE_CHUNK; ++t) { // Calculating a log estimate is expensive, so don't do it for every Y[t]. if(next_estimate <= (t + min_x)) { mpz_class y = (sqrt_N + t + min_x) * (sqrt_N + t + min_x) - N; // To estimate the 2 logarithm, just count the number of bits that v takes up. last_estimate = mpz_sizeinbase(y.get_mpz_t(), 2); // The higher t gets, the less the logarithm of Y[t] changes. next_estimate = next_estimate * 1.8 + 1; } Y[t] = last_estimate; } // Perform the actual sieve. for(uint32_t p = 0; p < factor_base.size(); ++p) { float lg = log(factor_base[p]) / log(2); for(uint32_t t = 0; t < 2; ++t) { while(fb_indexes[t][p] < max_x) { Y[fb_indexes[t][p] - min_x] -= lg; fb_indexes[t][p] += factor_base[p]; } // p = 2 only has one modular root. if(factor_base[p] == 2) break; } } // Factor all values whose logarithms were reduced to approximately zero using trial division. { float threshold = log(factor_base.back()) / log(2); for(uint32_t i = 0; i < SIEVE_CHUNK; ++i) { if(fabs(Y[i]) < threshold) { mpz_class y = (sqrt_N + i + min_x) * (sqrt_N + i + min_x) - N; smooth.push_back(std::vector<uint32_t>()); for(uint32_t p = 0; p < factor_base.size(); ++p) { while(mpz_divisible_ui_p(y.get_mpz_t(), factor_base[p])) { mpz_divexact_ui(y.get_mpz_t(), y.get_mpz_t(), factor_base[p]); smooth.back().push_back(p); } } if(y == 1) { // This V was indeed B-smooth. X.push_back(i + min_x); // Break out of trial division loop if we've found enou gh smooth numbers. if(smooth.size() >= (factor_base.size() + 20)) break; } else { // This V was apparently not B-smooth, remove it. smooth.pop_back(); ++fails; } } } } min_x += SIEVE_CHUNK; max_x += SIEVE_CHUNK; } uint64_t **matrix = new uint64_t*[factor_base.size()]; // The amount of words needed to accomodate a row in the augmented matrix. int row_words = (smooth.size() + sizeof(uint64_t)) / sizeof(uint64_t); for(uint32_t i = 0; i < factor_base.size(); ++i) { matrix[i] = new uint64_t[row_words]; memset(matrix[i], 0, row_words * sizeof(uint64_t)); } for(uint32_t s = 0; s < smooth.size(); ++s) { // For each factor in the smooth number, add the factor to the corresponding element in the matrix. for(uint32_t p = 0; p < smooth[s].size(); ++p) toggle_bit(s, matrix[smooth[s][p]]); } // Gauss elimination. The dimension of the augmented matrix is factor_base.size() x (smooth.size() + 1). { uint32_t i = 0, j = 0; while(i < factor_base.size() && j < (smooth.size() + 1)) { uint32_t maxi = i; // Find pivot element. for(uint32_t k = i + 1; k < factor_base.size(); ++k) { if(get_bit(j, matrix[k]) == 1) { maxi = k; break; } } if(get_bit(j, matrix[maxi]) == 1) { std::swap(matrix[i], matrix[maxi]); for(uint32_t u = i + 1; u < factor_base.size(); ++u) { if(get_bit(j, matrix[u]) == 1) { for(int32_t w = 0; w < row_words; ++w) matrix[u][w] ^= matrix[i][w]; } } ++i; } ++j; } } mpz_class a; mpz_class b; // A copy of matrix that we'll perform back-substitution on. uint64_t **back_matrix = new uint64_t*[factor_base.size()]; for(uint32_t i = 0; i < factor_base.size(); ++i) back_matrix[i] = new uint64_t[row_words]; uint32_t *x = new uint32_t[smooth.size()]; uint32_t *combination = new uint32_t[factor_base.size()]; // Loop until we've found a non-trivial factor. do { // Copy the gauss eliminated matrix. for(uint32_t i = 0; i < factor_base.size(); ++i) memcpy(back_matrix[i], matrix[i], row_words * sizeof(uint64_t)); // Clear the x vector. memset(x, 0, smooth.size() * sizeof(uint32_t)); // Perform back-substitution on our matrix that's now in row echelon form to get x. { int32_t i = factor_base.size() - 1; while(i >= 0) { // Count non-zero elements in current row. int32_t count = 0; int32_t current = -1; for(uint32_t c = 0; c < smooth.size(); ++c) { count += get_bit(c, back_matrix[i]); current = get_bit(c, back_matrix[i]) ? c : current; } // Empty row, advance to next. if(count == 0) { --i; continue; } // The system is underdetermined and we can choose x[current] freely. // To avoid the trivial solution we avoid always setting it to 0. uint32_t val = count > 1 ? rand() % 2 : get_bit(smooth.size(), back_matrix[i]); x[current] = val; for(int32_t u = 0; u <= i; ++u) { if(get_bit(current, back_matrix[u]) == 1) { if(val == 1) toggle_bit(smooth.size(), back_matrix[u]); unset_bit(current, back_matrix[u]); } } if(count == 1) --i; } } a = 1; b = 1; // The way to combine the factor base to get our square. memset(combination, 0, sizeof(uint32_t) * factor_base.size()); for(uint32_t i = 0; i < smooth.size(); ++i) { if(x[i] == 1) { for(uint32_t p = 0; p < smooth[i].size(); ++p) ++combination[smooth[i][p]]; b *= (X[i] + sqrt_N); } } for(uint32_t p = 0; p < factor_base.size(); ++p) { for(uint32_t i = 0; i < (combination[p] / 2); ++i) a *= factor_base[p]; } // If a = +/- b (mod N) we found a trivial factor, run the loop again to find a new a and b. } while(a % N == b % N || a % N == (- b) % N + N); b -= a; mpz_class factor; mpz_gcd(factor.get_mpz_t(), b.get_mpz_t(), N.get_mpz_t()); for(uint32_t i = 0; i < factor_base.size(); ++i) { delete[] matrix[i]; delete[] back_matrix[i]; } delete[] combination; delete[] Y; delete[] fb_indexes[0]; delete[] fb_indexes[1]; delete[] fb_indexes; delete[] matrix; delete[] back_matrix; delete[] x; return factor; }
int main(int argc, char *argv[]) { cwiid_wiimote_t *wiimote; /* wiimote handle */ struct cwiid_state state; /* wiimote state */ bdaddr_t bdaddr; /* bluetooth device address */ unsigned char mesg = 0; unsigned char led_state = 0; unsigned char rpt_mode = 0; unsigned char rumble = 0; int exit = 0; cwiid_set_err(err); /* Connect to address given on command-line, if present */ if (argc > 1) { str2ba(argv[1], &bdaddr); } else { bdaddr = *BDADDR_ANY; } /* Connect to the wiimote */ printf("Put Wiimote in discoverable mode now (press 1+2)...\n"); if (!(wiimote = cwiid_open(&bdaddr, 0))) { fprintf(stderr, "Unable to connect to wiimote\n"); return -1; } if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) { fprintf(stderr, "Unable to set message callback\n"); } printf("Note: To demonstrate the new API interfaces, wmdemo no longer " "enables messages by default.\n" "Output can be gathered through the new state-based interface (s), " "or by enabling the messages interface (c).\n"); /* Menu */ printf("%s", MENU); while (!exit) { switch (getchar()) { case '1': toggle_bit(led_state, CWIID_LED1_ON); set_led_state(wiimote, led_state); break; case '2': toggle_bit(led_state, CWIID_LED2_ON); set_led_state(wiimote, led_state); break; case '3': toggle_bit(led_state, CWIID_LED3_ON); set_led_state(wiimote, led_state); break; case '4': toggle_bit(led_state, CWIID_LED4_ON); set_led_state(wiimote, led_state); break; case '5': toggle_bit(rumble, 1); if (cwiid_set_rumble(wiimote, rumble)) { fprintf(stderr, "Error setting rumble\n"); } break; case 'a': toggle_bit(rpt_mode, CWIID_RPT_ACC); set_rpt_mode(wiimote, rpt_mode); break; case 'b': toggle_bit(rpt_mode, CWIID_RPT_BTN); set_rpt_mode(wiimote, rpt_mode); break; case 'e': /* CWIID_RPT_EXT is actually * CWIID_RPT_NUNCHUK | CWIID_RPT_CLASSIC */ toggle_bit(rpt_mode, CWIID_RPT_EXT); set_rpt_mode(wiimote, rpt_mode); break; case 'i': /* libwiimote picks the highest quality IR mode available with the * other options selected (not including as-yet-undeciphered * interleaved mode */ toggle_bit(rpt_mode, CWIID_RPT_IR); set_rpt_mode(wiimote, rpt_mode); break; case 'm': if (!mesg) { if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Error enabling messages\n"); } else { mesg = 1; } } else { if (cwiid_disable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Error disabling message\n"); } else { mesg = 0; } } break; case 'p': printf("%s", MENU); break; case 'r': if (cwiid_request_status(wiimote)) { fprintf(stderr, "Error requesting status message\n"); } break; case 's': if (cwiid_get_state(wiimote, &state)) { fprintf(stderr, "Error getting state\n"); } print_state(&state); break; case 't': toggle_bit(rpt_mode, CWIID_RPT_STATUS); set_rpt_mode(wiimote, rpt_mode); break; case 'x': exit = -1; break; case '\n': break; default: fprintf(stderr, "invalid option\n"); } } if (cwiid_close(wiimote)) { fprintf(stderr, "Error on wiimote disconnect\n"); return -1; } return 0; }
int main(int argc, char *argv[]) { cwiid_wiimote_t *wiimote; /* wiimote handle */ struct cwiid_state state; /* wiimote state */ bdaddr_t bdaddr; /* bluetooth device address */ unsigned char mesg = 0; unsigned char led_state = 0; unsigned char rpt_mode = 0; unsigned char rumble = 0; int exit = 0; deviceState.numButtons = NUM_BUTTONS; deviceState.numAxis = NUM_AXIS; deviceState.numSensors = NUM_SENSORS; deviceState.axisValues = NULL; deviceState.buttonValues = NULL; deviceState.sensorValues = new SensorData[NUM_SENSORS]; cwiid_set_err(err); if (argc < 2) { fprintf(stderr, "Usage: %s HOSTNAME\n\n", argv[0]); return 1; } // if // TODO: parse data from config file!!! /* Connect to address given on command-line, if present */ if (argc > 2) { str2ba(argv[2], &bdaddr); } else { bdaddr = *BDADDR_ANY; } /* Connect to the wiimote */ printf("Put Wiimote in discoverable mode now (press 1+2)...\n"); if (!(wiimote = cwiid_open(&bdaddr, 0))) { fprintf(stderr, "Unable to connect to wiimote\n"); return -1; } if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) { fprintf(stderr, "Unable to set message callback\n"); } toggle_bit(led_state, CWIID_LED1_ON); set_led_state(wiimote, led_state); if (cwiid_get_state(wiimote, &state)) { fprintf(stderr, "Error getting state\n"); } print_state(&state); toggle_bit(rpt_mode, CWIID_RPT_IR); toggle_bit(rpt_mode, CWIID_RPT_BTN); // toggle_bit(rpt_mode, CWIID_RPT_ACC); set_rpt_mode(wiimote, rpt_mode); if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) { fprintf(stderr, "Error enabling messages\n"); } else { mesg = 1; } if (initNetwork(argv[1], CLIENTPORT)) { fprintf(stderr, "Error at network initialization\n"); return -1; } // if bool running = true; while (running) {sleep(1);} if (cwiid_close(wiimote)) { fprintf(stderr, "Error on wiimote disconnect\n"); return -1; } return 0; }