File content as of revision 2:4f8930f623ca:

#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

    }
}