Benoît Roussel / Mbed 2 deprecated test_algo

Dependencies:   mbed

direction.h

Committer:
Mrlinkblue
Date:
2019-05-31
Revision:
3:97827746c632
Parent:
0:9eb40ee5ff41
Child:
4:c393c14f4502

File content as of revision 3:97827746c632:

#include "mbed.h"
#include <string>

int Periode = 20; //ms

// Paramètre de temps haut pour la vitesse
float Tmin_avant = 1.48; //ms
float Tmax_avant = 1.14; //ms
// Paramètre de temps haut pour la vitesse
float Tmin_direction = 1.09; //ms
float Tmax_direction = 1.5; //ms
// PWM du servo-moteur-vitesse
PwmOut pwm_servo_vitesse(PE_5);
// PWM du servo-moteur-direction
PwmOut pwm_servo_direction(PE_6); 

// Liaison série de la liaison bluetooth
Serial ComBT(PD_5,PD_6,115200);

// Nombres entre 0 et 200
// Format : a 147 (angle)
int a_com;
// Format : v 136 (vitesse)
int v_com;
    
// Pourcentage initiale de la vitesse
float pv=0.0; 
float pa=0.5;

// Liste avec a130v012
char data_direction[8];
    
    
// Fonction permettant d'envoyer le temps haut correspondant à un pourcentage de la vitesse max
void commande_servo_vitesse(float p) {
     int t1 = ((Tmax_avant-Tmin_avant)*p/2+Tmin_avant)*1000;
     pwm_servo_vitesse.pulsewidth_us(t1);
}

// Fonction permettant d'envoyer le temps haut correspondant à un pourcentage de l'angle
void commande_servo_direction(float p) {   
     int t1 = ((Tmin_direction-Tmax_direction)*p/2+Tmax_direction)*1000;
     pwm_servo_direction.pulsewidth_us(t1);
}

// Fonction permettant de récupérer la commande par bluetooth
void recuperer(char* data_direction){
    int i=0;
    char a = '0';
    bool state=false;
    while(i<8){
        if (ComBT.readable()){
            a = ComBT.getc();
            if (a=='a'){
                state=true;
                }
            if (state==true){
            data_direction[i] = a;
            i++;
            }   
            }
    }
}

void pourcentage_commande_bluetooth(){
    // Conversion char -> entier 
        int d1 = data_direction[1] - 48;
        int d2 = data_direction[2] - 48;
        int d3 = data_direction[3] - 48;
        // Détermination du nombre écrit sur l'écran (commande angle)
        if (data_direction[0]=='a'){
            a_com = d1*100+d2*10+d3;
            }
        // Pourcentage de l'angle
        pa=((float)a_com)/100.0;
            
        // Conversion char -> entier 
        int d5 = data_direction[5] - 48;
        int d6 = data_direction[6] - 48;
        int d7 = data_direction[7] - 48;
        // Détermination du nombre écrit sur l'écran (commande vitesse)
        if (data_direction[4]=='v'){
            v_com = d5*100+d6*10+d7;
            }
        // Pourcentage de la vitesse
        pv=((float)v_com)/100.0;
    }

void setup_direction(){
    //Initialisation de la PWM de direction
    pwm_servo_vitesse.period_ms(Periode); 
    pwm_servo_direction.period_ms(Periode);
    commande_servo_vitesse(pv);
    commande_servo_direction(pa);
    }

int convers_angle_tempsh(float angle){
    int t_angle;
    t_angle=int((-11.554)*angle+1308.4);
    return(t_angle);
    }

int convers_vitesse_tempsh(float vitesse){
    int t_vitesse;
    t_vitesse = int (vitesse *(1440.0-1480.0)/vitesse_max + 1480.0) ; // (us)
    if (t_vitesse>1480){
        return(1480);
        }
    return(t_vitesse);
    }