void QuaduinoFlight::init() {
	motorsActive = false;

	minPulseWidth = 1100;
	maxPulseWidth = 1600;
	gravityHover = 200;
	frontMotorOffset = 11;
	rearMotorOffset = 5;
	rightMotorOffset = 7;
	leftMotorOffset = 9;

	consKp = 0.3;
	consKi = 0.003;
	consKd = 0.192;

	aggKp = 0.93;
	aggKi = 0.1;
	aggKd = 0.2;

	angleToSwap = 15;

        writeMotor(FRONT, 0);
        writeMotor(RIGHT, 0);
        writeMotor(REAR, 0);
        writeMotor(LEFT, 0);

	yawPID = &PID(&yawInput, &yawOutput, &yawSetpoint, 4.0, 0.0, 0.0, DIRECT);
	pitchPID = &PID(&pitchInput, &pitchOutput, &pitchSetpoint, 4.0, 0.0, 0.0, DIRECT);
	rollPID = &PID(&rollInput, &rollOutput, &rollSetpoint, 4.0, 0.0, 0.0, DIRECT);

	setupPidControl();
}

float getDesiredMotorPWM(float setpoint, float measurement, volatile isNewMeasurementAvailable_t* isNewMeasurementAvailable, uint8_t channel)
{
	static struct persistantPIDVariables_s PIDVariables[] = {
	{ 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0 } };

	if (*isNewMeasurementAvailable == NEW_MEASUREMENT_AVAILABLE)
	{
		*isNewMeasurementAvailable = NO_NEW_MEASUREMENT_AVAILABLE;

		float dt = (TFC_Ticker[2] / 10000.0f);
		TFC_Ticker[2] = 0;
		if(dt > 0.1f)
		{
			dt = 0.1f;
		}
		
		// Integral wind-up protection
		if (PIDVariables[channel].PWM < MAX_PWM && PIDVariables[channel].PWM > MIN_PWM)
		{
			PID(setpoint, measurement, &PIDVariables[channel], INCLUDE_INTEGRAL, dt);
		}
		else
		{
			PID(setpoint, measurement, &PIDVariables[channel], NO_INTEGRAL, dt);
		}

		
		if (PIDVariables[channel].PWM < MIN_PWM)
			PIDVariables[channel].PWM = MIN_PWM;
		else if (PIDVariables[channel].PWM > MAX_PWM)
			PIDVariables[channel].PWM = MAX_PWM;
	}
	return PIDVariables[channel].PWM;
}

void runPIDs(){

        PID1.measuredOutput = Q15(v1) ;
        PID2.measuredOutput = Q15(v2) ;

        PID(&PID1);                           /*Call the PID controller using the new measured input */
        PID(&PID2);                           /*Call the PID controller using the new measured input */
                                                /*The user may place a breakpoint on "PID(&fooPID)", halt the debugger,*/
                                                /*tweak the measuredOutput variable within the watch window */
                                                /*and then run the debugger again */
}

static int
progwstat(Chan *c, uchar *db, int n)
{
	Dir d;
	Prog *p;
	char *u;
	Osenv *o;

	if(c->qid.type&QTDIR)
		error(Eperm);
	acquire();
	p = progpid(PID(c->qid));
	if(p == nil) {
		release();
		error(Ethread);
	}

	u = up->env->user;
	o = p->osenv;
	if(strcmp(u, o->user) != 0 && strcmp(u, eve) != 0) {
		release();
		error(Eperm);
	}

	n = convM2D(db, n, &d, nil);
	if(n == 0){
		release();
		error(Eshortstat);
	}
	if(d.mode != ~0UL)
		o->pgrp->progmode = d.mode&0777;
	release();
	return n;
}

void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
  int32_t pwm;
  MOT_Direction direction=MOT_DIR_FORWARD;
  MOT_MotorDevice *motHandle;

  pwm = PID(currPos, setPos, config);
  /* transform into motor pwm */
  pwm *= 1000; /* scale PID, otherwise we need high PID constants */
  if (pwm>=0) {
    direction = MOT_DIR_FORWARD;
  } else { /* negative, make it positive */
    pwm = -pwm; /* make positive */
    direction = MOT_DIR_BACKWARD;
  }
  /* pwm is now always positive, make sure it is within 16bit PWM boundary */
  if (pwm>0xFFFF) {
    pwm = 0xFFFF;
  }
  /* send new pwm values to motor */
  if (isLeft) {
    motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
  } else {
    motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
  }
  MOT_SetVal(motHandle, 0xFFFF-pwm); /* PWM is low active */
  MOT_SetDirection(motHandle, direction);
  MOT_UpdatePercent(motHandle, direction);
}

