Diff: main_copy.cpp

Revision:

2:4f8930f623ca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_copy.cpp	Mon Nov 09 14:12:30 2020 +0000
@@ -0,0 +1,133 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+
+m3pi m3pi;
+
+// Minimum and maximum motor speeds
+#define MAX 1.0
+#define MIN 0
+
+// PID terms
+#define P_TERM 0.9
+#define I_TERM 0.000
+#define D_TERM 0 //Ændret fra 4
+
+//Watt hour measurements
+#define AVERAGE 30 //No. of measurements
+#define AMPREL 24.07 //amps
+#define VOLTREL 8.984830805 
+#define SECDEC 1/3600
+#define AIOFFSET 0.45900 //Zeroing of amp (0 amp = 2.5V)
+
+AnalogIn ain_1(A0); //ADC for amps
+AnalogIn ain_2(A1); //ADC for volts
+Timer t; //Used for Watt hour measurement
+
+int main() {
+    
+    t.start(); //Starts timer function
+    float analogAmp; //Analog amp reading
+    float analogVolt; //Analog volt reading
+    float voltreading[AVERAGE]; //Volt array
+    float ampreading[AVERAGE]; //Amp array
+    float voltmean;
+    float ampsmean;
+    float wh = 0;
+    int measno = 0;
+    
+    for (int i = 1; i <= AVERAGE; i++)
+        {
+            ampreading[i] = 0;
+            voltreading[i] = 0;
+        }
+    
+    m3pi.locate(0,1);
+    m3pi.printf("Line PID");
+
+    wait(2.0);
+
+    m3pi.sensor_auto_calibrate();
+    float right;
+    float left;
+    float current_pos_of_line = 0.0;
+    float previous_pos_of_line = 0.0;
+    float derivative,proportional,integral = 0;
+    float power;
+    float speed = MAX;
+    
+    
+    while (1) {
+        
+        measno++; //Used for average amps & volts
+        
+        ampsmean = 0; //Zeroing mean amps before next calculation
+        
+        analogAmp = ain_1;
+        analogAmp -= AIOFFSET;
+        ampreading[measno] = analogAmp*AMPREL;
+        
+        for (int i = 1; i <= AVERAGE; i++)
+        {
+            ampsmean += ampreading[i];
+            voltmean += voltreading[i];
+        }
+        ampsmean /= AVERAGE;
+        voltmean /= AVERAGE;
+        
+        if (t > 1.0) //Zeroing timer
+        {
+            wh += ampsmean * voltmean * SECDEC;
+            t.stop();
+            t.reset();
+            t.start();
+        }
+        
+        analogVolt = ain_2; //distance between two ADC for stability
+        voltreading[measno] = analogVolt * VOLTREL;
+        m3pi.locate(1,0);
+        m3pi.printf("%.3f",integral);
+
+        // Get the position of the line.
+        current_pos_of_line = m3pi.line_position();        
+        proportional = current_pos_of_line;
+        
+        // Compute the derivative
+        derivative = current_pos_of_line - previous_pos_of_line;
+        
+        // Compute the integral
+        integral += proportional;
+        
+        // Remember the last position.
+        previous_pos_of_line = current_pos_of_line;
+        
+        // Compute the power
+        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+        
+        // Compute new speeds   
+        right = speed+power;
+        left  = speed-power;
+        
+        // limit checks
+        if (right < MIN)
+            right = MIN;
+        else if (right > MAX)
+            right = MAX;
+            
+        if (left < MIN)
+            left = MIN;
+        else if (left > MAX)
+            left = MAX;
+            
+       // set speed 
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+        
+        if (measno >= AVERAGE)
+        {
+            measno = 0;
+        }
+        voltmean = 0; //Zeroing mean volts before next calculation
+
+    }
+}
\ No newline at end of file