Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

here is my code:

int min_dist = 1e6;

if(key1 pressed)

       {
           joint1->SetForce(0,15);
           joint2->SetForce(0,15);
        }

        else if(key2 pressed)
        {
            joint1->SetForce(0,-40);
            joint2->SetForce(0,-40);
        }

         for(unsigned int i = 0; i < n; i++)
            {
              if(this->sicksensor->GetRange(i) < min_dist)
                min_dist = this->sicksensor->GetRange(i);

            }

            if(min_dist < this->sicksensor->GetRangeMax())
            {
              printf("object is detected");
              double velocity=this->joint1->GetForce(0);
              int t=1;
              double acceleration=(velocity/t);
              double target_dist=((velocity*velocity)/(2*acceleration));

              if(min_dist<=target_dist)
              {

double steeringRange=this->steering_joint->GetHighStop(0).Radian()-this->steering_joint->GetLowStop(0).Radian();

                  double steeringRatio= steeringRange / tireAngleRange;

                  double steeringAngle=this->steering_joint->GetAngle(0).Radian();

                  double wheelAngle=steeringAngle/steeringRatio;


                  this->joint1->SetLowStop(0,wheelAngle);
                  this->joint1->SetHighStop(0,wheelAngle);
                  this->joint1->SetLowStop(0,wheelAngle);
                  this->joint1->SetHighStop(0,wheelAngle);

                  this->joint2->SetHighStop(0,wheelAngle);
                  this->joint2->SetLowStop(0,wheelAngle);
                  this->joint2->SetHighStop(0,wheelAngle);
                  this->joint2->SetLowStop(0,wheelAngle);

              }
            }