Example #1
0
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;
}
Example #3
0
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 */
}
Example #4
0
File: devprog.c Project: 8l/inferno
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;
}
Example #5
0
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);
}
Example #6
0
/* 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");
}
Example #7
0
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);
}
Example #9
0
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);
}
Example #12
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;

}
Example #13
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);
}
Example #14
0
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));
}
Example #15
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);
  }
}
Example #16
0
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);
}
Example #17
0
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++;
}
Example #19
0
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);
}
Example #20
0
	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;
	}
Example #21
0
void Set_Speed(int v) 
{
  v_PID = PID(v);
  Motor_PWM = Motor_PWM+v_PID;
  if(Motor_PWM > Speed_Max)  Motor_PWM = Speed_Max;
  else if(Motor_PWM < -Speed_Max)  Motor_PWM = -Speed_Max;  
  /////////////////////////////////////////////////////////////////////////////
  SetSpeed(Motor_PWM); 
}
Example #22
0
void Client::SetFamiliar( gemActor *familiar )
{
    if ( familiar )
    {
        pets[0] = familiar->GetPID();
    }
    else
    {
        pets[0] = PID(0);
    }
}
Example #23
0
vm_exit_code set_act_state_graph(Graph g, char *action, vm_act_state state)
{
    int error;
    Node n;
    switch(state)
    {
        case ACT_DONE:
            return action_done(g,action);
        case ACT_READY:
            n = find_node(g,action);
            if (n!=NULL) {
                set_node_state(n, ACT_READY, &error);
                if (!error)
                    return VM_CONTINUE;
            }
            return VM_INTERNAL_ERROR;
        case ACT_RUN:
            if (action_run(g,action) >  0)
                return VM_CONTINUE;
            return VM_INTERNAL_ERROR;
        case ACT_NONE:
            n = find_node(g,action);
            if (n != NULL) {
                set_node_state(n, ACT_NONE, &error);
                if (!error)
                    return VM_CONTINUE;
            }
            return VM_INTERNAL_ERROR;
        case ACT_SUSPEND:
            n = find_node(g,action);
            if (n != NULL) {
                set_node_state(n, ACT_SUSPEND, &error);
                if (!error)
                    return VM_CONTINUE;
            }
            return VM_INTERNAL_ERROR;
        case ACT_ABORT:
            n = find_node(g,action);
            if (n != NULL) {
                if (STATE(n) == ACT_RUN) {
                    set_node_state(n, ACT_NONE, &error);
                    if (error)
                        return VM_INTERNAL_ERROR;
                    update_process_state(PID(n));
                }
                return VM_CONTINUE;
            }
            return VM_INTERNAL_ERROR;
        default :
            fprintf(stderr, "Error Changing Action : Invalid Action State\n");
            return -1;
    }
}
Example #24
0
Autopilot::Autopilot(FreeBoardModel* model) {
	this->model = model;
	autopilotTargetHeading = model->getAutopilotTargetHeading() + 720;
	autopilotCurrentHeading = autopilotTargetHeading + model->getAutopilotOffCourse();
	autopilotRudderCommand = model->getAutopilotRudderCommand();
	lastDirection=true;
	this->headingPid = PID(&autopilotCurrentHeading, &autopilotRudderCommand, &autopilotTargetHeading, P_Param, I_Param, D_Param, REVERSE);

	headingPid.SetOutputLimits(0.0, 66.0); //output limits
	headingPid.SetSampleTime(100);

}
Example #25
0
QString TSPacket::toString() const {
    QString str;
    str.append("TSPacket @0x%1  ").arg(long(&data()[0]),0,16);
    str.append("raw: 0x%1 0x%2 0x%3 0x%4\n").arg(int(data()[0]),0,16).
        arg(int(data()[1]),0,16).arg(int(data()[2]),0,16).arg(int(data()[3]),0,16);
    str.append("                 inSync: %1\n").arg( HasSync());
    str.append("         transportError: %1\n").arg( TransportError());
    str.append("           payloadStart: %1\n").arg( PayloadStart() );
    str.append("               priority: %1\n").arg( Priority() );
    str.append("                    pid: %1\n").arg( PID() );
    str.append("              scrampled: %1\n").arg( ScramplingControl() );
    str.append(" adaptationFieldControl: %1\n").arg( AdaptationFieldControl() );
    str.append("      continuityCounter: %1\n").arg( ContinuityCounter() );
    return str;
}
Example #26
0
File: branch.c Project: besco/binkd
static void *thread_start(void *arg)
{
  void (*F) (void*);
  void *args;

  F = ((thread_args_t *)arg)->F;
  args = ((thread_args_t *)arg)->args;
#ifdef HAVE_GETTID
  ((thread_args_t *)arg)->tid = PID();
#endif
  ReleaseSem(&((thread_args_t *)arg)->mutex);
  pthread_detach(pthread_self());
  F(args);
  return NULL;
}
Example #27
0
void CPIDCreateDlg::OnGridEndEdit(NMHDR *pNotifyStruct, LRESULT* pResult)
{
	pids.clear();

	for (int row = 1; row < m_cPidTable.GetRowCount(); ++row) {
		CString startTemp = m_cPidTable.GetItemText(row, 1);
		CString targetTemp = m_cPidTable.GetItemText(row, 2);
		CString kp = m_cPidTable.GetItemText(row, 3);
		CString kd = m_cPidTable.GetItemText(row, 4);
		CString ki = m_cPidTable.GetItemText(row, 5);

		pids.push_back( PID( _wtof(startTemp), _wtof(targetTemp), _wtof(kp), _wtof(kd), _wtof(ki) ) );
	}

	*pResult = 0;
}
Example #28
0
File: PID.c Project: zanderdk/P5
    S32 
    MotorPID(S32 target, U8 motor)
    {
        static S32 integrale[] = {0, 0, 0, 0};
        static S32 lastError[] = {0, 0, 0, 0};
        static S32 lastTarget[] = {0, 0, 0, 0};

        if(lastTarget[motor] != target)
        {
            integrale[motor] = 0;
            lastError[motor] = 0;
            lastTarget[motor] = target;            
        }

    	register S32 current = nxt_motor_get_count(motor);
    	register S32 getSpeed = PID(target, current, &integrale[motor], &lastError[motor]);
    	nxt_motor_set_speed(motor, getSpeed, 0);
    	return getSpeed;
    }
Example #29
0
void CPIDCreateDlg::OnBnClickedButtonPidCreateAdd()
{
	if( !UpdateData() )
		return;

	if( m_cStartTemp == m_cTargetTemp )
	{
		AfxMessageBox(L"시작온도와 타겟온도는 같은 값이 될 수 없습니다.");
		return;
	}

	int pidSize = pids.size();
	
	for(int i=0; i<pidSize; ++i){
		PID pid = pids[i];
		
		if( pid.startTemp == m_cStartTemp &&
			pid.targetTemp == m_cTargetTemp ){
				AfxMessageBox(L"이미 존재하는 pid parameter 입니다.");
				return;
		}
	}

	// Row size 를 변경한다
	m_cPidTable.SetRowCount(pidSize+2);

	// 현재 값을 저장한다. 
	// 제거할 수 있는 콤보 박스에도 넣어준다.
	pids.push_back( PID(m_cStartTemp, m_cTargetTemp, m_cKp, m_cKi, m_cKd) );

	CString label;
	label.Format(L"PID #%d", pidSize+1);
	m_cRemovalList.AddString(label);

	initPidTable();
	loadPidTable();

	m_cStartTemp = m_cTargetTemp = 25;
	m_cKp = m_cKd = m_cKi = 0;

	// 초기화된 변수 값을 control 에 넘겨준다.
	UpdateData(false);
}
Example #30
0
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
  int32_t speed, val;
  MOT_Direction direction=MOT_DIR_FORWARD;
  MOT_MotorDevice *motHandle;
  int error;
  #define POS_FILTER 5

  error = setPos-currPos;
  if (error>-POS_FILTER && error<POS_FILTER) { /* avoid jitter around zero */
    setPos = currPos;
  }
  speed = PID(currPos, setPos, config);
  /* transform into motor speed */
  speed *= 1000; /* scale PID, otherwise we need high PID constants */
  if (speed>=0) {
    direction = MOT_DIR_FORWARD;
  } else { /* negative, make it positive */
    speed = -speed; /* make positive */
    direction = MOT_DIR_BACKWARD;
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speed>0xFFFF) {
    speed = 0xFFFF;
  }
#if 1
  /* limit speed to maximum value */
  val = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
  speed = Limit(speed, -val, val);
#else
  /* limit speed to maximum value */
  speed = (speed*config->maxSpeedPercent)/100;
#endif
  /* 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);
}