struct args *new_args(struct expr *e)
{
struct args *aa = malloc(sizeof(struct args));
INIT_ARRAY(aa->v, 4);
ARRAY_APPEND(aa->v, e);
return aa;
}
struct stmts *new_stmts(struct stmt *s)
{
struct stmts *ss = malloc(sizeof(struct stmts));
INIT_ARRAY(ss->v, 4);
ARRAY_APPEND(ss->v, s);
return ss;
}
// ==================== //
// コンストラクタと代入 //
// ==================== //
// コンストラクタ。
Cache::Cache() :
enable_quiesce_search_(false),
enable_repetition_check_(false),
enable_check_extension_(false),
ybwc_limit_depth_(0),
ybwc_invalid_moves_(0),
enable_aspiration_windows_(false),
aspiration_windows_limit_depth_(0),
aspiration_windows_delta_(0),
enable_see_(false),
enable_history_(false),
enable_killer_(false),
enable_ttable_(false),
enable_iid_(false),
iid_limit_depth_(0),
iid_search_depth_(0),
enable_nmr_(false),
nmr_limit_depth_(0),
nmr_search_reduction_(0),
nmr_reduction_(0),
enable_probcut_(false),
probcut_limit_depth_(0),
probcut_margin_(0),
probcut_search_reduction_(0),
enable_history_pruning_(false),
history_pruning_limit_depth_(0),
history_pruning_threshold_(0),
history_pruning_reduction_(0),
enable_lmr_(false),
lmr_limit_depth_(0),
lmr_search_reduction_(0),
enable_futility_pruning_(false),
futility_pruning_depth_(0),
max_nodes_(0),
max_depth_(0),
thinking_time_(0) {
INIT_ARRAY(material_);
INIT_ARRAY(history_pruning_invalid_moves_);
INIT_ARRAY(lmr_invalid_moves_);
INIT_ARRAY(futility_pruning_margin_);
INIT_ARRAY(piece_hash_value_table_);
INIT_ARRAY(to_move_hash_value_table_);
INIT_ARRAY(castling_hash_value_table_);
INIT_ARRAY(en_passant_hash_value_table_);
INIT_ARRAY(eval_cache_);
}
int initSolver()
{
int i, j, _tmp;
/* initialise global counters */
current_node_stamp = 1;
lookDead = 0;
mainDead = 0;
#ifdef COUNT_SAT
count_sat = 0;
#endif
solution_bin = 0;
solution_bits = 63;
#ifdef DISTRIBUTION
first_time = 0;
skip_flag = 0;
first_depth = 20;
#endif
currentNodeNumber = 1;
UNSATflag = 0;
/* allocate recursion stack */
/* tree is max. nrofvars deep and we thus have max. nrofvars STACK_BLOCKS
-> 2 * nrofvars should be enough for everyone :)
*/
INIT_ARRAY( r , 3 * nrofvars + 1 );
INIT_ARRAY( imp , INITIAL_ARRAY_SIZE );
INIT_ARRAY( subsume , INITIAL_ARRAY_SIZE );
INIT_ARRAY( bieq , INITIAL_ARRAY_SIZE );
INIT_ARRAY( newbi , INITIAL_ARRAY_SIZE );
INIT_ARRAY( sub , INITIAL_ARRAY_SIZE );
MALLOC_OFFSET( bImp_satisfied, int, nrofvars, 2 );
MALLOC_OFFSET( bImp_start, int, nrofvars, 2 );
MALLOC_OFFSET( bImp_stamps, int, nrofvars, 0 );
MALLOC_OFFSET( node_stamps, tstamp, nrofvars, 0 );
tmpEqImpSize = (int*) malloc( sizeof(int) * (nrofvars+1) );
init_lookahead();
init_preselection();
#ifdef DISTRIBUTION
init_direction();
#endif
tmpTernaryImpSize = (int* ) malloc( sizeof(int ) * ( 2*nrofvars+1 ) );
#ifdef TERNARYLOOK
TernaryImp = (int**) malloc( sizeof(int*) * ( 2*nrofvars+1 ) );
TernaryImpSize = (int* ) malloc( sizeof(int ) * ( 2*nrofvars+1 ) );
for( i = 0; i <= 2 * nrofvars; i++ )
{
tmpTernaryImpSize[ i ] = 0;
TernaryImpSize [ i ] = 0;
}
for( i = 0; i < nrofclauses; i++ )
if( Clength[ i ] == 3 )
for( j = 0; j < 3; j++ )
TernaryImpSize[ Cv[ i ][ j ] + nrofvars ]++;
for( i = 0; i <= 2 * nrofvars; i++ )
TernaryImp[ i ] = (int*) malloc(sizeof(int)*(4*TernaryImpSize[i]+4));
tmpTernaryImpSize += nrofvars;
TernaryImp += nrofvars;
TernaryImpSize += nrofvars;
fill_ternary_implication_arrays();
for( i = -nrofvars; i <= nrofvars; i++ )
// tmpTernaryImpSize[ i ] = 2 * tmpTernaryImpSize[ i ] + 2;
tmpTernaryImpSize[ i ] = 4 * TernaryImpSize[ i ] + 2;
// branch_on_dummies_from_long_clauses();
while( AddTernaryResolvents() );
for( i = -nrofvars; i <= nrofvars; i++ )
free( TernaryImp[ i ] );
FREE_OFFSET( TernaryImp );
FREE_OFFSET( TernaryImpSize );
#else
tmpTernaryImpSize += nrofvars;
#endif
/* initialise global datastructures */
#ifdef GLOBAL_AUTARKY
MALLOC_OFFSET( TernaryImpReduction, int, nrofvars, 0 );
if( kSAT_flag )
{
int _nrofliterals = 0;
for( i = 0; i < nrofbigclauses; ++i )
_nrofliterals += clause_length[ i ];
clause_reduction = (int*) malloc( sizeof(int ) * nrofbigclauses );
clause_red_depth = (int*) malloc( sizeof(int ) * nrofbigclauses );
big_global_table = (int*) malloc( sizeof(int ) * _nrofliterals );
clause_SAT_flag = (int*) malloc( sizeof(int ) * nrofbigclauses );
MALLOC_OFFSET( big_to_binary, int*, nrofvars, NULL );
MALLOC_OFFSET( btb_size, int , nrofvars, 0 );
for( i = 0; i < nrofbigclauses; ++i )
{
clause_reduction[ i ] = 0;
clause_red_depth[ i ] = nrofvars;
clause_SAT_flag [ i ] = 0;
}
int tmp = 0;
for( i = 1; i <= nrofvars; i++ )
{
big_to_binary[ i ] = (int*) &big_global_table[ tmp ];
tmp += big_occ[ i ];
big_to_binary[ -i ] = (int*) &big_global_table[ tmp ];
tmp += big_occ[ -i ];
}
assert( tmp == _nrofliterals );
}
#endif
TernaryImp = (int**) malloc( sizeof(int*) * ( 2*nrofvars+1 ) );
TernaryImpSize = (int* ) malloc( sizeof(int ) * ( 2*nrofvars+1 ) );
TernaryImpLast = (int* ) malloc( sizeof(int ) * ( 2*nrofvars+1 ) );
TernaryImpTable = (int* ) malloc( sizeof(int ) * ( 6*nrofclauses+1 ) );
TernaryImp += nrofvars;
TernaryImpSize += nrofvars;
TernaryImpLast += nrofvars;
if( simplify_formula() == UNSAT ) return UNSAT;
for( i = -nrofvars; i <= nrofvars; i++ )
{
tmpTernaryImpSize[ i ] = 0;
TernaryImpSize [ i ] = 0;
bImp_satisfied [ i ] = 2; //waarom staat dit hier?
}
for( i = 0; i < nrofclauses; i++ )
if( Clength[ i ] == 3 )
for( j = 0; j < 3; j++ )
TernaryImpSize[ Cv[ i ][ j ] ]++;
_tmp = 0;
for( i = -nrofvars; i <= nrofvars; i++ )
{
TernaryImp[ i ] = TernaryImpTable + 2 * _tmp;
_tmp += TernaryImpSize[ i ];
TernaryImpLast[ i ] = TernaryImpSize[ i ];
}
fill_ternary_implication_arrays();
rebuild_BinaryImp();
init_freevars();
for( i = 0; i < nrofceq ; i++ )
assert( CeqSizes[ i ] != 1 );
#ifdef EQ
for( i = 0; i < nrofceq ; i++ )
if( CeqSizes[ i ] == 2 )
DPLL_propagate_binary_equivalence( i );
#endif
#ifdef DETECT_COMPONENTS
init_localbranching();
#endif
#ifdef CUT_OFF
solution_bits = CUT_OFF - 1;
#endif
push_stack_blocks();
return 1;
}
void init_lookahead()
{
#ifdef NO_TRANSLATOR
int i, longest_clause = 0; //dient goed geinit worden
for( i = 0; i < nrofclauses; i++ )
if( longest_clause < Clength[ i ] )
longest_clause = Clength[ i ];
printf("c longest clause has size %i\n", longest_clause);
size_diff = (float*) malloc(sizeof(float) * longest_clause );
size_diff[ 0 ] = 0.0;
size_diff[ 1 ] = 0.0;
for( i = 2; i < longest_clause; i++ )
size_diff[ i ] = pow(5.0, 2-i);
#endif
currentTimeStamp = 0;
lookaheadArray = (int*) malloc( sizeof( int ) * 2 * nrofvars );
#ifdef DL_STATIC
DL_trigger = DL_STATIC;
#else
DL_trigger = 0.0;
#endif
#ifdef DL_VARMULT
DL_trigger = freevars * DL_VARMULT * 0.01;
#endif
forced_literal_array = (int*) malloc( sizeof(int) * (nrofvars+1) );
treeArray = (struct treeNode*) malloc( sizeof(struct treeNode) * (2*nrofvars+1) );
MALLOC_OFFSET( EqDiff, float, nrofvars, 0 );
MALLOC_OFFSET( diff, float, nrofvars, 1 );
MALLOC_OFFSET( diff_tmp, float, nrofvars, 1 );
MALLOC_OFFSET( NBCounter, int, nrofvars, 0 );
MALLOC_OFFSET( WNBCounter, float, nrofvars, 0.0 );
MALLOC_OFFSET( failed_DL_stamp, int, nrofvars, 0 );
INIT_ARRAY( look_fix, nrofvars );
INIT_ARRAY( look_res, nrofvars );
diff_table = (float* ) malloc( sizeof(float ) * 100 * (2*nrofvars+1) );
diff_depth = (float**) malloc( sizeof(float*) * 100 );
for( i = 0; i < 100; i++ )
diff_depth[ i ] = diff_table + (i * (2*nrofvars+1) + nrofvars);
_diff = diff;
_diff_tmp = diff_tmp;
init_tree();
non_tautological_equivalences = check_non_tautological_equivalences();
#ifdef EQ
if( non_tautological_equivalences )
{
int i;
lengthWeight = (double *) malloc( sizeof( double ) * ( 100 ) );
lengthWeight[ 0 ] = 0.0;
lengthWeight[ 1 ] = 0.0;
for( i = 2; i < 100; i++ )
#ifdef QXBASE
lengthWeight[ i ] = QXCONST * 0.5 * pow( 0.6 + QXBASE*0.01, i );
#else
lengthWeight[ i ] = 0;
#endif
}
#endif
#ifdef EQ
if( non_tautological_equivalences )
{
if( kSAT_flag ) look_IUP = &look_IUP_w_eq_kSAT;
else look_IUP = &look_IUP_w_eq_3SAT;
}
else
#endif
{
if( kSAT_flag ) look_IUP = &look_IUP_wo_eq_kSAT;
else look_IUP = &look_IUP_wo_eq_3SAT;
}
#ifdef DOUBLELOOK
init_doublelook();
#endif
#ifdef LONG_LOOK
init_long_lookahead();
#endif
}
/***********************************************************************
This is the main controller loop.
sequences of operations:
- reads from CAN bus or serial port 1.
- reads encoders (or ADC).
- computes the control value (a PID in this version).
- checks limits and other errors.
- sets PWM
- does extra functions (e.g. communicate with neighboring cards).
***********************************************************************/
void main(void)
{
Int32 PWMoutput [JN];
Int32 PWMoutput_old [JN];
byte i=0;
byte wi=0;
byte k=0;
UInt16 *value=0;
Int32 t1val=0;
Int32 PID_R= 2;
Int32 kpp=1;
Int16 current_turn=0;
Int16 print_number=0;
Int16 real_pos=0;
byte first_step=0;
#if (VERSION == 0x0351)
#define winSizeMax 32
#define initialWindowSize 4
#else
#define winSizeMax 32
#define initialWindowSize 30
#endif
byte divJntPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divJntVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotPos[JN]=INIT_ARRAY(initialWindowSize-1);
byte divMotVel[JN]=INIT_ARRAY(initialWindowSize-1);
byte headJntPos[JN]=INIT_ARRAY(0); //current joint pos
byte tailJntPos[JN]=INIT_ARRAY(0);
byte headJntVel[JN]=INIT_ARRAY(0); //current joint vel
byte tailJntVel[JN]=INIT_ARRAY(0);
byte headMotPos[JN]=INIT_ARRAY(0); //current motor pos
byte tailMotPos[JN]=INIT_ARRAY(0);
byte headMotVel[JN]=INIT_ARRAY(0); //current motor vel
byte tailMotVel[JN]=INIT_ARRAY(0);
Int32 jntPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 jntVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motPosWindow[winSizeMax][JN]; //max window size = winSizeMax
Int32 motVelWindow[winSizeMax][JN]; //max window size = winSizeMax
Int16 _safeband[JN]; //it is a value for reducing the JOINT limit of 2*_safeband [tick encoder]
#ifdef TEMPERATURE_SENSOR
byte TempSensCount1 = 0;
UInt32 TempSensCount2 = 0;
byte temp_sens_status=0;
overtemp[0]=0;
overtemp[1]=0;
errortemp[0]=0;
errortemp[1]=0;
#endif
/* gets the address of flash memory from the linker */
_flash_addr = get_flash_addr();
/* enable interrupts */
setReg(SYS_CNTL, 0);
// IPL channels from 0 to 6 enabled
// external interrupts IRQA and IRQB disabled
setRegBits(IPR, 0xFE00);
// enable FAULT
__ENIGROUP (61, 3);
#if (VERSION == 0x0254)
#else
__ENIGROUP (60, 3);
#endif
// enable SCI
__ENIGROUP (52, 4);
__ENIGROUP (53, 4);
__ENIGROUP (50, 4);
__ENIGROUP (51, 4);
// enable data flash
__ENIGROUP (13, 4);
// enable CAN
__ENIGROUP (14, 6);
__ENIGROUP (15, 6);
__ENIGROUP (16, 6);
__ENIGROUP (17, 6);
// enable ADCA/ADCB
__ENIGROUP (55, 6);
__ENIGROUP (54, 6);
//enable PWM reload
__ENIGROUP (59, 7); // PMWA
#if (VERSION == 0x0254)
#else
__ENIGROUP (58, 7); // PWMB
#endif
// enable timers
// TIMER_A
__ENIGROUP (45, 7); //Timer for the encoder commutation if used
__ENIGROUP (44, 7); //
__ENIGROUP (43, 7); //
__ENIGROUP (42, 4); //TI1 1ms delay main loop
// TIMER_B
__ENIGROUP (41, 7); //
__ENIGROUP (40, 7); //
__ENIGROUP (39, 7); //
__ENIGROUP (38, 7);
// TIMER_C
__ENIGROUP (37, 1);
__ENIGROUP (36, 1);
__ENIGROUP (35, 1);
__ENIGROUP (34, 1);
// TIMER_D
__ENIGROUP (33, 7); //1ms delay duty cycle
__ENIGROUP (32, 1);
__ENIGROUP (31, 1);
__ENIGROUP (30, 1);
__EI();
flash_interface_init (JN);
readFromFlash (_flash_addr);
if (_version==_flash_version)
{
}
else
{
writeToFlash(_flash_addr);
}
__DI();
#warning "debug"// ;
__EI();
init_leds ();
#if (VERSION == 0x0254)
Init_Brushless_Comm (1,HALL);
#else
Init_Brushless_Comm (JN,HALL);
#endif
can_interface_init (JN);
init_strain ();
init_position_abs_ssi ();
#if VERSION ==0x0257
init_relative_position_abs_ssi();
#endif
init_faults (true,true,true);
init_position_encoder ();
TI1_init ();
//variable init
mainLoopOVF=0;
_count=0;
for(i=0;i<JN;i++)
{
_received_pid[i].rec_pid=0;
}
BUS_OFF=false;
#warning "debug"// ;
//__EI();
// print_version ();
/* initialization */
for (i=0; i<JN; i++) _calibrated[i] = false;
/* reset trajectory generation */
for (i=0; i<JN; i++) abort_trajectory (i, 0);
///////////////////////////////////////
// reset of the ABS_SSI
// this is needed because the AS5045 gives the first value wrong !!!
for (i=0; i<JN; i++) _position[i]=(Int32) Filter_Bit(get_position_abs_ssi(i));
for (i=0; i<JN; i++) _max_real_position[i]=Filter_Bit(4095);
//////////////////////////////////////
/* initialize speed and acceleration to zero (useful later on) */
for (i=0; i<JN; i++) _position_old[i] = 0;
for (i=0; i<JN; i++) _speed[i] = 0;
for (i=0; i<JN; i++) _accel[i] = 0;
for (i=0; i<JN; i++) _safeband[i] =5; //5 ticks => 1 grado di AEA.
for (i=0; i<JN; i++) PWMoutput [i] = PWMoutput_old[i] = 0;
/* reset the recursive windows for storage of position and velocity data */
/* (for velocity and position estimates) */
for(i=0;i<JN;i++)
{
for(wi=0;wi<winSizeMax;wi++)
{
jntPosWindow[wi][i]=_position[i];
jntVelWindow[wi][i]=0;
motPosWindow[wi][i]=0;
motVelWindow[wi][i]=0;
}
}
//set_relative_position_abs_ssi(1,get_absolute_real_position_abs_ssi(1));
/* main control loop */
for(_counter = 0;; _counter ++)
{
if (_counter >= CAN_SYNCHRO_STEPS) _counter = 0;
led3_on
while (_wait);
_count=0;
led3_off
// BUS_OFF check
if (getCanBusOffstatus() )
{
#ifdef DEBUG_CAN_MSG
can_printf("DISABLE BUS OFF");
#endif
for (i=0; i<JN; i++) put_motor_in_fault(i);
led1_off
}
else
led1_on
// READING CAN MESSAGES
can_interface();
for (i=0; i<JN; i++)
if (_pad_enabled[i]==false && _control_mode[i]!=MODE_HW_FAULT) _control_mode[i]=MODE_IDLE;
//Position calculation
// This is used to have a shift of the zero-cross out of the
// joint workspace
//
// max_real_position is the limit of the joint starting from
// 4095 and going to decrease this number without zero-cross
// untill the joint limit is reached
#if VERSION == 0x0257
_position_old[0]=_position[0];
if(get_error_abs_ssi(0)==ERR_OK)
_position[0]=Filter_Bit (get_position_abs_ssi(0));
_position_old[1]=_position[1];
if(get_error_abs_ssi(1)==ERR_OK)
_position[1]=Filter_Bit (get_position_abs_ssi(1));
#else
for (i=0; i<JN; i++)
{
_position_old[i]=_position[i];
if(get_error_abs_ssi(i)==ERR_OK)
_position[i]=Filter_Bit (get_position_abs_ssi(i));
}
#endif
// get_commutations() is used to read the incremental encoder of the motors.
// the variable _motor_position is then used to estimate the rotor speed and
// compensate the back-EMF of the motor.
for (i=0; i<JN; i++) _motor_position[i]=get_position_encoder(i);//get_commutations(i);
///////////////////////////////////////////DEBUG////////////
#if (VERSION !=0x0254)
for (i=0; i<JN; i++)
{
if (get_error_abs_ssi(i)==ERR_ABS_SSI)
{
put_motor_in_fault(i);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",i);
#endif
}
}
#endif
#if (VERSION ==0x0254)
if (get_error_abs_ssi(0)==ERR_ABS_SSI)
{
put_motor_in_fault(0);
#ifdef DEBUG_CAN_MSG
can_printf("ABS error %d",0);
#endif
}
#endif
//DO NOTHING
// decoupling the position
decouple_positions();
/* velocity and acceleration estimators */
{
for (i=0; i<JN; i++)
{
//joint velocity estimator
tailJntPos[i]=headJntPos[i]+(winSizeMax-divJntPos[i]); if(tailJntPos[i]>=winSizeMax) tailJntPos[i]=tailJntPos[i]%winSizeMax;
_speed_old[i] = _speed[i];
jntPosWindow[headJntPos[i]][i]=_position[i];
_speed[i] = (Int32) (((jntPosWindow[headJntPos[i]][i] - jntPosWindow[tailJntPos[i]][i] ))<<_jntVel_est_shift[i]);
// _speed[i] <<= _jntVel_est_shift[i];
_speed[i] = (Int32)(_speed[i]) / divJntPos[i];
headJntPos[i]=headJntPos[i]+1; if(headJntPos[i]>=winSizeMax) headJntPos[i]=0;
/*
//joint acceleration estimator
tailJntVel[i]=headJntVel[i]+(winSizeMax-divJntVel[i]); if(tailJntVel[i]>=winSizeMax) tailJntVel[i]=tailJntVel[i]%winSizeMax;
_accel_old[i] = _accel[i];
jntVelWindow[headJntVel[i]][i]=_speed[i];
_accel[i] = ((jntVelWindow[headJntVel[i]][i] - jntVelWindow[tailJntVel[i]][i] ));
_accel[i] << _jntAcc_est_shift[i];
_accel[i] = (Int32)(_accel[i]) / divJntVel[i];
headJntVel[i]=headJntVel[i]+1; if(headJntVel[i]>=winSizeMax) headJntVel[i]=0;
*/
//motor velocity estimator
tailMotPos[i]=headMotPos[i]+(winSizeMax-divMotPos[i]); if(tailMotPos[i]>=winSizeMax) tailMotPos[i]=tailMotPos[i]%winSizeMax;
_motor_speed_old[i] = _motor_speed[i];
motPosWindow[headMotPos[i]][i]=_motor_position[i];
_motor_speed[i] = ((motPosWindow[headMotPos[i]][i] - motPosWindow[tailMotPos[i]][i] ));
_motor_speed[i] <<= _motVel_est_shift[i];
_motor_speed[i] = (_motor_speed[i]) / divMotPos[i];
headMotPos[i]=headMotPos[i]+1; if(headMotPos[i]>=winSizeMax) headMotPos[i]=0;
}
}
/* in position? */
#if (VERSION != 0x0254)
for (i=0; i<JN; i++) _in_position[i] = check_in_position(i);
#else
_in_position[0] = check_in_position(0);
#endif
/* in reference configuration for calibration? */
//for (i=0; i<JN; i++) check_in_position_calib(i);
//******************************************* POSITION LIMIT CHECK ***************************/
for (i=0; i<JN; i++) check_range(i, _safeband[i], PWMoutput);
//******************************************* COMPUTES CONTROLS *****************************/
//FT sensor watchdog update
for (i=0; i<STRAIN_MAX; i++)
if (_strain_wtd[i]>0) _strain_wtd[i]--;
for (i=0; i<JN; i++)
{
//computing the PWM value (PID)
PWMoutput[i] = compute_pwm(i);
// PWM filtering in torque control if there is no bemf compensation
#if (VERSION != 0x0351)
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
if (_useFilter[i] == 3) PWMoutput[i] = lpf_ord1_3hz (PWMoutput[i], i);
}
// saving the PWM value before the decoupling
_bfc_PWMoutput[i] = PWMoutput_old[i] = PWMoutput[i];
// applying the saturation to the PWM
if (_bfc_PWMoutput[i] < -MAX_DUTY) _bfc_PWMoutput[i]=-MAX_DUTY;
else if (_bfc_PWMoutput[i] > MAX_DUTY) _bfc_PWMoutput[i]= MAX_DUTY;
#endif //(VERSION != 0x0351)
}
//decouple PWM
decouple_dutycycle(PWMoutput);
//******************************************* SATURATES CONTROLS ***************************/
/* back emf compensation + controls saturation (if necessary) */
for (i=0; i<JN; i++)
{
if (_control_mode[i] == MODE_TORQUE ||
_control_mode[i] == MODE_IMPEDANCE_POS ||
_control_mode[i] == MODE_IMPEDANCE_VEL)
{
#if (VERSION != 0x0351)
// Back emf compensation
//PWMoutput[i]+=compensate_bemf(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_bemf(i, _speed[i]); //use the joint speed
//add the coulomb friction compensation term
if (_kstp_torque[i] != 0 ||
_kstn_torque[i] != 0)
//PWMoutput[i]+=compensate_friction(i, _comm_speed[i]); //use the motor speed
PWMoutput[i]+=compensate_friction(i, _speed[i]); //use the joint speed
// Protection for joints out of the admissible range during force control
check_range_torque(i, _safeband[i], PWMoutput);
// PWM saturation
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit_torque[i] );
#else //(VERSION != 0x0351)
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
#endif //(VERSION != 0x0351)
}
else
{
ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] );
}
if (_pid[i] < -MAX_DUTY) _pid[i]=-MAX_DUTY;
else if (_pid[i] > MAX_DUTY) _pid[i]= MAX_DUTY;
}
/* generate PWM */
for (i=0; i<JN; i++)
{
if (!mode_is_idle(i)) {PWM_generate(i,_pid[i]);}
}
/* Check Current done in T1 */
/* do extra functions, communicate, etc. */
//send broadcast data
can_send_broadcast();
//send additional debug information
//can_send_broadcast_debug(1,1);
/***********************************************************************
// Check Current is made here
/***********************************************************************/
#if (VERSION != 0x0254)
for (i=0; i<JN; i++)
#else
for (i=0; i<1; i++)
#endif
{
if ((get_current(i)>=25000) || (get_current(i)<=-25000))
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d curr %f",i,get_current(i));
#endif
}
check_current(i, (_pid[i] > 0));
compute_i2t(i);
if (_filt_current[i] > MAX_I2T_CURRENT)
{
put_motor_in_fault(i);
highcurrent[i]=true;
#ifdef DEBUG_CAN_MSG
can_printf("j%d filtcurr %f",i,_filt_current[i]);
#endif
}
}
// Check for the MAIN LOOP duration
// t1val= (UInt16) TI1_getCounter();
if ( _count>0)
{
mainLoopOVF=1;
_count=0;
}
/* tells that the control cycle is completed */
_wait = true;
} /* end for(;;) */
#include "controller.h" #include "trajectory.h" #include "abs_ssi_interface.h" #include "can1.h" #include "can_interface.h" #include "pwm_interface.h" #include "encoders_interface.h" #include "calibration.h" #include "control_enable.h" #ifndef VERSION # error "No valid version specified" #endif Int16 _max_position_enc_tmp[JN] = INIT_ARRAY (0); Int32 _position_of_some_time_ago[JN] = INIT_ARRAY (0); //helper functions void helper_calib_hard_stops(byte channel, Int16 param1,Int16 param2, Int16 param3); void helper_calib_abs_digital(byte channel, Int16 param1,Int16 param2, Int16 param3); void helper_calib_hall_digital(byte channel, Int16 param1,Int16 param2, Int16 param3); void helper_calib_abs_and_incremental(byte channel, Int16 param1,Int16 param2, Int16 param3); void helper_calib_abs_digital_coupled (byte channel, Int16 param1,Int16 param2, Int16 param3); void helper_calib_eyes(byte channel, Int16 param1,Int16 param2, Int16 param3); /************************************************************ * this function checks if the calibration is terminated * and if calibration is terminated resets the encoder ************************************************************/ void check_in_position_calib(byte jnt)
void init_comment() { INIT_ARRAY(comment, 32); }
void init_sequence() { INIT_ARRAY(sequence, 128); }
// ==================== //
// コンストラクタと代入 //
// ==================== //
// コンストラクタ。
PVLine::PVLine() : last_(0), score_(0), mate_in_(-1) {
INIT_ARRAY(line_);
}
// ==================== //
// コンストラクタと代入 //
// ==================== //
// コンストラクタ。
FEN::FEN(const std::string fen_str) :
to_move_(WHITE),
castling_rights_(ALL_CASTLING),
en_passant_square_(0),
clock_(0),
ply_(0) {
// 駒の配置を初期化。
INIT_ARRAY(position_);
try {
// 構文木にパース。
std::map<std::string, std::string> tree = Util::ParseFEN(fen_str);
// 配置を解析。
std::string::iterator itr = tree["fen position"].begin();
FOR_SQUARES(square) {
Bitboard bb = Util::SQUARE[square][R0];
switch (*itr) {
case 'P':
position_[WHITE][PAWN] |= bb;
break;
case 'N':
position_[WHITE][KNIGHT] |= bb;
break;
case 'B':
position_[WHITE][BISHOP] |= bb;
break;
case 'R':
position_[WHITE][ROOK] |= bb;
break;
case 'Q':
position_[WHITE][QUEEN] |= bb;
break;
case 'K':
position_[WHITE][KING] |= bb;
break;
case 'p':
position_[BLACK][PAWN] |= bb;
break;
case 'n':
position_[BLACK][KNIGHT] |= bb;
break;
case 'b':
position_[BLACK][BISHOP] |= bb;
break;
case 'r':
position_[BLACK][ROOK] |= bb;
break;
case 'q':
position_[BLACK][QUEEN] |= bb;
break;
case 'k':
position_[BLACK][KING] |= bb;
break;
}
++itr;
}
// 手番を解析。
if (tree["fen to_move"] == "w") {
to_move_ = WHITE;
} else {
to_move_ = BLACK;
}
// キャスリングの権利を解析。
castling_rights_ = 0;
if (tree["fen castling"][0] != '-') {
castling_rights_ |= WHITE_SHORT_CASTLING;
}
if (tree["fen castling"][1] != '-') {
castling_rights_ |= WHITE_LONG_CASTLING;
}
if (tree["fen castling"][2] != '-') {
castling_rights_ |= BLACK_SHORT_CASTLING;
}
if (tree["fen castling"][3] != '-') {
castling_rights_ |= BLACK_LONG_CASTLING;
}
// アンパッサンのマスを解析。
std::string temp = tree["fen en_passant"];
if (temp != "-") {
en_passant_square_ =
Util::CoordToSquare(temp[0] - 'a', temp[1] - '1');
}
// 50手ルールの手数を解析。 あれば。
if (tree.find("fen clock") != tree.end()) {
clock_ = std::stoul(tree["fen clock"]);
}
// 手数を解析。 あれば。
if (tree.find("fen ply") != tree.end()) {
ply_ = std::stoul(tree["fen ply"]);
}
} catch (...) {
SetStartPosition();
}
}