/* Move() 
 * client:  client to connect to robot
 * distance: x-coordinate that robot should aim for;
 * angle: angle that robot should stay at;
 */
float Move(playerc_client_t *client, float distance, float angle)
{
	printf("Enter Move\n");

	error_x = error_tx(position2d, distance);   
  
	while(error_x > 0.1)
	{
		error_x = error_tx(position2d, distance);
		vx = PID(error_x);

		error_a = error_ta(position2d, angle);
		va = PID_A(error_a);

		playerc_position2d_set_cmd_vel(position2d, vx, 0, va, 1.0);
	   
		if(bumper->bumpers[0]!=0 || bumper->bumpers[1]!=0)
		{
			playerc_position2d_set_cmd_vel(position2d, 0.0, 0.0, 0.0, 0.0);
			break;
		}
		playerc_client_read(client);
		printf("Moving : x = %f y = %f a = %f\n", position2d->px, position2d->py, position2d->pa);   
	}
	printf("Leave Move\n");
}

TankDrive::TankDrive(int speedPinLeft, int speedPinRight, int forwardPinRight, int reversePinRight, int forwardPinLeft, int reversePinLeft)
{
	// connect motor controller pins to Arduino digital pins
	// motor Left
	_speedPinLeft = speedPinLeft;
	_forwardPinLeft = forwardPinLeft;
	_reversePinLeft = reversePinLeft;
	// motor two
	_speedPinRight = speedPinRight;
	_forwardPinRight = forwardPinRight;
	_reversePinRight = reversePinRight;

	// set all the motor control pins to outputs
	pinMode(speedPinLeft, OUTPUT);
    pinMode(speedPinRight, OUTPUT);
    pinMode(forwardPinLeft, OUTPUT);
    pinMode(reversePinLeft, OUTPUT);
    pinMode(forwardPinRight, OUTPUT);
    pinMode(reversePinRight, OUTPUT);

    _usePID=1; //Will not use PID by default.

    PID MotorPID = PID();
    Trapezoid TrapPath = Trapezoid();
}

int32 MotorCtrlUsePid()
{
	FreecaleConfig.Config.Motor.Pid.Pid.Now = Speed.Acturally;
	FreecaleConfig.Config.Motor.Pid.Pid.Target = Speed.Expect;
	
	return PID(&FreecaleConfig.Config.Motor.Pid.Pid);
}

void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
  int32_t pid, speed, speedL, speedR;
  MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;

  pid = PID(currLine, setLine, config);
  /*! \todo Apply PID values to speed values */

  speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
  pid = Limit(pid, -speed, speed);
  if (pid<0) { /* turn right */
    speedR = speed+pid;
    speedL = speed-pid;
  } else { /* turn left */
    speedR = speed+pid;
    speedL = speed-pid;
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speedL>0xFFFF) {
    speedL = 0xFFFF;
  } else if (speedL<0) {
    speedL = 0;
  }
  if (speedR>0xFFFF) {
    speedR = 0xFFFF;
  } else if (speedR<0) {
    speedR = 0;
  }
  /* send new speed values to motor */
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}

void loop()
{
  Bluetooth();
  Serial.print("CMSET:");
  Serial.print(cmset);
  Serial.println("cm");
  Serial.println();
delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  Serial.print("Ping: ");
  cm=uS / US_ROUNDTRIP_CM;
  Serial.print("high:");
  Serial.print(cm); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
//val=analogRead(potpin);// 读取传感器的模拟值并赋值给val
//Serial.println(val);//显示val 变量
//pwmval=val/4;
  PID();
  analogWrite(pwmpin,tempctrl);// 输出PWM（PWM 输出最大值255）
  Serial.print ("pwmval:");
  Serial.println (tempctrl);
  Serial.println ();
  delay(500);//延时0.01 秒
  lcd();

}

