/**
 * Run this Test
 */
void PWMBoardTest::spin()
{
  double rate = 5.0d;
  ros::Rate loop_rate(rate); //herz;

  double pwmSpeed = 0.0d;

  //Increment of each speed step
  double stepSize = 0.25;

  //Scale factor for setting speed at a lower rate
  //then the loop
  double modStepRate = 0.2;
  //For determining how many times the
  //loop has "looped"
  double cntRate = 0.0d;

  //Positive incrementing first
  if (maxPWM > 0)
  {
    //First, climb to max value
    bool toMax = true;

    //Set initial speed
    setSpeed(0, 0);

    while (ros::ok())
    {
      //Set speed to pwm speed
      setSpeed(pwmSpeed, pwmSpeed);
      ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);

      //Check for errors
      check_EM_STOP();

      //Check if it is time for setting new speed
      if (double_equals(cntRate, rate))
      {
        //Check if max is not reached
        if (!(double_equals(pwmSpeed, maxPWM)) && (toMax)){
        pwmSpeed += stepSize;
        printf("Stepping up Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
      }
      //Max is reached
      else
      {
        //Dont go to max value anymore
        toMax = false;

        //Check if 0 is not reached
        if (!double_equals(pwmSpeed, 0.0))
        {
          setSpeed(pwmSpeed,pwmSpeed);

          pwmSpeed -= stepSize;
          printf("Stepping down Speed from here: %f %f \n", pwmSpeed, 0.0);
        }
        //0 is reached, done
        else
        {
          printf("Done \n");
          break;
        }
      }

      //Reset countrate
      cntRate = 0.0d;
      std::cout << "cntrate " << cntRate << std::endl;
    }

    //Now it not the time for changing speed
    else
    {
      //Increment times loop has looped
      cntRate += modStepRate;
      std::cout << "cntrate " << cntRate << std::endl;
    }

      loop_rate.sleep();
    }

  }

  //Negative incrementing(decrementing) first
  else
  {
    //First go to low value
    bool toMin = true;

    //Set initial speed
    setSpeed(0, 0);
    ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);

    while (ros::ok())
    {
      //Set speed to pwm speed
      setSpeed(pwmSpeed, pwmSpeed);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        compass_angle = convert_bearing_to_degrees(compass_val);

        cout << "ds1: " << ir1_val << " ds14:" << ir14_val << endl;
        cout << "ds3: " << ir3_val << " ds12:" << ir12_val << endl;

        //Condition to choose between compass and sensor motion mode
        if ((ir1_val < DISTANCE_LIMIT) && (ir14_val < DISTANCE_LIMIT)&& (ir12_val < DISTANCE_LIMIT)&& (ir3_val < DISTANCE_LIMIT)){

            //Compass mode
            if ( compass_angle < (DESIRED_ANGLE - 2)) {

                _mode = TURN_RIGHT;
            }
            else{if(compass_angle > (DESIRED_ANGLE + 2)) {

                    _mode = TURN_LEFT;
                }
                else {

                    _mode = FORWARD;
                }
            }}else{

            //Sensor mode

            //Condition to stop and go backwards
            if (((ir1_val > 800) || (ir14_val > 800)||(ir3_val > 800) || (ir12_val > 800)) && ((ir1_val != 0) || (ir14_val != 0))){

                _mode = BACKWARDS;
                cout << "Backwards." << endl;}

            else{

                //Condition to turn using sensors
                if ((ir12_val > DISTANCE_LIMIT-50)|| (ir14_val > DISTANCE_LIMIT)) {
                    _mode = TURN_LEFT;
                    cout << "Turning left." << endl;
                }


                else {
                    if ((ir3_val > DISTANCE_LIMIT-50)||(ir1_val > DISTANCE_LIMIT)) {
                        _mode = TURN_RIGHT;
                        cout << "Turning right." << endl;
                    }
                    else {
                        _mode = FORWARD;
                        cout << "Moving forward." << endl;
                    }}}
        }













        // Send actuators commands according to the mode
        switch (_mode){
        case STOP:
            _left_speed = 0;
            _right_speed = 0;
            break;
        case FORWARD:
            _left_speed = MAX_SPEED;
            _right_speed = MAX_SPEED;
            break;
        case TURN_LEFT:
            _left_speed = MAX_SPEED/3.0;
            _right_speed = MAX_SPEED;
            break;
        case TURN_RIGHT:
            _left_speed = MAX_SPEED;
            _right_speed = MAX_SPEED/3.0;
            break;
        case BACKWARDS:
            _left_speed = -MAX_SPEED;
            _right_speed = -MAX_SPEED;
            break;

        default:
            break;
        }

        
        
        
        
        
        
        // Set the motor speeds
        setSpeed(_left_speed, _right_speed);
    }
}

