File content as of revision 17:d6e8234ffc91:

#include "mbed.h"
#include "m3pi.h"
//Mathias tester

m3pi m3pi;

// Minimum and maximum motor speeds
#define MAX 1.0
#define MIN 0

// PID terms
#define P_TERM 2.5
#define I_TERM 0.007
#define D_TERM 16 //Æ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) {

        ampsmean = 0; //Zeroing mean amps before next calculation

        analogAmp = ain_1;
        analogAmp -= AIOFFSET;
        ampreading[measno] = analogAmp*AMPREL;

        for (int i = 0; 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",wh);

        // 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);
        
        measno++; //Used for average amps & volts
        
        if (measno >= AVERAGE) {
            measno = 0;
        }
        voltmean = 0; //Zeroing mean volts before next calculation

    }
}