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
