aras-labs / turtle-plotter Goto Github PK

#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();
}