search

aras-labs / Turtle-Plotter

Python Codes for Plotting a SVG File or Robot Code using Turtle Graphics
MIT License
0 stars 0 forks source link

T9 #1

Open oudom021 opened 10 months ago

oudom021 commented 10 months ago

include

// 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 commented 10 months ago

include

// 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 commented 10 months ago

include

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