int main(){ int N; int cnd[3][4] = {0,}; int vote[3]= {0,}; scanf("%d",&N); for(int i = 0 ; i < N ; i++){ scanf("%d %d %d",&vote[0],&vote[1],&vote[2]); for(int j = 0 ; j < 3 ; j ++) cnd[i][vote[i]] ++; } for(int i = 0 ; i < 3 ; i++) cnd[i][0] = (cnd[i][1]) + (cnd[i][2])*2 (cnd[i][3])*3; if(cnd[0][0] == cnd[1][0] || cnd[1][0] == cnd[2][0] || cnd[2][0] == cnd[0][0]){ //총점이 같을때 } else{ //선출이 있을 때 } return 0; }
void trdosWriteSectors(unsigned char *src, unsigned char trck, unsigned char sec, unsigned char num) { src; trck; sec; num; __asm ld hl,#6 ;+6 (num) add hl,sp ld b,(hl) dec hl ; + 5 (sector) ld e,(hl) dec hl ; + 4 (track) ld d,(hl) dec hl ; + 3 ld a,(hl) dec hl ; + 2 (destination) ld h,(hl) ld l,a ld c,#6 di call #0x3d13 ei __endasm; }
my_double aspr, mp, a, rhop, mu; my_double k_xx, k_yy, k_zz; my_double k_tranx, k_trany, k_tranz; my_double fx, fy, fz; my_double v12x, v12y, v12z; pi = 3.1415; (tracer+ipart)->q[0] = q0; (tracer+ipart)->q[1] = q1; (tracer+ipart)->q[2] = q2; (tracer+ipart)->q[3] = q3; matQ[0][0] = 1-2(q2*q2+q3*q3); matQ[1][0] = 2*(q1*q2-q0*q3); matQ[2][0] = 2*(q1*q3+q0*q2); matQ[0][1] = 2*(q1*q2+q0*q3); matQ[1][1] = 1-2(q1*q1+q3*q3); matQ[2][1] = 2*(q2*q3-q0*q1); matQ[0][2] = 2*(q1*q3-q0*q2); matQ[1][2] = 2*(q2*q3+q0*q1); matQ[2][2] = 1-2(q1*q1+q2*q2); // aspect ratio is major axis / minor axis // already present in the code // (tracer+ipart)->aspect_ratio = b/a ; aspr = (tracer+ipart)->aspect_ratio ; // mp is the mass of the particle (zhang et al 2001) mp = 4/3*pi*pow(a,3)*aspr*rhop ; K_xx = (16*pi*a*pow((aspr*aspr - 1),3/2))/((2*aspr*aspr - 3)*log(aspr+pow((aspr*aspr-1),1/2)) + aspr*pow((aspr*aspr-1),1/2)); K_yy = K_xx;
/** A sample application to demonstrate a physics simulation in scl. * * Moving forward from tutorial 3, we will now control the 6 DOF * demo robot (r6bot) with the physics engine running. * * SCL Modules used: * 1. data_structs * 2. dynamics (physics) * 4. dynamics (control matrices) * 4. graphics (chai) * */ int main(int argc, char** argv) { std::cout<<"\n***************************************\n"; std::cout<<"Standard Control Library Tutorial #4"; std::cout<<"\n***************************************\n"; scl::SRobotParsed rds; //Robot data structure.... scl::SGraphicsParsed rgr; //Robot graphics data structure... scl::SGcModel rgcm; //Robot data structure with dynamic quantities... scl::SRobotIO rio; //I/O data structure scl::CGraphicsChai rchai; //Chai interface (updates graphics rendering tree etc.) scl::CDynamicsScl dyn_scl; //Robot kinematics and dynamics computation object... scl::CDynamicsTao dyn_tao; //Robot physics integrator scl::CParserScl p; //This time, we'll parse the tree from a file... sutil::CSystemClock::start(); /******************************Load Robot Specification************************************/ //We will use a slightly more complex xml spec than the first few tutorials bool flag = p.readRobotFromFile("../../specs/Kuka/KukaCfg.xml","../../specs/","KukaBot",rds); flag = flag && rgcm.init(rds); //Simple way to set up dynamic tree... flag = flag && dyn_tao.init(rds); //Set up integrator object flag = flag && dyn_scl.init(rds); //Set up kinematics and dynamics object flag = flag && rio.init(rds.name_,rds.dof_); for(unsigned int i=0;i<rds.dof_;++i){ rio.sensors_.q_(i) = rds.rb_tree_.at(i)->joint_default_pos_; } if(false == flag){ return 1; } //Error check. /******************************ChaiGlut Graphics************************************/ glutInit(&argc, argv); // We will use glut for the window pane (not the graphics). flag = p.readGraphicsFromFile("../../specs/Kuka/KukaCfg.xml","KukaBotStdView",rgr); flag = flag && rchai.initGraphics(&rgr); flag = flag && rchai.addRobotToRender(&rds,&rio); flag = flag && scl_chai_glut_interface::initializeGlutForChai(&rgr, &rchai); if(false==flag) { std::cout<<"\nCouldn't initialize chai graphics\n"; return 1; } /******************************Simulation************************************/ // Now let us integrate the model for a variety of timesteps and see energy stability std::cout<<"\nIntegrating the r6bot's physics. \nWill test two different controllers.\n Press (x) to exit at anytime."; long iter = 0, n_iters=200; double dt=0.0001; omp_set_num_threads(2); int thread_id; double tstart, tcurr; flag = false; const Eigen::Vector3d hpos0(0,1,0); //control position of op-point wrt. hand const Eigen::Vector3d hpos1(0,1,1); //control position of op-point wrt. hand Eigen::MatrixXd Jx0, Jx1; Eigen::Vector3d x0, x0_des=(0,0,0), x_init0, dx0; Eigen::Vector3d x1, x1_des1=x0_des+(hpos1-hpos0), x_init1, dx1; scl::SRigidBodyDyn *rhand = rgcm.rbdyn_tree_.at("hand"); #pragma omp parallel private(thread_id) { thread_id = omp_get_thread_num(); if(thread_id==1) //Simulate physics and update the rio data structure.. { // Controller 1 : gc controller std::cout<<"\n\n***************************************************************" <<"\n Starting joint space (generalized coordinate) controller..." <<"\n This will move the joints in a sine wave" <<"\n***************************************************************"; tstart = sutil::CSystemClock::getSimTime(); sutil::CSystemClock::tick(dt); while(iter < n_iters && true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running) { tcurr = sutil::CSystemClock::getSysTime(); // Controller : fgc = kp * (sin(t) - q) + kv(0 - dq) // Set the desired positions so that the joints move in a sine wave rio.actuators_.force_gc_commanded_ = 50* (sin(tcurr-tstart) - rio.sensors_.q_.array()) - 20 * rio.sensors_.dq_.array(); dyn_tao.integrate(rio,dt); iter++; const timespec ts = {0, 5000};/*.05ms*/ nanosleep(&ts,NULL); if(iter % 1000 == 0){ // std::cout<<"\n"<<(sin(tcurr-tstart) - rio.sensors_.q_.array()).transpose(); //std::cout<<"\n"<<rio.actuators_.force_gc_commanded_.transpose(); } } sleep(1); // Controller 2 : Operational space controller std::cout<<"\n\n***************************************************************" <<"\n Starting op space (task coordinate) controller..." <<"\n This will move the hand in a circle. x =sin(t), y=cos(t)." <<"\n***************************************************************"; tstart = sutil::CSystemClock::getSimTime(); iter = 0; Eigen::Vector3d ball_forces,ball_pos,ball_vel,ball_accel,paddle_orientation; Eigen::Vector3d ball_pos_int = (1, 1, 10); Eigen::Vector3d ball_vel_int =(0, 0, 0); Eigen::Vector3d gravity = (0,0,-9.8),ez=(0,0,1); Eigen::Vector3d ball_force_est,ball_pos_est, ball_vel_est,ball_accel_est,orientation; bool hitting = false; float ball_pos_threshold = 2,radius=.025,rho=1,Cd=.1,mass=1,impact_time=(10000*dt),Delta_T=.01; int est_iter = 0; ball_pos=ball_pos_int; ball_vel=ball_vel_int; while( iter<200000 && true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running) { tcurr = sutil::CSystemClock::getSimTime(); paddle_orientation=x1-x0; if(ball_pos(2)<x0(2)){ //checks for impact ball_vel=1.2*ball_vel-2(ball_vel.transpose()*paddle_orientation.normalized())*paddel_orientation.normalized(); rtask_hand->x0_des(2)=0; hitting=false; impact_time=(iter+10000)*dt; }else{ //if no impact if(hitting){ //check to see if hitting rtask_hand->x0_des(2)=100; }else if(iter*dt>imapct_time-Delta_T){ //check to see it should be hitting hitting=true; }else{ //if no impact and not hitting esitmate trejectory ball_pos_est=ball_pos; //intialize for estimation ball_vel_est=ball_vel; est_iter = 0; while(ball_pos_est(2) > ball_pos_threshold){//estimated trajectory ball_force_est=gravity-.5*rho*3.14159*radius*radius*Cd*ball_vel_est.squaredNorm()*ball_vel_est.Norm(); ball_accel_est=ball_force_est/mass; ball_vel_est=ball_vel_est+ball_accel_est*dt; ball_pos_est=ball_pos_est+ball_vel_est*dt; est_iter++; } impact_time=(iter+est_iter)*dt; x0_des(0) = ball_pos_est(0); //crossing location in z plane x0_des(1) = ball_pos_est(1); } //computing ball forces since no impact ball_force=gravity-.5*rho*3.14159*radius*radius*Cd*ball_vel.squaredNorm()*ball_vel.Norm(); ball_accel=ball_force/mass; ball_vel=ball_vel+ball_accel*dt; } orientation=(ez-ball_vel)/sqrt(2*ball_vel.norm()-ball_vel.transpose()*(0,0,1)); x1_des=x0_des+orientation.normalized(); // Compute control forces (note that these directly have access to the io data ds). rctr.computeDynamics(); rctr.computeControlForces(); // Integrate the dynamics dyn_tao.integrate(rio,dt); iter++; ball_pos=ball_pos+ball_vel*dt; // Compute kinematic quantities dyn_scl.computeTransformsForAllLinks(rgcm.rbdyn_tree_,rio.sensors_.q_); dyn_scl.computeJacobianWithTransforms(Jx0,*rhand,rio.sensors_.q_,hpos0); dyn_scl.computeJacobianWithTransforms(Jx1,*rhand,rio.sensors_.q_,hpos1); if(false == flag) { x_init = rhand->T_o_lnk_ * hpos0; flag = true; } x0 = rhand->T_o_lnk_ * hpos0; x1 = rhand->T_o_lnk_ * hpos1; dx0 = Jx0.block(0,0,3,rio.dof_) * rio.sensors_.dq_; dx1 = Jx1.block(0,0,3,rio.dof_) * rio.sensors_.dq_; rio.actuators_.force_gc_commanded_ = Jx0.block(0,0,3,rio.dof_).transpose() * (1000*(x0_des-x0) - 10 * dx0) - 200*rio.sensors_.dq_; rio.actuators_.force_gc_commanded_ += Jx1.block(0,0,3,rio.dof_).transpose() * (1000*(x1_des-x1) - 10 * dx1) - 200*rio.sensors_.dq_; // Integrate the dynamics dyn_tao.integrate(rio,dt); iter++; const timespec ts = {0, 5000};/*.05ms*/ nanosleep(&ts,NULL); sutil::CSystemClock::tick(dt); if(iter % 1000 == 0) { std::cout<<"\n"<<(x0_des-x0).transpose()<<" "<<(x0_des-x0).norm(); //std::cout<<"\n"<<rio.actuators_.force_gc_commanded_.transpose(); } } //Then terminate scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running = false; } else //Read the rio data structure and updated rendererd robot.. while(true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running) { glutMainLoopEvent(); const timespec ts = {0, 15000000};/*15ms*/ nanosleep(&ts,NULL); } } /******************************Exit Gracefully************************************/ std::cout<<"\n\nExecuted Successfully"; std::cout<<"\n**********************************\n"<<std::flush; return 0; }