File content as of revision 33:a4323c20494b:

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

#ifndef MACHINE_H
#define MACHINE_H

/**
 * Includes
 */
#include "mbed.h"
#include <stdlib.h>
#include <string.h>
#include "QEI.h"
#include "PID.h"
#include "PinDefinedSetting.h"
#include "prototype.h"
#include "communicate.h"
#include "Parameters_ps3.h"
#include "Swing.h"
#include "autoMode.h"
#include "manualMode.h"

/**
 * Functions.
 */
/***called by Com***/
void Call(){
    mesure_state();
    move_following();
    if(spcount<speed){
        spcount+=speed/100.0;
        targ_velocity=spcount;
    }
    if(dpcount>0.0){
        dpcount-=speed/100.0;
        targ_velocity=dpcount;
    }
    mesureSwing();
    mesure_state();
    if(enableSwing) swingFollowing();
}

/***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    = -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    = -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;
    if(flagf) velocity  = ( vr + vl ) / 2.0;
    else  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;
}

/***The function is PID controller initialize.***/
inline void initializeControllers()
{
    velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
    direction_controller.setInputLimits(-PI, PI); //x2

    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
    velocity_controller.setOutputLimits(0.0, 1.0);
    direction_controller.setOutputLimits(-1.0, 1.0);

    //set bias. 初期値
    velocity_controller.setBias(0.0);
    direction_controller.setBias(0.0);

    //set mode.
    velocity_controller.setMode(AUTO_MODE);
    direction_controller.setMode(AUTO_MODE);
}

/***The function is following move speed.***/
inline void velocity_following()
{
    velocity_controller.setSetPoint((float)targ_velocity);
    velocity_controller.setProcessValue((float)velocity);
    x1 = (double)velocity_controller.compute();
}

inline void arg_following()
{
    direction_controller.setSetPoint((float)targ_sita);
    direction_controller.setProcessValue((float)sita);
    x2 = (double)direction_controller.compute();
}

inline void move_following()
{
    velocity_following();
    arg_following();
    if(autoflag){
        if(flagf==0){
            if(x2>0.0){
                Vr = -x1 + x2;
                Vl = -x1;
            }
            else{
                Vr = -x1;
                Vl = -x1 - x2;
            }
        }
        else if(flagf==1){
            if(x2>0.0){
                Vr = x1;
                Vl = x1 - x2;
            }
            else{
                Vr = x1 + x2;
                Vl = x1;
            }
        }
    }
    if(!autoflag){
        if(flagf==0){
            Vr = ((-x1) + x2)/2.0;
            Vl = ((-x1) - x2)/2.0;
        }
        else if(flagf==1){
            Vr = (x1 + x2)/2.0;
            Vl = (x1 - x2)/2.0;
        }
    }
    Move_r( ( float ) Vr );
    Move_l( ( float ) Vl );
}

/***The function reset state.***/
void resetState(int d){
#ifdef BLUE
    x=0.0;
    y=0.0;
    if(d==1){
        sita=PI/4.0;
        targ_sita=PI/4.0;
    }
    else if(d==0){
        sita=3.0*PI/4.0;
        targ_sita=3.0*PI/4.0;
    }
    else if(d==2){
        sita=0.0;
        targ_sita=0.0;
    }
#else
    x=0.0;
    y=0.0;
    if(d==1){
        sita=-PI/4.0;
        targ_sita=-PI/4.0;
    }
    else if(d==0){
        sita=-3.0*PI/4.0;
        targ_sita=-3.0*PI/4.0;
    }
    else if(d==2){
        sita=0.0;
        targ_sita=0.0;
    }
#endif
    targ_velocity=0.0;
    Move_r_sense.reset();
    Move_l_sense.reset();
    velocity_controller.reset();
    direction_controller.reset();
}

#endif /*machine.h*/