/*	Return Values:
	0 = OK
	1 = too manny pulse lengths
	2 = abort
	3 = too long
*/
unsigned char captureIR(struct ir_raw_* capture) {
	
	unsigned long hightime, lowtime;
	unsigned long T0count;
	unsigned short highpulse, lowpulse;
	unsigned long T1count;
	unsigned long timeout;
	unsigned char status;
	unsigned char wide;
	
	while(ANYKEY);
	
	setSpeed(SPEED_60);
	disableIRQ();
	T0MCR = 0x00;
	
	FIOSET0 |= (1<<4);
	FIOCLR0 |= (1<<12);
	PINSEL1 |= (1<<27);
		
	T0CCR = (1<<9) | (1<<10) | (1<<11);
		
	capture->widetable[0].count = 0;
	capture->widetable[1].count = 0;
	capture->count = 0;
	status = 0;
	
	T0TC = 0;
	while (T0TC < 5000);
	
	T0IR = 0xff;
	while (!T0IR && !ANYKEY);
	if(!ANYKEY) {
		T0TC = 0;
		T0IR = 0xff;
		T0count = waitIrHi(&highpulse,&hightime,0);
		capture->pulsetime = (hightime * 1000) / highpulse;
		capture->data[0] = (decodewide(highpulse, &capture->widetable[1]) & 0x0F)<<4;
		
		while (!(T0IR));
		T0IR = 0xff;
		T1count= T0CR3;
		while ((capture->count < max_capture_data-1) && !status) {
			lowtime = T1count-T0count;
			T0count = waitIrHi(&highpulse,&hightime,T1count);		
			lowpulse = ((lowtime *1000)+(capture->pulsetime / 2)) / capture->pulsetime;
			wide = decodewide(lowpulse, &capture->widetable[1]);
			if(wide & 0xf0)
				status = 2;
			capture->data[capture->count] |= wide;
			wide = decodewide(highpulse, &capture->widetable[1]);
			if(wide & 0xf0)
				status = 2;
			capture->count++;
			capture->data[capture->count] = wide<<4;
			timeout = T0count + 500000;
			while (!(T0IR) && !status) {
				if (T0TC > timeout)
					status =1;
			}
			T0IR = 0xff;
			T1count= T0CR3;
		}
	}
	else
		status = 3;
	if(capture->count == max_capture_data-1)
		status = 4;
	T0TC = 0;
	T0MCR = 0x03;
	FIOSET0 |= (1<<12);
	T0CCR = 0;
	T0IR = 0xff;
	enableIRQ();
	setBacklight(BL_AUTO);
	setSpeed(SPEED_30);
	return status - 1;
} 

FirstPersonGravityCamera::FirstPersonGravityCamera() : FirstPersonCamera(glm::vec3(10,20,10), glm::vec3(0,0,0)){
	setSpeed(20.0f);
}

//.........这里部分代码省略.........
					Log(LOG_ERROR) << "Failed to load craft weapon " << type;
				}
			}
			j++;
		}
	}

	_items->load(node["items"]);
	// Some old saves have bad items, better get rid of them to avoid further bugs
	for (std::map<std::string, int>::iterator i = _items->getContents()->begin(); i != _items->getContents()->end();)
	{
		if (mod->getItem(i->first) == 0)
		{
			Log(LOG_ERROR) << "Failed to load item " << i->first;
			_items->getContents()->erase(i++);
		}
		else
		{
			++i;
		}
	}
	for (YAML::const_iterator i = node["vehicles"].begin(); i != node["vehicles"].end(); ++i)
	{
		std::string type = (*i)["type"].as<std::string>();
		if (mod->getItem(type))
		{
			Vehicle *v = new Vehicle(mod->getItem(type), 0, 4);
			v->load(*i);
			_vehicles.push_back(v);
		}
		else
		{
			Log(LOG_ERROR) << "Failed to load item " << type;
		}
	}
	_status = node["status"].as<std::string>(_status);
	_lowFuel = node["lowFuel"].as<bool>(_lowFuel);
	_mission = node["mission"].as<bool>(_mission);
	_interceptionOrder = node["interceptionOrder"].as<int>(_interceptionOrder);
	if (const YAML::Node &dest = node["dest"])
	{
		std::string type = dest["type"].as<std::string>();
		int id = dest["id"].as<int>();
		if (type == "STR_BASE")
		{
			returnToBase();
		}
		else if (type == "STR_UFO")
		{
			for (std::vector<Ufo*>::iterator i = save->getUfos()->begin(); i != save->getUfos()->end(); ++i)
			{
				if ((*i)->getId() == id)
				{
					setDestination(*i);
					break;
				}
			}
		}
		else if (type == "STR_WAYPOINT")
		{
			for (std::vector<Waypoint*>::iterator i = save->getWaypoints()->begin(); i != save->getWaypoints()->end(); ++i)
			{
				if ((*i)->getId() == id)
				{
					setDestination(*i);
					break;
				}
			}
		}
		else if (type == "STR_ALIEN_BASE")
		{
			for (std::vector<AlienBase*>::iterator i = save->getAlienBases()->begin(); i != save->getAlienBases()->end(); ++i)
			{
				if ((*i)->getId() == id)
				{
					setDestination(*i);
					break;
				}
			}
		}
		else
		{
			// Backwards compatibility
			if (type == "STR_ALIEN_TERROR")
				type = "STR_TERROR_SITE";
			for (std::vector<MissionSite*>::iterator i = save->getMissionSites()->begin(); i != save->getMissionSites()->end(); ++i)
			{
				if ((*i)->getId() == id && (*i)->getDeployment()->getMarkerName() == type)
				{
					setDestination(*i);
					break;
				}
			}
		}
	}
	_takeoff = node["takeoff"].as<int>(_takeoff);
	_inBattlescape = node["inBattlescape"].as<bool>(_inBattlescape);
	if (_inBattlescape)
		setSpeed(0);
}

