File content as of revision 9:f14532fd7a02:

#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 33.5 // radius = 33.5 mm on wheels
#define L 133 // 133 mm distance between wheels
#define PI 3.141592
int tickL = 0; // tick on left wheel
int tickR = 0; // tick on right wheel

/////////////////////////////////////////////////////////////////////////////
//  PID VALUES                                                             //
/////////////////////////////////////////////////////////////////////////////
#define P_TERM 0.13    
#define I_TERM 0
#define D_TERM 0

/////////////////////////////////////////////////////////////////////////////
//  GLOBAL DEFINITIONS                                                     //
/////////////////////////////////////////////////////////////////////////////
double right,left;  
double result;
double speed=0.5;
double distance = 4500;
/////////////////////////////////////////////////////////////////////////////
//  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
InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right
Wheel robot(PB_2, PB_1, PB_15, PB_14); 
// 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
    }
    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;
}
    
/////////////////////////////////////////////////////////////////////
//                      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) 
    {
        right = speed;     
        left = speed;
        e = right;      
        
        //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;
        left = speed + output*0.15; 
        
        robot.FW(right,left);
        printf("tickR: %d   rightSpeed: %.2lf   tickL: %d   leftSpeed: %.2lf",
        tickR, right, tickL, left);
        wait_ms(10); // should be adjusted to your context. Give the motor time
        // to do something before you react
    }
    t.stop(); // stop timer
} 
#endif