File content as of revision 29:6bce50d6530c:

#include "mbed.h"
#include "Robot.h"
#include "CAN_asser.h"

#include "Deplacement.h"
#include "Laser.h"
#include "Rouleau.h"
#include <string>

using namespace std;

DigitalIn Robot::Jack(PA_15);

Robot::Robot() : laserG(Laser(PB_1)), laserD(Laser(PC_5)), laserA(Laser(PC_4)),
                 rouleau(Rouleau(D5, D4, D6)),                                      /*Pins order : PWM, Ejecte, Gobe*/
                 BPs(DigitalIn(PC_2, PullDown)),
                 CNY_G(AnalogIn(A2)), CNY_D(AnalogIn(A3)),                           //Infos plus haut pour les pins
                 Cerveau_Bras(PwmOut(PB_7)), Disquette(PwmOut(PB_6))
{
    can_init();
    this->setPos(500,0,0);
    this->setSpeed(80,450); // vitesse de rotation de recherche de la balle
    
    this->Cerveau_Bras.period_ms(20);
    this->Cerveau_Bras = POS_BAS;
    
    this->Disquette.period_ms(20);
}

bool Robot::aBalle()
{
    return BPs.read();
}

/// Fonctions Odometrie ///
void Robot::setPos(unsigned short x, unsigned short y, signed short theta) const
{
    SetOdometrie(ODOMETRIE_POSITION, y, x, theta);   // Oui les x et les y sont inversés...
}

/// retourne l'adresse de la premiere case d'un tableau contenant dans l'ordre la position en x et en y et l'angle theta ///
short Robot::pos(T_POS pos)
{
    return this->deplacement.getPos(pos);
}

bool Robot::immobile()
{
    return this->deplacement.immobile();
}


/// Toutes les fonctions de déplacement ///
bool Robot::avance(signed short dist)
{
    return deplacement.avance(dist);
}

bool Robot::tourne(signed short angle)
{
    return deplacement.tourne(angle);
}

bool Robot::GoToXYT(unsigned short x, unsigned short y, unsigned short t, signed char sens)  //sens = 0 par défaut
{
    return deplacement.GoToXYT(x,y,t,sens);
}

void Robot::setSpeed(unsigned short vitesse, unsigned short acceleration) const
{
    SendSpeed(vitesse,acceleration);
}

void Robot::stop()
{
    deplacement.stop();
}


/// Fonction distance lasers ///
float Robot::getDist(int laserENUM, string unite) //="cm" par défaut
{
    switch(laserENUM) {
        case Laser::Gauche:
            return this->laserG.getDist(unite);

        case Laser::Droit:
            return this->laserD.getDist(unite);

        case Laser::Arriere:
            return this->laserA.getDist(unite);
    }

    return 0;
}

/// Fonction CNY surBlanc ou pas ///
bool Robot::surBlanc( T_CNY cny )
{
    static const float SEUIL_CNY_G = 0.5f,
                       SEUIL_CNY_D = 0.5f;
    
    //dbug.printf("Gauche : %f\n\rDroit : %f\n\r",this->CNY_G.read(), this->CNY_D.read());
    
    if( cny == Robot::CNY_GAUCHE )
        return ( this->CNY_G.read() >= SEUIL_CNY_G );
        
    else if( cny == Robot::CNY_DROIT )
        return ( this->CNY_D.read() >= SEUIL_CNY_D );
        
    else return false;
}

/// Fonctions Rouleau ///
void Robot::ejecte(int vitesse) // vitesse max = 100
{
    rouleau.ejecte(vitesse);
}

void Robot::gobe(int vitesse) // vitesse max = 100
{
    rouleau.gobe(vitesse);
}

void Robot::stopRouleau()
{
    rouleau.stop();
}

/// Bras ///
bool Robot::leveBras()
{
    static double posCerveau;
    posCerveau = this->Cerveau_Bras;
    
    if(posCerveau <= POS_HAUT)
    {
        posCerveau += PAS;
        this->Cerveau_Bras = posCerveau;
        return false;
    }
    else return true;
}

bool Robot::baisseBras()
{
    static double posCerveau;
    posCerveau = this->Cerveau_Bras;
    
    if(posCerveau >= POS_BAS)
    {
        posCerveau -= PAS;
        this->Cerveau_Bras = posCerveau;
        return false;
    }
    else return true;
}

void Robot::exploseBallon()
{
    this->Disquette.write(0.5);
}

void Robot::arreteDisquette()
{
    this->Disquette.write(0);
}