Post Reply 
 
Thread Rating:
  • 0 Votes - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
estimated robot speed
05-09-2017, 08:52 AM
Post: #1
estimated robot speed
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(angle_adjusted);
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);
}
Find all posts by this user
Quote this message in a reply
Post Reply 


Messages In This Thread
estimated robot speed - hordlord - 05-09-2017 08:52 AM

Forum Jump:


User(s) browsing this thread: 1 Guest(s)