Ejemplo n.º 1
0
int prIdentDict_PutGet(struct VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a, *b, *c, *d, *slot, *newslot;
	int i, index, size;
	PyrObject *dict;
	PyrObject *array;

	a = g->sp - 2;  // dict
	b = g->sp - 1;	// key
	c = g->sp;		// value
	d = ++g->sp;	// push the stack to save the receiver

	slotCopy(d,a);
	dict = slotRawObject(d);
	array = slotRawObject(&dict->slots[ivxIdentDict_array]);
	if (!isKindOf((PyrObject*)array, class_array)) {
		SetNil(a);
		--g->sp;
		return errFailed;
	}

	index = arrayAtIdentityHashInPairs(array, b);
	slot = array->slots + index;
	slotCopy(a,&slot[1]);
	slotCopy(&slot[1],c);
	g->gc->GCWrite(array, c);
	if (IsNil(slot)) {
		slotCopy(slot,b);
		g->gc->GCWrite(array, b);
		size = slotRawInt(&dict->slots[ivxIdentDict_size]) + 1;
		SetRaw(&dict->slots[ivxIdentDict_size], size);
		if (array->size < size*3) {
			PyrObject *newarray;
			newarray = newPyrArray(g->gc, size*3, 0, true);
			newarray->size = ARRAYMAXINDEXSIZE(newarray);
			nilSlots(newarray->slots, newarray->size);
			slot = array->slots;
			for (i=0; i<array->size; i+=2, slot+=2) {
				if (NotNil(slot)) {
					index = arrayAtIdentityHashInPairs(newarray, slot);
					newslot = newarray->slots + index;
					slotCopy(&newslot[0],&slot[0]);
					slotCopy(&newslot[1],&slot[1]);
				}
			}
			SetRaw(&dict->slots[ivxIdentDict_array], newarray);
			g->gc->GCWriteNew(dict, newarray); // we know newarray is white so we can use GCWriteNew
		}
	}
	--g->sp;
	return errNone;
}
Ejemplo n.º 2
0
int identDictPut(struct VMGlobals *g, PyrObject *dict, PyrSlot *key, PyrSlot *value)
{
	PyrSlot *slot, *newslot;
	int i, index, size;
	PyrObject *array;

	bool knows = IsTrue(dict->slots + ivxIdentDict_know);
	if (knows && IsSym(key)) {
		if (slotRawSymbol(key) == s_parent) {
			slotCopy(&dict->slots[ivxIdentDict_parent],value);
			g->gc->GCWrite(dict, value);
			return errNone;
		}
		if (slotRawSymbol(key) == s_proto) {
			slotCopy(&dict->slots[ivxIdentDict_proto],value);
			g->gc->GCWrite(dict, value);
			return errNone;
		}
	}
	array = slotRawObject(&dict->slots[ivxIdentDict_array]);
	if (array->IsImmutable()) return errImmutableObject;
	if (!isKindOf((PyrObject*)array, class_array)) return errFailed;

	index = arrayAtIdentityHashInPairs(array, key);
	slot = array->slots + index;
	slotCopy(&slot[1],value);
	g->gc->GCWrite(array, value);
	if (IsNil(slot)) {
		slotCopy(slot,key);
		g->gc->GCWrite(array, key);
		size = slotRawInt(&dict->slots[ivxIdentDict_size]) + 1;
		SetRaw(&dict->slots[ivxIdentDict_size], size);
		if (array->size < size*3) {
			PyrObject *newarray;
			newarray = newPyrArray(g->gc, size*3, 0, false);
			newarray->size = ARRAYMAXINDEXSIZE(newarray);
			nilSlots(newarray->slots, newarray->size);
			slot = array->slots;
			for (i=0; i<array->size; i+=2, slot+=2) {
				if (NotNil(slot)) {
					index = arrayAtIdentityHashInPairs(newarray, slot);
					newslot = newarray->slots + index;
					slotCopy(&newslot[0],&slot[0]);
					slotCopy(&newslot[1],&slot[1]);
				}
			}
			SetRaw(&dict->slots[ivxIdentDict_array], newarray);
			g->gc->GCWriteNew(dict, newarray); // we know newarray is white so we can use GCWriteNew
		}
	}
	return errNone;
}
Ejemplo n.º 3
0
int mathWrapInt(struct VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a, *b, *c;
	int err;

	a = g->sp - 2;
	b = g->sp - 1;
	c = g->sp;

	if (IsSym(b)) {
		*a = *b;
	} else if (IsSym(c)) {
		*a = *c;
	} else if (IsInt(b) && IsInt(c)) {
		SetRaw(a, sc_mod((int)(slotRawInt(a) - slotRawInt(b)), (int)(slotRawInt(c) - slotRawInt(b) + 1)) + slotRawInt(b));
	} else {
		double x, lo, hi;
		x = slotRawInt(a);
		err = slotDoubleVal(b, &lo);
		if (err) return err;
		err = slotDoubleVal(c, &hi);
		if (err) return err;
		SetFloat(a, sc_mod(x - lo, hi - lo) + lo);
	}
	return errNone;
}
Ejemplo n.º 4
0
// This is an internal helper function which assumes no NULL args are passed.
// It sets the given attribute values for a single entry at the given index.
static int SetEntryAttributes(struct drive *drive,
                              uint32_t index,
                              CgptAddParams *params) {
  if (params->set_raw) {
    SetRaw(drive, PRIMARY, index, params->raw_value);
  } else {
    if (params->set_successful)
      SetSuccessful(drive, PRIMARY, index, params->successful);
    if (params->set_tries)
      SetTries(drive, PRIMARY, index, params->tries);
    if (params->set_priority)
      SetPriority(drive, PRIMARY, index, params->priority);
  }

  // New partitions must specify type, begin, and size.
  if (IsUnused(drive, PRIMARY, index)) {
    if (!params->set_begin || !params->set_size || !params->set_type) {
      Error("-t, -b, and -s options are required for new partitions\n");
      return -1;
    }
    if (GuidIsZero(&params->type_guid)) {
      Error("New partitions must have a type other than \"unused\"\n");
      return -1;
    }
  }

  return 0;
}
Ejemplo n.º 5
0
int prSymbolAsSetter(struct VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a;
	char str[256];
	int len;

	a = g->sp;
	if (!(slotRawSymbol(a)->flags & sym_Setter)) {
		if ((slotRawSymbol(a)->flags & sym_Class) || (slotRawSymbol(a)->flags & sym_Primitive)) {
			error("Cannot convert class names or primitive names to setters.\n");
			return errFailed;
		}
		if (strlen(slotRawSymbol(a)->name)>255) {
			error("symbol name too long.\n");
			return errFailed;
		}
		strcpy(str, slotRawSymbol(a)->name);
		len = strlen(str);
		str[len] = '_';
		str[len+1] = 0;

		//postfl("prSymbolAsSetter %s\n", str);
		SetRaw(a, getsym(str));
	}
	return errNone;
}
Ejemplo n.º 6
0
int mathFoldInt(struct VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a, *b, *c;
	int err;

	a = g->sp - 2;
	b = g->sp - 1;
	c = g->sp;

	if (IsSym(b)) {
		*a = *b;
	} else if (IsSym(c)) {
		*a = *c;
	} else if (IsInt(b) && IsInt(c)) {
		SetRaw(a, sc_fold(slotRawInt(a), slotRawInt(b), slotRawInt(c)));
	} else {
		double x, lo, hi;
		x = slotRawInt(a);
		err = slotDoubleVal(b, &lo);
		if (err) return err;
		err = slotDoubleVal(c, &hi);
		if (err) return err;
		SetFloat(a, sc_fold(x, lo, hi));
	}
	return errNone;
}
Ejemplo n.º 7
0
void RarTime::SetAgeText(char *TimeText)
{
  uint Seconds=0,Value=0;
  for (int I=0;TimeText[I]!=0;I++)
  {
    int Ch=TimeText[I];
    if (IsDigit(Ch))
      Value=Value*10+Ch-'0';
    else
    {
      switch(etoupper(Ch))
      {
        case 'D':
          Seconds+=Value*24*3600;
          break;
        case 'H':
          Seconds+=Value*3600;
          break;
        case 'M':
          Seconds+=Value*60;
          break;
        case 'S':
          Seconds+=Value;
          break;
      }
      Value=0;
    }
  }
  SetCurrentTime();
  int64 RawTime=GetRaw();
  SetRaw(RawTime-INT32TO64(0,Seconds)*10000000);
}
Ejemplo n.º 8
0
/**
 * Set the PWM value based on a position.
 *
 * This is intended to be used by servos.
 *
 * @pre SetMaxPositivePwm() called.
 * @pre SetMinNegativePwm() called.
 *
 * @param pos The position to set the servo between 0.0 and 1.0.
 */
