Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define leftMotorPin 3;
- #define rightMotorPin 5;
- #define leftIRPin A0;
- #define centerIRPin A1;
- #define rightIRPin A2;
- #define setpoint 512; // analogRead returns 0-1023
- // PID
- #define Kp 0.5;
- #define Ki 0.0;
- #define Kd 0.0;
- double input, output;
- double previous_leftError = 0;
- double previous_centerError = 0;
- double previous_rightError = 0;
- double integral = 0;
- #define sampleTime 100; // in milliseconds
- #define maxOutput 255;
- unsigned long previousMillis = 0;
- void setup() {
- pinMode(leftMotorPin, OUTPUT);
- pinMode(rightMotorPin, OUTPUT);
- pinMode(leftIRPin, INPUT);
- pinMode(centerIRPin, INPUT);
- pinMode(rightIRPin, INPUT);
- }
- void loop() {
- unsigned long currentMillis = millis();
- if (currentMillis - previousMillis >= sampleTime) {
- int leftIR = analogRead(leftIRPin);
- int centerIR = analogRead(centerIRPin);
- int rightIR = analogRead(rightIRPin);
- int leftError = leftIR - setpoint;
- int centerError = centerIR - setpoint;
- int rightError = rightIR - setpoint;
- double dt = (currentMillis - previousMillis) / 1000.0; // Convert milliseconds to seconds
- double leftDerivative = (leftError - previous_leftError) / dt;
- double centerDerivative = (centerError - previous_centerError) / dt;
- double rightDerivative = (rightError - previous_rightError) / dt;
- output = Kp * (leftError + centerError + rightError) + Ki * integral + Kd * (leftDerivative + centerDerivative + rightDerivative);
- // Limit the output to the maximum value
- output = constrain(output, -maxOutput, maxOutput);
- integral += (leftError + centerError + rightError);
- previous_leftError = leftError;
- previous_centerError = centerError;
- previous_rightError = rightError;
- previousMillis = currentMillis;
- int leftSpeed = constrain(255 - output, -255, 255);
- int rightSpeed = constrain(255 + output, -255, 255);
- // Update motor speeds
- analogWrite(leftMotorPin, leftSpeed);
- analogWrite(rightMotorPin, rightSpeed);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement