/*
Firmware pro jednotku vozidla autonomního konvoje
@Max
*/

#include "mbed.h"
#include <ctype.h>                                          //including for USART interrupt  

#define SERVO_NULL      1600                                //Nulové hodnoty aktuátorů
#define MOTOR_NULL      1560                                

    DigitalOut led1(LED1);                                   //LED kontrolky
    DigitalOut led2(LED2);                                    
    DigitalOut led3(LED3);   
    DigitalOut led4(LED4); 
    DigitalOut srf_trigger(p10);                            //triger pin echolokátoru
    DigitalOut bat_enable(p19);                             //pin, který spouští měřící obvod
    
    Timer timer_SONAR;                                      //Timer pro měření vzdálenosti (délka pulzu)
    Timeout die;                                            //Timeout
    Ticker SRF04_Ticker;                                    //Ve funkci volané tickerem se nesmí vyskytovat printf ani wait!!!!!                                    
    Ticker BATTERY_Ticker;
     
    InterruptIn pin_echo(p9);                               //nastaví pin, aby reagoval na externí přerušení ( bude se měřit délka pulzu)
    AnalogIn   ain(p20);                                    //p20
    
    PwmOut Servo(p26);                                      //PWM výstup pro ovládání Serva
    PwmOut Motor(p21);                                      //PWM výstup pro ovládání Motoru
    PwmOut Motork(p24);                                     //PWM výstup pro ovládání Motoru

    Serial usart(p28, p27);                                 //HW UART 
    unsigned char inIdx;                                // Read by background, written by Int Handler. //For serial interrupt
    unsigned char outIdx;                               // Read by Int Handler, written by Background. //For serial interrupt   
    
    int x = 350;
    int y = 350;    
    int flag_trigger = 0;
    int flag_pulse = 0;
    int flag_serial = 0;
    int pulse_us;
    int napeti;
    float distance;    
    float value;
    
    int i=0;
    int a=0;
    char buffer[32];
    char message[16];
    
