unsigned short write_single_reg(request* req)
{
// значение регистра в req->cnt
unsigned short tmp;
//unsigned char err_cnt=0;
if(req->addr >= 128) return(get_error(req,0x02));
switch(req->addr) {
case 0:set_net_address(req->cnt);break;
case 3:set_platform_weight(req->cnt);break;
case 4:set_coeff(req->cnt);break;
}
//setParam(req->addr,req->cnt);
req->tx_buf[0]=cur_net_addr;
req->tx_buf[1]=0x06;
req->tx_buf[2]=req->addr>>8;
req->tx_buf[3]=req->addr&0xFF;
req->tx_buf[4]=req->cnt>>8;
req->tx_buf[5]=req->cnt&0xFF;
tmp=GetCRC16(req->tx_buf,6);
req->tx_buf[6]=tmp>>8;
req->tx_buf[7]=tmp&0xFF;
return(8);
}
unsigned short write_multi_regs(request* req)
{
unsigned short tmp,value;
//unsigned char err_cnt=0;
if((req->cnt >= 129)||(req->cnt == 0)) return(get_error(req,0x03));
if(req->addr+req->cnt>=129) return(get_error(req,0x02));
for(tmp=0;tmp<req->cnt;tmp++)
{
value = req->rx_buf[8+tmp*2] | ((unsigned short)req->rx_buf[7+tmp*2]<<8);
switch(req->addr + tmp) {
case 0:set_net_address(value);break;
case 3:set_platform_weight(value);break;
case 4:set_coeff(value);break;
}
}
req->tx_buf[0]=cur_net_addr;
req->tx_buf[1]=0x10;
req->tx_buf[2]=req->rx_buf[2];
req->tx_buf[3]=req->rx_buf[3];
req->tx_buf[4]=req->rx_buf[4];
req->tx_buf[5]=req->rx_buf[5];
tmp=GetCRC16(req->tx_buf,6);
req->tx_buf[6]=tmp>>8;
req->tx_buf[7]=tmp&0xFF;
return(8);
}
template <typename T, typename X> void core_solver_pretty_printer<T, X>::init_costs() {
vector<T> local_y(m_core_solver.m_m);
m_core_solver.solve_yB(local_y);
for (unsigned i = 0; i < ncols(); i++) {
if (m_core_solver.m_basis_heading[i] < 0) {
T t = m_core_solver.m_costs[i] - m_core_solver.m_A.dot_product_with_column(local_y, i);
set_coeff(m_costs, m_cost_signs, i, t, m_core_solver.column_name(i));
}
}
}
void map() {
int* arr;
double dt = 0.01;
int N = 100000;
Quadrotor* r = new Quadrotor(dt, N);
VQPoles(r);
int choices = 5;
int repeat = 3;
//int len = pow(choices, repeat);
product(choices, repeat, arr);
//double center[] = {10.0, 3.0, 7.0, 3.0};
//double length[] = { 9.9, 2.9, 6.9, 2.9};
double center[] = {-10.0, -10.0, -00.0};
double length[] = { 5.0, 5.0, 0.0};
double* coeff = new double[choices * repeat];
printf("map start\n");
for(int b = 0; b < 20; b++) {
int a = -1;
set_coeff(center, length, choices, repeat, coeff);
a = sub1(r, arr, coeff, choices, repeat);
if(a == -1) {
printf("failed\n");
return;
}
for(int c = 0; c < repeat; c++) {
center[c] = coeff[c*choices + arr[a*repeat + c]];
length[c] = length[c] * 0.8;
}
printf("center length\n");
print_arr(center, repeat);
print_arr(length, repeat);
//printf("a %i\n",a);
}
}
template <typename T, typename X> void core_solver_pretty_printer<T, X>::init_m_A_and_signs() {
for (unsigned column = 0; column < ncols(); column++) {
m_core_solver.solve_Bd(column); // puts the result into m_core_solver.m_ed
string name = m_core_solver.column_name(column);
for (unsigned row = 0; row < nrows(); row ++) {
set_coeff(
m_A[row],
m_signs[row],
column,
m_core_solver.m_ed[row],
name);
m_rs[row] += m_core_solver.m_ed[row] * m_core_solver.m_x[column];
}
m_exact_column_norms.push_back(current_column_norm() + 1);
}
}
void
Constraint::set_coeff(const Variable& v, real_t coeff)
{
set_coeff(v.get_name(), coeff);
}
// set the coefficient value of a ranked variable
int cplex_solver::set_constraint_coeff(int rank, CUDFcoefficient value) { set_coeff(rank, value); return 0; }
// solve the current problem
int cplex_solver::solve() {
int nb_objectives = objectives.size();
int mipstat, status;
// Presolving the problem
time_t ptime = time(NULL);
if (CPXpresolve(env, lp, CPX_ALG_NONE)) return 0;
time_t ctime = time(NULL);
_timeCount += difftime(ctime, ptime);
// Solve the objectives in a lexical order
for (int i = first_objective; i < nb_objectives; i++) {
ptime = ctime;
// Solve the mip problem
if (CPXmipopt (env, lp)) return ERROR;
ctime = time(NULL);
_solutionCount += CPXgetsolnpoolnumsolns(env, lp) + CPXgetsolnpoolnumreplaced(env, lp);
_timeCount += difftime(ctime, ptime);
_nodeCount += CPXgetnodecnt(env, lp);
// Get solution status
if ((mipstat = CPXgetstat(env, lp)) == CPXMIP_OPTIMAL) {
if (i < nb_objectives - 1) {
// Get next non empty objective
// (must be done here to avoid conflicting method calls
int previ = i, nexti, nexti_nb_coeffs = 0;
for (; i < nb_objectives - 1; i++) {
nexti = i + 1;
nexti_nb_coeffs = objectives[nexti]->nb_coeffs;
if (nexti_nb_coeffs > 0) break;
}
if (nexti_nb_coeffs > 0) { // there is one more objective to solve
// Set objective constraint value to objval
int index[1];
double values[1];
index[0] = previ;
values[0] = objective_value();
if (verbosity >= DEFAULT)
printf(">>>> Objective value %d = %f\n", previ, values[0]);
{
int status, begin[2];
double rhs[1];
begin[0] = 0;
rhs[0] = values[0]; //ub;
int n = objectives[previ]->nb_coeffs;
begin[1] = n - 1;
status = CPXaddrows(env, lp, 0, 1, n, rhs, "E", begin, objectives[previ]->sindex, objectives[previ]->coefficients, NULL, NULL);
if (status) {
fprintf(stderr, "cplex_solver: end_objective: cannot add %d objective as constraint.\n", i);
exit(-1);
}
}
// Set the new objective value
reset_coeffs();
// Set previous objective coefficients to zero
for (int k = 0; k < objectives[previ]->nb_coeffs; k++) set_coeff(objectives[previ]->sindex[k], 0);
// Set next objective coefficients to their actual values
for (int k = 0; k < nexti_nb_coeffs; k++) set_coeff(objectives[nexti]->sindex[k], objectives[nexti]->coefficients[k]);
// Do set the next objective
status = CPXchgobj(env, lp, nb_coeffs, sindex, coefficients);
if ( status ) {
fprintf (stderr,"Cannot change objective value. Exiting...\n");
exit(-1);
}
// Output model to file (when requested)
if (verbosity >= VERBOSE) {
char buffer[1024];
sprintf(buffer, "cplexpb-%d.lp", i);
writelp(buffer);
}
} else
return OPTIMUM;
} else
return OPTIMUM;
} else if( mipstat == CPXMIP_TIME_LIM_INFEAS ||
mipstat == CPXMIP_TIME_LIM_FEAS) {
return _solutionCount > 0 ? SAT : UNKNOWN;
} else {
if (verbosity >= DEFAULT)
fprintf(stderr, "CPLEX solution status = %d\n", mipstat);
return ERROR;
}
}
return 0;
}
