,,,,,,,,,,,,

Dependencies:   CMPS03 SRF05 mbed pixy

Fork of 0000Non_stop_code_v1 by Alexandre Pirotte

fct.cpp

Committer:
pirottealex
Date:
2017-06-10
Revision:
1:99f469d63b9a
Parent:
0:a8cee96c9250

File content as of revision 1:99f469d63b9a:

#include "mbed.h"
#include "fct.h"
#include "SRF05.h"

void vitmoteur(float VitG, float VitD)
{
    if(VitG<0) {
        VitG=-1*VitG;
        cmdI2C=cmdI2C&0xfe; //passe le moteur gauche en marche arriere 00000001
    } else {
        cmdI2C=cmdI2C|0x01; // marche avant 11110111 mot gauche
    }
    if(VitD<0) {
        VitD=-1*VitD;
        cmdI2C=cmdI2C&0xfd; //passe le moteur gauche en marche arriere 00000100
    } else {
        cmdI2C=cmdI2C|0x02;//marche avant 11111011 mot droit
    }
    monI2C.write(ADR_PCF,&cmdI2C,1);
    MotG.pulsewidth(((100-VitG)/100.0)*PERIOD);
    MotD.pulsewidth(((100-VitD)/100.0)*PERIOD);
}
void lecture_blanc(void)
{
    if(C1.read()>0.5) {
        captL1=0;
    } else {
        captL1=1;
    }
    if(C3.read()>0.5) {
        captL3=0;
    } else {
        captL3=1;
    }
}

void lecture_us(void)
{
    us_arriere=us_arr.read();
}

void init(void)
{
    bp.mode(PullUp);
    MotG.period(PERIOD);
    MotD.period(PERIOD);
    vitmoteur(0,0);
}

void lecture_boussole(void)
{
    gBoussole=Boussole.readBearing() / 10.0;
}