void PWM::SetPosition(float pos)
{
    if (pos < 0.0)
    {
        pos = 0.0;
    }
    else if (pos > 1.0)
    {
        pos = 1.0;
    }

    INT32 rawValue;
    // note, need to perform the multiplication below as floating point
    // before converting to int
    rawValue =
        (INT32) ((pos * (float)GetFullRangeScaleFactor()) +
                 GetMinNegativePwm());

    wpi_assert((rawValue >= GetMinNegativePwm())
               && (rawValue <= GetMaxPositivePwm()));
    wpi_assert(rawValue != kPwmDisabled);

    // send the computed pwm value to the FPGA
    SetRaw((UINT8) rawValue);
}
Ejemplo n.º 9
0
/**
 * Set the PWM value based on a speed.
 *
 * This is intended to be used by speed controllers.
 *
 * @pre SetMaxPositivePwm() called.
 * @pre SetMinPositivePwm() called.
 * @pre SetCenterPwm() called.
 * @pre SetMaxNegativePwm() called.
 * @pre SetMinNegativePwm() called.
 *
 * @param speed The speed to set the speed controller between -1.0 and 1.0.
 */
void PWM::SetSpeed(float speed) {
  if (StatusIsFatal()) return;
  // clamp speed to be in the range 1.0 >= speed >= -1.0
  if (speed < -1.0) {
    speed = -1.0;
  } else if (speed > 1.0) {
    speed = 1.0;
  }

  // calculate the desired output pwm value by scaling the speed appropriately
  int32_t rawValue;
  if (speed == 0.0) {
    rawValue = GetCenterPwm();
  } else if (speed > 0.0) {
    rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) +
                         ((float)GetMinPositivePwm()) + 0.5);
  } else {
    rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor()) +
                         ((float)GetMaxNegativePwm()) + 0.5);
  }

  // the above should result in a pwm_value in the valid range
  wpi_assert((rawValue >= GetMinNegativePwm()) &&
             (rawValue <= GetMaxPositivePwm()));
  wpi_assert(rawValue != kPwmDisabled);

  // send the computed pwm value to the FPGA
  SetRaw(rawValue);
}
Ejemplo n.º 10
0
inline int prOpFloat(VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a, *b;
	PyrSymbol *msg;

	a = g->sp - 1;
	b = g->sp;

	switch (GetTag(b)) {
		case tagInt :
			SetRaw(a, Functor::run(slotRawFloat(a), (double)slotRawInt(b)));
			break;
		case tagChar :
		case tagPtr :
		case tagNil :
		case tagFalse :
		case tagTrue :
			goto send_normal_2;
		case tagSym :
			SetSymbol(a, slotRawSymbol(b));
			break;
		case tagObj :
			if (isKindOf(slotRawObject(b), class_signal))
				SetObject(a, Functor::signal_fx(g, slotRawFloat(a), slotRawObject(b)));
			else
				goto send_normal_2;
			break;
		default :
			SetRaw(a, Functor::run(slotRawFloat(a), slotRawFloat(b)));
			break;
	}
	g->sp-- ; // drop
	g->numpop = 0;
#if TAILCALLOPTIMIZE
	g->tailCall = 0;
#endif
	return errNone;

	send_normal_2:
	if (numArgsPushed != -1)  // special case flag meaning it is a primitive
		return errFailed;	// arguments remain on the stack

	msg = gSpecialBinarySelectors[g->primitiveIndex];
	sendMessage(g, msg, 2);
	return errNone;
}
Ejemplo n.º 11
0
void offsetheap(VMGlobals *g, PyrObject *heap, double offset)
{
	long i;
	for (i=0; i<heap->size; i+=2) {
		SetRaw(&heap->slots[i], slotRawFloat(&heap->slots[i]) + offset);
		//post("%3d %9.2f %9.2f\n", i>>1, heap->slots[i].uf, offset);
	}
}
Ejemplo n.º 12
0
/**
 * Common initialization code called by all constructors.
 *
 * Note that the Victor uses the following bounds for PWM values.  These values were determined
 * empirically and optimized for the Victor 888. These values should work reasonably well for
 * Victor 884 controllers as well but if users experience issues such as asymmetric behavior around
 * the deadband or inability to saturate the controller in either direction, calibration is recommended.
 * The calibration procedure can be found in the Victor 884 User Manual available from IFI.
 *
 *   - 206 = full "forward"
 *   - 131 = the "high end" of the deadband range
 *   - 128 = center of the deadband range (off)
 *   - 125 = the "low end" of the deadband range
 *   - 56 = full "reverse"
 */
