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;
}
