/* * 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 ×tamp, 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; }
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(); } }
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; }
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 ) ); } } }
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; } }
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); } }
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); }
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!" ); } } }
static void setMapVoltage(float voltage) { setVoltage(engineConfiguration->map.sensor.hwChannel, voltage); }
static void setTpsVoltage(float voltage) { setVoltage(engineConfiguration->tpsAdcChannel, voltage); }
static void setAfrVoltage(float voltage) { setVoltage(engineConfiguration->afr.hwChannel, voltage); }
static void setMafVoltage(float voltage) { setVoltage(engineConfiguration->mafAdcChannel, voltage); }
static void setIatVoltage(float voltage) { setVoltage(engineConfiguration->iatAdcChannel, voltage); }
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; }
void Ati::setFT(void) { setVoltage(); ConvertToFT(cal,voltages,FT); }
///////////////////////////CONE MOVER/////////////////////// bool moveCone(void) { setVoltage((char)CONE_CH - 1, (float)KICK_CONE); return 0; }
//////////////////////////WALL//////////////////////////////// bool openWall(void) { setVoltage((char)WALL_CH - 1, (float)WALL_IS_OPEN); return 0; }
//==================================== 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 } } }
static void setCltVoltage(float voltage) { setVoltage(engineConfiguration->clt.adcChannel, voltage); }
bool closeWall(void) { setVoltage((char)WALL_CH - 1, (float)WALL_IS_CLOSED); return 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; }
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 } }
void TTerminal::setGnd() { mType = GND; setVoltage( 0.0 ); setCurrent( 0.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)); }