File content as of revision 15:49ed1bbf3c1d:

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

#ifndef MACHINE_H
#define MACHINE_H

/**
 * Includes
 */
#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "MachineState.h"
#include "PinDefined_and_Setting_ps3.h"
#include "Parameters_ps3.h"

Serial pc(USBTX, USBRX);

/**
 * Functions prototype.
 */
inline void initializeMotors();
inline void initializeControllers();
inline void mesure_move_r_velocity();
inline void mesure_move_l_velocity();
inline void Move_r_speed_following();
inline void Move_l_speed_following();
void        Move_r( float speed1 );
void        Move_l( float speed2 );

/**
 * include function header files.
 */
#include "communicate.h"

/**
 * Defines and const
 */
#define RATE  0.01
#define PI    3.14159265359
#define speed 10000.0
const double HalfTread   = 375.0;
const double WheelRadius = 34.0;
const double PulsePerRev = 400.0;
const double Xo          = 0.0;
const double Yo          = 0.0;
const double Sitao       = PI / 4.0;

/**
 * Class prototype.
 */
MachineState Machine( WheelRadius, HalfTread, PulsePerRev, ENCOD_R_A, ENCOD_R_B, ENCOD_L_A, ENCOD_L_B, RATE, Xo, Yo, Sitao );

/**
 * Set functions.
 */

/***PID Controller***/
PID velocity_controller( MOVE_VELOCITY_Kc, MOVE_VELOCITY_TAUi, MOVE_VELOCITY_TAUd, RATE );
PID direction_controller( MOVE_DIRECTION_Kc, MOVE_DIRECTION_TAUi, MOVE_DIRECTION_TAUd, RATE );
//PID direction_controller( 9.0, 2637.0, 0.0, RATE );

/***Variables***/
double targ_velocity        = 0.0;
double targ_sita            = 0.0;
double targ_swing_velocity  = 0.0;
double x1                   = 0.0;
double x2                   = 0.0;
double Vr                   = 0.0;
double Vl                   = 0.0;
int    step                 = 0;
int    cylinder_step        = 0;


/**
 * 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 PID controller initialize.***/
inline void initializeControllers()
{

    velocity_controller.setInputLimits(MOVE_VELOCITY_INPUT_LIMIT_BOTTOM, MOVE_VELOCITY_INPUT_LIMIT_TOP); //x1
    direction_controller.setInputLimits(MOVE_DIRECTION_INPUT_LIMIT_BOTTOM , MOVE_DIRECTION_INPUT_LIMIT_TOP); //x2

    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
    velocity_controller.setOutputLimits(MOVE_VELOCITY_OUTPUT_LIMIT_BOTTOM, MOVE_VELOCITY_OUTPUT_LIMIT_TOP);
    direction_controller.setOutputLimits(MOVE_DIRECTION_OUTPUT_LIMIT_BOTTOM, MOVE_DIRECTION_OUTPUT_LIMIT_TOP);
    
    //set bias. 初期値
    velocity_controller.setBias(MOVE_VELOCITY_BIAS);
    direction_controller.setBias(MOVE_DIRECTION_BIAS);

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

/***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.***/

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

void sita_following(double sita,double target_s)
{
    direction_controller.setSetPoint( ( float ) target_s );
    direction_controller.setProcessValue( ( float ) sita );
    x2 = ( double )direction_controller.compute();
}

inline void move_following()
{
    velocity_following( Machine.getVelocity(), targ_velocity );
    sita_following( Machine.getSita(), targ_sita );
    Vr = ( x1 + x2 ) / 2.0;
    Vl = ( x1 - x2 ) / 2.0;
    if( abs( Vr ) < 0.02 ) Vr = 0.0;
    if( abs( Vl ) < 0.02 ) Vl = 0.0;
    Move_r( ( float ) Vr );
    Move_l( ( float ) Vl );
}

/***Emergency stop.***/
void Emergency_toggle()
{
    if(edge_circle) {
        edge_circle=0;
        if(toggle) toggle=0,Emergency_stop=0;
        else toggle=1,Emergency_stop=1;
    }
}

#endif /*machine.h*/