Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

T9 #1

Open
oudom021 opened this issue Dec 6, 2023 · 2 comments
Open

T9 #1

oudom021 opened this issue Dec 6, 2023 · 2 comments

Comments

@oudom021
Copy link

oudom021 commented Dec 6, 2023

#include <QTRSensors.h>

// QTRSensors
QTRSensors qtr;

// Number of sensors to use
#define IR 8
unsigned short qtrValues[IR];
#define Kp 0.1 // 255: 0.1 110: 0.2
#define Ki 0.05 // 255: 0.05 110: 0.05
#define Kd 0.003 // 255: 0.003 110: 0.004
#define rightMaxSpeed 120 // 255 50
#define leftMaxSpeed 120 // 255 50
int SetPoint = (IR - 1) * 500;
//motorA
int enA = 6;
int in1 = 8;
int in2 = 7;
//motorB
int enB = 3;
int in3 = 4;
int in4 = 5;

int lastError = 0;
unsigned long cTime, pTime;
float eTime;
float P_error;
float I_error;
float D_error;
float PID_value;

void setup() {
Serial.begin(9600);
qtr.setTypeAnalog();

qtr.setSensorPins((const uint8_t[]){ A7, A6, A5, A4, A3, A2, A1, A0 }, IR);
for (uint8_t i = 0; i < 250; i++)
if (i % 5 == 0) {
qtr.calibrate();
delay(20);
}

pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}

void run_fwd(int valueSA, int valueSB) {
// Motor A
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}
void turn_right(int valueSA, int valueSB) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void turn_left(int valueSA, int valueSB) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void Run_robot() {
int med_Speed_R;
int med_Speed_L;
int position = qtr.readLineBlack(qtrValues);
P_error = position - SetPoint;
cTime = millis();
eTime = (float)(cTime - pTime) / 1000;
I_error = I_error * 2 / 3 + P_error * eTime;
D_error = (P_error - lastError) / eTime;
PID_value = Kp * P_error + Ki * I_error + Kd * D_error;
lastError = P_error;
pTime = cTime;

med_Speed_L = leftMaxSpeed - abs(PID_value);
med_Speed_R = rightMaxSpeed - abs(PID_value);
int leftMotorSpeed = med_Speed_L + PID_value;
int rightMotorSpeed = med_Speed_R - PID_value;
leftMotorSpeed = constrain(leftMotorSpeed, 0, leftMaxSpeed);
rightMotorSpeed = constrain(rightMotorSpeed, 0, rightMaxSpeed);
run_fwd(leftMotorSpeed, rightMotorSpeed);
if(qtrValues[0] > 950 && qtrValues[7] > 950 ){
run_fwd(0,0);
}
// Determine whether to turn left or right based on the PID value
// if (PID_value > 0) {
// // Turn left
// turn_left(leftMotorSpeed, rightMotorSpeed);
// } else {
// // Turn right
// turn_right(leftMotorSpeed, rightMotorSpeed);
// }

delayMicroseconds(80);
}

void loop() {
Run_robot();
}
@oudom021
Copy link
Author

oudom021 commented Dec 6, 2023

#include <QTRSensors.h>

// QTRSensors
QTRSensors qtr;

// Number of sensors to use
#define IR 8
unsigned short qtrValues[IR];
#define Kp 0.1 // 255: 0.1 110: 0.2
#define Ki 0.05 // 255: 0.05 110: 0.05
#define Kd 0.003 // 255: 0.003 110: 0.004
#define rightMaxSpeed 120 // 255 50
#define leftMaxSpeed 120 // 255 50
int SetPoint = (IR - 1) * 500;
//motorA
int enA = 6;
int in1 = 8;
int in2 = 7;
//motorB
int enB = 3;
int in3 = 4;
int in4 = 5;

int lastError = 0;
unsigned long cTime, pTime;
float eTime;
float P_error;
float I_error;
float D_error;
float PID_value;

void setup() {
Serial.begin(9600);
qtr.setTypeAnalog();

qtr.setSensorPins((const uint8_t[]){ A7, A6, A5, A4, A3, A2, A1, A0 }, IR);
for (uint8_t i = 0; i < 250; i++)
if (i % 5 == 0) {
qtr.calibrate();
delay(20);
}

pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}