bool Robot::processButton(int whichWay)            
{ 
  toggle(26);
      
  switch (whichWay) {
  
  case 'F': // forward
    #ifdef DEBUG
      Serial.println("Forward");
    #endif
    if (m_wheelLeft > m_wheelRight)
      m_wheelRight = m_wheelLeft;
    else if (m_wheelLeft < m_wheelRight) 
      m_wheelLeft = m_wheelRight;
    else {           
      m_wheelLeft = m_wheelLeft + 32;
      m_wheelRight = m_wheelRight + 32;
    }      
    break;    
    
  case 'R': // right
    #ifdef DEBUG
      Serial.println("Right");
    #endif
    m_wheelLeft = m_wheelLeft + 16;
    m_wheelRight = m_wheelRight - 16;
    break;
    
  case 'L': // left
    #ifdef DEBUG
      Serial.println("Left");
    #endif
    m_wheelLeft = m_wheelLeft - 16;
    m_wheelRight = m_wheelRight + 16;
    break;
    
  case 'B': // reverse
    #ifdef DEBUG
      Serial.println("Reverse");
    #endif
    if(m_wheelLeft < m_wheelRight)
      m_wheelRight = m_wheelLeft;
    else if (m_wheelLeft > m_wheelRight) 
      m_wheelLeft = m_wheelRight;
    else {           
      m_wheelLeft = m_wheelLeft - 32;
      m_wheelRight = m_wheelRight - 32;
    }
    break;  
        
  case 'S': // stop
    #ifdef DEBUG
      Serial.println("Stop");
    #endif
    m_wheelLeft = 0;
    m_wheelRight = 0;
    break;
    
  default:  // unknown request
    return false;
  }    
  
  if (m_wheelLeft > 128) m_wheelLeft = 128;
  if (m_wheelLeft < -128) m_wheelLeft = -128;
  if (m_wheelRight > 128) m_wheelRight = 128;
  if (m_wheelRight < -128) m_wheelRight = -128;
  
  setSpeed(m_wheelLeft, m_wheelRight);
    
  return true;
}