void fight()
{
  PID();
  if (g_diff > 30 || g_diff < -30) turn(g_pid);
  if (getBall(0) > g_debugBall[0] && getBall(0) > getBall(1) && getBall(0) >= getBall(2) && getBall(0) >= getBall(3) && getBall(0) >= getBall(4) && getBall(0) >= getBall(5) && getBall(0) >= getBall(6) && getBall(0) > getBall(7) ) moveAngle4ch(0,0,0);


  else if(getBall(1) > g_debugBall[1] && getBall(1) >= getBall(2) && getBall(1) > getBall(3) && getBall(1) > getBall(4) && getBall(1) > getBall(5) && getBall(1) > getBall(6) && getBall(1) > getBall(7))
  {
    //if (getPing(3) < 15) moveAngle4ch(50 , 0, 0); 
    if (getBall(1) > 600) moveAngle4ch(50, 0, 95);
    else moveAngle4ch(85 , 0, 95);
  } 
  else if(getBall(2) > g_debugBall[2] && getBall(2) >= getBall(3) && getBall(2) > getBall(4) && getBall(2) > getBall(5) && getBall(2) > getBall(6) && getBall(2) > getBall(7)) 
  {
    if(getBall(2) > 600)moveAngle4ch(50 , 0, 95);
    else moveAngle4ch(85, 0, 95);
  }
  else if (getBall(6) >  g_debugBall[6] && getBall(6) >= getBall(5)) 
  {
    if (getBall(6) > 600)moveAngle4ch(50 , 0, 265);
    else moveAngle4ch(85, 0, 265);
  }
  else if(getBall(7) > g_debugBall[7] && getBall(7) > getBall(1)) 
  {
    //if(getPing(1) < 15) moveAngle4ch(50 , 0, 0);
    if (getBall(7) > 600) moveAngle4ch(50, 0, 265);
    else moveAngle4ch(85, 0, 265);
  }
  else moveAngle4ch(0,0,0);
}

void FCInitOne()
{


   delay_nms(1000);

   EeSave1.Solo.BadSave = 0;
   EeReadTrg(&EeSave1);
   EePromDeal(&FC1.Run, sizeof(FC_RUNING_Def) - CRC_LEN, &EeSave1);
   if (EeSave1.Solo.BadSave)
   {
      uint08 tmp;

      uint16 *pdata = (uint16 *)&FC1.Run;
      EeSave1.Solo.BadSave = 0;

      for (tmp = 0; tmp < FC_SETTING_MAX; tmp++)
      {
         pdata[tmp] = FCParCfg[tmp].Default;
      }
   }
   EeWriteTrg(&EeSave1);


   FC1.State.Sys.All = 0;


   PID(&FC1.FCpid, &FC1.Run.FCpidPar);

   FC1.Buff.RoomTempAid = 200;
   FC1.Buff.DiTreat.All = 0;

}

void Program::startProgram() {

    time = 0;

//  Comms *c = new Comms();
//  c->openSerial();

    // Load config
    c.loadConfig();

    // Start oven controller
    oven = new OvenController();
    connect(oven, SIGNAL(updateTemp(float)), this, SLOT(updatePID(float)));
    oven->setConfig(&c);
    oven->openSerial();

    // Create PID controller
    pid = PID(&c);
    pid.initialise();

    // Create & connect timer
    timer = new QTimer(this);
    connect(timer,  SIGNAL(timeout()), this, SLOT(updateTick()));
    timer->start((1 / c.getSampleRate()) * 1000);
}

Client::Client ()
    : accumulatedLag(0), zombie(false), allowedToDisconnect(true), ready(false), mute(false),
      accountID(0), playerID(0), securityLevel(0), superclient(false),
      name(""), waypointEffectID(0), waypointIsDisplaying(false),
      pathEffectID(0), pathPath(NULL), pathIsDisplaying(false),
      locationEffectID(0), locationIsDisplaying(false),cheatMask(NO_CHEAT)
{
    actor           = 0;
    target          = 0;
    exchangeID      = 0;
    advisorPoints   = 0;
    lastInviteTime  = 0;
    spamPoints      = 0;
    clientnum       = 0;
    detectedCheatCount = 0;

    nextFloodHistoryIndex = 0;
    
    lastInventorySend = 0;
    lastGlyphSend = 0;

    isAdvisor           = false;
    isFrozen            = false;
    lastInviteResult    = true;
    hasBeenWarned       = false;
    hasBeenPenalized    = false;
    valid               = false;
    isBuddyListHiding   = false;

    // pets[0] is a special case for the players familiar.
    pets.Insert(0, PID(0));
}

