File content as of revision 1:396a582e8861:

/*
 ============================================================================
 Name        : robot.cpp
 Author      : Henning Slavensky
 Version     : 0.1
 Date        : 13102016
 Copyright   : Open for all
 Description : Program to serve af platform for Pro1 2016
 ============================================================================
 */
#include "mbed.h"
#include "HCSR04.h"
#include "nucleo_servo.h"
#include "hack_motor.h"
#include <math.h> // nice when you got to work with real geometry stuff

#define PI 3.141592 // definition of PI

// PID definitions
#define P_TERM 0.1
#define I_TERM 0
#define D_TERM 0.0
#define MAX 1.0
#define MIN 0


/* ------------------------------
    Definitions 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 with x and y
#define starting_x = 0
#define starting_y = 0 
#define second_x = 5
#define second_y = 4
#define third_x = 7
#define third_y = 1

double distcalc(double, double)
{
    
}

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

/*  -----------
    Prototypes
    -----------
*/
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
void get_to_goal ( ); // function to get to goal
void read_analog(); // comes here every second

Ticker T1; // create an object T1 of Ticker

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", 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", sensor.getDistance_mm());
    servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
    wait_ms (500);
    printf("\r\nDistance: %5.1f mm", 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
    // ------------------------------------------------------------
    while (Dcenter() <= CLOSE_ENOUGH) { // while distance traveled is less than
        //target
        
            robot.FW(0.5,0.5); // set new values half speed on both wheels
            wait_ms(20);
    }
    // --------------------------------------------------------------

    robot.stop(); // stop the robot again
    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
           tickR);

}

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;
}


void get_to_goal ( ) // function to get to goal
{

    // PID init
    double power, derivative, proportional, integral, e_old;
    power=derivative=proportional=integral=e_old=0;

    while (Dcenter() < CLOSE_ENOUGH) {

        e = tickR-tickL; // 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()

        /* --------------------------
         * 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 adusted 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;
}