تطبيق خوارزمية لتحديد عرض كائن باستخدام محدد المدى والمشفرات بالموجات فوق الصوتية بلغة RobotC

عصور ما قبل التاريخ

قاعدة عنصرية

مهمة

اساس نظرى

معلمات الإدخال

• dist = 100 مم (المسافة التي يتوقف عندها الروبوت للقياس ، يتم تحديد المعلمة بناءً على وجود أشياء أخرى حول الجسم قد تقع في مجال رؤية المستشعر وتتداخل مع القياس)
• d_wheel = 102 مم (قطر عجلة الروبوت)
• d_ball = 102 مم (قطر الكرة الفعلي)

• الخطأ = القيمة المطلقة (d_ball_exper - d_ball)

خوارزمية تشغيل البرنامج

1. الحركة إلى الكائن.
2. قم بالتدوير إلى أقصى نقطة في الجسم.
3. استدارة إلى أقصى يسار الكائن.
4. حساب الزاوية باستخدام قراءات مأخوذة من برامج التشفير.
5. حساب الخطأ.

تنفيذ RobotC الخوارزمية

#pragma config(StandardModel, "VEX Launchbot")

int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}

return getMotorEncoder(leftMotor);
}

• leftSpeed ​​- سرعة محرك الأقراص الأيسر
• rightSpeed ​​- سرعة القيادة الصحيحة
• rightSpeed ​​- سرعة القيادة الصحيحة
• المسافة - المسافة إلى الكائن حيث توقف الروبوت
• need_object - وضع الحركة: صحيح ، إذا كانت هناك حاجة إلى كائن في مجال رؤية المستشعر ، خطأ إذا كانت هناك حاجة إلى كائن
• الجانب الأيسر - اتجاه دوران الروبوت. صحيح - استدر يسارًا ، خطأ - استدر يمينًا

task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;

int dist = 10; //sm
//  
setMotor(armMotor, -50);
wait(1);

while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();

float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
	
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}

بيانات تلخيصية

الآفاق