Scribd
Upload
19 views

ทดสอบ

The document defines constants for motor driver pins and sensor pins. It initializes arrays for sensor minimum and maximum values and reference values. Functions are defined for PID control, reading sensor values, driving motors in different directions, and line following behavior. The setup function initializes serial communication and calls functions to drive in different patterns. The loop function calls the readanalog function.

Uploaded by

ธวัชชัย ตรีศูนย์
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
19 views

ทดสอบ

The document defines constants for motor driver pins and sensor pins. It initializes arrays for sensor minimum and maximum values and reference values. Functions are defined for PID control, reading sensor values, driving motors in different directions, and line following behavior. The setup function initializes serial communication and calls functions to drive in different patterns. The loop function calls the readanalog function.

Uploaded by

ธวัชชัย ตรีศูนย์
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 5

#define DR1 7

#define DR2 8
#define PWMR 6
#define DL1 9
#define DL2 4
#define PWML 5
int button = 2;
int numSensor = 6;

int sensor_pin[] = {0, 1, 2, 3, 4, 5};


int min_sensor_values[] = {260, 375, 77, 337, 718, 217,};
int max_sensor_values[] = {979, 981, 520, 982, 985, 979};
int REF01 = (min_sensor_values[0]+max_sensor_values[0])/2;
int REF02 = (min_sensor_values[1]+max_sensor_values[1])/2;
int REF03 = (min_sensor_values[2]+max_sensor_values[2])/2;
int REF04 = (min_sensor_values[3]+max_sensor_values[3])/2;
int REF05 = (min_sensor_values[4]+max_sensor_values[4])/2;
int REF06 = (min_sensor_values[5]+max_sensor_values[5])/2;
int state_on_Line = 1;
float Kp = 3;
float Ki = 0;
float Kd = 16;

int speed_max =100;


uint16_t setpoint;
uint32_t _lastPosition;
float present_position;
float errors = 0;
float output = 0;
float integral ;
float derivative ;
float previous_error ;
boolean buttonState = 0;

//unsigned long long int time = 10000;


void setup() {
Lotus_nano();

//////////////////////////////////////////////////

LR();LL();LL();LR();

/////////////////////////////////////////////////

run(0,0); delay(1);

void loop() {
// pid();
readanalog();
}
void readanalog()
{
int S1 = analogRead(0);
int S2 = analogRead(1);
int S3 = analogRead(2);
int S4 = analogRead(3);
int S5 = analogRead(4);
int S6 = analogRead(5);
Serial.print("S1="); Serial.print(S1);
Serial.print("||"); Serial.print("S2="); Serial.print(S2);
Serial.print("||"); Serial.print("S3="); Serial.print(S3);
Serial.print("||"); Serial.print("S4="); Serial.print(S4);
Serial.print("||"); Serial.print("S5="); Serial.print(S5);
Serial.print("||"); Serial.print("S6="); Serial.print(S6);
Serial.println(); delay(1);
}
void TN()
{
while (1)
{
if (analogRead(0) < REF01 && analogRead(5) < REF06 )
{
while (1)
{
if (analogRead(0) > REF01 && analogRead(5) > REF06 )
{

break;
}
pid();
}
break;
}
pid();
}
}

void LL()
{
while(1){
if(analogRead(0) < REF01 and analogRead(1)< REF02 and analogRead(2)<REF03)
{delay(100);l();break;}
pid();
}
}

void LR()
{
while(1){
if(analogRead(3) < REF04 and analogRead(4)< REF05 and analogRead(5)<REF06)
{delay(100);r();break;}
pid();
}
}

void T()
{
while(1){
if((analogRead(1)< REF02) and (analogRead(2)<REF03) and ((analogRead(3)< REF04)
or (analogRead(2)< REF03) and (analogRead(3)<REF04) and (analogRead(4)< REF05)))
{run(speed_max,speed_max);delay(200);break;}
pid();
}
}

//void T()
//{ int j = 0;
// while(1)
// {
// if(analogRead(3) < REF04 and analogRead(4)< REF05 and analogRead(5)<REF06)
{ R(); }
// if(analogRead(0) < REF01 and analogRead(1)< REF02 and analogRead(2)<REF03)
{ L(); }
//
// if(analogRead(1)< REF02 and analogRead(2)<REF03 and
// analogRead(3)< REF04 and analogRead(4)<REF05)
{ run(speed_max,speed_max);delay(200); }
//
// if (j >= time)
// {
// /////
// run(0,0); delay(100*100);
// /////
// break;
// }
// j++;
// pid();
// }
//}
void l()
{
while(1){
if(analogRead(0)<REF01){run(-350,350);}
if(analogRead(1)<REF02){run(-350,350);}
if(analogRead(3)<REF04){run(-200,200);break;}
else {run(-350,350);}
}
}
void r()
{ run(0,0);delay(20);
while(1){
if(analogRead(5)<REF06){run(350,-350);}
if(analogRead(4)<REF05){run(350,-350);}
if(analogRead(2)<REF03){run(200,-200);break;}
else {run(350,-350);}
}
}
void sw_button()
{
while (1)
{
buttonState = digitalRead(2);
if (buttonState == LOW) {
break;
}
else {
run(0, 0);
}
}
}
int readline()
{
bool onLine = false;
long avg = 0;
long sum = 0;
for (uint8_t i = 0; i < numSensor ; i++)
{
long value = map(analogRead(sensor_pin[i]), min_sensor_values[i],
max_sensor_values[i], 1000, 0);
if (value > 200) {
onLine = true;
}
if (value > 50)
{
avg += (long)value * (i * 1000);
sum += value;
}
}
if (!onLine)
{
if (_lastPosition < (numSensor - 1) * 1000 / 2)
{
return 0 - 2000;
}
else
{
return ((numSensor - 1) * 1000) + 2000 ;
}
}
_lastPosition = avg / sum;
return _lastPosition;
}
void pid() {

present_position = readline() / ((numSensor - 1) * 10) ;


setpoint = 50.0;
errors = setpoint - present_position;
integral = integral + errors ;
derivative = (errors - previous_error) ;
output = Kp * errors + Ki * integral + Kd * derivative;
previous_error = errors;
float motorL = speed_max - output;
float motorR = speed_max + output;
motorL = constrain(motorL, -100, 250);
motorR = constrain(motorR, -100, 250);
run(motorL, motorR);

void run(int spl, int spr)


{
if (spl > 0)
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spl);
}
else if (spl < 0)
{
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spl);
}
else
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, LOW);
analogWrite(PWML, -255);
}
//////////////////////////////////////
if (spr > 0)
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
}
else if (spr < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}
}
void Lotus_nano()
{
Serial.begin(9600);
pinMode(2, INPUT);
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
sw_button();
}

You might also like