void LocalPlayer::applyControl(float dtime)
{
	// Clear stuff
	swimming_up = false;

	// Random constants
	f32 walk_acceleration = 4.0 * BS;
	f32 walkspeed_max = 4.0 * BS;
	
	setPitch(control.pitch);
	setYaw(control.yaw);
	
	v3f move_direction = v3f(0,0,1);
	move_direction.rotateXZBy(getYaw());
	
	v3f speed = v3f(0,0,0);
	
	bool fly_allowed = m_gamedef->checkLocalPrivilege("fly");
	bool fast_allowed = m_gamedef->checkLocalPrivilege("fast");

	bool free_move = fly_allowed && g_settings->getBool("free_move");
	bool fast_move = fast_allowed && g_settings->getBool("fast_move");
	bool continuous_forward = g_settings->getBool("continuous_forward");

	if(free_move || is_climbing)
	{
		v3f speed = getSpeed();
		speed.Y = 0;
		setSpeed(speed);
	}

	// Whether superspeed mode is used or not
	bool superspeed = false;
	
	// If free movement and fast movement, always move fast
	if(free_move && fast_move)
		superspeed = true;
	
	// Auxiliary button 1 (E)
	if(control.aux1)
	{
		if(free_move)
		{
			// In free movement mode, aux1 descends
			v3f speed = getSpeed();
			if(fast_move)
				speed.Y = -20*BS;
			else
				speed.Y = -walkspeed_max;
			setSpeed(speed);
		}
		else if(is_climbing)
		{
		        v3f speed = getSpeed();
			speed.Y = -3*BS;
			setSpeed(speed);
		}
		else
		{
			// If not free movement but fast is allowed, aux1 is
			// "Turbo button"
			if(fast_move)
				superspeed = true;
		}
	}

	if(continuous_forward)
		speed += move_direction;

	if(control.up)
	{
		if(continuous_forward)
			superspeed = true;
		else
			speed += move_direction;
	}
	if(control.down)
	{
		speed -= move_direction;
	}
	if(control.left)
	{
		speed += move_direction.crossProduct(v3f(0,1,0));
	}
	if(control.right)
	{
		speed += move_direction.crossProduct(v3f(0,-1,0));
	}
	if(control.jump)
	{
		if(free_move)
		{
			v3f speed = getSpeed();
			if(fast_move)
				speed.Y = 20*BS;
			else
				speed.Y = walkspeed_max;
			setSpeed(speed);
		}
		else if(touching_ground)
//.........这里部分代码省略.........

bool CCParticleExplosion::initWithTotalParticles(unsigned int numberOfParticles)
{
    if( CCParticleSystemQuad::initWithTotalParticles(numberOfParticles) ) 
    {
        // duration
        m_fDuration = 0.1f;

        setEmitterMode(kCCParticleModeGravity);

        // Gravity Mode: gravity
        setGravity(ccp(0,0));

        // Gravity Mode: speed of particles
        setSpeed(70);
        setSpeedVar(40);

        // Gravity Mode: radial
        setRadialAccel(0);
        setRadialAccelVar(0);

        // Gravity Mode: tangential
        setTangentialAccel(0);
        setTangentialAccelVar(0);

        // angle
        m_fAngle = 90;
        m_fAngleVar = 360;

        // emitter position
        CCSize winSize = CCDirector::sharedDirector()->getWinSize();
        this->setPosition(ccp(winSize.width/2, winSize.height/2));
        setPosVar(CCPointZero);

        // life of particles
        m_fLife = 5.0f;
        m_fLifeVar = 2;

        // size, in pixels
        m_fStartSize = 15.0f;
        m_fStartSizeVar = 10.0f;
        m_fEndSize = kCCParticleStartSizeEqualToEndSize;

        // emits per second
        m_fEmissionRate = m_uTotalParticles/m_fDuration;

        // color of particles
        m_tStartColor.r = 0.7f;
        m_tStartColor.g = 0.1f;
        m_tStartColor.b = 0.2f;
        m_tStartColor.a = 1.0f;
        m_tStartColorVar.r = 0.5f;
        m_tStartColorVar.g = 0.5f;
        m_tStartColorVar.b = 0.5f;
        m_tStartColorVar.a = 0.0f;
        m_tEndColor.r = 0.5f;
        m_tEndColor.g = 0.5f;
        m_tEndColor.b = 0.5f;
        m_tEndColor.a = 0.0f;
        m_tEndColorVar.r = 0.5f;
        m_tEndColorVar.g = 0.5f;
        m_tEndColorVar.b = 0.5f;
        m_tEndColorVar.a = 0.0f;

        CCTexture2D* pTexture = getDefaultTexture();
        if (pTexture != NULL)
        {
            setTexture(pTexture);
        }

        // additive
        this->setBlendAdditive(false);
        return true;
    }
    return false;
}

void LocalPlayer::applyControl(float dtime, ClientEnvironment *env)
{
	// Clear stuff
	swimming_vertical = false;

	setPitch(control.pitch);
	setYaw(control.yaw);

	// Nullify speed and don't run positioning code if the player is attached
	if(isAttached)
	{
		setSpeed(v3f(0,0,0));
		return;
	}

	v3f move_direction = v3f(0,0,1);
	move_direction.rotateXZBy(getYaw());

	v3f speedH = v3f(0,0,0); // Horizontal (X, Z)
	v3f speedV = v3f(0,0,0); // Vertical (Y)

	bool fly_allowed = m_gamedef->checkLocalPrivilege("fly");
	bool fast_allowed = m_gamedef->checkLocalPrivilege("fast");

	free_move = fly_allowed && g_settings->getBool("free_move");
	bool fast_move = fast_allowed && g_settings->getBool("fast_move");
	// When aux1_descends is enabled the fast key is used to go down, so fast isn't possible
	bool fast_climb = fast_move && control.aux1 && !g_settings->getBool("aux1_descends");
	bool continuous_forward = g_settings->getBool("continuous_forward");
	bool fast_pressed = false;
	bool always_fly_fast = g_settings->getBool("always_fly_fast");

	// Whether superspeed mode is used or not
	superspeed = false;

	if (always_fly_fast && free_move && fast_move)
		superspeed = true;

	// Old descend control
	if(g_settings->getBool("aux1_descends"))
	{
		// If free movement and fast movement, always move fast
		if(free_move && fast_move)
			superspeed = true;

		// Auxiliary button 1 (E)
		if(control.aux1)
		{
			if(free_move)
			{
				// In free movement mode, aux1 descends
				if(fast_move)
					speedV.Y = -movement_speed_fast;
				else
					speedV.Y = -movement_speed_walk;
			}
			else if(in_liquid || in_liquid_stable)
			{
				speedV.Y = -movement_speed_walk;
				swimming_vertical = true;
			}
			else if(is_climbing)
			{
				speedV.Y = -movement_speed_climb;
			}
			else
			{
				// If not free movement but fast is allowed, aux1 is
				// "Turbo button"
				if(fast_allowed)
					superspeed = true;
			}
		}
	}
	// New minecraft-like descend control
	else
	{
		// Auxiliary button 1 (E)
		if(control.aux1)
		{
			if(!is_climbing)
			{
				// aux1 is "Turbo button"
				if(fast_allowed)
					superspeed = true;
			}
			if(fast_allowed)
				fast_pressed = true;
		}

		if(control.sneak)
		{
			if(free_move)
			{
				// In free movement mode, sneak descends
				if (fast_move && (control.aux1 || always_fly_fast))
					speedV.Y = -movement_speed_fast;
				else
					speedV.Y = -movement_speed_walk;
			}
//.........这里部分代码省略.........

//.........这里部分代码省略.........
				node =map->getNodeNoEx(p + v3s16(0,2,0), &is_valid_position);
				if (!is_valid_position || nodemgr->get(node).walkable)
					continue;
			}

			min_distance_f = distance_f;
			new_sneak_node = p;
		}

		bool sneak_node_found = (min_distance_f < 100000.0 * BS * 0.9);

		m_sneak_node = new_sneak_node;
		m_sneak_node_exists = sneak_node_found;

		if (sneak_node_found) {
			f32 cb_max = 0;
			MapNode n = map->getNodeNoEx(m_sneak_node);
			std::vector<aabb3f> nodeboxes;
			n.getCollisionBoxes(nodemgr, &nodeboxes);
			for (std::vector<aabb3f>::iterator it = nodeboxes.begin();
					it != nodeboxes.end(); ++it) {
				aabb3f box = *it;
				if (box.MaxEdge.Y > cb_max)
					cb_max = box.MaxEdge.Y;
			}
			m_sneak_node_bb_ymax = cb_max;
		}

		/*
			If sneaking, the player's collision box can be in air, so
			this has to be set explicitly
		*/
		if(sneak_node_found && control.sneak)
			touching_ground = true;
	}

	/*
		Set new position
	*/
	setPosition(position);

	/*
		Report collisions
	*/

	// Dont report if flying
	if(collision_info && !(g_settings->getBool("free_move") && fly_allowed)) {
		for(size_t i=0; i<result.collisions.size(); i++) {
			const CollisionInfo &info = result.collisions[i];
			collision_info->push_back(info);
		}
	}

	if(!result.standing_on_object && !touching_ground_was && touching_ground) {
		MtEvent *e = new SimpleTriggerEvent("PlayerRegainGround");
		m_gamedef->event()->put(e);

		// Set camera impact value to be used for view bobbing
		camera_impact = getSpeed().Y * -1;
	}

	{
		camera_barely_in_ceiling = false;
		v3s16 camera_np = floatToInt(getEyePosition(), BS);
		MapNode n = map->getNodeNoEx(camera_np);
		if(n.getContent() != CONTENT_IGNORE){
			if(nodemgr->get(n).walkable && nodemgr->get(n).solidness == 2){
				camera_barely_in_ceiling = true;
			}
		}
	}

	/*
		Update the node last under the player
	*/
	m_old_node_below = floatToInt(position - v3f(0,BS/2,0), BS);
	m_old_node_below_type = nodemgr->get(map->getNodeNoEx(m_old_node_below)).name;

	/*
		Check properties of the node on which the player is standing
	*/
	const ContentFeatures &f = nodemgr->get(map->getNodeNoEx(getStandingNodePos()));
	// Determine if jumping is possible
	m_can_jump = touching_ground && !in_liquid;
	if(itemgroup_get(f.groups, "disable_jump"))
		m_can_jump = false;
	// Jump key pressed while jumping off from a bouncy block
	if (m_can_jump && control.jump && itemgroup_get(f.groups, "bouncy") &&
		m_speed.Y >= -0.5 * BS) {
		float jumpspeed = movement_speed_jump * physics_override_jump;
		if (m_speed.Y > 1) {
			// Reduce boost when speed already is high
			m_speed.Y += jumpspeed / (1 + (m_speed.Y / 16 ));
		} else {
			m_speed.Y += jumpspeed;
		}
		setSpeed(m_speed);
		m_can_jump = false;
	}
}

int main(void)
{
	
    /* Configure Oscillator to operate the device at 30Mhz
       Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
       Fosc= 7.37*(32)/(2*2)=58.96Mhz for Fosc, Fcy = 29.48Mhz */

    /* Configure PLL prescaler, PLL postscaler, PLL divisor */
    //PLLFBDbits.PLLDIV=38;   /* M = PLLFBD + 2 */ // izlazna frekvencija = 30Mhz
    //Fin=8MHz, Fcy=30MHz 
	// Configure PLL prescaler, PLL postscaler, PLL divisor
	PLLFBD = 28; 				// M=40    ---> PLLFBD + 2 = M
	CLKDIVbits.PLLPOST = 0; 	// N2=2    ---> 2x(PLLPOST + 2) = N2
	CLKDIVbits.PLLPRE = 0; 	// N1=2    ---> PLLPRE + 2 = N1

	//new oscillator selection
	__builtin_write_OSCCONH(0b011);  				//0b011 ---> XT with PLL
	//enable oscillator source switch
	__builtin_write_OSCCONL (OSCCONL | (1<<0)); 	//OSWEN 

	//wait for PLL lock -> wait to new settings become available
	while (OSCCONbits.COSC != 0b011); 
	//wait for PLL lock
	while (OSCCONbits.LOCK != 0b1); 
    
    AD1PCFGL = 0xFFFF;// all PORT Digital

   
    RPINR18bits.U1RXR = 0;		//UART1 RX na RP0- pin 4
    RPOR0bits.RP1R = 3;			//UART1 TX na RP1- pin 5
    RPINR14bits.QEA1R = 2;		//QEI1A na RP2
    RPINR14bits.QEB1R = 3;		//QEI1B na RP3

    RPINR16bits.QEA2R = 4;		//QEI2A na RP4
    RPINR16bits.QEB2R = 7;		//QEI2B na RP7
    
    CAN_init(DRIVER_IDENTIFICATOR); // inicijalizacija CAN BUS- a-> argument je adresa drajvera

    int tmp;
    char komanda, v, smer;
    int Xc, Yc, ugao;
    
    NewLine();

    PortInit();
    //UARTinit();
    TimerInit();
    QEIinit();
    PWMinit();
   // CloseMCPWM();

    resetDriver();

    setSpeed(0x80);
    setSpeedAccel(K2);	//K2 je za 1m/s /bilo je 2
    int tmpX, tmpY, tmpO;
    unsigned char rxBuffer[8];
    while(1)
    {

        __delay_ms(1000);
        setSpeed(30);
       // kretanje_pravo(-1000, 0);
        if(getStatus() == STATUS_MOVING)
            CAN_getLastMessage(rxBuffer);
        else
            CAN_read(rxBuffer);

        komanda = rxBuffer[0];

        switch(komanda)
        {
            // zadavanje pozicije
            case 'I':
                tmpX = rxBuffer[1] << 8;
                tmpX |= rxBuffer[2];

                tmpY = rxBuffer[3] << 8;
                tmpY |= rxBuffer[4];

                tmpO = rxBuffer[5] << 8;
                tmpO |= rxBuffer[6];

                setPosition(tmpX, tmpY, tmpO);

                break;

            // citanje pozicije i statusa
            case 'P':
                sendStatusAndPosition();

                break;

            //zadavanje max. brzine (default K2/2)
            case 'V':
                tmp = rxBuffer[1];
                setSpeed(tmp);

                break;

//.........这里部分代码省略.........

void GLWorld::mousePressEvent(QMouseEvent *event)
{
    last_pos = event->pos();

    if(!(event->buttons() & Qt::RightButton && current_selection.type != TYPE_NOTHING && editable))
        return;

    WorldObject &here = getObject(current_selection.coords);
    bool steve_is_here = current_selection.coords == steve;
    QPoint pos = mapToGlobal(event->pos());

    QMenu menu;
    QAction steve_here(trUtf8("Steve hierher teleportieren"), &menu);
    steve_here.setDisabled(here.has_cube || steve_is_here);
    menu.addAction(&steve_here);

    QAction cube_here(trUtf8("Würfel"), &menu);
    cube_here.setCheckable(true);
    cube_here.setChecked(here.has_cube);
    cube_here.setDisabled(steve_is_here); //No cube into steve
    menu.addAction(&cube_here);

    QAction stack_here(trUtf8("Stapel"), &menu);
    stack_here.setCheckable(true);
    if(here.stack_size > 0)
    {
        stack_here.setChecked(true);
        stack_here.setText(trUtf8("Stapel (%1)").arg(here.stack_size));
    }
    menu.addAction(&stack_here);

    QAction mark_here(trUtf8("Markierung"), &menu);
    mark_here.setCheckable(true);
    mark_here.setChecked(here.has_mark);
    menu.addAction(&mark_here);

    QAction *selected = menu.exec(pos);
    if(selected == &steve_here)
    {
        steve = current_selection.coords;

        //The user can't be really fast, so animations always on
        setSpeed(2000);

        setAnimation(ANIM_STEP);
        updateAnimationTarget();
        updateFront();

        fbo_dirty = true;
    }
    else if(selected == &cube_here)
    {
        here.has_cube = !here.has_cube;
        here.stack_size = 0;
        here.has_mark = false;

        fbo_dirty = true;

        emit changed();
    }
    else if(selected == &stack_here)
    {
        bool ok;
        int s = QInputDialog::getInt(this, trUtf8("Stapelhöhe"), trUtf8("Stapelhöhe auswählen:"), here.stack_size, 0, World::max_height, 1, &ok);
        if(ok)
        {
            if(s > 0)
                here.has_cube = false;

            here.stack_size = s;

            if(steve == current_selection.coords)
            {
                setAnimation(ANIM_STEP);
                updateAnimationTarget();
            }

            fbo_dirty = true;

            emit changed();
        }
    }
    else if(selected == &mark_here)
    {
        here.has_mark = !here.has_mark;
        here.has_cube = false;

        fbo_dirty = true;

        emit changed();
    }

    updateSelection();
}

void DreamWebEngine::processEvents() {
	if (_eventMan->shouldQuit()) {
		quit();
		return;
	}

	soundHandler();
	Common::Event event;
	int softKey, hardKey;
	while (_eventMan->pollEvent(event)) {
		switch(event.type) {
		case Common::EVENT_RTL:
			quit();
			break;
		case Common::EVENT_KEYDOWN:
			if (event.kbd.flags & Common::KBD_CTRL) {
				switch (event.kbd.keycode) {

				case Common::KEYCODE_d:
					_console->attach();
					_console->onFrame();
					break;

				case Common::KEYCODE_f:
					setSpeed(_speed != 20? 20: 1);
					break;

				case Common::KEYCODE_g:
					_turbo = !_turbo;
					break;

				case Common::KEYCODE_c: //skip statue puzzle
					_symbolBotNum = 3;
					_symbolTopNum = 5;
					break;

				default:
					break;
				}

				return; //do not pass ctrl + key to the engine
			}

			// Some parts of the ASM code uses the hardware key
			// code directly. We don't have that code, so we fake
			// it for the keys where it's needed and assume it's
			// 0 (which is actually an invalid value, as far as I
			// know) otherwise.

			hardKey = 0;

			switch (event.kbd.keycode) {
			case Common::KEYCODE_ESCAPE:
				hardKey = 1;
				break;
			case Common::KEYCODE_SPACE:
				hardKey = 57;
				break;
			default:
				hardKey = 0;
				break;
			}

			_lastHardKey = hardKey;

			// The rest of the keys are converted to ASCII. This
			// is fairly restrictive, and eventually we may want
			// to let through more keys. I think this is mostly to
			// keep weird glyphs out of savegame names.

			softKey = 0;

			if (event.kbd.keycode >= Common::KEYCODE_a && event.kbd.keycode <= Common::KEYCODE_z) {
				softKey = event.kbd.ascii & ~0x20;
			} else if (event.kbd.keycode == Common::KEYCODE_MINUS ||
				event.kbd.keycode == Common::KEYCODE_SPACE ||
				(event.kbd.keycode >= Common::KEYCODE_0 && event.kbd.keycode <= Common::KEYCODE_9)) {
				softKey = event.kbd.ascii;
			} else if (event.kbd.keycode >= Common::KEYCODE_KP0 && event.kbd.keycode <= Common::KEYCODE_KP9) {
				softKey = event.kbd.keycode - Common::KEYCODE_KP0 + '0';
			} else if (event.kbd.keycode == Common::KEYCODE_KP_MINUS) {
				softKey = '-';
			} else if (event.kbd.keycode == Common::KEYCODE_BACKSPACE ||
				event.kbd.keycode == Common::KEYCODE_DELETE) {
				softKey = 8;
			} else if (event.kbd.keycode == Common::KEYCODE_RETURN
				|| event.kbd.keycode == Common::KEYCODE_KP_ENTER) {
				softKey = 13;
			}

			if (softKey)
				keyPressed(softKey);
			break;
		default:
			break;
		}
	}
}

void UrRealtimeCommunication::run() {
	uint8_t buf[2048];
	int bytes_read;
	memset(buf, 0, 2048);
	struct timeval timeout;
	fd_set readfds;
	FD_ZERO(&readfds);
	FD_SET(sockfd_, &readfds);
	print_debug("Realtime port: Got connection");
	connected_ = true;
	while (keepalive_) {
		while (connected_ && keepalive_) {
			timeout.tv_sec = 0; //do this each loop as selects modifies timeout
			timeout.tv_usec = 500000; // timeout of 0.5 sec
			select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
			bytes_read = recv(sockfd_, (char*) buf, 2048, 0);
			if (bytes_read > 0) {
				setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
				robot_state_->unpack(buf);
				if (safety_count_ == safety_count_max_) {
					setSpeed(0., 0., 0., 0., 0., 0.);
				}
				safety_count_ += 1;
			} else {
				connected_ = false;
				CloseSocket(sockfd_);
			}
		}
		if (keepalive_) {
			//reconnect
            ofLog()<<"Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."<<endl;
			sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
			if (sockfd_ < 0) {
				print_fatal("ERROR opening socket");
			}
			flag_ = 1;
			setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
					sizeof(int));
			setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, 
					sizeof(int));
	
			setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
					sizeof(int));
			SetNonBlocking(sockfd_, true);
			while (keepalive_ && !connected_) {
				std::this_thread::sleep_for(std::chrono::seconds(10));
				fd_set writefds;

				connect(sockfd_, (struct sockaddr *) &serv_addr_,
						sizeof(serv_addr_));
				FD_ZERO(&writefds);
				FD_SET(sockfd_, &writefds);
				select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
				int flag_len;
				getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, (char*)&flag_, &flag_len);
				if (flag_ < 0) {
					print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
				} else {
					connected_ = true;
					print_info("Realtime port: Reconnected");
				}
			}
		}
	}
	setSpeed(0., 0., 0., 0., 0., 0.);
	CloseSocket(sockfd_);
}