void Victor::InitVictor() {
	SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);

	SetPeriodMultiplier(kPeriodMultiplier_2X);
	SetRaw(m_centerPwm);

	LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
}
Ejemplo n.º 13
0
/**
 * Constructor for a VictorSP
 * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
 * on-board, 10-19 are on the MXP port
 */
VictorSP::VictorSP(uint32_t channel) : SafePWM(channel) {
  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
  SetPeriodMultiplier(kPeriodMultiplier_1X);
  SetRaw(m_centerPwm);
  SetZeroLatch();

  HALReport(HALUsageReporting::kResourceType_VictorSP, GetChannel());
  LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this);
}
Ejemplo n.º 14
0
/**
 * Constructor for a SD540
 * @param channel The PWM channel that the SD540 is attached to. 0-9 are
 * on-board, 10-19 are on the MXP port
 */
SD540::SD540(uint32_t channel) : SafePWM(channel) {
  SetBounds(2.05, 1.55, 1.50, 1.44, .94);
  SetPeriodMultiplier(kPeriodMultiplier_1X);
  SetRaw(m_centerPwm);
  SetZeroLatch();

  HALReport(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
  LiveWindow::GetInstance()->AddActuator("SD540", GetChannel(), this);
}
Ejemplo n.º 15
0
/**
 * Common initialization code called by all constructors.
 *
 * Note that the Talon uses the following bounds for PWM values. These values should work reasonably well for
 * most controllers, but if users experience issues such as asymmetric behavior around
 * the deadband or inability to saturate the controller in either direction, calibration is recommended.
 * The calibration procedure can be found in the Talon User Manual available from CTRE.
 *
 *   2.037ms = full "forward"
 *   1.539ms = the "high end" of the deadband range
 *   1.513ms = center of the deadband range (off)
 *   1.487ms = the "low end" of the deadband range
 *   0.989ms = full "reverse"
 */
void Talon::InitTalon() {
	SetBounds(2.037, 1.539, 1.513, 1.487, .989);
	SetPeriodMultiplier(kPeriodMultiplier_1X);
	SetRaw(m_centerPwm);
	SetZeroLatch();

	HALReport(HALUsageReporting::kResourceType_Talon, GetChannel());
	LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
}
Ejemplo n.º 16
0
/**
 * Common initialization code called by all constructors.
 *
 * Note that the Victor uses the following bounds for PWM values.  These values were determined
 * empirically and optimized for the Victor 888. These values should work reasonably well for
 * Victor 884 controllers as well but if users experience issues such as asymmetric behavior around
 * the deadband or inability to saturate the controller in either direction, calibration is recommended.
 * The calibration procedure can be found in the Victor 884 User Manual available from IFI.
 *
 *   - 206 = full "forward"
 *   - 131 = the "high end" of the deadband range
 *   - 128 = center of the deadband range (off)
 *   - 125 = the "low end" of the deadband range
 *   - 56 = full "reverse"
 */
void Victor::InitVictor() {
    SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);

    SetPeriodMultiplier(kPeriodMultiplier_2X);
    SetRaw(m_centerPwm);

    LiveWindow::GetInstance()->AddActuator("Victor", GetModuleNumber(), GetChannel(), this);
    nUsageReporting::report(nUsageReporting::kResourceType_Victor, GetChannel(), GetModuleNumber() - 1);
}
Ejemplo n.º 17
0
/**
 * Common initialization code called by all constructors.
 *
 * Note that the Victor uses the following bounds for PWM values.  These values were determined
 * empirically and optimized for the Victor 888. These values should work reasonably well for
 * Victor 884 controllers as well but if users experience issues such as asymmetric behavior around
 * the deadband or inability to saturate the controller in either direction, calibration is recommended.
 * The calibration procedure can be found in the Victor 884 User Manual available from IFI.
 * 
 *   - 206 = full "forward"
 *   - 131 = the "high end" of the deadband range
 *   - 128 = center of the deadband range (off)
 *   - 125 = the "low end" of the deadband range
 *   - 56 = full "reverse"
 */
