示例#1
0
int8_t task_speedreg(int8_t state) //Period: 25Hz
{
	//Turn motor off?
	if(mot.off)// || mot.off_invisible)
	{
		mot.d[LEFT].speed.to = 0;
		mot.d[RIGHT].speed.to = 0;
	}

	if(mot_driver_standby)
		motor_activate(0); //Shut down motor driver
	else
	{
		motor_activate(1); //Activate motor driver
		controlSpeed(); //Speed Regulation
	}

	return 0;
}
示例#2
0
/* calculate distances frontDistance, leftDistance, rightDistance for detecting corners */
void control::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    const unsigned int N = msg->ranges.size();
    const double angleResolution = msg->angle_increment *180/M_PI; //TO DO: test!

    //std::cout << "laserCallback: N " << N<< std::endl;
    //std::cout << "laserCallback: angleResolution " << angleResolution << std::endl;

    double frontDistance = 0.0; // average distance in front of the car for a 10 degree angle
	double minimumDistance = 10.0; // minimum distance in front of the car for a 10 degree angle
	
    // frontDistance
    int counter = 0;
    for (int i=N/2-5*(int)(1/angleResolution); i < N/2+5*(1/angleResolution); i++)
    {
        if (msg->ranges[i]>0)
        {
        //std::cout << "laserCallback: i " << i<< std::endl;
        
		// approximation by computing average
		frontDistance += msg->ranges[i];
        //std::cout << "laserCallback: msg->ranges[i] " << msg->ranges[i]<< std::endl;

        counter ++;

        if(msg->ranges[i] < minimumDistance)
            minimumDistance = msg->ranges[i];
        }
    }
    //std::cout << "laserCallback: Summe " << frontDistance<< std::endl;

    frontDistance = frontDistance/((double) counter);
    //std::cout << "laserCallback: frontDistance " << frontDistance<< std::endl;

    /* now speed is adjusted to distances */
    controlSpeed(frontDistance);
}