# JJRobots COMMUNITY

Full Version: estimated robot speed
You're currently viewing a stripped down version of our content. View the full version with proper formatting.
Hi if there is someone to explain me this part of the program please

// We calculate the estimated robot speed:
// Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
//actual_robot_speed_Old = actual_robot_speed;
actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward units: 200steps*rev*16(microstepping)= 3200steps/seg/rev = (/50 factor) = 64steps/seg/360ÃƒÆ’Ã¢â‚¬Å¡Ãƒâ€šÃ‚Âº =>

int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 25.0; // 25 is an empirical extracted factor to adjust for real units
int16_t estimated_speed = -actual_robot_speed + angular_velocity;
//int16_t estimated_speed = -actual_robot_speed;
//estimated_speed_filtered = (estimated_speed_filtered*9 + estimated_speed)/10; // low pass filter on estimated speed
estimated_speed_filtered = estimated_speed_filtered * 0.9 + (float)estimated_speed * 0.1; // low pass filter on estimated speed

#if DEBUG==2
Serial.print(" ");
Serial.println(estimated_speed_filtered);
#endif

if (positionControlMode)
{
// POSITION CONTROL. INPUT: Target steps for each motor. Output: motors speed
motor1_control = positionPDControl3(steps1, target_steps1, Kp_position, Kd_position, speed_M1);
motor2_control = positionPDControl3(steps2, target_steps2, Kp_position, Kd_position, speed_M2);

//Serial.print(motor1_control);
//Serial.print(" ");
//Serial.println(motor2_control);

// Convert from motor position control to throttle / steering commands
throttle = (motor1_control + motor2_control) / 2;
throttle = constrain(throttle, -190, 190);
steering = motor2_control - motor1_control;
steering = constrain(steering, -50, 50);
}
Reference URL's
• JJRobots COMMUNITY: http://forums.jjrobots.com/index.php
• :