void Victor::InitVictor() {
	// TODO: compute the appropriate values based on digital loop timing
	SetBounds(206, 131, 128, 125, 56);
	SetPeriodMultiplier(kPeriodMultiplier_2X);
	SetRaw(m_centerPwm);

	LiveWindow::GetInstance()->AddActuator("Victor", GetModuleNumber(), GetChannel(), this);
	nUsageReporting::report(nUsageReporting::kResourceType_Victor, GetChannel(), GetModuleNumber() - 1);
}
Ejemplo n.º 18
0
/**
 * Constructor for a Spark
 * @param channel The PWM channel that the Spark is attached to. 0-9 are
 * on-board, 10-19 are on the MXP port
 */
Spark::Spark(uint32_t channel) : PWMSpeedController(channel) {
  SetBounds(2.003, 1.55, 1.50, 1.46, .999);
  SetPeriodMultiplier(kPeriodMultiplier_1X);
  SetRaw(m_centerPwm);
  SetZeroLatch();

  HALReport(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
  LiveWindow::GetInstance()->AddActuator("Spark", GetChannel(), this);
}
Ejemplo n.º 19
0
/**
 * Common initialization code called by all constructors.
 * 
 * Note that the Victor uses the following bounds for PWM values.  These values were determined
 * empirically through experimentation during the 2008 beta testing of the new control system.
 * Testing during the beta period revealed a significant amount of variation between Victors.
 * The values below are chosen to ensure that teams using the default values should be able to
 * get "full power" with the maximum and minimum values.  For better performance, teams may wish
 * to measure these values on their own Victors and set the bounds to the particular values
 * measured for the actual Victors they were be using. 
 *   - 210 = full "forward"
 *   - 138 = the "high end" of the deadband range
 *   - 132 = center of the deadband range (off)
 *   - 126 = the "low end" of the deadband range
 *   - 56 = full "reverse"
 */
void Victor::InitVictor()
{
	// TODO: compute the appropriate values based on digital loop timing
	SetBounds(210, 138, 132, 126, 56);
	SetPeriodMultiplier(kPeriodMultiplier_2X);
	SetRaw(m_centerPwm);

	nUsageReporting::report(nUsageReporting::kResourceType_Victor, GetChannel(), GetModuleNumber() - 1);
}
Ejemplo n.º 20
0
/**
 * Common initialization code called by all constructors.
 *
 * Note that the Talon uses the following bounds for PWM values. These values should work reasonably well for
 * most controllers, but if users experience issues such as asymmetric behavior around
 * the deadband or inability to saturate the controller in either direction, calibration is recommended.
 * The calibration procedure can be found in the Talon User Manual available from CTRE.
 * 
 *   - 211 = full "forward"
 *   - 133 = the "high end" of the deadband range
 *   - 129 = center of the deadband range (off)
 *   - 125 = the "low end" of the deadband range
 *   - 49 = full "reverse"
 */
void Talon::InitTalon() {
	// TODO: compute the appropriate values based on digital loop timing
	SetBounds(211, 133, 129, 125, 49);
	SetPeriodMultiplier(kPeriodMultiplier_2X);
	SetRaw(m_centerPwm);

	// TODO: Add Talon to Usage Reporting
	nUsageReporting::report(nUsageReporting::kResourceType_Talon, GetChannel(), GetModuleNumber() - 1);
}
Ejemplo n.º 21
0
void PriorityQueuePostpone(PyrObject* queueobj, double time)
{
	PyrSlot *schedqSlot = queueobj->slots;

	if (IsObj(schedqSlot)) {
		PyrObject *schedq = slotRawObject(schedqSlot);
		PyrSlot* slots = schedq->slots;
		for (int i=1; i < schedq->size; i+=3) {
			SetRaw(&slots[i], slotRawFloat(&slots[i]) + time);
		}
	}
}
Ejemplo n.º 22
0
int prSignalRotate(struct VMGlobals *g, int numArgsPushed)
{
	PyrSlot *a, *b;
	int err, rot;

	a = g->sp - 1;
	b = g->sp;

	err = slotIntVal(b, &rot);
	if (err) return err;

	SetRaw(a, signal_rotate(g, slotRawObject(a), rot));
	return errNone;
}
Ejemplo n.º 23
0
/**
 * Common initialization code called by all constructors.
 */
void Jaguar::InitJaguar()
{
	/*
	 * Input profile defined by Luminary Micro.
	 * 
	 * Full reverse ranges from 0.671325ms to 0.6972211ms
	 * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
	 * Neutral ranges from 1.4482078ms to 1.5517922ms
	 * Proportional forward ranges from 1.5517922ms to 2.3027789ms
	 * Full forward ranges from 2.3027789ms to 2.328675ms
	 */
	SetBounds(251, 135, 128, 120, 4);
	SetPeriodMultiplier(kPeriodMultiplier_1X);
	SetRaw(m_centerPwm);
}
Ejemplo n.º 24
0
/**
 * @param channel The PWM channel that the Jaguar is attached to.
 */
Jaguar::Jaguar(uint32_t channel) : SafePWM(channel)
{
	/*
	 * Input profile defined by Luminary Micro.
	 *
	 * Full reverse ranges from 0.671325ms to 0.6972211ms
	 * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
	 * Neutral ranges from 1.4482078ms to 1.5517922ms
	 * Proportional forward ranges from 1.5517922ms to 2.3027789ms
	 * Full forward ranges from 2.3027789ms to 2.328675ms
	 */
	SetBounds(2.31, 1.55, 1.507, 1.454, .697);
	SetPeriodMultiplier(kPeriodMultiplier_1X);
	SetRaw(m_centerPwm);

	LiveWindow::GetInstance().AddActuator("Jaguar", GetChannel(), this);
}
Ejemplo n.º 25
0
/**
 * Constructor for a Jaguar connected via PWM
 * @param channel The PWM channel that the Jaguar is attached to. 0-9 are
 * on-board, 10-19 are on the MXP port
 */
Jaguar::Jaguar(uint32_t channel) : PWMSpeedController(channel) {
  /**
   * Input profile defined by Luminary Micro.
   *
   * Full reverse ranges from 0.671325ms to 0.6972211ms
   * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
   * Neutral ranges from 1.4482078ms to 1.5517922ms
   * Proportional forward ranges from 1.5517922ms to 2.3027789ms
   * Full forward ranges from 2.3027789ms to 2.328675ms
   */
  SetBounds(2.31, 1.55, 1.507, 1.454, .697);
  SetPeriodMultiplier(kPeriodMultiplier_1X);
  SetRaw(m_centerPwm);
  SetZeroLatch();

  HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel());
  LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
}
Ejemplo n.º 26
0
bool addheap(VMGlobals *g, PyrObject *heapArg, double schedtime, PyrSlot *task)
{
	PyrHeap * heap = (PyrHeap*)heapArg;
#ifdef GC_SANITYCHECK
	g->gc->SanityCheck();
#endif
	if (heap->size >= ARRAYMAXINDEXSIZE(heap))
		return false;
	assert(heap->size);

// 	post("->addheap\n");
// 	dumpheap(heapArg);

	/* parent and sibling in the heap, not in the task hierarchy */
	int mom = heap->size - 1;
	PyrSlot * pme = heap->slots + mom;
	int stabilityCount = slotRawInt(&heap->count);
	SetRaw(&heap->count, stabilityCount + 1);

	for (; mom>0;) {	/* percolate up heap */
		int newMom = ((mom - 3) / 2);
		mom = newMom - newMom % 3; /// LATER: we could avoid the division by using 4 slots per element
		PyrSlot * pmom = heap->slots + mom;
		if (schedtime < slotRawFloat(pmom)) {
			assert(slotRawInt(pmom + 2) < stabilityCount);
			slotCopy(&pme[0], &pmom[0]);
			slotCopy(&pme[1], &pmom[1]);
			slotCopy(&pme[2], &pmom[2]);
			pme = pmom;
		} else break;
	}
	SetFloat(&pme[0], schedtime);
	slotCopy(&pme[1], task);
	SetInt(&pme[2], stabilityCount);
	g->gc->GCWrite(heap, task);
	heap->size += 3;

#ifdef GC_SANITYCHECK
	g->gc->SanityCheck();
#endif
// 	dumpheap(heapArg);
// 	post("<-addheap %g\n", schedtime);
	return true;
}
Ejemplo n.º 27
0
/**
 * Sets NAOs body stiffness to x percent.
 */
