#include #include #include "BasicStepperDriver.h" // Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step #define MOTOR_STEPS 200 // All the wires needed for full functionality #define ENBL 7 #define DIR 8 #define STEP 9 // Since microstepping is set externally, make sure this matches the selected mode // 1=full step, 2=half step etc. #define MICROSTEPS 1 // 2-wire basic config, microstepping is hardwired on the driver BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP); //Mechanical Sytem Properties //full pendulum rotation =8000 Encoder counts #define rot_Encoder_A 20 // rotary Encoderpin A an Pin 20 #define rot_Encoder_B 21 // rotary Encoderpin B an Pin 21 #define lin_Encoder_A 18 // linear Encoderpin A an Pin 18 #define lin_Encoder_B 19 // linear Encoderpin B an Pin 19 Encoder rotEnc(rot_Encoder_A, rot_Encoder_B); Encoder linEnc(lin_Encoder_A, lin_Encoder_B); //Variable Definitions //Pin Definitions byte pin_photoint=13; double a = 0.80; // Dämpfungsparameter im Filter //Linear and angle Limits for Controller long limit_lin_min= -1800 ; //1600 min Limit (in direction torwards limit switch--> right) long limit_lin_max= 1800 ; // 40000 max Limit (left) double limit_angle= 300; //limit at which it becomes impossible to catch the pendulum again if exeeded, active control of pendulum is stopped and cart is stopped if exeeded. Limit works in both direction (+ and -) at the equilibrium point //Deadband Variables byte output_deadband= 40; //Miscellaneous Variables int rot_offset=0; //Offset from Vertical, System inherent error. Use if System always tends to run in one direction double angle, y=0; double lin_position; //PID Setup //PID Variables double rot_kp= 6.2; double rot_ki= 84.0; double rot_kd= 0.0; double lin_kp= 0.0000001; double lin_ki= 0.00000001; double lin_kd= 0.0; double Output= 0.0; double rot_Setpoint = 0.0; double lin_Setpoint = 0.0; // 21000 is approx. middle of track //PID Controller initialisation PID rot_Controller(&angle,&Output,&rot_Setpoint,rot_kp,rot_ki,rot_kd,DIRECT); PID lin_Controller(&lin_position,&rot_Setpoint,&lin_Setpoint,lin_kp,lin_ki,lin_kd,DIRECT); void setup() { pinMode(ENBL, OUTPUT); digitalWrite(ENBL,1); Serial.begin(115200); pinMode(pin_photoint,INPUT); //Initialize Encoder counts rotEnc.write(-4000); //set when pendulum is hanging straight down due to gravity linEnc.write(lin_Setpoint); //PID Settings rot_Controller.SetMode(AUTOMATIC); rot_Controller.SetOutputLimits(-500,500); rot_Controller.SetSampleTime(5); //2 rot_Controller.SetTunings(rot_kp,rot_ki,rot_kd); lin_Controller.SetMode(AUTOMATIC); lin_Controller.SetOutputLimits(-300,300); lin_Controller.SetSampleTime(5); //2 lin_Controller.SetTunings(lin_kp,lin_ki,lin_kd); } void loop() { angle = rotEnc.read(); lin_position = linEnc.read(); if ((lin_position>limit_lin_min) && (lin_position (limit_angle*-1)){ //perform as long as pendulum is within the angular limits //Execute linear PID controller lin_Controller.Compute(); //Execute angular PID controller rot_Controller.Compute(); y = a * y + (1.0 - a) * Output; Serial.println(y); // Ausgabe der Stellgröße auf den sereillen Plotter digitalWrite(ENBL,0); if(y > output_deadband){ stepper.begin(y, MICROSTEPS); stepper.move(-MICROSTEPS); } else if(y < -output_deadband){ stepper.begin(abs(y), MICROSTEPS); stepper.move(MICROSTEPS); } } //stop carrigage if pendulum has tipped out of the angular limits else { digitalWrite(ENBL,1); } } //stop carrigage out of the linear limits else { digitalWrite(ENBL,1); } } //End Loop