main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 //Mathias tester
00004 
00005 m3pi m3pi;
00006 
00007 // Minimum and maximum motor speeds
00008 #define MAX 1.0
00009 #define MIN 0
00010 
00011 // PID terms
00012 #define P_TERM 2.5
00013 #define I_TERM 0.007
00014 #define D_TERM 16 //Ændret fra 4
00015 
00016 //Watt hour measurements
00017 #define AVERAGE 30 //No. of measurements
00018 #define AMPREL 24.07 //amps
00019 #define VOLTREL 8.984830805
00020 #define SECDEC 1/3600
00021 #define AIOFFSET 0.45900 //Zeroing of amp (0 amp = 2.5V)
00022 
00023 AnalogIn ain_1(A0); //ADC for amps
00024 AnalogIn ain_2(A1); //ADC for volts
00025 Timer t; //Used for Watt hour measurement
00026 
00027 int main()
00028 {
00029 
00030     t.start(); //Starts timer function
00031     float analogAmp; //Analog amp reading
00032     float analogVolt; //Analog volt reading
00033     float voltreading[AVERAGE]; //Volt array
00034     float ampreading[AVERAGE]; //Amp array
00035     float voltmean;
00036     float ampsmean;
00037     float wh = 0;
00038     int measno = 0;
00039 
00040     for (int i = 1; i <= AVERAGE; i++) {
00041         ampreading[i] = 0;
00042         voltreading[i] = 0;
00043     }
00044 
00045     m3pi.locate(0,1);
00046     m3pi.printf("Line PID");
00047 
00048     wait(2.0);
00049 
00050     m3pi.sensor_auto_calibrate();
00051     float right;
00052     float left;
00053     float current_pos_of_line = 0.0;
00054     float previous_pos_of_line = 0.0;
00055     float derivative,proportional,integral = 0;
00056     float power;
00057     float speed = MAX;
00058 
00059 
00060     while (1) {
00061 
00062         ampsmean = 0; //Zeroing mean amps before next calculation
00063 
00064         analogAmp = ain_1;
00065         analogAmp -= AIOFFSET;
00066         ampreading[measno] = analogAmp*AMPREL;
00067 
00068         for (int i = 0; i < AVERAGE; i++) {
00069             ampsmean += ampreading[i];
00070             voltmean += voltreading[i];
00071         }
00072         ampsmean /= AVERAGE;
00073         voltmean /= AVERAGE;
00074 
00075         if (t > 1.0) { //Zeroing timer
00076             wh += ampsmean * voltmean * SECDEC;
00077             t.stop();
00078             t.reset();
00079             t.start();
00080         }
00081 
00082         analogVolt = ain_2; //distance between two ADC for stability
00083         voltreading[measno] = analogVolt * VOLTREL;
00084         m3pi.locate(1,0);
00085         m3pi.printf("%.3f",wh);
00086 
00087         // Get the position of the line.
00088         current_pos_of_line = m3pi.line_position();
00089         proportional = current_pos_of_line;
00090 
00091         // Compute the derivative
00092         derivative = current_pos_of_line - previous_pos_of_line;
00093 
00094         // Compute the integral
00095         integral += proportional;
00096 
00097         // Remember the last position.
00098         previous_pos_of_line = current_pos_of_line;
00099 
00100         // Compute the power
00101         power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00102 
00103         // Compute new speeds
00104         right = speed+power;
00105         left  = speed-power;
00106 
00107         // limit checks
00108         if (right < MIN)
00109             right = MIN;
00110         else if (right > MAX)
00111             right = MAX;
00112 
00113         if (left < MIN)
00114             left = MIN;
00115         else if (left > MAX)
00116             left = MAX;
00117 
00118         // set speed
00119         m3pi.left_motor(left);
00120         m3pi.right_motor(right);
00121         
00122         measno++; //Used for average amps & volts
00123         
00124         if (measno >= AVERAGE) {
00125             measno = 0;
00126         }
00127         voltmean = 0; //Zeroing mean volts before next calculation
00128 
00129     }
00130 }