/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * Author: Jay Weeks * Created: 10-May-2016 * * Description: * This is a very basic signal filter and PID for controlling the * speed of a motor equiped with the super high-tech encoder from * that one Instructable I made! * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * Variables * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ #define memMax 16 // This will define how many values we can have // in our PID memory #define SCALE 250000 // Scale our PID values down to a PWM value #define PWMmax 255 // Define limits for our PWM values #define PWMmin 45 #define Min_time 36000 // PID target value bounds #define Max_time 400000 /* ~~~~~~~~~~~~~~~~ * Sliding Cutoff Values * ~~~~~~~~~~~~~~~~ */ #define k_cutoff 0.50 #define curvature 2112000 /* ~~~~~~~~~~~~~~~~ * PID Variables * ~~~~~~~~~~~~~~~~ */ // PID constants. Adjust these to tune the PID. #define k_P 100 // Proportional #define k_I 0 // Integral #define k_D 50 // Derivative // For now we will just make a PD controller. // You can make a true PID later, but PD will do. // Our calculated PID values int P = 0, I = 0, D = 0; // The PID value being sent to the motor int PIDoutput = 255; int PIDtarget = Max_time; // This is the target our PID is trying to // hit. unsigned int PIDupdate = 0; // This will record when our PID memory // has been updated, so we can update our // PID value, so we can increment our PWM // output. /* ~~~~~~~~~~~~~~~~ * PWM Variables * ~~~~~~~~~~~~~~~~ */ int PWMoutput = 255; // The PWM value being sent to the motor /* ~~~~~~~~~~~~~~~~ * Pot Variables * ~~~~~~~~~~~~~~~~ */ int POTread = 0; // This records the value we get from the onboard // pot. /* ~~~~~~~~~~~~~~~~ * Data Collection Variables * ~~~~~~~~~~~~~~~~ */ int PIDmemory[memMax]; // This will store our PID's memory unsigned int memoryIndex = 0; // This will store our current position // in the PID memory // Our sliding cutoff value int slidingCutoff = k_cutoff * ((curvature/(PWMoutput - 39))+Min_time); // Variables to record encoder times unsigned int encoder_prev = 0, encoder_dif = 0; /* ~~~~~~~~~~~~~~~~ * Other Variables * ~~~~~~~~~~~~~~~~ */ // Generic incrementer unsigned int i = 0; // Generic index. /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * Main Code Segment * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* ~~~~~~~~~~~~~~~~ * Setup * ~~~~~~~~~~~~~~~~ */ void setup() { // Begin serial communication Serial.begin(9600); // Set our inputs pinMode(PIN_INT3, INPUT); // We're using interrupt pins 1 and 3 pinMode(PIN_INT1, INPUT); pinMode(A2, INPUT); // We're also using the onboard POT // Set our outputs pinMode(PIN_OC5, OUTPUT); // Create our interrupts attachInterrupt(1, logR, RISING); // int1: rising attachInterrupt(3, logF, FALLING); // int3: falling // Clear our PID memory for (i = 0; i < memMax; i ++) { PIDmemory[i] = 0; } // Initialize our timers encoder_prev = micros(); } /* ~~~~~~~~~~~~~~~~ * Loop * ~~~~~~~~~~~~~~~~ */ void loop() { // set the speed of the motor analogWrite(PIN_OC5, PWMoutput); // Update our PID target // Get our current POT value POTread = analogRead(A2); PIDtarget = Min_time + (Max_time - Min_time)/1023 * POTread; // Check to see if the PID memory has been updated if(PIDupdate) { // Reset PIDupdate PIDupdate = 0; // Throw some debug information at that serial connection /* // Output our Pot value Serial.print("Pot:\t"); Serial.print(POTread); Serial.print("\t"); // Output the target value Serial.print("Tar:\t"); Serial.print(PIDtarget); Serial.print("\t"); */ // Output our PID memory Serial.print("Mem:\t"); for (i = 0; i < memMax; i ++) { Serial.print(PIDmemory[i]); Serial.print("\t"); } /* // Output our P Serial.print("P:\t"); Serial.print(P); Serial.print("\t"); // Output our I Serial.print("I:\t"); Serial.print(I); Serial.print("\t"); // Output our D Serial.print("D:\t"); Serial.print(D); Serial.print("\t"); */ // Output our PID output Serial.print("Out:\t"); Serial.print(PIDoutput); Serial.print("\t"); // Output our PWM output Serial.print("PWM:\t"); Serial.println(PWMoutput); // Calculate our PID // P P = k_P * PIDmemory[memoryIndex] / SCALE; // I I = 0; for (i = 0; i < memMax; i ++) I = I + PIDmemory[i]; I = k_I * I / SCALE; // D if (memoryIndex == 0) D = k_D * (PIDmemory[memMax - 1] - PIDmemory[0]) / SCALE; else D = k_D * (PIDmemory[memoryIndex - 1] - PIDmemory[memoryIndex]) / SCALE; PIDoutput = P + I + D; // Increment our PWM PWMoutput = PWMoutput + PIDoutput; // Now check it's bounds if (PWMoutput > PWMmax) PWMoutput = PWMmax; if (PWMoutput < PWMmin) PWMoutput = PWMmin; // Now recalculate our sliding cutoff slidingCutoff = k_cutoff * ((curvature/(PWMoutput - 39))+Min_time); } } /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * Interrupt Service Routines * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* ~~~~~~~~~~~~~~~~ * Falling * ~~~~~~~~~~~~~~~~ */ void logF() { // We only want to record the start time of the low segments encoder_prev = micros(); } /* ~~~~~~~~~~~~~~~~ * Rising * ~~~~~~~~~~~~~~~~ */ void logR() { // We've reached the end of our low segment // Get the time since we went low encoder_dif = micros() - encoder_prev; // Discard anything lower than our sliding cutoff if (encoder_dif >= slidingCutoff) { // If our measure is greater than our sliding cutoff PIDupdate ++; // Increment our PIDupdate // Also record it in PID memory memoryIndex ++; // Keep memory index looping if (memoryIndex >= memMax) memoryIndex = 0; PIDmemory[memoryIndex] = encoder_dif - PIDtarget; } } // I hate typing at the very bottom of the page, don't you?