Advertisement
microrobotics

Untitled

Apr 30th, 2023
1,078
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2.  
  3. Servo myServo;        // create a servo object
  4. int feedbackPin = 0;  // analog input pin to read feedback signal
  5. int targetAngle = 90; // target angle for the servo
  6. int feedbackValue;    // variable to store feedback signal
  7.  
  8. void setup() {
  9.   myServo.attach(9);  // attach the servo to pin 9
  10.   Serial.begin(9600); // initialize serial communication
  11. }
  12.  
  13. void loop() {
  14.   feedbackValue = analogRead(feedbackPin);  // read the feedback signal
  15.   int currentAngle = map(feedbackValue, 0, 1023, 0, 180); // map the feedback signal to an angle value
  16.   int error = targetAngle - currentAngle; // calculate the error between target and current angle
  17.   int offset = map(error, -90, 90, -30, 30); // map the error to a servo offset value
  18.   int servoPos = targetAngle + offset; // calculate the new servo position
  19.  
  20.   myServo.write(servoPos); // set the servo position
  21.   delay(10); // wait for the servo to move
  22.  
  23.   Serial.print("Target Angle: ");
  24.   Serial.print(targetAngle);
  25.   Serial.print("\t Current Angle: ");
  26.   Serial.print(currentAngle);
  27.   Serial.print("\t Error: ");
  28.   Serial.print(error);
  29.   Serial.print("\t Servo Position: ");
  30.   Serial.println(servoPos);
  31. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement