BABER
Hectopat
Daha fazla
- Cinsiyet
- Erkek
Bu kodu çizgi izleyen robotumda kullanıyorum ancak sadece sürekli sola dönüyor. Motorları, motor test kodu ile denedim sorun yok olması gerektiği gibi dönüyorlar. Diyagramdaki bağlantılar ile birebir aynı bağlı. Sıkıntı ne olabilir?
Kod:
#include <QTRSensors.h>
#define NUM_SENSORS 8
#define TIMEOUT 2500
#define EMITTER_PIN QTR_NO_EMITTER_PIN
QTRSensorsRC qtrrc((unsigned char[]) {2, 4, 5, 6, 7, 8, 9,10},NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS]; // array with individual sensor reading values
unsigned int line_position=0;
int pwm_a = 3;
int pwm_b = 11;
int dir_a = 12;
int dir_b = 13;
int calSpeed = 85;
float error=0;
float PV =0 ;
float kp = 1;
int m1Speed=0;
int m2Speed=0;
void setup()
{
pinMode(pwm_a, OUTPUT);
pinMode(pwm_b, OUTPUT);
pinMode(dir_a, OUTPUT);
pinMode(dir_b, OUTPUT);
analogWrite(pwm_a, 0);
analogWrite(pwm_b, 0);
delay(2000);
for (int i = 0; i <= 100; i++) // begin calibration cycle to last about 2.5 seconds (100*25ms/call)
{
if (i==0 || i==60)
{
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, calSpeed);
digitalWrite(dir_b, HIGH);
analogWrite(pwm_b, calSpeed);
}
else if (i==20 || i==100)
{
digitalWrite(dir_a, HIGH);
analogWrite(pwm_a, calSpeed);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, calSpeed);
}
qtrrc.calibrate();
}
line_position = qtrrc.readLine(sensorValues);
while (sensorValues[6] < 200)
{
line_position = qtrrc.readLine(sensorValues);
}
while (line_position > 4350)
{
line_position = qtrrc.readLine(sensorValues);
}
analogWrite(pwm_b, 0);
analogWrite(pwm_a, 0);
delay(1000);
}
void loop() // main loop
{
unsigned int line_position = qtrrc.readLine(sensorValues);
follow_line(line_position);
}
void follow_line(int line_position)
{
switch(line_position)
{
// Line has moved off the left edge of sensor
// This will make it turn fast to the left
case 7000:
digitalWrite(dir_a, HIGH);
analogWrite(pwm_a, 200);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, 200);
break;
case 0:
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, 200);
digitalWrite(dir_b, HIGH);
analogWrite(pwm_b, 200);
break;
default:
error = (float)line_position - 3500;
PV = kp * error;
if (PV > 55)
{
PV = 55;
}
if (PV < -55)
{
PV = -55;
}
m1Speed = 200 - PV;
m2Speed = 200 + PV;
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, m1Speed);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, m2Speed);
break;
}
}
