File content as of revision 7:a852e63cac3d:

#ifndef ODOMETRY_H
#define ODOMETRY_H
#include "mbed.h"
#include "hack_motor.h"
#include "math.h"   

//  ODOMETRY VALUES
#define N 20 // ticks on wheel
#define R 32.5 // radius = 32.5 mm on wheels
#define L 133 // 133 mm distance between wheels
#define PI 3.141592
#define DISTANCE 3000
int tickL = 0; // tick on left wheel
int tickR = 0; // tick on right wheel

//  PID VALUES
#define P_TERM 1.0
#define I_TERM 0
#define D_TERM 0
#define MIN 0
#define MAX 1.0

//  GLOBAL DEFINITIONS
double right,left;  
double result;
double speed=0.5;  
DigitalOut LED(LED1);

//  ANALOG POWER
AnalogIn   ain(PC_4);
DigitalOut dout(PB_13);
DigitalOut greenout(PB_12);
int stop=0;         //DigitalOut DEFINITION OF RED LED!!
DigitalOut LedFlash(PA_3);  //  RED LED WILL FLASH IF ROBOT STOPS 

/////////////////////////////////////////////////////////////////////////////
//              TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE               //
/////////////////////////////////////////////////////////////////////////////
Timer t; // DEFINE A TIMER T
Serial pc(USBTX, USBRX); // not used here because we only have one serial 
// connection
InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå    
Wheel robot(PB_2, PB_1, PB_15, PB_14);  //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
// create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---

    //  GETTING INFO FROM SENSORS 
void read_analog() // comes here every second in this case
// could be flagged with global variables like "stop"
{
    if(ain > 0.6f) { // 60% power, time for recharge
        dout = 1;
        stop=0; 
    } else {
        dout = not dout;
        stop=1; // flash red led
        LED = 1;
    }
    if (ain==1.0f) greenout = 1; // full power
    else greenout = 0;
}
//  INIT YOUR PARAMETERS
void init()
{
    tickL=0;
    tickR=0;
}
//  SENSOR INFO FROM TACO SENSORS
void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
{
    tickL++;
}

void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
{
    tickR++;
}
    
float Dleft() // DISTANCE FOR LEFT WHEEL
{
    return 2*PI*R*tickL/N;
}

float Dright() // DISTANCE FOR RIGHT WHEEL
{
    return 2*PI*R*tickR/N;
}

float Dcenter() // DISTANCE FOR CENTER
{
    return (Dleft()+Dright())/2;
}
void turn( )
{
    
}

/////////////////////////////////////////////////////////////////////
//                      PID REGULATOR                              //
/////////////////////////////////////////////////////////////////////
void get_to_goal ( )    
{   
    double e = 0; 
    double output = 0;
    double derivative = 0;
    double proportional = 0;
    double integral = 0;
    double e_old = 0;

    while (Dcenter() <= DISTANCE) 
    {    
        e = tickR - tickL;  //error calculation
        if(e < 0)
        {
            e = 0;   
        }
        else if(e > 0)
        {
         e = 0;   
        }
    
        //PID calculation
        proportional = e; // GETTING THE ERROR VALUE
        integral += proportional;   // THE ERROR NEVER GETS TOO LOW
        derivative = e - e_old;
        e_old = e;
        
        // COMPUTING THE OUTPUT SIGNAL
        output = (proportional * (P_TERM)) + (integral * (I_TERM))+ 
        (derivative * (D_TERM));
        
        // COMPUTING NEW SPEEDS ON WHEELS
        right = speed - output;//if power is 0, no error->same speed on wheels
        left = speed + output;
        
        //CHECK YOUR LIMITS
        if(right < MIN)
        {
            right = MIN;
        }
        else if(right > MAX)
        {
            right = MAX;
        }
        if(left < MIN)
        {
            left = MIN;
        }
        else if(left > MAX)
        {
            left = MAX;
        }
        printf("\n\r RightSpeed: %.2lf  RightTicks: %d  LeftSpeed: %.2lf"  
        "LeftTicks: %d", right, tickR, left, tickL);    
    
        // set new positions
        robot.FW(right,left); 
        wait_ms(25); // should be adjusted to your context. Give the motor time
        // to do something before you react
    }
    t.stop(); // stop timer
}
#endif