/*
 ============================================================================
 Name        : robot.cpp
 Author      : Team 1
 Version     : 0.3
 Date        : 02112016
 Copyright   : Team ones
 Description : Controle program for projekt 1 robot
 ============================================================================
 */
#include "mbed.h"
#include "uSound.h"
#include "nucleo_servo.h"
#include "engineControle.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  // 1?
#define I_TERM 0
#define D_TERM 0.0
#define MAX 1.0
#define MIN 0

#define N 20 // ticks on wheel
#define R 32.5 // radius = 32.5 mm
#define L 133 // 133 mm distance between wheels
#define GOAL_CLOSE_ENOUGH 250 // 1 m to be used when you approach target

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
//         (M1A,  M1B,  M2A,   M2B)

AnalogIn   ain(PC_4);   //kan bruges til at måle spænding
DigitalOut dout(PB_13); // lysdioder out
DigitalOut greenout(PB_12);

int stop=0;

double right,left;
double speed=0.5;
double goalangle(); //angle to goal
float Dangle = 0;
float angle = 0;
double read_deltaticks();
double g = 0;
double distancegoal(); //distance to goal
double e = 0; // angle error
int tickL = 0; // tick on left wheel
int tickLold = 0;
int tickR = 0; // tick on right wheel
int tickRold = 0;
int loop = 0;
double x = 0;
double y = 0;
double dx = 0;
double dy = 0;

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

    uSound sensor(PC_5,PC_6); // Create an object of uSound 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(2000); //wait 2 secs here before you go
    t.start();      // start timer (just demo of how you can use a timer)

    get_to_goal ();
    

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

} // End Main

///// machine to control goal and what is next

double xgoal = 300;
double ygoal = 500;

void tickLeft() // update left tick on tacho interrupt
{
    tickL++;
}
void tickRight() // update right tick on tacho interrupt
{
    tickR++;
}
float Dleft() // Distance for left wheel
{

    return 2*PI*R*tickL/N; //delta tick tjek tickL
}

float Dright() // Distance for right wheel 
{

    return 2*PI*R*tickR/N; //delta tick tjek tickR
}

float Dcenter() // Distance for center
{
    return (Dleft()+Dright())/2;
}

double Gangle() //goal angle
{
    return atan2(ygoal - y, xgoal - x);
}
double distancegoal() //calc distance to target
{
    return sqrt(pow(xgoal - x, 2) + pow(ygoal - y, 2));
}

void get_to_goal() // function to get to goal
{
    loop=0; // loop??
    g = distancegoal();
    // PID init
    double power, derivative, proportional, integral, e_old;
    power=derivative=proportional=integral=e_old=0;

    while (g>GOAL_CLOSE_ENOUGH) {
    
    //    float distance;
    
    read_deltaticks();
    Dangle = (Dright() - Dleft()) / L; 
    // delta angle
    dx = Dcenter() + cos (angle + Dangle/2); 
    // delta x
    dy = Dcenter() + sin (angle + Dangle/2);
    // delta y

    x = x + dx;
    // x now
    y = y + dy;
    // y now
    angle = angle+Dangle;
    // angle now

    if (angle > PI)
    angle = angle - 2 * PI;
    if (angle < -PI)
    angle = 2 * PI + angle;

    g = distancegoal(); // distance goal
    Dangle = Gangle(); // goal angle
    e = angle - Dangle; // error angle
    if (e < -PI)
    e = 2 * PI + e; // short angle
    if (e > PI)
    e = e - 2 * PI; // short angle

        // 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;
    tickLold=0;
    tickR=0;
    tickRold=0;
    x = 0; // Start position
    y = 0; // Start position
    angle = 0; // Start angle
}
