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;
}
/* 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);
}