void MpvHandler::Speed(double d)
{
    if(playState > 0)
        mpv_set_property_async(mpv, MPV_REPLY_PROPERTY, "speed", MPV_FORMAT_DOUBLE, &d);
    setSpeed(d);
}

Robot::~Robot()
{
  setSpeed(0, 0);
}

Scene* Chapter10_5::createScene()
{
    //cocos2d::Rect visibleRect = Director::getInstance()->getOpenGLView()->getVisibleRect();
    Size visibleSize = Director::getInstance()->getVisibleSize();
    Vec2 origin = Director::getInstance()->getVisibleOrigin();
    auto winSize = Director::getInstance()->getWinSize();
    
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // create a scene
    // 'scene' is an autorelease object
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    auto scene = Scene::create();
    
    // add title
    auto label = LabelTTF::create("camera test", "Arial", 24);
    label->setPosition(Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).x,
                       Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height).y - 30);
    
    scene->addChild(label, -1);
    
    //add the menu item for back to main menu
    label = LabelTTF::create("MainMenu", "Arial", 24);
    
    auto menuItem = MenuItemLabel::create(label);
    menuItem->setCallback([&](cocos2d::Ref *sender) {
        Director::getInstance()->replaceScene(Chapter10::createScene());
    });
    auto menu = Menu::create(menuItem, nullptr);
    
    menu->setPosition( Vec2::ZERO );
    menuItem->setPosition( Vec2( Vec2(origin.x+visibleSize.width, origin.y+visibleSize.height/2).x - 80, Vec2(origin.x+visibleSize.width/2, origin.y).y + 25) );
    
    scene->addChild(menu, 1);
    
    auto layer3D=Layer::create();
    scene->addChild(layer3D,2);
    
    std::string fileName = "orc.c3b";
    
    auto sprite = Sprite3D::create(fileName);
    sprite->setScale(5.f);
    sprite->setRotation3D(Vec3(0,180,0));
    sprite->setPosition( Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).x,
                        Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).y );
    
    // play animation
    auto animation = Animation3D::create(fileName);
    if (animation)
    {
        auto animate = Animate3D::create(animation);
        animate->setSpeed(1);
        sprite->runAction(RepeatForever::create(animate));
    }
    
    //add to scene
    layer3D->addChild(sprite);
    
    // add camera
    auto camera=Camera::createPerspective(60, (GLfloat)winSize.width/winSize.height, 1, 1000);
    camera->setCameraFlag(CameraFlag::USER1);// set camera flag
    camera->setPosition3D(Vec3(0, 0, 230) + sprite->getPosition3D());
    camera->lookAt(sprite->getPosition3D(), Vec3(0,1,0));
    
    // create camera action
    auto action = MoveBy::create(3, Vec2(100, 0));
    auto action_back = action->reverse();
    auto action1 = MoveBy::create(3, Vec2(0, 100));
    auto action_back1 = action1->reverse();
    auto seq = Sequence::create( action, action_back, action1, action_back1, nullptr );
    
    // run camera action
    camera->runAction( RepeatForever::create(seq) );
    
    layer3D->addChild(camera);
    
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // set camera mask
    // when node's camera-mask & camer-flag result is true, the node is visible for this camera.
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    layer3D->setCameraMask(0x2);
    
    // return the scene
    return scene;
}

Robot::Robot()
  : m_wheelLeft(0), m_wheelRight(0)
{
  high(26);
  setSpeed(m_wheelLeft, m_wheelRight);
}

Tricycle::Tricycle(int initialSpeed)
{
	setSpeed(initialSpeed);
}

Enemy::Enemy() {
	// TODO 自動生成されたコンストラクター・スタブ
	setSpeed(0);
	mCount = 0;
}