File content as of revision 8:5ca76759a67e:

#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
#define DISTANCE 10000
#define TURN 282  
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.9
#define MAX 1.0
#define WheelFactor 0.10
#define T_MIN 0.00
#define T_MAX 1.00

//  GLOBAL DEFINITIONS
double right,left;  
double result;
double speed=0.5;
double speed_turn=1;   
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;
}

/////////////////////////////////////////////////////////////////////
//                      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)
        {
            tickR = tickL;   
        }
        else if(e > 0)
        {
         tickR = tickL;   
        }
    
        //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
        right = right - WheelFactor*right;
        left = speed + output;
        
        //CHECK YOUR LIMITS
        if(right < MIN)
        {
            right = MIN- WheelFactor*MIN;
        }
        else if(right > MAX)
        {
            right = MAX- WheelFactor*MAX;
        }
        if(left < MIN)
        {
            left = MIN;
        }
        else if(left > MAX)
        {
            left = MAX;
        }
        printf("\n\r RightSpeed: %.2lf  RightTicks: %d  LeftSpeed: %.2lf"  
        "LeftTicks: %d  Output: %lf", right, tickR, left, tickL, output);    
    
        // set new positions
        robot.FW(right,left); 
        wait_ms(10); // should be adjusted to your context. Give the motor time
        // to do something before you react
    }
    t.stop(); // stop timer
} 

void turn()
{
    double e = 0; 
    double output = 0;
    double derivative = 0;
    double proportional = 0;
    double integral = 0;
    double e_old = 0;
    
    while(Dright() <= TURN) 
   {
       e = tickR - tickL;
        if(e < 0)
        {
            tickR = tickL;   
        }
        else if(e > 0)
        {
         tickR = tickL;   
        }
        proportional = e; 
        integral += proportional;
        derivative = e - e_old;
        e_old = e;
        
        output = (proportional * (P_TERM)) + (integral * (I_TERM))+ 
        (derivative * (D_TERM));
        right = speed + output;
        left = 0;
       
       if(right <= T_MIN)
       {
        right = T_MAX;
        }
        
        else if(right >= T_MAX)
        {
            right = T_MAX;
        }
       robot.FW(right, left); 
       wait_ms(10);
   }   
}
#endif