//*****************************************************************************
//Kontrolování dat pro úhel natočení nápravy
int Angle(int alfa)                    
{
    int servo_us;
    
    if(alfa>=0 && alfa<=700){
        if(alfa<350){
        servo_us = alfa+1240; //1240+350 = 1590   
        }
        else if(alfa>350){
        servo_us = alfa+1270; //1270+350 = 1610
        }   
        else{                        
        servo_us = SERVO_NULL;//servo null
    }
    return servo_us;  
}
//*****************************************************************************
// Kontrolování dat pro rychlost vozidla
int Speed(int beta)                       
{
    int motor_us;
    
    if(beta>=0 && beta<=700){
        if(beta<348){
        motor_us = beta+1140;//nemněnit, naladěno!!!!!!!!!!!!!!!!!!!!!!
        }
        else if(beta>352){
        motor_us = beta+1285; //nemněnit, naladěno!!!!!!!!!!!!!!!!!!!!!
        }
        else{                        
        motor_us = MOTOR_NULL;//motor NULL
        }
      }  
      return motor_us;   
}   
//*****************************************************************************
// přerušení z UART
void SerialRecvInterrupt (void)                             
{   
    char c;
    c = usart.getc();
    buffer[i] = c;
    i=i+1;
    
    if (c == '&'){//když přijde ukončovací znak, nastaví se vlajka a index bufferu se nuluje
        i = 0;
        flag_serial = 1;
        }
}
//*****************************************************************************
// BATTERY_Ticker                                 
void BATTERY_MEASURE()                                          
{
   led2 = !led2;
   bat_enable = 1;                                    //aktivuje se obvod pro měření napětí baterie
   value = ain.read();                                //čtení hodnoty z AD převodníku  
   bat_enable = 0;                                    //vypnutí obvodu
} 
//*****************************************************************************
// Nulování trigger pinu po timeoutu
void zero()                                            
{
    led1 = !led1;
    srf_trigger = 0;
}
//*****************************************************************************
//sestupná hrana pulzu z ultrazvukového senzoru
void fall()                                             
{ 
    timer_SONAR.stop();                                 //zastavím časovač
    pulse_us = timer_SONAR.read_us();                   //získám délku pulzu
    pin_echo.rise(NULL);                                //nulování přerušení
    timer_SONAR.reset();                                //Reset resetuji časovač pro měření délky pulzu
    flag_pulse = 1;
}
//*****************************************************************************
//vzestupná hrana pulzu z ultrazvukového senzoru
void rise()                                                   
{ 
    timer_SONAR.start();                                      //Spustím časovač pro měření délky pulzu
    pin_echo.fall(&fall);                                     //Nastavím přerušení pinu echo na sestupnou hranu (volá se funkce fall)
}
//*****************************************************************************
// SRF04_Ticker    
void SRF04_RANGE()                                            
{   
    pin_echo.rise(&rise);                                     //Nastavím přerušení pinu echo na náběžnou hranu (volá se funkce rise)
    srf_trigger = 1;                                          //vyšlu imlupz na trigger pin SRF04
    die.attach(&zero, 0.00002);                               //funkce zero se zavolá po 20 us z timeoutu die
}
//*****************************************************************************
//Odesílání naměřených dat zpět do raspberry pi 
void posli(int l)                                            
{   
        int i;
        for(i=0;i<l;i++){
        usart.putc(message[i]);
        }
        memset(&message[0], 0, sizeof(message));            //mazání zprávy  
}
//*****************************************************************************
// Main Program                                                            
//*****************************************************************************
int main() 
{
    inIdx  = 0;                                             // Initialize bfr input and output pointers. 
    outIdx = 0; 
    usart.baud(115200);                                     //Setting baudrate for USART communication
    
    Servo.period_ms(20);                                    // servo requires a 20ms period
    Motor.period_ms(20);                                    // motor requires a 20ms period
                            
    Servo.pulsewidth_us(SERVO_NULL);                        //nastavení klidových hodnot
    Motor.pulsewidth_us(MOTOR_NULL); 

    usart.attach (&SerialRecvInterrupt, usart.RxIrq);                 //Recv interrupt handler                 
    SRF04_Ticker.attach(&SRF04_RANGE, 0.5);                           //Ultrazvuk volám každých 0,5s         
    BATTERY_Ticker.attach(&BATTERY_MEASURE, 3);                       //Baterii měřím každé 3s

    while(1){
        
    if (flag_pulse == 1){
        led3 = !led3;                                     //výpočet vzdálenosti a odesílání zprávy (v přerušení nelze použít printf!)   
        if (pulse_us <19000){                             //pokud je délka pulzu kratší než 19 ms, tak to zpracuj
            distance = pulse_us/58;                   //výpočet vzdálenosti [cm]
            }
            else{
            distance = 0;                                 //Out of range
            }
        if (value < 1.0){
            napeti = 0;
            }
            else{
            napeti = 1;
            }   
        posli(sprintf(message,"S%d.%d&", (int)distance, napeti));    //odeslání zprávy NECHAT CO NEJKRATŠÍ!
        flag_pulse = 0;                                             //nulování vlajky
        }
    
    if (flag_serial == 1){//dekódování řetězece pro kontrolu
        led4 = !led4;
        if(buffer[0] == '/' && buffer[1] == '/'){ //premble je v pořádku
            sscanf(buffer,"//A%d.%d", &x ,&y); //dekodování řetězce, '.' je oddělovací znak 
                int PWM_SERVO = Angle(x); //kontrolování dat serva
                int PWM_MOTOR = Speed(y); //kontrolování dat motoru
                Servo.pulsewidth_us(PWM_SERVO);//nastavení PWM serva
                Motor.pulsewidth_us(PWM_MOTOR);//nastavení PWM motoru
            memset(&buffer[0], 0, sizeof(buffer)); //mazání bufferu
            }
    flag_serial = 0;
    }

    }//while
}//main