Ejemplo n.º 1
2
/*
 * 	to sort a band, we have
 */
void	band::sortBand (void) {
int	MadeChange = true;
int	i;
/*
 * 	just a simple bubblesort, since the amount of elements
 * 	is limited, and the function is only called on set up
 */
	while (MadeChange) {
	   MadeChange = false;
	   for (i = 0; i < amount - 1; i ++)
	      if (getFrequency (i) > getFrequency (i + 1)) {
	         int f1, v1;
	         f1 = getFrequency (i + 1);
	         v1 = getVoltage   (i + 1);
	         setFrequency (i + 1, getFrequency (i));
	         setVoltage   (i + 1, getVoltage   (i));
	         setFrequency (i, f1);
	         setVoltage   (i, v1);
	         MadeChange = true;
	      }
	}

	setLowerBound (getFrequency (0));
	setUpperBound (getFrequency (amount - 1));
}
bool QuadrotorPropulsion::processQueue(const ros::Time &timestamp, const ros::Duration &tolerance, const ros::Duration &delay, const ros::WallDuration &wait, ros::CallbackQueue *callback_queue) {
  boost::mutex::scoped_lock lock(command_queue_mutex_);
  bool new_command = false;

  ros::Time min(timestamp), max(timestamp);
  try {
    min = timestamp - delay - tolerance /* - ros::Duration(dt) */;
  } catch (std::runtime_error &e) {}

  try {
    max = timestamp - delay + tolerance;
  } catch (std::runtime_error &e) {}

  do {

    while(!command_queue_.empty()) {
      hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = command_queue_.front();
      ros::Time new_time = new_motor_voltage->header.stamp;

      if (new_time.isZero() || (new_time >= min && new_time <= max)) {
        setVoltage(*new_motor_voltage);
        command_queue_.pop();
        new_command = true;

      // new motor command is too old:
      } else if (new_time < min) {
        ROS_DEBUG_NAMED("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).toSec());
        command_queue_.pop();

      // new motor command is too new:
      } else {
        break;
      }
    }

    if (command_queue_.empty() && !new_command) {
      if (!motor_status_.on || wait.isZero()) break;

      ROS_DEBUG_NAMED("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.toSec(), (timestamp - last_command_time_).toSec());
      if (!callback_queue) {
        if (command_condition_.timed_wait(lock, wait.toBoost())) continue;
      } else {
        lock.unlock();
        callback_queue->callAvailable(wait);
        lock.lock();
        if (!command_queue_.empty()) continue;
      }

      ROS_ERROR_NAMED("quadrotor_propulsion", "Command timed out. Disabled motors.");
      shutdown();
    }

  } while(false);

  if (new_command) {
      ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)");
  }

  return new_command;
}
Ejemplo n.º 3
0
bool pidLowLevelManipulator(void) //вычисление ПИД регулятора манипулятора
{
    cubesCatcherPID.current = adcData[(char)CUBES_CATCHER_ADC - 1] * 360.0 / 4096.0 - 22.0; // current manipulator's position
    pidCalc(&cubesCatcherPID);
    setVoltage((char)CUBES_CATCHER_MOTOR_CH - 1, cubesCatcherPID.output);

    return 0;
}
void AppleACPIBatteryDevice::clearBatteryState(bool do_set)
{
    // Only clear out battery state; don't clear manager state like AC Power.
    // We just zero out the int and bool values, but remove the OSType values.
    fBatteryPresent = false;
    fACConnected = false;
    fACChargeCapable = false;
	
    setBatteryInstalled(false);
    setIsCharging(false);
    setCurrentCapacity(0);
    setMaxCapacity(0);
    setTimeRemaining(0);
    setAmperage(0);
    setVoltage(0);
    setCycleCount(0);
    setAdapterInfo(0);
    setLocation(0);
	
    properties->removeObject(manufacturerKey);
    removeProperty(manufacturerKey);
    properties->removeObject(serialKey);
    removeProperty(serialKey);
    properties->removeObject(batteryInfoKey);
    removeProperty(batteryInfoKey);
    properties->removeObject(errorConditionKey);
    removeProperty(errorConditionKey);
	// setBatteryBIF
	properties->removeObject(_DesignCapacitySym);
    removeProperty(_DesignCapacitySym);
	properties->removeObject(_DeviceNameSym);
    removeProperty(_DeviceNameSym);
	properties->removeObject(_ManufactureDateSym);
    removeProperty(_ManufactureDateSym);
	properties->removeObject(_SerialNumberSym);
    removeProperty(_SerialNumberSym);
	// setBatteryBST
	properties->removeObject(_AvgTimeToEmptySym);
    removeProperty(_AvgTimeToEmptySym);
	properties->removeObject(_AvgTimeToFullSym);
    removeProperty(_AvgTimeToFullSym);
	properties->removeObject(_InstantTimeToEmptySym);
    removeProperty(_InstantTimeToEmptySym);
	properties->removeObject(_InstantTimeToFullSym);
    removeProperty(_InstantTimeToFullSym);
	properties->removeObject(_InstantAmperageSym);
    removeProperty(_InstantAmperageSym);
	properties->removeObject(_QuickPollSym);
    removeProperty(_QuickPollSym);
	properties->removeObject(_SoftwareSerialSym);
    removeProperty(_SoftwareSerialSym);
	
    rebuildLegacyIOBatteryInfo();
	
    if(do_set) {
        updateStatus();
    }
}
Ejemplo n.º 5
0
void TTerminal::setSource(TTerminal::TERMINAL_SOURCE source, double value)
{
    if(source == TTerminal::SOURCE_CURRENT){
       setCurrent( value );
       setVoltage( 0.0 );
       mType = source;
    }else if(source == TTerminal::SOURCE_VOLTAGE){
        setVoltage( value );
        setCurrent( 0.0 );
        mType = source;
    }else{
        setVoltage( 0.0 );
        setCurrent( 0.0 );
        mType = GND;
    }

    return;
}
Ejemplo n.º 6
0
void eapsUta12Port::adjustValues()
{
    if( _setValueType == setValueType::setTypeVoltage )
    {
        setVoltage( calcAdjustedValue( _setVoltage, _lastVoltage ) );
    }
    else if( _setValueType == setValueType::setTypeCurrent )
    {
        setCurrent( calcAdjustedValue( _setCurrent, _lastCurrent ) );
    }
    else if( _setValueType == setValueType::setTypePowerByVoltage )
    {
        if( _lastVoltage > 0.0 && _lastCurrent > 0.0 )
        {
            setVoltage( std::sqrt( calcAdjustedValue( _setPower, _lastPower )*
                                   _lastVoltage/_lastCurrent ) );
        }
    }
    else if( _setValueType == setValueType::setTypePowerByCurrent )
    {
        if( _lastVoltage > 0.0 && _lastCurrent > 0.0 )
        {
            setCurrent( std::sqrt( calcAdjustedValue( _setPower, _lastPower )/
                                   (_lastVoltage/_lastCurrent) ) );
        }
    }
    else if( _setValueType == setValueType::setTypeResistanceByVoltage )
    {
        if( _lastVoltage > 0.0 && _lastResistance > 0.0 )
        {
            setVoltage( _lastVoltage*
                        std::sqrt( _setResistance/_lastResistance ) );
        }
    }
    else if( _setValueType == setValueType::setTypeResistanceByCurrent )
    {
        if( _lastVoltage > 0.0 && _lastResistance > 0.0 )
        {
            setCurrent( _lastCurrent*
                        std::sqrt( _setResistance/_lastResistance ) );
        }
    }
}
Ejemplo n.º 7
0
double Translator::takeInput(int pidSetting, float rawSetting) {
  if(pidSetting == 0 ) {
    double setpoint = setVoltage(rawSetting);
    return setpoint;
  }
  else if(pidSetting == 1) {
    double setpoint = setWattage(rawSetting);
    return setpoint;
  }
}
Ejemplo n.º 8
0
void pidLowLevel(void) //вычисление ПИД регулятора колес
{
//Low level pid target values are set here__________________________________
  char i;
  for(i = 0; i < 4; i++)
  {

    wheelsPidStruct[i].target = regulatorOut[i];//передача требуемых значений от траекторного регулятора
    wheelsPidStruct[i].current = motorSpeed[i]; // текущие занчения скоростей колес
    pidCalc(&wheelsPidStruct[i]);
    if (curState.pidEnabled) setVoltage(WHEELS[i], wheelsPidStruct[i].output);
  }
}
Ejemplo n.º 9
0
Archivo: Ati.cpp Proyecto: cyhd/HaCoE
void Ati::nano17Init(char *filename)
{
	float Total[6];
    unsigned short index = 1;   // index of calibration in file (second parameter; default = 1)	

	cal=createCalibration(filename,index);

//	Taking Average samples for bias
	for(int j=0;j<AVERAGE_NUM;j++)
	{
		setVoltage();

		for(int k=0;k<6;k++)
			Total[k]=Total[k]+voltages[k];
	}
	
	for(int k=0;k<6;k++)
     Total[k]=Total[k]/AVERAGE_NUM;

	Bias(cal,Total);
}
Ejemplo n.º 10
0
void eapsUta12Port::setValue( setValueType type, double value, bool autoAdjust )
{    
    _setValueType = type;
    if( type == setValueType::setTypeNone )
    {
        return;
    }
    _autoAdjust = autoAdjust;

    if( type == setValueType::setTypeVoltage )
    {
        LOG(INFO) << "eaps uta 12 set voltage: " << value
                  << " adjust: " << autoAdjust;
        _setVoltage = value;
        _lastVoltage = value;
        setVoltage( value );
    }
    else if( type == setValueType::setTypeCurrent )
    {
        LOG(INFO) << "eaps uta 12 set current: " << value
                  << " adjust: " << autoAdjust;
        _setCurrent = value;
        _lastCurrent = value;
        setCurrent( value );
    }
    else if( type == setValueType::setTypePowerByVoltage
             || type == setValueType::setTypePowerByCurrent )
    {
        LOG(INFO) << "eaps uta 12 set power: " << value
                  << " adjust: " << autoAdjust << " by voltage: "
                  << (type == setValueType::setTypePowerByVoltage);
        _setPower = value;
        _lastPower = value;
        if( _lastVoltage > 0.0 && _lastCurrent > 0.0 )
        {
            double resistance = _lastVoltage/_lastCurrent;
            if( type == setValueType::setTypePowerByVoltage )
            {
                setVoltage( std::sqrt( value*resistance ) );
            }
            else if( type == setValueType::setTypePowerByCurrent )
            {
                setCurrent( std::sqrt( value/resistance ) );
            }
        }
        else
        {
            emit portError( "Measure before setting power!" );
        }
    }
    else if( type == setValueType::setTypeResistanceByVoltage
             || type == setValueType::setTypeResistanceByCurrent )
    {
        LOG(INFO) << "eaps uta 12 set resistance: " << value << " adjust: "
                  << autoAdjust << " by voltage: "
                  << (type == setValueType::setTypeResistanceByVoltage);
        _setResistance = value;
        if( _lastVoltage > 0.0 && _lastCurrent > 0.0 && _lastResistance > 0.0 )
        {
            if( type == setValueType::setTypeResistanceByVoltage )
            {
                setVoltage( _lastVoltage*
                            std::sqrt( _setResistance/_lastResistance ) );
            }
            else if( type == setValueType::setTypeResistanceByCurrent )
            {
                setCurrent( _lastCurrent*
                            std::sqrt( _setResistance/_lastResistance ) );
            }
        }
        else
        {
            emit portError( "Measure before setting power!" );
        }
    }
}
Ejemplo n.º 11
0
static void setMapVoltage(float voltage) {
	setVoltage(engineConfiguration->map.sensor.hwChannel, voltage);
}
Ejemplo n.º 12
0
static void setTpsVoltage(float voltage) {
	setVoltage(engineConfiguration->tpsAdcChannel, voltage);
}
Ejemplo n.º 13
0
static void setAfrVoltage(float voltage) {
	setVoltage(engineConfiguration->afr.hwChannel, voltage);
}
Ejemplo n.º 14
0
static void setMafVoltage(float voltage) {
	setVoltage(engineConfiguration->mafAdcChannel, voltage);
}
Ejemplo n.º 15
0
static void setIatVoltage(float voltage) {
	setVoltage(engineConfiguration->iatAdcChannel, voltage);
}
Ejemplo n.º 16
0
static void setAfrVoltage(float voltage) {
	setVoltage(engineConfiguration->afrSensor.afrAdcChannel, voltage);
}
IOReturn AppleACPIBatteryDevice::setBatteryBST(OSArray *acpibat_bst)
{
	UInt32 currentStatus= OSDynamicCast(OSNumber, acpibat_bst->getObject(BST_STATUS))->unsigned32BitValue();
	fCurrentRate		= OSDynamicCast(OSNumber, acpibat_bst->getObject(BST_RATE))->unsigned32BitValue();
	fCurrentVoltage		= OSDynamicCast(OSNumber, acpibat_bst->getObject(BST_VOLTAGE))->unsigned32BitValue();
	fCurrentCapacity	= OSDynamicCast(OSNumber, acpibat_bst->getObject(BST_CAPACITY))->unsigned32BitValue();

#if TEST
    //test
#if CHARGING
    currentStatus = BATTERY_CHARGING;
    fCurrentCapacity = fMaxCapacity*(i%100)/100;
#else
    currentStatus = BATTERY_DISCHARGING;
    fCurrentCapacity = fMaxCapacity*(100-(i%100))/100;
#endif
    
    i++;
#endif

    setCurrentCapacity(fCurrentCapacity);
	setVoltage(fCurrentVoltage);
    
	if(fCurrentRate	& 0x8000)		fCurrentRate = 0xFFFF - (fCurrentRate);
	
	if (fCurrentRate <= 0x00000000) fCurrentRate = fMaxCapacity / 2;
	
	if (fAverageRate)	fAverageRate = (fAverageRate + fCurrentRate) / 2;
	else				fAverageRate = fCurrentRate;
	
	DEBUG_LOG("AppleACPIBatteryDevice: Battery State 0x%x.\n", currentStatus);
	
	if (currentStatus ^ fStatus) {		// The battery has changed states
		fStatus = currentStatus;
		fAverageRate = 0;
	}
	
	if ((currentStatus & BATTERY_DISCHARGING) && (currentStatus & BATTERY_CHARGING)) {		// This will NEVER happen
		const OSSymbol *permanentFailureSym = OSSymbol::withCString(kErrorPermanentFailure);
		logReadError( kErrorPermanentFailure, 0, NULL);
		setErrorCondition( (OSSymbol *)permanentFailureSym );
		permanentFailureSym->release();
		
		/* We want to display the battery as present & completely discharged, not charging */
		setFullyCharged(false);
		setIsCharging(false);
		
		fACConnected = true;
		setExternalConnected(fACConnected);
		fACChargeCapable = false;
		setExternalChargeCapable(fACChargeCapable);
		
		setAmperage(0);
		setInstantAmperage(0);
		
		setTimeRemaining(0);
		setAverageTimeToEmpty(0);
		setAverageTimeToFull(0);
		setInstantaneousTimeToFull(0);
		setInstantaneousTimeToEmpty(0);
		
		DEBUG_LOG("AppleACPIBatteryDevice: Battery Charging and Discharging?\n");
	} else if (currentStatus & BATTERY_DISCHARGING) {
		setFullyCharged(false);
		setIsCharging(false);
		
		fACConnected = false;
		setExternalConnected(fACConnected);
		fACChargeCapable = false;
		setExternalChargeCapable(fACChargeCapable);
		
		setAmperage(fAverageRate * -1);
		setInstantAmperage(fCurrentRate * -1);
		
		if (fAverageRate)	setTimeRemaining((60 * fCurrentCapacity) / fAverageRate);
		else				setTimeRemaining(0xffff);
		
		
		if (fAverageRate)	setAverageTimeToEmpty((60 * fCurrentCapacity) / fAverageRate);
		else				setAverageTimeToEmpty(0xffff);
		
		if (fCurrentRate)	setInstantaneousTimeToEmpty((60 * fCurrentCapacity) / fCurrentRate);
		else				setInstantaneousTimeToEmpty(0xffff);

		setAverageTimeToFull(0xffff);
		setInstantaneousTimeToFull(0xffff);		
		
		DEBUG_LOG("AppleACPIBatteryDevice: Battery is discharging.\n");
	} else if (currentStatus & BATTERY_CHARGING) {
		setFullyCharged(false);
		setIsCharging(true);
		
		fACConnected = true;
		setExternalConnected(fACConnected);
		fACChargeCapable = true;
		setExternalChargeCapable(fACChargeCapable);
		
		setAmperage(fAverageRate);
		setInstantAmperage(fCurrentRate);
		
		if (fAverageRate)	setTimeRemaining((60 * (fMaxCapacity - fCurrentCapacity)) / fAverageRate);
		else				setTimeRemaining(0xffff);
		
		if (fAverageRate)	setAverageTimeToFull((60 * (fMaxCapacity - fCurrentCapacity)) / fAverageRate);
		else				setAverageTimeToFull(0xffff);
		
		
		if (fCurrentRate)	setInstantaneousTimeToFull((60 * (fMaxCapacity - fCurrentCapacity)) / fCurrentRate);
		else				setInstantaneousTimeToFull(0xffff);
		
		setAverageTimeToEmpty(0xffff);
		setInstantaneousTimeToEmpty(0xffff);
		
		DEBUG_LOG("AppleACPIBatteryDevice: Battery is charging.\n");
	} else {	// BATTERY_CHARGED
		setFullyCharged(true);
		setIsCharging(false);
		
		fACConnected = true;
		setExternalConnected(fACConnected);
		fACChargeCapable = true;
		setExternalChargeCapable(fACChargeCapable);
		
		setAmperage(0);
		setInstantAmperage(0);
		
		setTimeRemaining(0xffff);
		setAverageTimeToFull(0xffff);
		setAverageTimeToEmpty(0xffff);
		setInstantaneousTimeToFull(0xffff);
		setInstantaneousTimeToEmpty(0xffff);
		
		fCurrentCapacity = fMaxCapacity;
		setCurrentCapacity(fCurrentCapacity);
		
		DEBUG_LOG("AppleACPIBatteryDevice: Battery is charged.\n");
	}
	
	if (!fPollingOverridden && fMaxCapacity) {
		/*
		 * Conditionally set polling interval to 5 second if we're
		 *      on AC power
		 * i.e. we're doing an Inflow Disabled discharge
		 */
        if(fACConnected){
			setProperty("Quick Poll", true);
			fPollingInterval = kQuickPollInterval;
        }
        else
        {
			setProperty("Quick Poll", false);
			fPollingInterval = kDefaultPollInterval;
        }
	}

	/* construct and publish our battery serial number here */
	constructAppleSerialNumber();
	
	/* Cancel read-completion timeout; Successfully read battery state */
	fBatteryReadAllTimer->cancelTimeout();
	
	rebuildLegacyIOBatteryInfo();
	
	updateStatus();
	
	return kIOReturnSuccess;
}
Ejemplo n.º 18
0
Archivo: Ati.cpp Proyecto: cyhd/HaCoE
void Ati::setFT(void)
{
	setVoltage();
	ConvertToFT(cal,voltages,FT);

}
Ejemplo n.º 19
0
///////////////////////////CONE MOVER///////////////////////
bool moveCone(void)
{
    setVoltage((char)CONE_CH - 1, (float)KICK_CONE);
    return 0;
}
Ejemplo n.º 20
0
//////////////////////////WALL////////////////////////////////
bool openWall(void)
{
    setVoltage((char)WALL_CH - 1, (float)WALL_IS_OPEN);
    return 0;
}
Ejemplo n.º 21
0
Archivo: main.c Proyecto: ednspace/EAR
//==================================== 
void main() 
{ 


int16 local_ccp_delta;
got_pulse_width = FALSE; 
first_press = TRUE;

//DAC_address = 0xC4; //Address of the DAC don't ever forget this please
DAC_address = 0xC0; //I2C Address of parts with marking AJ

//Oscillator Config

//setup_oscillator(OSC_8MHZ|OSC_INTRC|OSC_PLL_ON); //I am giving it all shes got, she can't take any more Captain
//setup_oscillator(OSC_500KHZ|OSC_INTRC|OSC_PLL_OFF);  //Can measure a little over 4 seconds with timer1
setup_oscillator(OSC_2MHZ|OSC_INTRC|OSC_PLL_OFF);  //Can measure a little over 1 second with timer1 Use this one

//Capture Compare Config
setup_CCP1(CCP_CAPTURE_FE); //Sets up Capture Compare for Falling Edge - Reads Tap Input
bit_set(APFCON,0); //Set CCP1 Pin to A5 instead of A2

//ADC Config
SETUP_ADC(ADC_CLOCK_INTERNAL);
SETUP_ADC_PORTS(sAN3); //Analog in on RA4
set_adc_channel(3);
delay_us(10);



setup_timer_1(T1_INTERNAL | T1_DIV_BY_8 ); 
setup_timer_2(T2_DIV_BY_16 , 31, 1);  //Need a real understanding of what this lines does here
enable_interrupts(INT_CCP1);
enable_interrupts(INT_TIMER2);
enable_interrupts(GLOBAL);

//DAC Test Code
//setVoltage(4095, FALSE); //Sets DAC output to Max voltage
//setVoltage(0, FALSE); 

mode = 1; //Start off in POT mode

while(1) 
  { 
  pot_val = read_adc(); //Read the ADC everytime
  
  if (mode == 1){ //We are in POT mode just set the value
      pot_save = pot_val;
      setVoltage(pot_val * 4, FALSE); //scale to 12bit DAC
  }
  
  if (((pot_val > pot_save + 10 && mode ==0)) || ((pot_val < pot_save - 10) && (mode == 0))){ //We are in TAP mode only set voltage if POT moves a lot
      mode = 1; //Jump out of tap mode and enter pot mode
  }
 
   
  
  if(got_pulse_width) 
     { 
      disable_interrupts(GLOBAL); 
      local_ccp_delta = ccp_delta;  
      enable_interrupts(GLOBAL);
      
      pulse_width_ms = local_ccp_delta / 62; //2E-6 per tick * 8 Prescale = 1.6E-5 so .001 / 1.6E-5 = 62.5 
      got_pulse_width = FALSE;
      
     // For Testing set Pulse width ms here
     // pulse_width_ms = 524;
      
      LUT_count = 1;
      while   (millisecond_delay[LUT_count] >= pulse_width_ms){
      LUT_count = LUT_count + 1;
      }
      calculated_voltage = map(pulse_width_ms, millisecond_delay[LUT_count - 1], millisecond_delay[LUT_count] , dac_out[LUT_count-1],dac_out[LUT_count]);
      
      
     // printf("%lu ms \n\r", pulse_width_ms); //Debug Message
     // printf("%lu dac \n\r", calculated_voltage); //Debug Message
      setVoltage(calculated_voltage,FALSE);
      mode = 0; //Set Mode to tap mode, requires larger movement of POT to break out
     } 
  } 

}
Ejemplo n.º 22
0
static void setCltVoltage(float voltage) {
	setVoltage(engineConfiguration->clt.adcChannel, voltage);
}
Ejemplo n.º 23
0
bool closeWall(void)
{
    setVoltage((char)WALL_CH - 1, (float)WALL_IS_CLOSED);
    return 0;
}
Ejemplo n.º 24
0
int main () {
  initSocket();
  sleep(3);
  encodersReset();
  double toFront = 0;
  dist toBack = {0,0}, toSide = {0,0}, scale = {1,1};
  volts speed = {30,30};

  // This is a new cycle, first check and then move
  while(1) {

    if (req.setWallAuto) {
      if (r.s.wall == LEFT) req.considerSide = LEFT;
      else if (r.s.wall == RIGHT) req.considerSide = RIGHT;
      else req.considerSide = 0;
      req.setWallAuto = false;
    }

    if (req.checkSideStatic == true) { toSide = checkSideStatic(ACCURATE); printf("toSide %lF %lF ", toSide.l, toSide.r); req.checkSideStatic == false; }
    if (req.checkFrontStatic == true) { toFront = checkFrontStatic(ACCURATE); printf("toFront2 %lF ", toFront); req.checkFrontStatic = false; }
    if (req.checkFront == true) { toFront = checkFront(ACCURATE); printf("toFront %lF ", toFront); req.checkFront = false; }
    if (req.checkSide == true) { toSide = checkSide(ACCURATE, 0, 0); printf("toSide %lF %lF ", toSide.l, toSide.r); req.checkSide == false; }
    if (req.checkBack == true) { toBack = checkBack(ACCURATE); printf("toBack %lF %lF ", toBack.l, toBack.r); req.checkBack = false; }

    if (req.checkEncoders == true) { encodersGet(&r.s);  printf("encoders %i %i\n", r.s.encodersL, r.s.encodersR);}

      /**/printf("frontRisk %d frontLRisk %d frontLRisk %d sideLRisk %d sideRRisk %d backLRisk %d backRRisk %d wall %d\n", frontRisk, frontLRisk, frontLRisk, sideLRisk, sideRRisk, backLRisk, backRRisk, r.s.wall);/**/

    if (req.rotate180 == true) {
      if (frontRisk && sideLRisk && sideRRisk) {
        reposition(&r.s, 400, 400, 10, -10); 
        req = DEFAULT_REQUEST;
        continue;
      }
    }

    if (req.calculateBack == true) {
      
      // In case of no risk
      if (!frontLRisk && !frontRRisk && !sideLRisk && !sideRRisk && !backLRisk && !backRRisk) {

        // Case 1: No risks. No wall. Go straight
        if (r.s.wall == 0) { backVal = (dist) {0,0}; /**/printf("No risks. No wall. Go straight\n"); }/**/
        
        // Case 2: No risks. Left wall. Touching back. We are in a deep corner. Turn
        else if (r.s.wall == LEFT && sideLFar && !backLisInfinite) { /**/printf("No risks. Left wall. Touching back. Turn\n");/**/

          backConsidered = toBack.l;
          backDif = 0 - backConsidered;
          
          if (backConsidered < 38 && backConsidered >= 25)      {backVal = (dist) {-15,-15};}                 // Too far
          else if (backConsidered < 25 && backConsidered >= 18) {backVal = (dist) {backDif/1.5,backDif/1.5};} // Far
          else if (backConsidered < 18 && backConsidered >= 8) {backVal = (dist) {backDif/1.5,backDif/1.5};} // In range

          
        // Case 3: No risks. Left wall. Touching back. We are in a deep corner. Turn
        } else if (r.s.wall == RIGHT && sideLInfinite && !backLisInfinite) {/**/printf("No Risks. Right Wall is being followed %i.\n", r.s.wall);/**/ 

          backConsidered = toBack.r;
          backDif = 0 - backConsidered;

          if (backConsidered < 38 && backConsidered >= 25)      {backVal = (dist) {+15,+15};}                 // Too far
          else if (backConsidered < 25 && backConsidered >= 18) {backVal = (dist) {-backDif/1.5,-backDif/1.5};} // Far
          else if (backConsidered < 18 && backConsidered >= 8) {backVal = (dist) {-backDif/1.5,-backDif/1.5};} // In range

        }
        // Default
        else {backVal = (dist){999, 999};}
        
      // Risks, skip
      } else { /**/printf("Risks, skip.\n");/**/ backVal = (dist){999, 999}; }
      
      printf("backVal %lF %lF\n", backVal.l, backVal.r);// TODO check the other side tooooooo!
      req.calculateBack = false;
    }
    
    
    // might be used for scale in tunnels to moderate velocity
    if (req.calculateSide == true) {


        if (r.s.wall == LEFT) {
          sideConsidered = toSide.l;
          sideDif = 27 - sideConsidered; /*/printf("sideDif %i\n", sideDif);/**/

          if (sideConsidered < 100 && sideConsidered >= 60)     {  sideVal = (dist) {sideDif, sideDif};}          // Too far
          else if (sideConsidered < 60 && sideConsidered >= 30) { sideVal = (dist) {sideDif, sideDif};} // Far
          else if (sideConsidered < 30 && sideConsidered >= 21) { sideVal = (dist) {0, 0};}             // In range
          else if (sideConsidered < 21)                         { sideVal = (dist) {sideDif, sideDif};} // Close

        } else {
          sideConsidered = toSide.r;
          sideDif = 27 - sideConsidered; /*/printf("sideDif %i\n", sideDif);/**/

          if (sideConsidered < 100 && sideConsidered >= 60)     {  sideVal = (dist) {-sideDif, -sideDif};}          // Too far
          else if (sideConsidered < 60 && sideConsidered >= 30) { sideVal = (dist) {-sideDif, -sideDif};} // Far
          else if (sideConsidered < 30 && sideConsidered >= 21) { sideVal = (dist) {0, 0};}             // In range
          else if (sideConsidered < 21)                         { sideVal = (dist) {-sideDif, -sideDif};} // Close
        }

      /**/printf("sideVal %lF %lF\n", sideVal.l, sideVal.r);/**/ // TODO check the other side tooooooo!
      req.calculateSide = false;
    }
    
    // might be used for scale in tunnels to moderate velocity
    if (req.calculateFront == true) {

      frontConsidered = toFront;
      frontDif = frontOk - frontConsidered; /*/printf("frontDif %i\n", frontDif);/**/
      frontVal = frontDif;
      if (-frontRange <= frontDif && frontDif <= frontRange) { frontVal = 0; /*/printf("front in Range\n");/**/ }
      if (frontVal <= -frontMinimum) { }
    
      /*/printf("frontVal %i\n", frontVal);/**/ // TODO check the other side tooooooo!
      req.calculateFront = false;
    }
    
    // Calculate scale
    
    // Check for risk
      // - recalculate speed

    if (frontVal != 999) speed = (volts){frontVal,-frontVal};
    if (sideVal.l != 999 && sideVal.r != 999) speed = (volts){sideVal.l,-sideVal.r};
    if (backVal.l != 999 && backVal.r != 999) speed = (volts){backVal.l,-backVal.r};

    // Assign voltage
    r.v = setVoltage(speed, scale);

    // Finally arrived here
    moveAtVoltage(r.v.l, r.v.r);
    printf("*  Speed chosen: %i %i\n\n", r.v.l, r.v.r );
    req = DEFAULT_REQUEST;
  }

  return 0;
}
Ejemplo n.º 25
0
bool closeCone(void)
{
    setVoltage((char)CONE_CH - 1, (float)CONE_BACK);
    return 0;
}
int main() {

    __builtin_disable_interrupts();

    // set the CP0 CONFIG register to indicate that kseg0 is cacheable (0x3)
    __builtin_mtc0(_CP0_CONFIG, _CP0_CONFIG_SELECT, 0xa4210583);

    // 0 data RAM access wait states
    BMXCONbits.BMXWSDRM = 0x0;

    // enable multi vector interrupts
    INTCONbits.MVEC = 0x1;

    // disable JTAG to get pins back
    DDPCONbits.JTAGEN = 0;


    __builtin_enable_interrupts();

    initSPI1();
    initI2C2();
    i2c_master_setup();
    initExpander();

    //sine wave
    int sine[1000];
    int i;
    for(i = 0; i < 1000; i++){
      sine[i] = 128 + 127*sin(2*3.14*10*i/1000);
    }

    int triangle[1000];
    i = 0;
    for(i = 0; i < 1000; i++){
      triangle[i] = .256*i;
    }

    i = 0;

    while(1) {
      _CP0_SET_COUNT(0);

      if(_CP0_GET_COUNT() > 24000){
        i++;
        setVoltage(0, sine[i]);
        setVoltage(1, triangle[i]);
        _CP0_SET_COUNT(0);
      }

      if(i > 1000){
        i = 0;
      }

      char status = getExpander();          //read the expander
      char g7 = (status & 0x80) >> 7;       //get level of pin g7
      setExpander(0, g7);                   //set pin 0 to level of g7



    }


}
Ejemplo n.º 27
0
void TTerminal::setGnd()
{
    mType = GND;
    setVoltage( 0.0 );
    setCurrent( 0.0 );
}
Ejemplo n.º 28
0
/**
* @brief Event method that is fired when voltage is changed
*
* Event method that fires when voltage input textbox is changed. It updates the model data with the setVoltage method.
* @return void
**/
static void voltageChanged (GtkEditable *editable,
               gpointer     user_data) {
    setVoltage(gtk_editable_get_chars(editable, 0,-1));
}