File content as of revision 3:30bdc3d9e1c6:

/*
 ============================================================================
 Name        : robot.cpp
 Author      : Team 5
 Version     : 0.1
 Date        : 13-10-2016
 Copyright   : Open for all
 Description : Program to serve af platform for Pro1 2016
 ============================================================================
 */
    
/*
=============================================================================
Including the necessary header files
=============================================================================
*/
#include "mbed.h"
#include "HCSR04.h"
#include "nucleo_servo.h"
#include "hack_motor.h"
#include <math.h> 

/*
=============================================================================
PID definitions
=============================================================================
*/
#define P_TERM 0.2
#define I_TERM 0.1
#define D_TERM 0.3
#define MAX 1.0 
#define MIN 0
#define PI 3.141592

/*
=============================================================================
Geometry for robot:
note that N,R,L maybe not have the most meaningful name
but they follow the names from the theory for our
robot lectures, and can be used in youa application
=============================================================================
*/         
#define N 20 // ticks on wheel
#define R 32.5 // radius = 32.5 mm
#define L 133 // 133 mm distance between wheels
 
/*
=============================================================================
Coordinates for robot to reach
=============================================================================
*/
//Start position:
#define first_x 0
#define first_y 0
#define first_angle 0.785  //45 deg

//Second position:
#define second_x 50
#define second_y 40
#define second_angle 1.047 //60 deg

//Third position:
#define third_x 70
#define third_y 10
#define third_angle 1.396 //80 deg

//fourth position:
#define fourth_x 70
#define fourth_y 50
  
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_3 to be interupt in for tacho left
InterruptIn tacho_right(PC_2);// Set PC_2 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)

/*
=============================================================================
definition for analog power
=============================================================================
*/
AnalogIn   ain(PC_4);
DigitalOut dout(PB_13);
DigitalOut greenout(PB_12);

int stop=0;
/*
=============================================================================
Global variables
=============================================================================
*/    
double right,left;
double speed=0.5;
double e = 0; // angle error
int tickL = 0; // tick on left wheel
int tickR = 0; // tick on right wheel

/*
=============================================================================
Prototype defined functions
=============================================================================
*/ 
void init();
void tickLeft(); // read tick left
void tickRight(); // read tick right
float Dleft(); // distance left wheel
float Dright(); // distance right wheel
float Dcenter(); // Distance for center

//functions for calculating desired distances from defined points
float Dist_reach1();
float Dist_reach2();
float Dist_reach3();

void get_to_goal ( ); // function to get to goal
void read_analog(); // comes here every second

Ticker T1; // create an object T1 of Ticker

/*
----------------------------------------------------------------------------
                            Accessing the int main
----------------------------------------------------------------------------
*/
int main()
{
    T1.attach(&read_analog, 1.0); // attach the address of the read_analog
    //function to read analog in every second

    tacho_left.rise(&tickLeft);  // attach the address of the count function
    //to the falling edge
    tacho_right.rise(&tickRight);  // attach the address of the count function
    //to the falling edge

    HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
    // (echo,trigger) on pin PC_5, PC6
    Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8

    sensor.setRanges(2, 400); // set the range for Ultrasonic

/*
=============================================================================
Demo of the servor and ulta sonic sensor
=============================================================================
*/  
    wait_ms(2000); // just get time for you to enable your screeen
    servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
    wait_ms (500);
    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
    //readings from ultra sonic at this position
    servo1.set_position(180); // Servo left position (angle = 180 degree)
    //for servo
    wait_ms (500);
    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
    servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
    wait_ms (500);
    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());

    init(); // initialise parameters (just for you to remember if you need to)

    wait_ms(1000); //wait 1 secs here before you go
    t.start();      // start timer (just demo of how you can use a timer)

    /*  ------------------
            with PID
        -----------------
    --  delete get_to_goal(); when you use without pid
    get_to_goal ();
    */

    /* ---------------------------
           without pid
       ------------------------
    */
    // delete between --- when you use it with pid
    // ------------------------------------------------------------
/*
=============================================================================
Driving and stopping the robot with member functions from the wheel class
=============================================================================
*/
    while(Dcenter() <= Dist_reach1())
    {
        robot.FW(0.5, 0.5);
        wait_ms(5000);
    }
    robot.stop(); // stop the robot again
    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
           tickR);    
           
} 
/*  
----------------------------------------------------------------------------
                            END OF MAIN FUNCTION
----------------------------------------------------------------------------
*/

/*
=============================================================================
Calculations of distances traveled by both wheels and robot
=============================================================================
*/
void tickLeft() // udtate left tick on tacho interrupt
{
    tickL++;
}
void tickRight() // udtate right tick on tacho 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;
}

//Calculation of current position and new position with angles!!!

float angle_new()
{
    return (Dright()-Dleft())/L;   
}

float x_position()
{
    return Dcenter()*sin(angle_new());   
}

float y_position()
{
    return Dcenter()*cos(angle_new());
}

//slet efterfølgende
float Dist_reach1() 
{ 
  double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
  double dist_firstTosecond = sqrt(firstTosecond_calc); 
  
  return (dist_firstTosecond);
}

/*
=============================================================================
PID init
=============================================================================
*/
void get_to_goal ( ) // function to get to goal
{
    double power = 0;
    double derivative = 0;
    double proportional = 0;
    double integral = 0;
    double e_old = 0;

    while (Dcenter() < Dist_reach1()) {
/*
-------------------------------------------------
error is difference between right tick and left tick
should be ajusted to your context, angle for robot
and instead of test Dcenter() in your while loop, test
for distance_to_goal()
-------------------------------------------------
*/
        e = tickR-tickL; 
/*
=============================================================================
PID regulator
=============================================================================
*/

        // Compute the proportional
        proportional = e; // get the error

        // Compute the integral
        integral += proportional;

        // Compute the derivative
        derivative = e - e_old;

        // Remember the last error.
        e_old = e;

        // Compute the power
        power = (proportional * (P_TERM)) + (integral * (I_TERM))
                + (derivative * (D_TERM));

        // Compute new speeds

        right = speed - power;//if power is 0, no error and same speed on wheels
        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;

        robot.FW(right,left); // set new positions
        printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
               Dcenter(),tickL,tickR,left,right);

        wait_ms(20); // should be adjusted to your context. Give the motor time
        // to do something before you react
    }
    t.stop(); // stop timer
}

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;
}
void init() // init your parameters
{
    tickL=0;
    tickR=0;
}