void CNaoCommand::SetBodyStiffness(int percent) {
    
    if (!SETSTIFFNESS.Release()) return;

    if (percent<0) percent=0;
    if (percent>100) percent=100;

    char buf[50];
    sprintf_s(buf,"%d",percent);
    
    int l=strlen(SETSTIFFNESS.Cmd());
    char *cmd=new char[l+50];
    strcpy_s(cmd,l+50,SETSTIFFNESS.Cmd());
    strcat_s(cmd,l+50,buf);
    
    SetRaw(cmd);   

    delete cmd;
}
Ejemplo n.º 28
0
/**
 * NAO says the passed text.
 */
void CNaoCommand::Say(TCHAR* text) {

    if (!SAY.Release()) return;

    if (text!=NULL && wcslen(text)>0) {

        size_t converted;
        int l=strlen(SAY.Cmd());
        int tot=30+l+_tcslen(text)*2;

        char *raw=new char[tot+50];
        strcpy_s(raw,tot,SAY.Cmd());
        wcstombs_s(&converted,raw+l,tot,text,tot);

        SetRaw(raw);   
        
        delete raw;
    }
}
Ejemplo n.º 29
0
/**
 * Common initialization code called by all constructors.
 */
void Jaguar::InitJaguar()
{
	/*
	 * Input profile defined by Luminary Micro.
	 * 
	 * Full reverse ranges from 0.671325ms to 0.6972211ms
	 * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
	 * Neutral ranges from 1.4482078ms to 1.5517922ms
	 * Proportional forward ranges from 1.5517922ms to 2.3027789ms
	 * Full forward ranges from 2.3027789ms to 2.328675ms
	 * TODO: compute the appropriate values based on digital loop timing
	 */
	SetBounds(251, 135, 128, 120, 4);
	SetPeriodMultiplier(kPeriodMultiplier_1X);
	SetRaw(m_centerPwm);

	nUsageReporting::report(nUsageReporting::kResourceType_Jaguar, GetChannel(), GetModuleNumber() - 1);
	LiveWindow::GetInstance()->AddActuator("Jaguar", GetModuleNumber(), GetChannel(), this);
}
Ejemplo n.º 30
0
/**
 * Construct a TalonSRX connected via PWM
 * @param channel The PWM channel that the TalonSRX is attached to. 0-9 are
 * on-board, 10-19 are on the MXP port
 */
TalonSRX::TalonSRX(uint32_t channel) : SafePWM(channel) {
  /* Note that the TalonSRX uses the following bounds for PWM values. These
   * values should work reasonably well for most controllers, but if users
   * experience issues such as asymmetric behavior around the deadband or
   * inability to saturate the controller in either direction, calibration is
   * recommended. The calibration procedure can be found in the TalonSRX User
   * Manual available from Cross The Road Electronics.
   *   2.004ms = full "forward"
   *   1.52ms = the "high end" of the deadband range
   *   1.50ms = center of the deadband range (off)
   *   1.48ms = the "low end" of the deadband range
   *   0.997ms = full "reverse"
   */
  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
  SetPeriodMultiplier(kPeriodMultiplier_1X);
  SetRaw(m_centerPwm);
  SetZeroLatch();

  HALReport(HALUsageReporting::kResourceType_TalonSRX, GetChannel());
  LiveWindow::GetInstance()->AddActuator("TalonSRX", GetChannel(), this);
}