Hello!
We are writing a PID control for a line following robot and we know what we want it to do but are completely new to Arduino and have very little other programming background. We are using the Qtrarc optical array sensors.
Initially I am trying to convert to sensor readings into a digital array and then give it a position value and use that for the PID.
My first problem is assigning the sensor readings to a binary value.
The code is attached, any help would be much appreciated!
#include <QTRSensors.h>
//define pins
#define Left_Direc 4
#define Left_PMW 5
#define Right_Direc 6
#define Right_PMW 7
#define NUM_SENSORS 8 //number of sensors
#define TIMEOUT 2500 //waits this amount of time for sensor outputs to go low
#define EMITTER_PIN A5 //assigned but wont be used
int dt= 50;
int setpoint= 0;
int previous_error=0;
int error=0;
int LDR =0;
//define the sensor array
QTRSensorsRC qtrrc((unsigned char[]) {A4, A3, A2, 8, 9, 10, 11, 12}, //assign sensors to pins
NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensors[NUM_SENSORS]; //puts sensor values into a list the size of the number of sensors
void setup()
{
Serial.begin(9600); //baud rate
// set pins for I/O
pinMode(Left_Direc, OUTPUT);
pinMode(Left_PMW, OUTPUT);
pinMode(Right_Direc, OUTPUT);
pinMode(Right_PMW, OUTPUT);
Motors_Stop();
delay(2000);
Sensor_Calibration(); //create a calibration function!!!
delay(2000);
}
void Sensor_Calibration()
{
qtrrc.read(sensors);
int light_sensitivity = 700;
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
if ( sensors[i] > light_sensitivity)
{
sensors[i] = HIGH;
}
else
{
sensors[i] = LOW;
}
}
}
//main function
void loop()
{
FOLLOW_LINE(); //the main function to follow the line!
// read raw sensor values
qtrrc.read(sensors);
// print the sensor values as numbers from 0 to 2500, where 0 means maximum reflectance and
// 1023 means minimum reflectance
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensors[i]);
Serial.print('\t'); // tab to format the raw data into columns in the Serial monitor
}
Serial.println();
delay(250);
}
//set PID paramaters -- currently unknown!! Will find through testing
int Kp=20;
int Ki=0;
int Kd=0;
//define follow line function
void FOLLOW_LINE()
{
int position_value = qtrrc.readLine(sensors);
//define position values
if (sensors[7]= HIGH)
{
position_value = 4;
}
if (sensors[6,7] = HIGH)
{
position_value = 3;
}
if (sensors[5,6] = HIGH)
{
position_value = 2;
}
if (sensors[4,5] = HIGH)
{
position_value = 1;
}
if (sensors[3,4] = HIGH)
{
position_value = 0;
}
if (sensors[2,3] = HIGH)
{
position_value = -1;
}
if (sensors[1,2] = HIGH)
{
position_value = -2;
}
if (sensors[0,1] = HIGH)
{
position_value = -3;
}
if (sensors[0] = HIGH)
{
position_value = -4;
}
Serial.print(sensors[0,1,2,3,4,5,6,7]);
Serial.print('\t'); // tab to format the raw data into columns in the Serial monitor
int error = setpoint - position_value;
int integral = integral + error*dt;
int derivative = (error - previous_error)*dt;
int PID_out = Kp*error + Ki*integral + Kd*derivative;
int previous_error = error;
int PID = PID_out/25;
delay(dt);
const int max_speed = 255;
if (PID > 0)
{
if (PID > max_speed) PID =max_speed;
digitalWrite(Left_Direc, LOW);
analogWrite(Left_PMW, max_speed);
digitalWrite(Right_Direc, LOW);
analogWrite(Right_PMW, max_speed-PID);
}
else {
if (PID < -max_speed) PID=-max_speed;
digitalWrite(Left_Direc, LOW);
analogWrite (Left_PMW, max_speed+PID);
digitalWrite(Right_Direc, LOW);
analogWrite (Right_PMW, max_speed);
}
}
//MOTOR MOVEMENTS
void Motors_Stop()
{
digitalWrite(Left_Direc, LOW);
analogWrite(Left_PMW, 0);
digitalWrite(Right_Direc, LOW);
analogWrite(Right_PMW, 0);
}
Moderator edit: To save other users the hassle of downloading your code, I put it here, in the post, where it should have been in the first place.
PIDcontrol2.ino (3.67 KB)