Example #1
0
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;
}
Example #2
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;
}