void Robot::loadSettings(XMLDocument* doc)
{
printf("Robot::%s::%d: Loading Settings...\n",__INFO__);
XMLElement* robot = doc->FirstChildElement("Robot");
if(robot != NULL)
{
XMLElement* main_thread = robot->FirstChildElement("MainThread");
if(main_thread != NULL)
{
double period = 0.0;
int priority = 0;
GET_XML_FLOAT(main_thread,Period,period,DEFUALT_MAIN_THREAD_PERIOD);
setPeriod(period);
GET_XML_FLOAT(main_thread,Priority,priority,DEFUALT_MAIN_THREAD_PRIORITY);
setPriority(static_cast<gsi::Thread::ThreadPriority>(priority));
//printf("Period is: %.3f\n",period);
//printf("Values: %s\n",main_thread->FirstChildElement("Period")->GetText());
//setPeriod()
}
else
{
setPeriod(DEFUALT_MAIN_THREAD_PERIOD);
setPriority(static_cast<gsi::Thread::ThreadPriority>(DEFUALT_MAIN_THREAD_PRIORITY));
}
}
else
{
printf("Robot::%s::%d: Error, no <Robot> element\n",__INFO__);
printf("Robot::%s::%d: Assigning defualt settings\n",__INFO__);
settings_file_exists = false;
}
}
void LEDPixels::initialize(long microseconds, int * DisplayAddress,unsigned int LEDCount , char clkPin, char dPin )
{
byte Counter;
clockPin = clkPin;
dataPin = dPin;
Display = DisplayAddress;
NoOfLEDs = LEDCount;
pinMode(clockPin, OUTPUT);
pinMode(dataPin, OUTPUT);
// Adjust as you please. Too slow makes LEDs flicker.
// Too fast and the interrupt may chew into your processing speed!
//Timer1.attachInterrupt( LedOut ) ; // attaches routine to drive LEDs
show(); // Kick off display.
TCCR1A = 0; // clear control register A
TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer
if(microseconds > 0) setPeriod(microseconds);
//isrCallback = isr; // register the user's callback with the real ISR
TIMSK1 = _BV(TOIE1); // sets the timer overflow interrupt enable bit
sei(); // ensures that interrupts are globally enabled
start();
}
void CompositionDBG::changeNumPeaks(){
CompositionDBG* r_dbg=new CompositionDBG(m_id,m_numDim,m_numPeaksTemp,m_changeType.type,*ms_funID,
m_changePeakRatio,m_flagDimensionChange,m_flagNumPeaksChange,
m_numPeaksChangeMode,m_noiseFlag,m_timeLinkageFlag);
if(m_changeType.type==CT_Recurrent||m_changeType.type==CT_RecurrentNoisy)
r_dbg->setPeriod(m_period);
r_dbg->parameterSetting(this);
r_dbg->calculateGlobalOptima();
freeMemory();
RealDBG::freeMemory();
DynamicContinuous::freeMemory();
DynamicContinuous::allocateMemory(m_numDim,m_numPeaksTemp);
RealDBG::allocateMemory(m_numDim,m_numPeaksTemp);
allocateMemory(m_numDim,m_numPeaksTemp);
m_numPeaks=m_numPeaksTemp;
setPeriod(m_period);
*this=*r_dbg;
delete r_dbg;
r_dbg=0;
}
void CompositionDBG::changeDimension(){
/// no need to preserve previous information, e.g., positions, heights, width....
CompositionDBG* r_dbg=new CompositionDBG(m_id,m_dimNumberTemp,m_numPeaks,m_changeType.type,*ms_funID,m_changePeakRatio,m_flagDimensionChange,
m_flagNumPeaksChange,m_numPeaksChangeMode,m_noiseFlag,m_timeLinkageFlag);
if(m_changeType.type==CT_Recurrent||m_changeType.type==CT_RecurrentNoisy){
r_dbg->setPeriod(m_period);
}
r_dbg->parameterSetting(this);
r_dbg->calculateGlobalOptima();
freeMemory();
RealDBG::freeMemory();
DynamicContinuous::freeMemory();
Problem::freeMemory();
Problem::allocateMemory(m_dimNumberTemp);
ContinuousProblem::allocateMemory(m_dimNumberTemp);
DynamicContinuous::allocateMemory(m_dimNumberTemp,m_numPeaks);
RealDBG::allocateMemory(m_dimNumberTemp,m_numPeaks);
allocateMemory(m_dimNumberTemp,m_numPeaks);
m_numDim=m_dimNumberTemp;
setPeriod(m_period);
*this=*r_dbg;
delete r_dbg;
r_dbg=0;
}
/*!
Constructs an easing curve of the given \a type, with the given \a
amplitude and \a period. You only have to use this constructor if
\a type is one of QtEasingCurve::InElastic, QtEasingCurve::OutElastic,
QtEasingCurve::InBounce or QtEasingCurve::OutBounce.
*/
QtEasingCurve::QtEasingCurve(Type type, qreal amplitude, qreal period)
: d_ptr(new QtEasingCurvePrivate)
{
setType(type);
setAmplitude(amplitude);
setPeriod(period);
}
void LEDService::begin() {
if (state != LED_STATE_INVALID)
return; // Already been here
state = LED_STATE_OFF;
config = new Configuration("LED",
new ConfigurationItem("pin", LED_PIN_DEFAULT),
new ConfigurationItem("name", NULL),
new ConfigurationItem("passive", LED_PASSIVE_DEFAULT),
new ConfigurationItem("active", LED_ACTIVE_DEFAULT),
NULL);
UPnPService::begin(config);
led = config->GetValue("pin");
#ifdef DEBUG
DEBUG.printf("LEDService::begin (pin %d)\n", led);
#endif
pinMode(led, OUTPUT);
state = LED_STATE_OFF;
if (config->configured("active") && config->configured("passive")) {
setPeriod(config->GetValue("active"), config->GetValue("passive"));
SetState(LED_STATE_BLINK);
#ifdef DEBUG
DEBUG.printf("LEDService blink %d %d\n",
config->GetValue("active"),
config->GetValue("passive"));
#endif
}
}
void PeriodicThreadImplQNX::makePeriodic()
{
if (m_chid == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"No timer channel available! Cannot make this thread periodic!" << endl);
return;
}
struct sigevent event;
int coid;
coid = ConnectAttach(0, 0, m_chid, 0, 0);
if (coid == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"Could not attach to the timer channel! Cannot make this thread periodic!" << endl);
return;
}
SIGEV_PULSE_INIT(&event, coid, SIGEV_PULSE_PRIO_INHERIT, ePT_PULSE_TIMER, 0);
int res = timer_create(CLOCK_REALTIME, &event, &m_timerid);
if (res == -1)
{
LOGGING_ERROR_C(Thread, PeriodicThreadImplQNX,
"Could not create timer! Cannot make this thread periodic!" << endl);
return;
}
m_made_periodic = true;
setPeriod(m_period);
}
// Start the timer
// If a period is set, then sets the period and start the timer
DueTimer DueTimer::start(long microseconds){
if(microseconds > 0)
setPeriod(microseconds);
NVIC_EnableIRQ(Timers[timer].irq);
return *this;
}
PwmSysfsDriverNode::PwmSysfsDriverNode(ros::NodeHandle const& nh,
ros::NodeHandle const& nh_rel):
nh_(nh), nh_rel_(nh_rel)
{
// get reqired path to sysfs pwm directory parameter
std::string pwm_sysfs_dir;
if (!nh_rel_.getParam("pwm_sysfs_dir", pwm_sysfs_dir)) {
ROS_FATAL("Path to sysfs pwm directory required.");
ros::shutdown();
return;
}
// attemp to create driver
try {
driver_.reset(new PwmSysfsDriver(pwm_sysfs_dir));
}
catch (PwmSysfsDriverException e) {
driver_.reset();
ROS_FATAL("Failed to initialize driver: %s", e.what());
ros::shutdown();
return;
}
// set initial pwm values
bool enable;
if (nh_rel_.getParam("initial_enable", enable))
setEnable(enable);
bool invert_polarity;
if (nh_rel_.getParam("initial_invert_polarity", invert_polarity))
setInvertPolarity(invert_polarity);
double period;
if (nh_rel_.getParam("initial_period", period))
setPeriod(period);
double duty;
if (nh_rel_.getParam("initial_duty", duty))
setDuty(duty);
// boolean pwm enable
enable_sub_ = nh_.subscribe("enable", 10,
&PwmSysfsDriverNode::enableCallback, this);
// boolean pwm invert polarity
invert_polarity_sub_ =
nh_.subscribe("invert_polarity", 10,
&PwmSysfsDriverNode::invertPolarityCallback, this);
// pwm cycle period, in nanoseconds
period_sub_ = nh_.subscribe("period", 10,
&PwmSysfsDriverNode::periodCallback, this);
// pwm duty cycle fraction, 0 to 1
duty_sub_ = nh_.subscribe("duty", 10,
&PwmSysfsDriverNode::dutyCallback, this);
}
PID::PID(double kp, double ki, double kd, uint8_t direction, uint32_t period, uint32_t time){
setOutputLimits(0, 255);
setPeriod(100);
setDirection(direction);
setTunings(kp, ki, kd);
if (time > period) lastTime = time - period;
else lastTime = 0;
}
void SPIPWMworker(void)
{
unsigned char token, instruction, fInst=0,fb=0,bf=0;
initSPI();
SSP2BUF = 0xFD;
enablePWM();
while (1)
{
while(PORTCbits.RC0 ==0)//CS LOW
{
if(SSP2STATbits.BF) // we received something :)
{
bf=1;
if(fb==0)
{
token = SSP2BUF;
instruction = token & SPIEEMASK;
}
fb=1;
switch (instruction)
{
case 0:
if(fInst)
{
if(fInst==1)setPeriod(SSP2BUF);
fInst++;
}
else
{
fInst=1;
}
SSP2BUF = PWMperiod;
break;
case 1:
if(fInst)
{
if(fInst==1)setDuty(SSP2BUF);
fInst++;
}
else
{
fInst=1;
}
SSP2BUF = PWMduty;
break;
}
}
}
if(bf) //CS HIGH
{
fb=0;
bf=0;
fInst = 0;
}
}
}
::Ice::DispatchStatus
RoboCompCommonBehavior::CommonBehavior::___setPeriod(::IceInternal::Incoming& __inS, const ::Ice::Current& __current)
{
__checkMode(::Ice::Normal, __current.mode);
::IceInternal::BasicStream* __is = __inS.is();
__is->startReadEncaps();
::Ice::Int period;
__is->read(period);
__is->endReadEncaps();
setPeriod(period, __current);
return ::Ice::DispatchOK;
}
void initialize(struct Sched* sched, uint32_t time){
if (sched == NULL)
return;
sched->period = time;
sched->nb_fn = 0;
sched->first_fn = NULL;
sched->last_fn = NULL;
setPeriod(time);
}
void PeriodicThreadImplTimerfd::makePeriodic()
{
/* Create the timer */
int fd = timerfd_create(CLOCK_MONOTONIC, 0);
m_info->wakeups_missed = 0;
m_info->timer_fd = fd;
if (fd != -1)
{
timer_created = true;
}
setPeriod(m_period);
}
int main()
{
//int blinkHz = 1;
//int nsec = 5;
//s_println("Busy blink yellow LED at %d Hz for %d seconds", blinkHz, nsec);
//busy_blink(5, 1, GREEN_PIN);
setup_CTC_timer0(1, redCallback, 0);
setPeriod(kRED, 500);
setPeriod(kYELLOW, 500);
setPeriod(kGREEN, 500);
sei(); // enable interrupts
CommandIO cio;
CIOReset(&cio);
CIORegisterCommand(&cio, "zero", zero_cmd);
CIORegisterCommand(&cio, "toggle", toggle_cmd);
CIORegisterCommand(&cio, "print", print_cmd);
CIORegisterCommand(&cio, "stop", stop_cmd);
s_println("Entring while (1)");
while (1)
{
if (CIOCheckForCommand(&cio))
CIORunCommand(&cio);
if (redInterruptCount >= redNextRelease)
{
redTask();
redNextRelease += redInterruptsPerRelease;
}
}
}
ActuatorPwm::ActuatorPwm(ActuatorDigital* _target, uint16_t _period) :
ActuatorDriver(_target) {
periodStartTime = ticks.millis();
periodLate = 0;
dutyLate = 0;
value = 0.0;
minVal = 0.0;
maxVal = 100.0;
target->setActive(false);
setPeriod(_period);
// at init, pretend last high period was tiny spike in the past
lowToHighTime = periodStartTime - period_ms;
highToLowTime = lowToHighTime + 2;
cycleTime = period_ms;
dutyTime = calculateDutyTime(period_ms);
}
DueTimer& DueTimer::start(long microseconds){
/*
Start the timer
If a period is set, then sets the period and start the timer
*/
if(microseconds > 0)
setPeriod(microseconds);
if(_frequency[timer] <= 0)
setFrequency(1);
NVIC_ClearPendingIRQ(Timers[timer].irq);
NVIC_EnableIRQ(Timers[timer].irq);
TC_Start(Timers[timer].tc, Timers[timer].channel);
return *this;
}
//!***************************************************************
//! @details:
//! constructor
//!
//! @param[in]: camera
//! the camera that will be manipulated
//!
//! @param[in]: eventManager
//! the engine's event manager that will send events
//!
//! @return:
//!
//!
//!***************************************************************
ScreenScroller::ScreenScroller(FIFE::Camera* camera, FIFE::EventManager* eventManager, FIFE::TimeManager* timeManager)
: m_camera(camera), m_eventManager(eventManager), m_timeManager(timeManager), ScrollAmount(20),
ScrollActivationPercent(0.02f), m_eventRegistered(false)
{
// set the period for timing event in ms
setPeriod(20);
if (m_camera)
{
// get the viewport for the camera
const FIFE::Rect& viewport = m_camera->getViewPort();
// calculate borders to activate automatic scrolling
m_scrollAreaTop = static_cast<int>(viewport.h - (viewport.h*ScrollActivationPercent));
m_scrollAreaBottom = static_cast<int>(viewport.y + (viewport.h*ScrollActivationPercent));
m_scrollAreaRight = static_cast<int>(viewport.w - (viewport.w*ScrollActivationPercent));
m_scrollAreaLeft = static_cast<int>(viewport.x + (viewport.w*ScrollActivationPercent));
}
}
/** Resets the electometer, rebooting the device, sleeping for 1 second, and then downloading all of the settings.
*/
asynStatus drvAPS_EM::reset()
{
int range;
double integrationTime;
getIntegerParam(P_Range, &range);
getDoubleParam(P_IntegrationTime, &integrationTime);
writeMeter(REBOOT_COMMAND, 0);
epicsThreadSleep(1.0);
setRange(range);
epicsThreadSleep(0.01);
setPulse();
epicsThreadSleep(0.01);
setPeriod();
epicsThreadSleep(0.01);
setIntegrationTime(integrationTime);
epicsThreadSleep(0.01);
setGo();
return asynSuccess;
}
void RecurrenceWidget::set(bool recurring, int frequency, QString period, QDateTime start, QDateTime end, int max)
{
if (DEBUG)
qDebug() << objectName() << "::set(" << recurring << ", "
<< frequency << ", " << period << ", "
<< start << ", " << end << ", "
<< max << ") entered";
setRecurring(recurring);
setPeriod(period);
setFrequency(frequency);
setStartDateTime(start);
setEndDateTime(end);
setMax(max);
_prevEndDateTime = end.isValid() ? end : _eot ;
_prevFrequency = frequency;
_prevPeriod = stringToPeriod(period);
_prevRecurring = recurring;
_prevStartDateTime = start;
_prevMax = max;
}
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
openDevice();
initializeStreams();
///INICIALIZACION ATRIBUTOS GENERALES
registration=RoboCompRGBD::None; //A QUE INICIALIZO REGISTRATION????
///INICIALIZACION MUTEX
usersMutex = new QMutex();
RGBMutex = new QMutex();
depthMutex = new QMutex();
pointsMutex = new QMutex();
///INICIALIZACION ATRIBUTOS PARA PINTAR
mColor = new uint16_t [IMAGE_WIDTH*IMAGE_HEIGHT*3];
auxDepth = new uint8_t [IMAGE_WIDTH*IMAGE_HEIGHT*3];
qImgDepth = new QImage(IMAGE_WIDTH,IMAGE_HEIGHT,QImage::Format_Indexed8);
printf("\nStart moving around to get detected...\n(PSI pose may be required for skeleton calibration, depending on the configuration)\n");
setPeriod(15);
}
IsoMonitor_c::IsoMonitor_c() :
SchedulerTask_c( 125, true ),
mvec_isoMember(),
mt_handler(*this),
mt_customer(*this),
CONTAINER_CLIENT1_CTOR_INITIALIZER_LIST
{
}
void
IsoMonitor_c::init()
{
isoaglib_assert (!initialized());
isoaglib_assert (mvec_isoMember.empty());
mi32_lastSaRequest = -1; // not yet requested. Do NOT use 0, as the first "setLastRequest()" could (and does randomly) occur at time0 as it's called at init() time.
mc_tempIsoMemberItem.set( 0, IsoName_c::IsoNameUnspecified(), 0xFE, IState_c::Active, getMultitonInst() );
setPeriod( 125, false );
getSchedulerInstance().registerTask( *this, 0 );
CNAMESPACE::memset( &m_isoItems, 0x0, sizeof( m_isoItems ) );
// add filter REQUEST_PGN_MSG_PGN via IsoRequestPgn_c
getIsoRequestPgnInstance4Comm().registerPGN (mt_handler, ADDRESS_CLAIM_PGN);
#ifdef USE_WORKING_SET
getIsoRequestPgnInstance4Comm().registerPGN (mt_handler, WORKING_SET_MASTER_PGN);
getIsoRequestPgnInstance4Comm().registerPGN (mt_handler, WORKING_SET_MEMBER_PGN);
#endif
getIsoBusInstance4Comm().insertFilter( mt_customer, IsoAgLib::iMaskFilter_c( 0x3FFFF00UL, ((ADDRESS_CLAIM_PGN)+0xFF)<<8 ), 8 );
#ifdef USE_WORKING_SET
getIsoBusInstance4Comm().insertFilter( mt_customer, IsoAgLib::iMaskFilter_c( 0x3FFFF00UL, (WORKING_SET_MASTER_PGN<<8) ), 8 );
getIsoBusInstance4Comm().insertFilter( mt_customer, IsoAgLib::iMaskFilter_c( 0x3FFFF00UL, (WORKING_SET_MEMBER_PGN<<8) ), 8 );
#endif
setInitialized();
}
void Qd::setMode ()
{
//===Setings pin===//
//pha
pha.settingPinPort(QdDef::PhaPort);
pha.settingPin(QdDef::PhaPin, QdDef::PhaAlt);
//phb
phb.settingPinPort(QdDef::PhbPort);
phb.settingPin(QdDef::PhbPin, QdDef::PhbAlt);
//===Settings timer===//
FTM_SC_REG(ftm_ptr[num_ftm]) = 0;
setPeriod(FTM_MOD_MOD_MASK);
setInitValue(0);
FTM_MODE_REG (ftm_ptr[num_ftm]) |= FTM_MODE_WPDIS_MASK;
FTM_MODE_REG (ftm_ptr[num_ftm]) |= FTM_MODE_FTMEN_MASK;
FTM_CnSC_REG(ftm_ptr[num_ftm], 0) = 0;
FTM_CnSC_REG(ftm_ptr[num_ftm], 1) = 0;
FTM_QDCTRL_REG(ftm_ptr[num_ftm]) |= FTM_QDCTRL_QUADEN_MASK|FTM_QDCTRL_PHAFLTREN_MASK|FTM_QDCTRL_PHBFLTREN_MASK;
FTM_FILTER_REG (ftm_ptr[num_ftm]) |= FTM_FILTER_CH0FVAL(2) | FTM_FILTER_CH1FVAL(2) ;
start ();
}
void TimerOne::initialize(long microseconds)
{
TCCR1A = 0; // clear control register A
TCCR1B = _BV(WGM13); // set mode 8: phase and frequency correct pwm, stop the timer
setPeriod(microseconds);
}
void toggleFunc(int color, void* arg)
{
int msecPerCycle = *(int*)arg;
setPeriod(color, msecPerCycle);
}
/*MAIN*/
int main()
{
//Declarations
int i, j;
int targets;
int center, error, most_blob;
int num_balls = 5;
int dist, prev;
int n = 0;
//Set up GPIO
gpio_export(POWER_LED_PIN);
gpio_set_dir(POWER_LED_PIN, OUTPUT_PIN);
gpio_export(CAMERA_LED_PIN);
gpio_set_dir(CAMERA_LED_PIN, OUTPUT_PIN);
gpio_export(BUTTON_PIN);
gpio_set_dir(BUTTON_PIN, INPUT_PIN);
gpio_export(RELOAD_LED_PIN);
gpio_set_dir(RELOAD_LED_PIN, OUTPUT_PIN);
gpio_export(ARDUINO_PIN);
gpio_set_dir(ARDUINO_PIN, OUTPUT_PIN);
gpio_set_value(ARDUINO_PIN, LOW);
//Set up PWM
initPWM();
usleep(microsecondsToMilliseconds(500));
//Dropper
setPeriod(DROPPER_MOTOR_INDEX, PWM_PERIOD);
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION));
setRun(DROPPER_MOTOR_INDEX, 1);
//Grabber
setPeriod(GRABBER_MOTOR_INDEX, PWM_PERIOD);
setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION);
setRun(GRABBER_MOTOR_INDEX, 1);
//Right wheel
setPeriod(RIGHT_WHEEL_INDEX, PWM_PERIOD);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(RIGHT_WHEEL_INDEX, 0);
//Left wheel
setPeriod(LEFT_WHEEL_INDEX, PWM_PERIOD);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(LEFT_WHEEL_INDEX, 0);
//Roller
setRun(ROLLER_MOTOR_INDEX, 0);
setPeriod(ROLLER_MOTOR_INDEX, PWM_PERIOD);
setDuty(ROLLER_MOTOR_INDEX, PWM_PERIOD*ROLLER_SPEED);
//Set up serial
serial* arduinoSerial;
if (serial_open(&arduinoSerial, ARDUINO_SERIAL_PORT, ARDUINO_BAUD_RATE) == 0){
printf("Opened Ardunio serial port sucessfully\n");
}
else{
printf("Failed to open Arduino serial port\n");
}
/*for(i = 0; i < 50; i++)
{
dist = getDistance(arduinoSerial);
printf("Instensity: %d\n", dist);
usleep(microsecondsToMilliseconds(100));
}
usleep(microsecondsToMilliseconds(5000));*/
//Turn on power LED
gpio_set_value(POWER_LED_PIN, HIGH);
printf("Systems are GO for launch!\n");
waitForButtonPress();
getDistance(arduinoSerial);
//Move to center of the board
goForward();
usleep(microsecondsToMilliseconds(1800));
stopWheels();
//Aim at right target
rotate(180);
if(aimAtSideMostTarget(1))
{
for (; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Move toward left target
rotate(-550);
goForward();
usleep(microsecondsToMilliseconds(2200));
stopWheels();
//Aim at left target
rotate(300);
if(aimAtSideMostTarget(0))
{
for (; num_balls > 1; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(260);
aimAtBiggestTarget();
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
//Move to sideline for reload
rotate(-600);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("%d, ", dist);
usleep(microsecondsToMilliseconds(9));
}
while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1100);
//Turn on reload LED
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
while (n < 3) //big while loop thing!
{
//start from left
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1200));
stopWheels();
rotate(-420);
goForward();
usleep(microsecondsToMilliseconds(590));
stopWheels();
//Aim at left target
if(aimAtSideMostTarget(0))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(420);
goForward();
usleep(microsecondsToMilliseconds(600));
stopWheels();
rotate(-150);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at right target
rotate(400);
goForward();
usleep(microsecondsToMilliseconds(2700));
stopWheels();
rotate(-370);
if(aimAtSideMostTarget(1))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(320);
//usleep(microsecondsToMilliseconds(50));
goForward();
buffer_clear(arduinoSerial);
dist = 0;
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("And another thing....\n");
//printf("%d!!! \n", dist);
usleep(microsecondsToMilliseconds(10));
}while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(-900);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
//start from right
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1350));
stopWheels();
rotate(570);
//Aim at right target
if(aimAtSideMostTarget(1))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(-500);
goForward();
usleep(microsecondsToMilliseconds(4500));
stopWheels();
rotate(650);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at left target
rotate(-200);
if(aimAtSideMostTarget(0))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(-400);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
usleep(microsecondsToMilliseconds(9));
}
while(dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1000);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
} //end of while loop
//Turn off gpio power
gpio_set_value(POWER_LED_PIN, LOW);
gpio_set_value(RELOAD_LED_PIN, LOW);
gpio_set_value(ARDUINO_PIN, LOW);
//Turn off motors
setRun(GRABBER_MOTOR_INDEX, 0);
setRun(DROPPER_MOTOR_INDEX, 0);
setRun(ROLLER_MOTOR_INDEX, 0);
setRun(RIGHT_WHEEL_INDEX, 0);
setRun(LEFT_WHEEL_INDEX, 0);
//Remove GPIO files
//gpio_unexport(PING_PIN);
return 0;
}
void PeriodicTask(efitime_t nowNt) override {
UNUSED(nowNt);
setPeriod(GET_PERIOD_LIMITED(&engineConfiguration->etb));
// set debug_mode 17
if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) {
#if EFI_TUNER_STUDIO
pid.postState(&tsOutputChannels);
tsOutputChannels.debugIntField5 = feedForward;
#endif /* EFI_TUNER_STUDIO */
} else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) {
#if EFI_TUNER_STUDIO
// set debug_mode 29
tsOutputChannels.debugFloatField1 = directPwmValue;
#endif /* EFI_TUNER_STUDIO */
}
if (shouldResetPid) {
pid.reset();
shouldResetPid = false;
}
if (!cisnan(directPwmValue)) {
etb1.dcMotor.Set(directPwmValue);
return;
}
if (boardConfiguration->pauseEtbControl) {
etb1.dcMotor.Set(0);
return;
}
percent_t actualThrottlePosition = getTPS();
if (engine->etbAutoTune) {
autoTune.input = actualThrottlePosition;
bool result = autoTune.Runtime(&logger);
tuneWorkingPid.updateFactors(autoTune.output, 0, 0);
float value = tuneWorkingPid.getOutput(50, actualThrottlePosition);
scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input,
autoTune.output,
value);
scheduleMsg(&logger, "AT PID=%f", value);
etb1.dcMotor.Set(PERCENT_TO_DUTY(value));
if (result) {
scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd());
}
return;
}
percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE);
feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH);
pid.iTermMin = engineConfiguration->etb_iTermMin;
pid.iTermMax = engineConfiguration->etb_iTermMax;
currentEtbDuty = feedForward +
pid.getOutput(targetPosition, actualThrottlePosition);
etb1.dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty));
if (engineConfiguration->isVerboseETB) {
pid.showPidStatus(&logger, "ETB");
}
}
void UARTworker(void)
{
unsigned char c,mode=0,addr=0,instruction=0,EEaddrF=0,EEaddr=0,adcc=0,helpC;
initUART();
//write start message (menu)
UARTwriteString(msgMenu[0]);
UARTwrite('\n');
while(1)
{
if(RCIF)
{
RCIF=0;
LED2ON;
if(!(RCSTA&0b00000110))
{ rhead++;
rhead&=RINGBUFFMASK;
ringbuff[rhead]=RCREG;
}
LED2OFF;
c=UARTread();
UARTwrite(c);
//c=UARTcharFromString(c);
switch (mode)
{
case 0:
mode=c-48;
UARTwriteString(msgMenu[c-48]);
if(mode==2)enablePWM();
else if(mode==3)enableDAC();
break;
case 1://ADC
switch(c)
{
case 'r'://single read
UARTwriteString("\n\nADC value: ");
helpC=getADC(adcc);
UARTwriteDecimal(helpC);
UARTwriteString(msgMenu[1]);
break;
case '1'://chanell one
UARTwriteString("\n\nchannel 1 selected");
adcc=0;
UARTwriteString(msgMenu[1]);
break;
case '2'://chanel two
UARTwriteString("\n\nchannel 2 selected");
adcc=1;
UARTwriteString(msgMenu[1]);
break;
case '3'://chanell three
UARTwriteString("\n\nchannel 3 selected");
adcc=2;
UARTwriteString(msgMenu[1]);
break;
case 't'://temp
UARTwriteString("\n\nTemp sensor selected");
adcc=3;
UARTwriteString(msgMenu[1]);
break;
case 'm'://back to start
mode = 0;
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
break;
case 2://PWM
if(instruction)
{
switch(instruction)
{
case 'p':
//pwm period = c;
setPeriod(UARTcharFromString(c));
UARTwriteString(msgMenu[2]);
break;
case 'd':
setDuty(UARTcharFromString(c));
UARTwriteString(msgMenu[2]);
//pwm period =c;
break;
case 'm':
mode =0;
//pwm off
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
instruction = 0;
}
else
{
instruction = c; //loads the instruction
if(instruction == 'p')
{
UARTwriteString("\n\nEnter the PWM Period: ");
}
else if(instruction == 'd')
{
UARTwriteString("\n\nEnter the PWM Duty Cycle: ");
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
disablePWM();
UARTwriteString(msgMenu[0]);
}
}
break;
case 3://DAC
if(instruction)
{
switch(instruction)
{
case 'v': //enter woltage
setDAC(UARTcharFromString(c));
UARTwriteString(msgMenu[3]);
break;
case 'm':
mode = 0;
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
instruction =0;
}
else
{
instruction = c; //loads the instruction
if(instruction == 'v')
{
UARTwriteString(msgDACsetV);
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
disableDAC();
UARTwriteString(msgMenu[0]);
}
}
break;
case 4://MEM
if(instruction) //if instruction has been sent previusly
{
if(EEaddrF) //instruction was sent previusly, check if address was sent
{ //address was sent
if(instruction == 'w') //if instruction was W-writes recived character to EEProm[ADDR]
{
EEPROMwrite(EEaddr,UARTcharFromString(c));
UARTwriteString(msgMenu[4]);
//write c to eeprom
}
else if (instruction == 'r') //if instruction was R-reads EEprom[addr] from eeprom
{
UARTwriteDecimal(EEPROMread(EEaddr));
UARTwriteString(msgMenu[4]);
}
else if (instruction == 'm') //if instruction was m --returns to start menu...
{
mode = 0;
UARTwriteString(msgMenu[0]);
}
EEaddrF=0; //clears the addressing flag
instruction =0; //clears the istruction flag
}
else
{
EEaddrF=1; //sets the address flage
EEaddr=UARTcharFromString(c);
if(instruction=='w')UARTwriteString(msgEEw);
else if(instruction == 'r')UARTwriteString("\n\nHit any key to read from EEPROM.\n\n");
}
}
else
{
instruction = c; //loads the instruction
if((instruction == 'w')||(instruction == 'r'))
{
UARTwriteString(msgEEaddr);
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
UARTwriteString(msgMenu[0]);
}
}
break;
default:
mode=0;
UARTwriteString(msgMenu[0]);
break;
}
}
}
}
SDController::SDController(): PERIODMAX(10), FAC(8)
{
sFile.open("neuron.txt",ios::out);
/**************************** two neuron network setup **************************************/
//outputs
o1_=0;
o2_=0;
//counter
n_= 0;
//difference values
diff_n1_=0;
diff_n2_=0;
diffi_ = 0;
//maximum period
percount = 0;
//arrays for two neuron network output
data_ = new data_t[PERIODMAX+1]; //(data_t*) calloc (periodmax_+1,sizeof(data_t));
data_[0].o1 = 0;
data_[0].o2 = 0;
//control matrix (unity)
NDIRS_ = 4;
DIRS_ = new int[NDIRS_]; //(int*) calloc (4,sizeof(int));
DIRS_[0]=1;
DIRS_[1]=0;
DIRS_[2]=0;
DIRS_[3]=1;
//two neuron connection weights
w11_ = -22.;
w21_ = -6.6 ;
w12_ = 5.9;
//two neuron bias terms
theta1_ = -3.4;
theta2_ = 3.8;
//learning rate
lr_ = 0.05; //0.05;//0.08;
//initial period
setPeriod(1);
//control inputs
input1_ = input2_ = 0;
//control factor
cL_ =0;
update_ = 2;
/**************************** motor control **************************************/
lin_[0].setInverse(+1.);
lin_[1].setInverse(-1.);
thetahys_[0] = -0.5;
thetahys_[1] = -0.6;
}
void PwmSysfsDriverNode::periodCallback(std_msgs::Float64::ConstPtr const& msg)
{
setPeriod(msg->data);
}