 estimated robot speed - Printable Version +- JJRobots COMMUNITY (http://forums.jjrobots.com) +-- Forum: JJrobots (/forumdisplay.php?fid=1) +--- Forum: B-Robot (/forumdisplay.php?fid=2) +--- Thread: estimated robot speed (/showthread.php?tid=1251) estimated robot speed - hordlord - 05-09-2017 08:52 AM 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); }