void run_fwd(int valueSA, int valueSB) {
// Motor A
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}
void turn_right(int valueSA, int valueSB) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void turn_left(int valueSA, int valueSB) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void Run_robot() {
int med_Speed_R;
int med_Speed_L;
int position = qtr.readLineBlack(qtrValues);
P_error = position - SetPoint;
cTime = millis();
eTime = (float)(cTime - pTime) / 1000;
I_error = I_error * 2 / 3 + P_error * eTime;
D_error = (P_error - lastError) / eTime;
PID_value = Kp * P_error + Ki * I_error + Kd * D_error;
lastError = P_error;
pTime = cTime;

med_Speed_L = leftMaxSpeed - abs(PID_value);
med_Speed_R = rightMaxSpeed - abs(PID_value);
int leftMotorSpeed = med_Speed_L + PID_value;
int rightMotorSpeed = med_Speed_R - PID_value;
leftMotorSpeed = constrain(leftMotorSpeed, 0, leftMaxSpeed);
rightMotorSpeed = constrain(rightMotorSpeed, 0, rightMaxSpeed);
run_fwd(leftMotorSpeed, rightMotorSpeed);
if(qtrValues[0] > 950 && qtrValues[7] > 950 ){
run_fwd(0,0);
}
// Determine whether to turn left or right based on the PID value
// if (PID_value > 0) {
// // Turn left
// turn_left(leftMotorSpeed, rightMotorSpeed);
// } else {
// // Turn right
// turn_right(leftMotorSpeed, rightMotorSpeed);
// }

delayMicroseconds(80);
}

void loop() {
Run_robot();
}

1 similar comment
@oudom021
Copy link
Author

oudom021 commented Dec 6, 2023

#include <QTRSensors.h>

// QTRSensors
QTRSensors qtr;

// Number of sensors to use
#define IR 8
unsigned short qtrValues[IR];
#define Kp 0.1 // 255: 0.1 110: 0.2
#define Ki 0.05 // 255: 0.05 110: 0.05
#define Kd 0.003 // 255: 0.003 110: 0.004
#define rightMaxSpeed 120 // 255 50
#define leftMaxSpeed 120 // 255 50
int SetPoint = (IR - 1) * 500;
//motorA
int enA = 6;
int in1 = 8;
int in2 = 7;
//motorB
int enB = 3;
int in3 = 4;
int in4 = 5;

int lastError = 0;
unsigned long cTime, pTime;
float eTime;
float P_error;
float I_error;
float D_error;
float PID_value;

void setup() {
Serial.begin(9600);
qtr.setTypeAnalog();

qtr.setSensorPins((const uint8_t[]){ A7, A6, A5, A4, A3, A2, A1, A0 }, IR);
for (uint8_t i = 0; i < 250; i++)
if (i % 5 == 0) {
qtr.calibrate();
delay(20);
}

pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}

void run_fwd(int valueSA, int valueSB) {
// Motor A
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}
void turn_right(int valueSA, int valueSB) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void turn_left(int valueSA, int valueSB) {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, valueSA);
analogWrite(enB, valueSB);
}

void Run_robot() {
int med_Speed_R;
int med_Speed_L;
int position = qtr.readLineBlack(qtrValues);
P_error = position - SetPoint;
cTime = millis();
eTime = (float)(cTime - pTime) / 1000;
I_error = I_error * 2 / 3 + P_error * eTime;
D_error = (P_error - lastError) / eTime;
PID_value = Kp * P_error + Ki * I_error + Kd * D_error;
lastError = P_error;
pTime = cTime;

med_Speed_L = leftMaxSpeed - abs(PID_value);
med_Speed_R = rightMaxSpeed - abs(PID_value);
int leftMotorSpeed = med_Speed_L + PID_value;
int rightMotorSpeed = med_Speed_R - PID_value;
leftMotorSpeed = constrain(leftMotorSpeed, 0, leftMaxSpeed);
rightMotorSpeed = constrain(rightMotorSpeed, 0, rightMaxSpeed);
run_fwd(leftMotorSpeed, rightMotorSpeed);
if(qtrValues[0] > 950 && qtrValues[7] > 950 ){
run_fwd(0,0);
}
// Determine whether to turn left or right based on the PID value
// if (PID_value > 0) {
// // Turn left
// turn_left(leftMotorSpeed, rightMotorSpeed);
// } else {
// // Turn right
// turn_right(leftMotorSpeed, rightMotorSpeed);
// }

delayMicroseconds(80);
}

void loop() {
Run_robot();
}

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
1 participant
@oudom021