Project description
Code
Code for 30 cm beam with setpoint at 15 cm
c_cpp
1#include<Servo.h> 2#include<PID_v1.h> 3#include<SoftwareSerial.h> 4 5int set=6,neg=-24,pos=24,base=130;//Setpoint,Negative,Positive,Base values.neg shows tilt on other side of ultrasonic sensor 6long cm1=set; //For filtering purpose 7const double a=0.5; //Exponential filter parameter 8const int servoPin = 9; //Servo pin 9 10 11float Kp = 0.8; //Initial Proportional Gain 12float Ki = 0.02; //Initial Integral Gain 13float Kd = 0.75; //Intitial Derivative Gain 14double Setpoint, Input, Output, ServoOutput; 15 16 17 18PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE); //Initialize PID object, which is in the class PID. 19 20 21 22 23Servo myServo; //Initialize Servo. 24 25 26void setup() { 27 28 Serial.begin(9600); //Begin Serial 29 myServo.attach(servoPin); //Attach Servo 30 31 Input = readPosition(); //Calls function readPosition() and sets the balls 32 // position as the input to the PID algorithm 33 34 35 36 myPID.SetMode(AUTOMATIC); //Set PID object myPID to AUTOMATIC 37 myPID.SetOutputLimits(neg,pos); //Set Output limits to neg and pos degrees. 38} 39 40void loop() 41{ 42 43 Setpoint = set; //Give value for setpoint 44 Input = readPosition(); 45 46 myPID.Compute(); //computes Output in range of neg to pos degrees 47 48 ServoOutput=base+Output; // value in base is my horizontal 49 myServo.write(ServoOutput); //Writes value of Output to servo 50 51 52} 53 54 55 56 57float readPosition() { 58 delay(40); 59 60 61const int TrigPin = 11;//Trig 62const int EchoPin = 12;//Echo 63 64long duration, cm,cmn; 65unsigned long now = millis(); 66pinMode(TrigPin, OUTPUT); 67digitalWrite(TrigPin, LOW); 68delayMicroseconds(2); 69digitalWrite(TrigPin, HIGH); 70delayMicroseconds(5); 71digitalWrite(TrigPin, LOW); 72 73 74pinMode(EchoPin, INPUT); 75duration = pulseIn(EchoPin, HIGH); 76 77 cm = duration/(15*2); 78 79 80 if(cm > 30) // 30 cm is the maximum position for the ball 81 {cm=30;} //Signal Conditioning for ultrasonic sensor 82 83 cmn = a * cm + (1 - a) * cm1; //Exponential filter- signal conditioning 84 Serial.print(cm); Serial.print("\ "); 85 Serial.println(cmn); //cmn is filtered value 86 delay(10); 87 cm1 = cmn; //saved to cm1 which is used as history in exponential filter 88 89 return (cmn); //Returns filtered distance value in cm 90}
Downloadable files
schematic
schematic
schematic
schematic
Comments
Only logged in users can leave comments
nihalsuri
0 Followers
•0 Projects
0
0