void main(int argc, char**argv){
if(argc != 2){
fprintf(stderr,"Error: usage: ./calibrate <file_name>\n");
exit(EXIT_FAILURE);
}
// Init
printf("init\n");
init_gui();
init_motors();
init_sensor();
printf("+---+---+\n");
printf("| 2 | 3 |\n");
printf("+---+---+\n");
printf("| 0 | 1 |\n");
printf("+---+---+\n");
printf(" ^ \n");
printf(" | \n");
printf("Init compass\n");
init_compass();
printf("Calibrate compass\n");
calibrate_location(argv[1]);
destroy_sensors();
destroy_motors();
}
void update_simbody_kinematics(SimbodyBodiesStruct* simbodyBodiesStruct, MBSdataStruct *MBSdata)
{
int i, j;
MBSsensorStruct CoM_sensor;
allocate_sensor(&CoM_sensor,NB_JOINTS);
init_sensor(&CoM_sensor,NB_JOINTS);
for(i=0; i<simbodyBodiesStruct->nb_contact_bodies; i++)
{
sensor(&CoM_sensor, MBSdata, simbodyBodiesStruct->S_sensor_Robotran_index[i]);
for(j=0; j<3; j++)
{
simbodyBodiesStruct->abs_pos_CoM[i][j] = CoM_sensor.P[j+1];
simbodyBodiesStruct->lin_vel_CoM[i][j] = CoM_sensor.V[j+1];
simbodyBodiesStruct->ang_vel_CoM[i][j] = CoM_sensor.OM[j+1];
}
simbodyBodiesStruct->rot_matrix[i][0] = CoM_sensor.R[1][1];
simbodyBodiesStruct->rot_matrix[i][1] = CoM_sensor.R[1][2];
simbodyBodiesStruct->rot_matrix[i][2] = CoM_sensor.R[1][3];
simbodyBodiesStruct->rot_matrix[i][3] = CoM_sensor.R[2][1];
simbodyBodiesStruct->rot_matrix[i][4] = CoM_sensor.R[2][2];
simbodyBodiesStruct->rot_matrix[i][5] = CoM_sensor.R[2][3];
simbodyBodiesStruct->rot_matrix[i][6] = CoM_sensor.R[3][1];
simbodyBodiesStruct->rot_matrix[i][7] = CoM_sensor.R[3][2];
simbodyBodiesStruct->rot_matrix[i][8] = CoM_sensor.R[3][3];
}
free_sensor(&CoM_sensor);
}
void r3d2_init(void)
{
NOTICE(0,"Initializing timer");
// initialise timer used to get ticks of sensor detection and motor rotation
timer_init();
NOTICE(0,"timer1 starting");
timer1_start();
// initialise port to power on and get signal of the sensor
NOTICE(0,"Initializing sensor");
init_sensor();
// power on the motor that drive the mirror
NOTICE(0,"Initializing motor");
motor_init();
// first there is no robot detected, neither motor error
robot_detected_value = robot_not_detected;
motor_error_value = no_motor_error;
robot_detected_timout_treshold = eeprom_read_byte(&eep_robot_detected_timout_treshold);
surface_reflection_ratio = eeprom_read_float((float*)&eep_surface_reflection_ratio);
robot_detected_angle_offset = eeprom_read_float((float*)&eep_robot_detected_angle_offset);
motor_rotating_timout_treshold = eeprom_read_byte(&eep_motor_rotating_timout_treshold);
motor_rotating_timout_value = motor_rotating_timout_treshold;
robot_detected_timout_value = 0;
}
static void rna_Sensor_type_set(struct PointerRNA *ptr, int value)
{
bSensor *sens = (bSensor *)ptr->data;
if (value != sens->type) {
sens->type = value;
init_sensor(sens);
}
}
void prvHardwareSetup(void) {
// set GPIO direction
init_gpio();
// create mutex and semaphore
init_locks();
// enable interrupt for sensors
init_sensor();
}
void Sensor::init(){
init_sensor();
if(pthread_create( &tCollector, NULL, &collector_task, (void *)(this)) != 0) {
perror("Error creating collector thread for sensor! ");
}
if(pthread_create( &tAnalysis, NULL, &analysis_task, (void *)(this)) != 0) {
perror("Error creating analysis thread for sensor! ");
}
}
void prvHardwareSetup(void) {
// init shard_buf
volatile unsigned long *addr = IO_TYPE;
*addr = EMPTY_REQ;
// set GPIO direction
init_gpio();
// create mutex and semaphore
init_locks();
// enable interrupt for sensors
init_sensor();
}
int main(void)
{
// engine ctrl led and reset indicator
DDRC |= (1 << DDC1);
PORTC |= (1 << PC1);
// inicator for: received package is ok
DDRC |= (1 << PC4);
PORTC &= ~(1 << PC4);
/* wait 400ms to give the rf12's POR
* (Power-On Reset) time to
* initialize the registers etc..
* (initializing wouldnt work without)
*
* .. this is NOT documented in the datasheet :\
*
* notice:
* the producer did the same in the example code
* but let it uncommented
*
*/
_delay_ms(400);
rfxx_init();
// init rf12 as receiver
rf12_init(0);
init_sensor();
init_control();
// enable external interrupt 2
GICR = (1 << INT2);
// Interrupt PIN is Input
RFXX_nIRQ_PORT &= ~(1 << RFXX_nIRQ);
// enable interrupts (global)
sei();
// enable receiver's FIFO
rfxx_wrt_cmd(0xCA83);
// init finished
PORTC &= ~(1 << PC1);
while (1);
}
bSensor *new_sensor(int type)
{
bSensor *sens;
sens= MEM_callocN(sizeof(bSensor), "Sensor");
sens->type= type;
sens->flag= SENS_SHOW;
init_sensor(sens);
strcpy(sens->name, "sensor");
// XXX make_unique_prop_names(sens->name);
return sens;
}
void init(int use_http, char *address, int port){
// Setup periferal and library
init_gui();
init_motors();
init_sensor();
init_compass();
calibrate_compass(0); // 0 = do not execute a new calibration of the compass
// Load ball predefined positions
get_positions(POINTS_FILE_NAME, &positions, &n_points);
// Start bluetooth
if(use_http)
bluetooth_test_init(address, port);
else
bluetooth_init();
//bluetooth_register_and_start(fAction_t, fAck_t, fLead_t, fStart_t, fStop_t, fWait_t, fKick_t, fCancel_t);
bluetooth_register_and_start(&follower_action_cb, NULL, &lead_cb, &start_cb, &stop_cb, NULL, NULL, &cancel_cb);
// Reset global variables
send_action_flag = 1;
act = 0;
end_game = 0;
start_game = 0;
wait = 0;
cancel = 0;
// Waiting for the game start
set_light(LIT_LEFT, LIT_GREEN);
set_light(LIT_RIGHT, LIT_GREEN);
while(!start_game); //start game, robot rank and snake size initialized by start_cb
// Initial position
home.x = BORDER_X_MAX/2;
home.y = (snake_size - robot_rank -1)*40 + 20;
center.x = BORDER_X_MAX/4;
center.y = BORDER_Y_MAX/2;
robot = home;
printf("[DEBUG] Starting position X: %d, Y: %d, T: %d\n", robot.x , robot.y, robot.t);
update_sensor(SENSOR_GYRO);
gyro_init_val = get_sensor_value(SENSOR_GYRO);
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
}
int main(void){
twi_master_init();
init_sensor();
ble_stack_init();
timers_init();
gpio_init();
gap_params_init();
services_init();
advertising_init();
conn_params_init();
sec_params_init();
advertising_start();
while(true){
power_manage();
}
}
int main(int argc,char** argv){
int mot_value[] = {200, 400, 600, 1000, 1200};
int us_value[NUM_TESTS];
int i;
// Init
init_gui();
init_motors();
init_sensor();
// US setup
set_motors_speed(NORMAL_SPEED);
printf("mot\tus\n");
for(i=0; i<NUM_TESTS; i++){
us_value[i] = do_test(mot_value[i]);
}
return 0;
}
int main (void)
{
//Initialize UART with 8-bit character sizes,no parity,1 stop bit
uart_init();
sei();
init_motor(); //This should be called first before using any motor command macro
init_sensor();
TCNT0 = 0x63; // for speed control
TCCR0 = 0x05;
X=1;
Y=1;
curHead = East;
while(1) {
trans('1');
_delay_ms(1);
trans('2');
_delay_ms(1);
trans('3');
_delay_ms(1);
fwd();
//move(0x22);
//moveStraight();
//turnRight();
//stop();
//while(1);
//moveStraight();
// Now ISR will initiate the actions
}
}
/*
update the state of the sensor
*/
void AP_RangeFinder_BLPing::update(void)
{
if (uart == nullptr) {
return;
}
const uint32_t now = AP_HAL::millis();
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
state.last_reading_ms = now;
update_status();
} else if (now - state.last_reading_ms > BLPING_TIMEOUT_MS) {
set_status(RangeFinder::RangeFinder_NoData);
// initialise sensor if no distances recently
if (now - last_init_ms > BLPING_INIT_RATE_MS) {
last_init_ms = now;
init_sensor();
}
}
}
int main(void)
{
sensor_t sensor;
core_s_t *core = NULL;
init_sensor(&sensor);
update_sensor(&sensor);
core = max_temp_core(&sensor.cpu_list, core);
if (core)
printf("label = %s, input = %d\n", core->label, core->input);
while (1) {
printf("====================\n");
update_sensor(&sensor);
core = max_temp_core(&sensor.cpu_list, core);
if (core)
printf("label = %s, input = %d\n", core->label, core->input);
sleep(1);
}
free_sensor(&sensor);
}
int main(void) {
unsigned short handle = -1;
int reset = 0;
if (!avr_read_eeprom()) {
PERR_STR("Could not read stored eeprom values");
}
uart_init();
init_sensor();
led_init();
sei();
while (1) {
while (!avr_init_dev(&dev)) {
bt_dev_pack_reset(&dev);
bt_dev_flush_hci(&dev);
}
avr_main_loop(&dev);
}
return 0;
}
int main() {
char i = 0x00;
ADCTRIS |= ADCPIN;
i = 0;
SNSTRIS |= LVLONE|LVLTWO;
LEDTRIS &= ~(POWLED|ALMLED);
ALMTRIS &= ~(LGTOUT|ALMOUT);
init_sensor(&levelSensors[0]);
init_sensor(&levelSensors[1]);
timer0_init();
timer1_init();
timer2_init();
adc_init_CH0();
LEDPORT |= POWLED;
ALMPORT &= ~(ALMOUT|LGTOUT);
timer0_start(); //Blinking Timer
timer2_start(); //Start Timer Counting TODO: Shutoff if Both Sensors dont use
while(1)
{
levelSensors[0].sensorRead = (SNSPORT & LVLONE);
levelSensors[1].sensorRead = (SNSPORT & LVLTWO);
for(i = 0; i < 2;i++)
{
checkTankStatus(&levelSensors[i]);
checkSensorState(&levelSensors[i]);
checkAlarmState(&levelSensors[i], &theAlarm);
}
// checkTankStatus(&levelSensors[0]);
// checkTankStatus(&levelSensors[1]);
//
// checkSensorState(&levelSensors[0]);
// checkSensorState(&levelSensors[1]);
//
// checkAlarmState(&levelSensors[0], &theAlarm);
// checkAlarmState(&levelSensors[1], &theAlarm);
blinkLed(&(levelSensors[0].LEVEL_STATE), &(levelSensors[1].LEVEL_STATE), &(theAlarm.ALARM_STATE), &blinkState);
}
//NOT A GOOD SIGNAL!! ERROR!
while(1)
{
LEDPORT^=POWLED;
for(int i=0;i<10000;i++);
}
return (EXIT_SUCCESS);
}
ENTRYPOINT void
draw_sonar (ModeInfo *mi)
{
sonar_configuration *sp = &sps[MI_SCREEN(mi)];
Display *dpy = MI_DISPLAY(mi);
Window window = MI_WINDOW(mi);
if (!sp->glx_context)
return;
glXMakeCurrent(MI_DISPLAY(mi), MI_WINDOW(mi), *(sp->glx_context));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix ();
{ GLfloat s = 7; glScalef (s,s,s); }
gltrackball_rotate (sp->trackball);
if (wobble_p)
{
double x, y, z;
double max = 40;
get_position (sp->rot, &x, &y, &z, !sp->button_down_p);
glRotatef (max/2 - x*max, 1, 0, 0);
glRotatef (max/2 - z*max, 0, 1, 0);
}
mi->polygon_count = 0;
glPushMatrix(); /* table */
glCallList (sp->table_list);
mi->polygon_count += sp->table_polys;
glPopMatrix();
glPushMatrix(); /* text */
glTranslatef (0, 0, -0.01);
mi->polygon_count += draw_bogies (mi);
glPopMatrix();
glCallList (sp->screen_list); /* glass */
mi->polygon_count += sp->screen_polys;
glTranslatef (0, 0, 0.004); /* sweep */
glPushMatrix();
glRotatef ((sp->sweep_th * 180 / M_PI), 0, 0, 1);
if (sp->sweep_th >= 0)
glCallList (sp->sweep_list);
mi->polygon_count += sp->sweep_polys;
glPopMatrix();
glTranslatef (0, 0, 0.004); /* grid */
glCallList (sp->grid_list);
mi->polygon_count += sp->screen_polys;
if (! sp->ssd || sp->error)
draw_startup_blurb(mi);
glPopMatrix ();
if (mi->fps_p) do_fps (mi);
glFinish();
glXSwapBuffers(dpy, window);
if (! sp->ssd)
/* Just starting up. "Resolving hosts" text printed. Go stall. */
init_sensor (mi);
else
sweep (sp);
}
int main(void)
{
//*******************************Timer setup**********************************
WDTCTL = WDTPW + WDTHOLD; // Stop Watchdog Timer
P1SEL = 0;
P2SEL = 0;
P1IE = 0;
P1IFG = 0;
P2IFG = 0;
DRIVE_ALL_PINS // set pin directions correctly and outputs to low.
// Check power on bootup, decide to receive or sleep.
if(!is_power_good())
sleep();
RECEIVE_CLOCK;
#if DEBUG_PINS_ENABLED
#if USE_2618
DEBUG_PIN5_LOW;
#endif
#endif
#if ENABLE_SLOTS
// setup int epc
epc = ackReply[2]<<8;
epc |= ackReply[3];
// calculate RN16_1 table
for (Q = 0; Q < 16; Q++)
{
rn16 = epc^Q;
lfsr();
if (Q > 8)
{
RN16[(Q<<1)-9] = __swap_bytes(rn16);
RN16[(Q<<1)-8] = rn16;
}
else
{
RN16[Q] = rn16;
}
}
#endif
TACTL = 0;
asm("MOV #0000h, R9");
// dest = destorig;
#if READ_SENSOR
init_sensor();
#endif
#if !(ENABLE_SLOTS)
queryReplyCRC = crc16_ccitt(&queryReply[0],2);
queryReply[3] = (unsigned char)queryReplyCRC;
queryReply[2] = (unsigned char)__swap_bytes(queryReplyCRC);
#endif
#if SENSOR_DATA_IN_ID
// this branch is for sensor data in the id
ackReply[2] = SENSOR_DATA_TYPE_ID;
state = STATE_READ_SENSOR;
timeToSample++;
#else
ackReplyCRC = crc16_ccitt(&ackReply[0], 14);
ackReply[15] = (unsigned char)ackReplyCRC;
ackReply[14] = (unsigned char)__swap_bytes(ackReplyCRC);
#endif
#if ENABLE_SESSIONS
initialize_sessions();
#endif
state = STATE_READY;
setup_to_receive();
while (1)
{
// TIMEOUT! reset timer
if (TAR > 0x256 || delimiterNotFound) // was 0x1000
{
if(!is_power_good()) {
sleep();
}
#if SENSOR_DATA_IN_ID
// this branch is for sensor data in the id
if ( timeToSample++ == 10 ) {
state = STATE_READ_SENSOR;
timeToSample = 0;
}
#elif SENSOR_DATA_IN_READ_COMMAND
if ( timeToSample++ == 10 ) {
state = STATE_READ_SENSOR;
timeToSample = 0;
}
#else
#if !(ENABLE_READS)
if(!is_power_good())
sleep();
#endif
inInventoryRound = 0;
state = STATE_READY;
#endif
#if ENABLE_SESSIONS
handle_session_timeout();
#endif
#if ENABLE_SLOTS
if (shift < 4)
shift += 1;
else
shift = 0;
#endif
setup_to_receive();
}
switch (state)
{
case STATE_READY:
{
inInventoryRound = 0;
//////////////////////////////////////////////////////////////////////
// process the QUERY command
//////////////////////////////////////////////////////////////////////
if ( bits == NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
{
handle_query(STATE_REPLY);
setup_to_receive();
}
//////////////////////////////////////////////////////////////////////
// process the SELECT command
//////////////////////////////////////////////////////////////////////
// @ short distance has slight impact on performance
else if ( bits >= 44 && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
{
handle_select(STATE_READY);
delimiterNotFound = 1;
} // select command
//////////////////////////////////////////////////////////////////////
// got >= 22 bits, and it's not the beginning of a select. just reset.
//////////////////////////////////////////////////////////////////////
else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) )
{
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
}
break;
}
case STATE_ARBITRATE:
{
//////////////////////////////////////////////////////////////////////
// process the QUERY command
//////////////////////////////////////////////////////////////////////
if ( bits == NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
{
handle_query(STATE_REPLY);
setup_to_receive();
}
//////////////////////////////////////////////////////////////////////
// got >= 22 bits, and it's not the beginning of a select. just reset.
//////////////////////////////////////////////////////////////////////
//else if ( bits >= NUM_QUERY_BITS )
else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) )
{
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
}
// this state handles query, queryrep, queryadjust, and select commands.
//////////////////////////////////////////////////////////////////////
// process the QUERYREP command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
{
handle_queryrep(STATE_REPLY);
delimiterNotFound = 1;
} // queryrep command
//////////////////////////////////////////////////////////////////////
// process the QUERYADJUST command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYADJ_BITS && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
{
handle_queryadjust(STATE_REPLY);
setup_to_receive();
} // queryadjust command
//////////////////////////////////////////////////////////////////////
// process the SELECT command
//////////////////////////////////////////////////////////////////////
// @ short distance has slight impact on performance
else if ( bits >= 44 && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
{
handle_select(STATE_READY);
delimiterNotFound = 1;
} // select command
break;
}
case STATE_REPLY:
{
// this state handles query, query adjust, ack, and select commands
///////////////////////////////////////////////////////////////////////
// process the ACK command
///////////////////////////////////////////////////////////////////////
if ( bits == NUM_ACK_BITS && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
{
#if ENABLE_READS
handle_ack(STATE_ACKNOWLEDGED);
setup_to_receive();
#elif SENSOR_DATA_IN_ID
handle_ack(STATE_ACKNOWLEDGED);
delimiterNotFound = 1; // reset
#else
// this branch for hardcoded query/acks
handle_ack(STATE_ACKNOWLEDGED);
//delimiterNotFound = 1; // reset
setup_to_receive();
#endif
}
//////////////////////////////////////////////////////////////////////
// process the QUERY command
//////////////////////////////////////////////////////////////////////
if ( bits == NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
{
// i'm supposed to stay in state_reply when I get this, but if I'm
// running close to 1.8v then I really need to reset and get in the
// sleep, which puts me back into state_arbitrate. this is complete
// a violation of the protocol, but it sure does make everything
// work better. - polly 8/9/2008
handle_query(STATE_REPLY);
setup_to_receive();
}
//////////////////////////////////////////////////////////////////////
// process the QUERYREP command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
{
do_nothing();
state = STATE_ARBITRATE;
setup_to_receive();
} // queryrep command
//////////////////////////////////////////////////////////////////////
// process the QUERYADJUST command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYADJ_BITS && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
{
handle_queryadjust(STATE_REPLY);
delimiterNotFound = 1;
} // queryadjust command
//////////////////////////////////////////////////////////////////////
// process the SELECT command
//////////////////////////////////////////////////////////////////////
else if ( bits >= 44 && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
{
handle_select(STATE_READY);
delimiterNotFound = 1;
} // select command
else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) &&
( ( cmd[0] & 0xF0 ) != 0x80 ) )
{
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
}
break;
}
case STATE_ACKNOWLEDGED:
{
// responds to query, ack, request_rn cmds
// takes action on queryrep, queryadjust, and select cmds
/////////////////////////////////////////////////////////////////////
// process the REQUEST_RN command
//////////////////////////////////////////////////////////////////////
if ( bits >= NUM_REQRN_BITS && ( cmd[0] == 0xC1 ) )
{
#if 1
handle_request_rn(STATE_OPEN);
setup_to_receive();
#else
handle_request_rn(STATE_READY);
delimiterNotFound = 1;
#endif
}
#if 1
//////////////////////////////////////////////////////////////////////
// process the QUERY command
//////////////////////////////////////////////////////////////////////
if ( bits == NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
{
handle_query(STATE_REPLY);
delimiterNotFound = 1;
}
///////////////////////////////////////////////////////////////////////
// process the ACK command
///////////////////////////////////////////////////////////////////////
// this code doesn't seem to get exercised in the real world. if i ever
// ran into a reader that generated an ack in an acknowledged state,
// this code might need some work.
//else if ( bits == 20 && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
else if ( bits == NUM_ACK_BITS && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
{
handle_ack(STATE_ACKNOWLEDGED);
setup_to_receive();
}
//////////////////////////////////////////////////////////////////////
// process the QUERYREP command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
{
// in the acknowledged state, rfid chips don't respond to queryrep
// commands
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
} // queryrep command
//////////////////////////////////////////////////////////////////////
// process the QUERYADJUST command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYADJ_BITS && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
{
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
} // queryadjust command
//////////////////////////////////////////////////////////////////////
// process the SELECT command
//////////////////////////////////////////////////////////////////////
else if ( bits >= 44 && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
{
handle_select(STATE_READY);
delimiterNotFound = 1;
} // select command
//////////////////////////////////////////////////////////////////////
// process the NAK command
//////////////////////////////////////////////////////////////////////
else if ( bits >= 10 && ( cmd[0] == 0xC0 ) )
{
do_nothing();
state = STATE_ARBITRATE;
delimiterNotFound = 1;
}
//////////////////////////////////////////////////////////////////////
// process the READ command
//////////////////////////////////////////////////////////////////////
// warning: won't work for read addrs > 127d
if ( bits == NUM_READ_BITS && ( cmd[0] == 0xC2 ) )
{
handle_read(STATE_ARBITRATE);
state = STATE_ARBITRATE;
delimiterNotFound = 1 ;
}
// FIXME: need write, kill, lock, blockwrite, blockerase
//////////////////////////////////////////////////////////////////////
// process the ACCESS command
//////////////////////////////////////////////////////////////////////
if ( bits >= 56 && ( cmd[0] == 0xC6 ) )
{
do_nothing();
state = STATE_ARBITRATE;
delimiterNotFound = 1 ;
}
#endif
else if ( bits >= MAX_NUM_READ_BITS )
{
state = STATE_ARBITRATE;
delimiterNotFound = 1 ;
}
#if 0
// kills performance ...
else if ( bits >= 44 )
{
do_nothing();
state = STATE_ARBITRATE;
delimiterNotFound = 1;
}
#endif
break;
}
case STATE_OPEN:
{
// responds to query, ack, req_rn, read, write, kill, access,
// blockwrite, and blockerase cmds
// processes queryrep, queryadjust, select cmds
//////////////////////////////////////////////////////////////////////
// process the READ command
//////////////////////////////////////////////////////////////////////
// warning: won't work for read addrs > 127d
if ( bits == NUM_READ_BITS && ( cmd[0] == 0xC2 ) )
{
handle_read(STATE_OPEN);
// note: setup_to_receive() et al handled in handle_read
}
//////////////////////////////////////////////////////////////////////
// process the REQUEST_RN command
//////////////////////////////////////////////////////////////////////
else if ( bits >= NUM_REQRN_BITS && ( cmd[0] == 0xC1 ) )
{
handle_request_rn(STATE_OPEN);
setup_to_receive();
}
//////////////////////////////////////////////////////////////////////
// process the QUERY command
//////////////////////////////////////////////////////////////////////
if ( bits == NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
{
handle_query(STATE_REPLY);
delimiterNotFound = 1;
}
//////////////////////////////////////////////////////////////////////
// process the QUERYREP command
//////////////////////////////////////////////////////////////////////
else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
{
do_nothing();
state = STATE_READY;
setup_to_receive();
} // queryrep command
//////////////////////////////////////////////////////////////////////
// process the QUERYADJUST command
//////////////////////////////////////////////////////////////////////
else if ( bits == 9 && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
{
do_nothing();
state = STATE_READY;
delimiterNotFound = 1;
} // queryadjust command
///////////////////////////////////////////////////////////////////////
// process the ACK command
///////////////////////////////////////////////////////////////////////
else if ( bits == NUM_ACK_BITS && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
{
handle_ack(STATE_OPEN);
delimiterNotFound = 1;
}
//////////////////////////////////////////////////////////////////////
// process the SELECT command
//////////////////////////////////////////////////////////////////////
else if ( bits >= 44 && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
{
handle_select(STATE_READY);
delimiterNotFound = 1;
} // select command
//////////////////////////////////////////////////////////////////////
// process the NAK command
//////////////////////////////////////////////////////////////////////
else if ( bits >= 10 && ( cmd[0] == 0xC0 ) )
{
handle_nak(STATE_ARBITRATE);
delimiterNotFound = 1;
}
break;
}
case STATE_READ_SENSOR:
{
#if SENSOR_DATA_IN_READ_COMMAND
read_sensor(&readReply[0]);
// crc is computed in the read state
RECEIVE_CLOCK;
state = STATE_READY;
delimiterNotFound = 1; // reset
#elif SENSOR_DATA_IN_ID
read_sensor(&ackReply[3]);
RECEIVE_CLOCK;
ackReplyCRC = crc16_ccitt(&ackReply[0], 14);
ackReply[15] = (unsigned char)ackReplyCRC;
ackReply[14] = (unsigned char)__swap_bytes(ackReplyCRC);
state = STATE_READY;
delimiterNotFound = 1; // reset
#endif
break;
} // end case
} // end switch
} // while loop
}
void controller_inputs(MBSdataStruct *MBSdata)
{
UserIOStruct *uvs;
ControllerStruct *cvs;
ControllerInputsStruct *ivs;
MBSsensorStruct S_MidWaist;
int i;
double R_11, R_21;
int robotran_id;
int robotran_id_table[] = {
WAIST_YAW, // 1. TORSO_YAW
WAIST_SAG, // 2. TORSO_PITCH
WAIST_LAT, // 3. TORSO_ROLL
R_HIP_SAG, // 4. RIGHT_HIP_PITCH
L_HIP_SAG, // 5. LEFT_HIP_PITCH
R_HIP_LAT, // 6. RIGHT_HIP_ROLL
R_HIP_YAW, // 7. RIGHT_HIP_YAW
R_KNEE_SAG, // 8. RIGHT_KNEE_PITCH
R_ANK_SAG, // 9. RIGHT_FOOT_PITCH
R_ANK_LAT, // 10. RIGHT_FOOT_ROLL
L_HIP_LAT, // 11. LEFT_HIP_ROLL
L_HIP_YAW, // 12. LEFT_HIP_YAW
L_KNEE_SAG, // 13. LEFT_KNEE_PITCH
L_ANK_SAG, // 14. LEFT_FOOT_PITCH
L_ANK_LAT, // 15. LEFT_FOOT_ROLL
R_SH_SAG, // 16. RIGHT_SHOULDER_PITCH
R_SH_LAT, // 17. RIGHT_SHOULDER_ROLL
R_SH_YAW, // 18. RIGHT_SHOULDER_YAW
R_ELB, // 19. RIGHT_ELBOW_PITCH
L_SH_SAG, // 20. LEFT_SHOULDER_PITCH
L_SH_LAT, // 21. LEFT_SHOULDER_ROLL
L_SH_YAW, // 22. LEFT_SHOULDER_YAW
L_ELB, // 23. LEFT_ELBOW_PITCH
};
uvs = MBSdata->user_IO;
cvs = uvs->cvs;
ivs = cvs->Inputs;
// -- Time -- //
ivs->t = MBSdata->tsim; // time [s]
// ---- Forces under the feet ---- //
// Vertical forces [N]
for (i=0; i<3; i++)
{
ivs->F_Rfoot[i] = uvs->GRF_r[i+1];
ivs->F_Lfoot[i] = uvs->GRF_l[i+1];
ivs->T_Rfoot[i] = uvs->GRM_r[i+1];
ivs->T_Lfoot[i] = uvs->GRM_l[i+1];
#ifdef COMP_COMAN
ivs->F_Rfoot[i] += uvs->GRF_r_dist[i+1];
ivs->F_Lfoot[i] += uvs->GRF_l_dist[i+1];
ivs->T_Rfoot[i] += uvs->GRM_r_dist[i+1];
ivs->T_Lfoot[i] += uvs->GRM_l_dist[i+1];
#endif
}
// -- Joint positions, velocities and torques -- //
for(i=0; i<COMAN_NB_JOINT_ACTUATED; i++)
{
robotran_id = robotran_id_table[i];
ivs->q[i] = MBSdata->q[robotran_id]; // position [rad]
ivs->qd[i] = MBSdata->qd[robotran_id]; // velocity [rad/s]
ivs->Qq[i] = MBSdata->Qq[robotran_id]; // torque [Nm]
}
// -- IMU -- //
// allocation
allocate_sensor(&S_MidWaist,COMAN_NB_JOINT_TOTAL);
init_sensor(&S_MidWaist,COMAN_NB_JOINT_TOTAL);
sensor(&S_MidWaist, MBSdata, S_MIDWAIST); // IMU located in the MidWaist body
ivs->IMU_Orientation[0] = S_MidWaist.R[1][1];
ivs->IMU_Orientation[1] = S_MidWaist.R[1][2];
ivs->IMU_Orientation[2] = S_MidWaist.R[1][3];
ivs->IMU_Orientation[3] = S_MidWaist.R[2][1];
ivs->IMU_Orientation[4] = S_MidWaist.R[2][2];
ivs->IMU_Orientation[5] = S_MidWaist.R[2][3];
ivs->IMU_Orientation[6] = S_MidWaist.R[3][1];
ivs->IMU_Orientation[7] = S_MidWaist.R[3][2];
ivs->IMU_Orientation[8] = S_MidWaist.R[3][3];
ivs->IMU_Angular_Rate[0] = S_MidWaist.OM[1];
ivs->IMU_Angular_Rate[1] = S_MidWaist.OM[2];
ivs->IMU_Angular_Rate[2] = S_MidWaist.OM[3];
free_sensor(&S_MidWaist);
// -- IMU not available on the real CoMan -- //
// rotation matrix
R_11 = ivs->IMU_Orientation[0];
R_21 = ivs->IMU_Orientation[3];
// absolute orientation [rad]
uvs->real_theta_3_waist = atan2(-R_21,R_11);
// derivative of absolute orientation [rad/s]
uvs->real_omega_3_waist = ivs->IMU_Angular_Rate[2];
}
