File content as of revision 22:3996c3f41922:

/**
 * This library is included main.cpp.
 * class "machine" whitch machine functions are structured.
 */

#ifndef MACHINE_H
#define MACHINE_H

/**
 * Functions prototype.
 */


/**
 * Includes
 */
#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "PinDefined_and_Setting_ps3.h"
#include "prototype.h"
#include "communicate.h"
#include "Parameters_ps3.h"
#include "manualMode.h"
#include "autoMode.h"

/**
 * Functions.
 */

/***The function is motors initialize.***/
inline void initializeMotors()
{
    Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
    Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
}

/***The function is Move on field.***/
//Right
void Move_r(float speed1)
{
    if(speed1<0) {
        Move_r_Motor_CCW    = 1;
        Move_r_Motor_CW     = 0;
        Move_r_Motor_PWM    = abs(speed1);
    } else {
        Move_r_Motor_CCW    = 0;
        Move_r_Motor_CW     = 1;
        Move_r_Motor_PWM    = speed1;
    }
}
//Left
void Move_l(float speed2)
{
    if(speed2<0) {
        Move_l_Motor_CCW    = 1;
        Move_l_Motor_CW     = 0;
        Move_l_Motor_PWM    = abs(speed2);
    } else {
        Move_l_Motor_CCW    = 0;
        Move_l_Motor_CW     = 1;
        Move_l_Motor_PWM    = speed2;
    }
}

/***Caluculate state.***/
inline void mesure_state()
{
    Pulses_move_r       = ( double )Move_r_sense.getPulses();
    Pulses_move_l       = ( double )Move_l_sense.getPulses();
    dlr                 = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
    dll                 = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
    PrefPulses_move_r   = Pulses_move_r;
    PrefPulses_move_l   = Pulses_move_l;
    vr                  = dlr / RATE;
    vl                  = dll / RATE;
    velocity            = ( vr + vl ) / 2.0;
    dsita               = ( dlr - dll ) / ( 2.0 * d );
    dx                  = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
    dy                  = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
    sita                += dsita;
    x                   += dx;
    y                   += dy;
}

/***Emergency stop.***/
void Emergency_toggle()
{
    if(edge_circle) {
        edge_circle     = 0;
        Emergency_stop  = !Emergency_stop;
    }
}

/***Interrupter to shoot.***/
inline void initialize_interrupter() {
    interrupter.rise(&shootEsaka);
}

void shootEsaka() {
    Indicator1 = !Indicator1;
}

#endif /*machine.h*/