int executeAutomovement(motorGroup *group, int debugStartCol) {
switch (group->moving) {
case TARGET:
if (group->posPID.kP != 0) {
int powerLimit = (group->autoStillSpeeding && errorLessThan(group, group->autoSSmargin) ? group->stillSpeed : 127); //limit power if autoStillSpeeding conditions are met
group->posPID.integralMax = powerLimit;
int PIDpower = PID_runtime(group->posPID, getPosition(group), debugStartCol);
setPower(group, copysign(PIDpower, limit(fabs(PIDpower), 0, powerLimit)));
}
break;
case MANEUVER:
if (group->forward == (getPosition(group) < group->targetPos)) {
group->maneuverTimer = resetTimer();
setPower(group, group->movePower);
}
else if (time(group->maneuverTimer) > group->maneuverTimeout) {
setPower(group, group->endPower);
group->moving = NO;
}
break;
case DURATION:
if (time(group->maneuverTimer) > group->moveDuration) {
setPower(group, group->endPower);
group->moving = NO;
}
else {
setPower(group, group->movePower);
}
break;
}
return getPower(group); //TODO: be better
}
int initDEVICE(DEVICE *ret) {
int i=0;
if( startConnectionUE9(&ret->ue9) != 0 )
{
return -1;
}
for(i=0;i<4;i++)
{
ret->activeTrack[i]=false;
ret->activeTrackSensor[i]=false;
}
ret->trafficLightStatus=4;
if(setLights(ret) != 0)
return -2;
if( setPower( ret,0,false) !=0)
return -2;
if( setPower( ret,1,false) !=0)
return -2;
if( setPower( ret,2,false) !=0)
return -2;
if( setPower( ret,3,false) !=0)
return -2;
return 0;
}
int handle_power_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
{
if (argc > 1)
{
return ERROR_INVALID_ARGUMENTS;
}
if (argc == 1)
{
if (strcmp(args[0], "on") == 0)
{
setPower(1);
}
else if (strcmp(args[0], "off") == 0)
{
setPower(0);
} else
{
command_print(cmd_ctx, "arg is \"on\" or \"off\"");
return ERROR_INVALID_ARGUMENTS;
}
}
command_print(cmd_ctx, "Target power %s", savePower ? "on" : "off");
return ERROR_OK;
}
bool ElevatorModule::calibrate() {
if(!m_Enabled)
return false;
std::cout << "in calibrate" << std::endl;
Timer timeOut, timeOut2;
timeOut.Start();
bool renablePID = false;
disablePID();
if(m_PIDController->IsEnabled()){
renablePID = true;
}
while(!getButton()) {
if(timeOut.Get() > MAX_ELEVATOR_CALIBRATE_TIME_DOWN) {
throw CalibrationError(this, "ElevatorModule::calibrate()" , "calibrate timed out");
}
setPower(CALIBRATE_ELEVATOR_DOWN);
Wait(0.005);
}
std::cout << "After first loop" << std::endl;
setPower(0);
m_Encoder->Reset();
timeOut2.Start();
while(timeOut2.Get() < MAX_ELEVATOR_CALIBRATE_TIME_UP) {
setPower(CALIBRATE_ELEVATOR_UP);
Wait(0.005);
}
std::cout << "after second" << std::endl;
setPower(0);
if(getButton()) {
throw CalibrationError(this, "ElevatorModule::calibrate()" ,"check your button");
}
if(m_Encoder->PIDGet() < MIN_ELEVATOR_DISTANCE_CALIBRATE) {
throw CalibrationError(this, "ElevatorModule::calibrate()" , "check your encoder");
}
if(renablePID)
m_PIDController->Enable();
m_Calibration_Is_Done = true;
std::cout << "Done with Calibrate" << std::endl;
return true;
}
/*! \brief main
*/
int main(void)
{
//! temporary duty value
uint8_t duty;
//! temporary time counter
uint32_t time_counter;
init();
startFan(potzRead());
while (1)
{
duty = potzRead();
if (duty < MIN_PWM_DUTY)
{
duty = MIN_PWM_DUTY;
}
// do not update power when starting
if (g_fan.starting == FALSE)
{
setPower(duty);
}
// stalled rotor : power off, wait and restart
if (g_fan.stall == TRUE)
{
// power off
setPower(0);
// reset time counter
ATOMIC_BLOCK(ATOMIC_FORCEON)
{
g_time_counter = 0;
}
// wait until delay finished
do
{
ATOMIC_BLOCK(ATOMIC_FORCEON)
{
time_counter = g_time_counter;
}
}
while (time_counter <= STALL_RETRY_TIME);
// start up the fan again
startFan(duty);
}
}
void CVFD::wake_up() {
printf("CVFD::wake_up>\n");
if (atoi(g_settings.lcd_setting_dim_time) > 0) {
printf("1\n");
timeout_cnt = atoi(g_settings.lcd_setting_dim_time);
atoi(g_settings.lcd_setting_dim_brightness) > 0 ?
setBrightness(g_settings.lcd_setting[SNeutrinoSettings::LCD_BRIGHTNESS]) : setPower(1);
}
else
{
printf("2\n");
setPower(1);
}
printf("CVFD::wake_up<\n");
}
//#subregion durational movement
void moveForDuration(motorGroup *group, int power, int duration, bool runConcurrently=true, int endPower=128) { //if endPower>127, will finish with stillSpeed
group->movePower = power;
group->moveDuration = duration;
group->moving = DURATION;
group->maneuverTimer = resetTimer();
group->endPower = (endPower>127 ? calcStillSpeed(group, power>0) : endPower);
setPower(group, group->movePower);
if (!runConcurrently) {
wait1Msec(duration);
setPower(group, group->endPower);
group->moving = NO;
}
}
void CLCD::wake_up()
{
if (g_info.hw_caps->display_type == HW_DISPLAY_LINE_TEXT) {
if (atoi(g_settings.lcd_setting_dim_time.c_str()) > 0) {
timeout_cnt = atoi(g_settings.lcd_setting_dim_time.c_str());
g_settings.lcd_setting_dim_brightness > -1 ?
setBrightness(g_settings.lcd_setting[SNeutrinoSettings::LCD_BRIGHTNESS]) : setPower(1);
}
else
setPower(1);
if(g_settings.lcd_info_line){
switch_name_time_cnt = g_settings.timing[SNeutrinoSettings::TIMING_INFOBAR] + 10;
}
}
}
void HueLight::updateStates(const QVariantMap &statesMap)
{
// color mode
if (statesMap.value("colormode").toString() == "hs") {
setColorMode(ColorModeHS);
} else if (statesMap.value("colormode").toString() == "ct") {
setColorMode(ColorModeCT);
} else if (statesMap.value("colormode").toString() == "xy") {
setColorMode(ColorModeXY);
}
// effect (none, colorloop)
if (statesMap.value("effect").toString() == "none") {
setEffect("none");
} else if (statesMap.value("effect").toString() == "colorloop") {
setEffect("color loop");
}
setReachable(statesMap.value("reachable").toBool());
// alert (none, select, lselect)
setAlert(statesMap.value("alert").toString());
setBrigtness(statesMap.value("bri").toInt());
setCt(statesMap.value("ct").toInt());
setPower(statesMap.value("on").toBool());
setSat(statesMap.value("sat").toInt());
setHue(statesMap.value("hue").toInt());
if (!statesMap.value("xy").toList().isEmpty())
setXy(QPointF(statesMap.value("xy").toList().first().toFloat(), statesMap.value("xy").toList().last().toFloat()));
emit stateChanged();
}
void A110x2500Radio::begin(uint8_t address, channel_t channel, power_t power)
{
gDataTransmitting = false;
gDataReceived = false;
Task_Params taskParams;
Error_Block eb;
Error_init(&eb);
GateMutex_construct(&mygate, NULL);
sem = Semaphore_create(0, NULL, &eb);
// Configure the radio and set the default address, channel, and TX power.
A110LR09Init(&gPhyInfo, &gSpi, gGdo);
setAddress(address);
setChannel(channel);
setPower(power);
Task_Params_init(&taskParams);
taskParams.priority = Task_numPriorities - 1;
taskParams.stackSize = 0x800;
Task_create(serviceInterrupt, &taskParams, &eb);
attachInterrupt(RF_GDO0, gdo0Isr, FALLING);
sleep();
}
void DriveModule::drive(double throttle, double angle) {
throttle = driveFunc.transformThrottle(throttle);
angle = driveFunc.transformAngle(angle);
double leftMotorOutput = 0;
double rightMotorOutput = 0;
if(throttle > 0.0) {
angle = -angle;
if(angle < 0.0) {
leftMotorOutput = (throttle + angle);
rightMotorOutput = fmax(throttle, -angle);
}
else {
leftMotorOutput = fmax(throttle, angle);
rightMotorOutput = (throttle - angle);
}
}
else {
if(angle > 0.0) {
leftMotorOutput = -fmax(-throttle, angle);
rightMotorOutput = throttle + angle;
//std::cout << rightMotorOutput << std::endl;
}
else {
leftMotorOutput = throttle - angle;
rightMotorOutput = -fmax(-throttle,-angle);
}
}
setPower(leftMotorOutput, rightMotorOutput);
}
void initPower( void )
{
chMtxInit( &g_mutex );
setPower( 0 );
chThdCreateStatic( waPower, sizeof(waPower), NORMALPRIO, Power, NULL );
}
void Motor::setControlPin(int control_pin_)
{
controlPin = control_pin_;
pinMode(controlPin, OUTPUT);
setPower(0);
}
void nrf24l01p::rxMode()
{
setCeLow();
writeRegister(CONFIG, _BV(EN_CRC) | _BV(CRCO)); // Enable CRC (2bytes)
delayMicroseconds(100);
writeRegister(EN_AA, 0x00); // Disable auto acknowledgment
writeRegister(EN_RXADDR, 0x01); // Enable first data pipe
writeRegister(SETUP_AW, 0x03); // 5 bytes address
writeRegister(SETUP_RETR, 0xFF); // 15 retransmit, 4000us pause
writeRegister(RF_CH, 0x00); // channel 8
setBitrate(NRF24L01_BR_250K);
setPower(mPower);
writeRegister(STATUS, 0x70); // Clear status register
writeRegister(RX_PW_P0, 0x0A); // RX payload of 10 bytes
writeRegister(FIFO_STATUS, 0x00); // Nothing useful for write command
delay(50);
flushTx();
flushRx();
delayMicroseconds(100);
writeRegister(CONFIG, _BV(EN_CRC) | _BV(CRCO) | _BV(PWR_UP) );
delayMicroseconds(100);
writeRegister(CONFIG, _BV(EN_CRC) | _BV(CRCO) | _BV(PWR_UP) | _BV(PRIM_RX) );
delayMicroseconds(130);
setCeHigh();
delayMicroseconds(100);
}
void * reduce_phase2(const void * _exp)
{
if(isA(_exp, Integer()))
return domainCast(_exp, Real());
else if(isA(_exp, Real()) || isA(_exp, Var()))
return copy(_exp);
else if(isA(_exp, Sum()) || isA(_exp, Product()))
{
void * s = copy(_exp);
size_t i;
for(i = 0; i < size(s); ++i)
{
delete(argv(s, i));
setArgv(s, i, reduce_phase2(argv(_exp, i)));
}
return s;
}
else if(isA(_exp, Pow()))
{
void * p = copy(_exp);
delete(base(p));
delete(power(p));
setBase(p, reduce_phase2(base(_exp)));
setPower(p, reduce_phase2(power(_exp)));
return p;
}
else if(isOf(_exp, Apply_1()))
{
void * f = copy(_exp);
delete(arg(f));
setArg(f, reduce_phase2(arg(_exp)));
return f;
}
assert(0);
}
/** Set mode for a feature.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param mode DC1394 mode desired
* @return true if mode set successfully
*/
bool Features::setMode(dc1394feature_info_t *finfo, dc1394feature_mode_t mode)
{
dc1394feature_t feature = finfo->id;
if (hasMode(finfo, mode))
{
// first, make sure the feature is powered on
setPower(finfo, DC1394_ON);
ROS_DEBUG_STREAM("setting feature " << featureName(feature)
<< " mode to " << modeName(mode));
if (DC1394_SUCCESS !=
dc1394_feature_set_mode(camera_, feature, mode))
{
ROS_WARN_STREAM("failed to set feature " << featureName(feature)
<< " mode to " << modeName(mode));
return false;
}
}
else
{
// device does not support this mode for this feature
ROS_DEBUG_STREAM("no " << modeName(mode)
<< " mode for feature " << featureName(feature));
return false;
}
return true;
}
int termPower(bus_t bus)
{
if (1 == getPower(bus))
return setPower(bus, 0, "Device Termination");
return SRCP_OK;
}
bool CCommand::Open()
{
// Device is initialized with 9600 bauds ??? conection
int retry = 0;
bool opened = false;
// If device open failed retry three times
while ((retry < 3) && (!opened))
{
if (m_device->open()) {
opened = true;
qDebug() << "Connected";
break;
}
sleep(1);
m_device->close();
sleep(1);
retry++;
}
// If not opened cancel
qDebug() << "Opened ? " << opened;
if (opened == false) return false;
// Try to power it on
retry = 0;
while (retry < 3) {
// Try to power it on
qDebug() << "setPower();";
setPower(true);
// If powered end loop
if (getPower()) {
qDebug() << "Power on is OK";
qDebug() << "so change baudrate";
sleep(3);
qDebug() << "change it on computer side too";
//m_device->setBaudRate("38400");
setBaudRate(CCommand::b38400);
getPower();
sleep(10);
// Baud rate is now 38400
return true;
}
sleep(3);
retry++;
}
setPower(false);
return false;
}
void cmd_pwr_en( BaseChannel *chp, int argc, char * argv [] )
{
(void)chp;
if ( argc > 0 )
{
if ( argv[0][0] != '0' )
{
setPower( 1 );
chprintf( chp, "ok:pwron" );
}
else
{
setPower( 0 );
chprintf( chp, "ok:pwroff" );
}
}
}
void CVFD::wake_up() {
if(fd < 0) return;
if (atoi(g_settings.lcd_setting_dim_time.c_str()) > 0) {
timeout_cnt = atoi(g_settings.lcd_setting_dim_time.c_str());
g_settings.lcd_setting_dim_brightness > -1 ?
setBrightness(g_settings.lcd_setting[SNeutrinoSettings::LCD_BRIGHTNESS]) : setPower(1);
}
else
setPower(1);
if(g_settings.lcd_info_line){
switch_name_time_cnt = g_settings.timing[SNeutrinoSettings::TIMING_INFOBAR] + 10;
}
#if defined (BOXMODEL_OCTAGON1008)
ShowIcon(ICON_COLON2, false);
#endif
}
void TCS34725Driver::initialize() {
setGain(gain_1);
setIntegrationTime(time_700ms);
setPower(true);
// power up cycle with 2,4 ms integration time is 2,4 ms
QThread::msleep(3);
setAENbit(true);
}
void RenderParams::update(ConstCDLOpDataRcPtr & cdl)
{
double slope[4], offset[4], power[4];
cdl->getSlopeParams().getRGBA(slope);
cdl->getOffsetParams().getRGBA(offset);
cdl->getPowerParams().getRGBA(power);
const float saturation = (float)cdl->getSaturation();
const CDLOpData::Style style = cdl->getStyle();
m_isReverse
= (style == CDLOpData::CDL_V1_2_REV)
|| (style == CDLOpData::CDL_NO_CLAMP_REV);
m_isNoClamp
= (style == CDLOpData::CDL_NO_CLAMP_FWD)
|| (style == CDLOpData::CDL_NO_CLAMP_REV);
if (isReverse())
{
// Reverse render parameters
setSlope(Reciprocal((float)slope[0]),
Reciprocal((float)slope[1]),
Reciprocal((float)slope[2]),
Reciprocal((float)slope[3]));
setOffset((float)-offset[0], (float)-offset[1], (float)-offset[2], (float)-offset[3]);
setPower(Reciprocal((float)power[0]),
Reciprocal((float)power[1]),
Reciprocal((float)power[2]),
Reciprocal((float)power[3]));
setSaturation(Reciprocal(saturation));
}
else
{
// Forward render parameters
setSlope((float)slope[0], (float)slope[1], (float)slope[2], (float)slope[3]);
setOffset((float)offset[0], (float)offset[1], (float)offset[2], (float)offset[3]);
setPower((float)power[0], (float)power[1], (float)power[2], (float)power[3]);
setSaturation(saturation);
}
}
void P2OSPtz::shutdown()
{
sendAbsPanTilt(0,0);
usleep(SLEEP_TIME_USEC);
sendAbsZoom(0);
usleep(SLEEP_TIME_USEC);
setPower(POWER_OFF);
usleep(SLEEP_TIME_USEC);
ROS_INFO("PTZ camera has been shutdown");
}
void ShootingWeapon::initSW(int power, int range, int frequency){
timerID = 0;
timerGarbageCollectorID = 0;
unitToShoot = 0;
timerInterval = 20;
incProyectile = 0;
setPower(power);
setRange(range);
setFrequency(frequency);
setProyectileVelocity(1000);
}
Ally::Ally()
:max_life(1)
{
setLife(max_life);
setPower(1);
setSpeed(15.0f);
setEffectiveArea(Director::getInstance()->getWinSize());
setTag(kTagAlly);
setZOrder(kZOrderMovableSprite);
setStatus(Ally::Status::Normal);
setSpecialEffect(BattleManager::SpecialEffect::Nothing);
}
void A110x2500Radio::begin(uint8_t address, channel_t channel, power_t power)
{
gDataTransmitting = false;
gDataReceived = false;
// Configure the radio and set the default address, channel, and TX power.
A110LR09Init(&gPhyInfo, &gSpi, gGdo);
setAddress(address);
setChannel(channel);
setPower(power);
attachInterrupt(RF_GDO0, gdo0Isr, FALLING);
sleep();
}
void onPower(HttpRequest &request, HttpResponse &response)
{
setPower(!power);
JsonObjectStream* stream = new JsonObjectStream();
JsonObject& json = stream->getRoot();
json["status"] = (bool)true;
json["state"] = (bool)power;
response.setAllowCrossDomainOrigin("*");
response.setHeader("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,OPTIONS");
response.setHeader("Access-Control-Allow-Headers", "Content-Type, Access-Control-Allow-Headers, Authorization, X-Request, X-Request, X-Requested-With");
response.sendJsonObject(stream);
}
void MotorMixer::computePower(double throttle)
{
//Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
double basePower = MOTORS_MIN + (throttle * 800);
MotorPower motorPower = MotorPower();
motorPower.motor1 = basePower;
motorPower.motor2 = basePower;
motorPower.motor3 = basePower;
motorPower.motor4 = basePower;
//Set motor power
setPower(motorPower);
}
void CVFD::count_down() {
if (timeout_cnt > 0) {
timeout_cnt--;
if (timeout_cnt == 0) {
if (atoi(g_settings.lcd_setting_dim_brightness) > 0) {
// save lcd brightness, setBrightness() changes global setting
int b = g_settings.lcd_setting[SNeutrinoSettings::LCD_BRIGHTNESS];
setBrightness(atoi(g_settings.lcd_setting_dim_brightness));
g_settings.lcd_setting[SNeutrinoSettings::LCD_BRIGHTNESS] = b;
} else {
setPower(0);
}
}
}
}
static QScriptValue js_setPower(QScriptContext *context, QScriptEngine *engine)
{
int power = context->argument(0).toInt32();
int player;
if (context->argumentCount() > 1)
{
player = context->argument(0).toInt32();
}
else
{
player = engine->globalObject().property("me").toInt32();
}
setPower(player, power);
return QScriptValue();
}
