|
Fizik jarayonlarni kompyuterda modellashtirish
|
| bet | 123/135 | | Sana | 22.05.2024 | | Hajmi | 15,08 Mb. | | #250347 |
Bog'liq Fizik jarayonlarni kompyuterda modellashtirishRobot_v02.ino
// Motorlar M1+,M1-,M2+,M2- klemmalarga ulanadi
// shield motori 6,5,7,4 to’rtta kontaktni boshqarish uchun qo’llaydi
#define SPEED_LEFT 6
#define SPEED_RIGHT 5
#define DIR_LEFT 7
#define DIR_RIGHT 4
#define LEFT_SENSOR_PIN 8
#define RIGHT_SENSOR_PIN 9
// Oldinga biz harakat qiladigan tezlik(0-255)
#define SPEED 100
// Murakkab jarayonlarni o’tash uchun tezlik
#define SLOW_SPEED
#define BACK_SLOW_SPEED 30
#define BACK_FAST_SPEED 50
// Necha martaga tormoz qilish zarurligini aniqlaydigan koeffitsient
// burish uchun g’ildiraklarning biri
#define BRAKE_K 4
#define STATE_FORWARD 0
#define STATE_RIGHT 1
#define STATE_LEFT 2
#define SPEED_STEP 2
#define FAST_TIME_THRESHOLD 500
int state = STATE_FORWARD;
int currentSpeed = SPEED;
int fastTime = 0;
void runForward()
{
state = STATE_FORWARD;
fastTime += 1;
if (fastTime < FAST_TIME_THRESHOLD) {
currentSpeed = SLOW_SPEED;
} else {
currentSpeed = min(currentSpeed + SPEED_STEP, SPEED);
}
analogWrite(SPEED_LEFT, currentSpeed);
analogWrite(SPEED_RIGHT, currentSpeed);
digitalWrite(DIR_LEFT, HIGH);
digitalWrite(DIR_RIGHT, HIGH);
}
void steerRight()
{
state = STATE_RIGHT;
fastTime = 0;
// burishni boshlash uchun chap g’ildirakni,
// o'ng g'ildirak hisobiga harakatini kamaytiramiz
analogWrite(SPEED_RIGHT, 0);
analogWrite(SPEED_LEFT, SPEED);
digitalWrite(DIR_LEFT, HIGH);
digitalWrite(DIR_RIGHT, HIGH);
}
void steerLeft()
{
state = STATE_LEFT;
fastTime = 0;
analogWrite(SPEED_LEFT, 0);
analogWrite(SPEED_RIGHT, SPEED);
digitalWrite(DIR_LEFT, HIGH);
digitalWrite(DIR_RIGHT, HIGH);
}
void stepBack(int duration, int state) {
if (!duration)
return;
// Burilish yo’nalishiga bog’liq orqaga harakat qilganda kata bo’lmagan burilish qilamiz
// burilish qilamiz
int leftSpeed = (state == STATE_RIGHT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
int rightSpeed = (state == STATE_LEFT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
analogWrite(SPEED_LEFT, leftSpeed);
analogWrite(SPEED_RIGHT, rightSpeed);
// реверс колёс
digitalWrite(DIR_RIGHT, LOW);
digitalWrite(DIR_LEFT, LOW);
delay(duration);
}
void setup()
{
// Signallarni chiqishiga 4,5,6,7 platalarning chiqishini to’g’irlaymiz
for(int i = 4; i <= 7; i++)
pinMode(i, OUTPUT);
// Darrov oldinga boramiz
runForward();
}
void loop()
{
// Robotimiz qora trekli oq maydon bo’yicha harakat qilyaptiAks holda
// datchiklardan qiymatlarni invariantalash keragi yo’q
boolean left = !digitalRead(LEFT_SENSOR_PIN);
boolean right = !digitalRead(RIGHT_SENSOR_PIN);
// Qanday holga o’tish kerak?
int targetState;
if (left == right) {
// sensorlar ostida hamma narsa oq yoki qora
// oldinga boramiz
targetState = STATE_FORWARD;
} else if (left) {
// chap sonsor trekga tiralgan
// chapga buramiz
targetState = STATE_LEFT;
} else {
targetState = STATE_RIGHT;
}
if (state == STATE_FORWARD && targetState != STATE_FORWARD) {
int brakeTime = (currentSpeed > SLOW_SPEED) ?
currentSpeed : 0;
stepBack(brakeTime, targetState);
}
switch (targetState) {
case STATE_FORWARD:
runForward();
break;
case STATE_RIGHT:
steerRight();
break;
case STATE_LEFT:
steerLeft();
break;
}
}
|
| |