void PID_PosCfg(int16_t currPos, int16_t setPos, bool isLeft, PID_Config *config) {
  int32_t pid, speed;
  MOT_Direction direction=MOT_DIR_FORWARD;
  
  pid = PID(currPos, setPos, config);

  /* transform into motor speed */
  speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* limit the speed */
  pid = Limit(pid, -speed, speed);
  if (pid<0) { /* move forward */
    speed -= pid;
    direction = MOT_DIR_FORWARD;
  } else { /* move backward */
    speed += pid;
    direction = MOT_DIR_BACKWARD;
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speed>0xFFFF) {
    speed = 0xFFFF;
  } else if (speed<0) {
    speed = 0;
  }
  /* send new speed values to motor */
  if (isLeft) {
    MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speed); /* PWM is low active */
    MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), direction);
  } else {
    MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speed); /* PWM is low active */
    MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), direction);
  }
}

void PID_SpeedCfg(int32_t currSpeed, int32_t setSpeed, bool isLeft, PID_Config *config) {
  int32_t speed;
  MOT_Direction direction=MOT_DIR_FORWARD;
  MOT_MotorDevice *motHandle;
  
  speed = PID(currSpeed, setSpeed, config);
  if (speed>=0) {
    direction = MOT_DIR_FORWARD;
  } else { /* negative, make it positive */
    speed = -speed; /* make positive */
    direction = MOT_DIR_BACKWARD;
  }
  /* speed shall be positive here, make sure it is within 16bit PWM boundary */
  if (speed>0xFFFF) {
    speed = 0xFFFF;
  }
  /* send new speed values to motor */
  if (isLeft) {
    motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
  } else {
    motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
  }
  MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
  MOT_SetDirection(motHandle, direction);
  MOT_UpdatePercent(motHandle, direction);
}

fractional pidHWRun(tPID* controller, fractional feedback) {

    controller->measuredOutput = feedback;
    PID(controller);

    return controller->controlOutput;
}

void Keeper()
{
  static int g_loop = 0;
  getMaxball();
  PID();
  if (g_diff > 40 || g_diff < -40) turn(g_pid);
  else
  {
    g_integral == 0;
    if(g_no == 0 && g_ball[0] > 300) 
    {
      moveAngle( 80, 0 , 0);
      kick();
    } 
    else if(g_no == 1 && g_ball[1] > 300) moveAngle( 80 , 0 , 270 );

    else if(g_no == 2 && g_ball[2] > 300) moveAngle( 80 , 0 , 270 );

    else if(g_no == 3 && g_ball[3] > 300) moveAngle( 50 , 0 , 315 );

    else if(g_no == 4 && g_ball[4] > 300) moveAngle( 0 , 0 , 180 );

    else if(g_no == 5 && g_ball[5] > 300) moveAngle( 50 , 0 , 45 );

    else if(g_no == 6 && g_ball[6] > 300) moveAngle( 80 , 0 , 80 );

    else if(g_no == 7 && g_ball[7] > 300) moveAngle( 80 , 0 , 80 );

    else goHome();

  }
  g_loop++;
}

int is_provides_true(Node n) {
    int i;
    int num_resources = 0;
    peos_resource_t* resources;
    peos_resource_t* proc_resources;
    peos_context_t* context = peos_get_context(PID(n));
    resources = get_resource_list_action_provides(PID(n), n->name, &num_resources);
    if (context && context->num_resources > 0) {
        proc_resources = (peos_resource_t *) calloc(context->num_resources, sizeof(peos_resource_t));
       for (i = 0; i < context->num_resources; i++) {
            strcpy(proc_resources[i].name, context->resources[i].name);
         strcpy(proc_resources[i].value, context->resources[i].value);
       }
       eval_resource_list(&proc_resources, context->num_resources);
       fill_resource_list_value(proc_resources, context->num_resources, &resources, num_resources);
    }
    return eval_predicate(resources, num_resources, n->provides);
}

	SlottedPage* SSI::operator->() const {
		if (page == nullptr) {
			SSI* that = const_cast<SSI*>(this);
			that->frame = &segment->fixPage(PID(segment->getID(), pageId), false);
			that->page = new SlottedPage(segment, *frame);
		}

		return page;
	}