void Shooter::run () { while(true) { computeTurn(); // for the speed of the shooter if(getoShooterSpeed != -1) pid->SetSetpoint(getoShooterSpeed/-60.); else { //pid->SetSetpoint(0); pid->SetSetpoint(computeSpeed(computeDistance())/-60.); //printf("encoder %f %f\n", encoder.GetRate()*60, distance.GetVoltage()/.0098); } cout << encoder.GetRate() << endl; /*double intPart; turret->Set(modf(TurretLocation, &intPart)); if(LimitTurret.Get() == 1) { // I am rezeroing the turret because it might slip on the turning and this will reset it as it should be turret->EnableControl(0); // I think that this has to get reset to the jaguar after it is enabled turret->ConfigEncoderCodesPerRev((int)(500 * config->GetValue("turretMotorTurns"))); turret->SetPID(config->GetValue("turretP"), config->GetValue("turretI"), config->GetValue("turretD")); }*/ Wait(.05); } }
bool ControlTrajectoryJulian::getSpeed(float& forward,float& turn) { bool automatic=computeSpeed(); forward=speed; turn=rot; return automatic; }
void Odometer::imu_cb(const sensor_msgs::Imu::ConstPtr &msg) { if (!m_bReady) { m_vBaseAcc.push_back(msg->linear_acceleration); genMinMaxAcc(); } if (( msg->header.stamp - m_LastTime).toSec() > 0.1) { geometry_msgs::Vector3 v3 = linearAccFilter(msg->linear_acceleration); computeOrientation(msg->orientation); computeDeltaPose(v3, msg->header.stamp); computeSpeed(v3, msg->header.stamp); m_LastTime = msg->header.stamp; if (m_bReady) m_Pub.publish(m_Odom); } }
void ControlManagerJulian::getSpeed(float& forward,float& turn) { computeSpeed(); forward=speed; turn=rot; }
void antSpeedOnlyProcessing::createSPB7ResultString ( double &speed, unsigned int dataPage, unsigned int deltaBikeSpeedEventTime, unsigned int deltaWheelRevolutionCount, unsigned int additionalData1, unsigned int additionalData2, unsigned int additionalData3, double wheelCircumference, unsigned int numberOfMagnets, unsigned int &zeroTime ) { if ( !semiCookedOut ) { speed = computeSpeed ( speed, deltaWheelRevolutionCount, deltaBikeSpeedEventTime, wheelCircumference, numberOfMagnets, zeroTime, maxZeroTime ); } if ( outputAsJSON ) { if ( semiCookedOut ) { appendJSONItem( "delta bike speed event time", deltaBikeSpeedEventTime ); appendJSONItem( "delta wheel revolution count", deltaWheelRevolutionCount ); } else { appendJSONItem( C_SPEED_JSON, speed, getValuePrecision() ); appendJSONItem( "wheel circumference", wheelCircumference, getValuePrecision() ); appendJSONItem( "number of magnets", numberOfMagnets ); } appendJSONItem( C_DATA_PAGE_JSON, dataPage ); if ( ( dataPage & 0x0F ) == 1 ) { if ( semiCookedOut ) { appendJSONItem( "delta operating time", additionalData1 ); } else { appendJSONItem( "operating time", 2 * additionalData1 ); } } else if ( ( dataPage & 0x0F ) == 2 ) { appendJSONItem( C_MANUFACTURER_JSON, additionalData1 ); appendJSONItem( C_SERIAL_NUMBER_JSON, additionalData2 ); } else if ( ( dataPage & 0x0F ) == 3 ) { appendJSONItem( C_MANUFACTURER_JSON, additionalData1 ); appendJSONItem( C_HARDWARE_REVISION_JSON, additionalData2 ); appendJSONItem( C_MODEL_NUMBER_JSON, additionalData3 ); } else { appendJSONItem( "additional data 1", additionalData1 ); appendJSONItem( "additional data 1", additionalData2 ); appendJSONItem( "additional data 3", additionalData3 ); } } else { if ( semiCookedOut ) { appendOutput( ( unsigned int ) dataPage ); appendOutput( deltaBikeSpeedEventTime ); appendOutput( deltaWheelRevolutionCount ); if ( ( dataPage & 0x0F ) != 0 ) { appendOutput( additionalData1 ); // deltaOperatingTime (1) / manufacturerID (2) / hwVersion (3) if ( ( dataPage & 0x0F ) != 1 ) { appendOutput( additionalData2 ); // serialNumber (2) / swVersion (3) if ( ( dataPage & 0x0F ) != 2 ) { appendOutput( additionalData3 ); // modelNumber (3) } } } } else { appendOutput( speed, getValuePrecision() ); appendOutput( wheelCircumference, getValuePrecision() ); appendOutput( numberOfMagnets ); appendOutput( dataPage ); if ( ( dataPage & 0x0F ) != 0 ) { if ( ( dataPage & 0x0F ) == 1 ) { // operatingTimeSeconds appendOutput( 2 * additionalData1 ); } else { appendOutput( additionalData1 ); // manufacturerID (2) / hwVersion (3) appendOutput( additionalData2 ); // serialNumber (2) / swVersion (3) if ( ( dataPage & 0x0F ) != 2 ) { appendOutput( additionalData3 ); // modelNumber (3) } } } } } }
/** * Runs of the SMPSO algorithm. * @return a <code>SolutionSet</code> that is a set of non dominated solutions * as a result of the algorithm execution */ SolutionSet * PSO::execute() { initParams(); success_ = false; globalBest_ = NULL; //->Step 1 (and 3) Create the initial population and evaluate for (int i = 0; i < particlesSize_; i++) { Solution * particle = new Solution(problem_); problem_->evaluate(particle); evaluations_ ++; particles_->add(particle); if ((globalBest_ == NULL) || (particle->getObjective(0) < globalBest_->getObjective(0))) { if (globalBest_!= NULL) { delete globalBest_; } globalBest_ = new Solution(particle); } } //-> Step2. Initialize the speed_ of each particle to 0 for (int i = 0; i < particlesSize_; i++) { speed_[i] = new double[problem_->getNumberOfVariables()]; for (int j = 0; j < problem_->getNumberOfVariables(); j++) { speed_[i][j] = 0.0; } } //-> Step 6. Initialize the memory of each particle for (int i = 0; i < particles_->size(); i++) { Solution * particle = new Solution(particles_->get(i)); localBest_[i] = particle; } //-> Step 7. Iterations .. while (iteration_ < maxIterations_) { int * bestIndividualPtr = (int*)findBestSolution_->execute(particles_); int bestIndividual = *bestIndividualPtr; delete bestIndividualPtr; computeSpeed(iteration_, maxIterations_); //Compute the new positions for the particles_ computeNewPositions(); //Mutate the particles_ //mopsoMutation(iteration_, maxIterations_); //Evaluate the new particles_ in new positions for (int i = 0; i < particles_->size(); i++) { Solution * particle = particles_->get(i); problem_->evaluate(particle); evaluations_ ++; } //Actualize the memory of this particle for (int i = 0; i < particles_->size(); i++) { //int flag = comparator_.compare(particles_.get(i), localBest_[i]); //if (flag < 0) { // the new particle is best_ than the older remember if ((particles_->get(i)->getObjective(0) < localBest_[i]->getObjective(0))) { Solution * particle = new Solution(particles_->get(i)); delete localBest_[i]; localBest_[i] = particle; } // if if ((particles_->get(i)->getObjective(0) < globalBest_->getObjective(0))) { Solution * particle = new Solution(particles_->get(i)); delete globalBest_; globalBest_ = particle; } // if } iteration_++; } // Return a population with the best individual SolutionSet * resultPopulation = new SolutionSet(1); int * bestIndexPtr = (int *)findBestSolution_->execute(particles_); int bestIndex = *bestIndexPtr; delete bestIndexPtr; cout << "Best index = " << bestIndex << endl; Solution * s = particles_->get(bestIndex); resultPopulation->add(new Solution(s)); // Free memory deleteParams(); return resultPopulation; } // execute
/** * Runs of the SMPSO algorithm. * @return a <code>SolutionSet</code> that is a set of non dominated solutions * as a result of the algorithm execution */ SolutionSet *SMPSOhv::execute() { initParams(); success = false; //->Step 1 (and 3) Create the initial population and evaluate for (int i = 0; i < swarmSize; i++){ Solution *particle = new Solution(problem_); problem_->evaluate(particle); problem_->evaluateConstraints(particle); particles->add(particle); } //-> Step2. Initialize the speed of each particle to 0 for (int i = 0; i < swarmSize; i++) { speed[i] = new double[problem_->getNumberOfVariables()]; for (int j = 0; j < problem_->getNumberOfVariables(); j++) { speed[i][j] = 0.0; } } // Step4 and 5 for (int i = 0; i < particles->size(); i++){ Solution *particle = new Solution(particles->get(i)); if (leaders->add(particle) == false){ delete particle; } } //-> Step 6. Initialize the memory of each particle for (int i = 0; i < particles->size(); i++){ Solution *particle = new Solution(particles->get(i)); best[i] = particle; } //Crowding the leaders_ //distance->crowdingDistanceAssignment(leaders, problem_->getNumberOfObjectives()); leaders->computeHVContribution(); //-> Step 7. Iterations .. while (iteration < maxIterations) { //Compute the speed_ computeSpeed(iteration, maxIterations); //Compute the new positions for the particles_ computeNewPositions(); //Mutate the particles_ mopsoMutation(iteration,maxIterations); //Evaluate the new particles in new positions for (int i = 0; i < particles->size(); i++){ Solution *particle = particles->get(i); problem_->evaluate(particle); problem_->evaluateConstraints(particle); } //Update the archive for (int i = 0; i < particles->size(); i++) { Solution *particle = new Solution(particles->get(i)); if (leaders->add(particle) == false) { delete particle; } } //Update the memory of this particle for (int i = 0; i < particles->size(); i++) { int flag = dominance->compare(particles->get(i), best[i]); if (flag != 1) { // the new particle is best_ than the older remembered Solution *particle = new Solution(particles->get(i)); delete best[i]; best[i] = particle; } } iteration++; } // Build the solution set result SolutionSet * result = new SolutionSet(leaders->size()); for (int i=0;i<leaders->size();i++) { result->add(new Solution(leaders->get(i))); } // Free memory deleteParams(); return result; } // execute