File content as of revision 11:ce3083681efa:

/**
 * 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 "PinDefined_and_Setting_ps3.h"
#include "Parameters_ps3.h"

/**
 * Functions prototype.
 */
inline void initializeMotors();
inline void initializeControllers();
void Move_r(float speed1);
void Move_l(float speed2);
inline void mesure_move_r_velocity();
inline void mesure_move_l_velocity();
inline void mesure_swing_velocity();
inline void data_clear();
inline void data_check();
inline void SBDBT_interrupt();
inline void initializeSBDBT();
inline void Move_r_speed_following();
inline void Move_l_speed_following();
inline void Swing_speed_following();
void Emergency_toggle();
short toggle=0,edge_circle=0,edge_triangle=0,edge_r1=0,edge_l1=0,edge_l_up=0,edge_l_down=0,edge_r_up=0,edge_r_down=0,edge_right=0,edge_left=0,edge_up=0;
int step=0,cylinder_step=0;

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

/**
 * Defines
 */
#define RATE 0.01
#define PI 3.14159265359
#define speed 10000.0

/**
 * Set functions.
 */

/***PID Controller***/
PID velocity_controller(18.0,5274.0 ,0.0,RATE);
PID direction_controller(18.0,5274.0,0.0,RATE);
PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);

/***Variables***/
double Pulses_move_r=0.0;
double Pulses_move_l=0.0;
double Pulses_swing=0.0;
double PrefPulses_move_r=0.0;
double PrefPulses_move_l=0.0;
double PrefPulses_swing=0.0;
double swing_velocity=0.0;
double velocity=0.0;
double targ_velocity=0.0;
double targ_sita=0.0;
double targ_swing_velocity=0.0;
double dsita=0.0,dx=0.0,dy=0.0,sita=0.0,x=0.0,y=0.0,dlr=0.0,dll=0.0,vr=0.0,vl=0.0; //state
double x1=0.0,x2=0.0;
double Vr=0.0,Vl=0.0;
const double d=375.0;
const double r_wheel=34.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);
    Motor_swing_pwm.period_us(SWING_PERIOD_US);
}

/***The function is PID controller initialize.***/
inline void initializeControllers()
{

    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
    direction_controller.setInputLimits(-10.0, 10.0); //x2
    swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);

    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
    velocity_controller.setOutputLimits(-1.0, 1.0);
    direction_controller.setOutputLimits(-1.0, 1.0);
    swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
    
    //set bias. 初期値
    velocity_controller.setBias(0.0);
    direction_controller.setBias(0.0);
    swing_controller.setBias(SWING_BIAS);

    //set mode.
    velocity_controller.setMode(AUTO_MODE);
    direction_controller.setMode(AUTO_MODE);
    swing_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.***/
inline void mesure_state()
{
    Pulses_move_r = (double)Move_r_sense.getPulses();
    dlr = (((Pulses_move_r - PrefPulses_move_r)/400.0)*2.0*PI)*r_wheel;
    PrefPulses_move_r = Pulses_move_r;
    Pulses_move_l = (double)Move_l_sense.getPulses();
    dll = (((Pulses_move_l - PrefPulses_move_l)/400.0)*2.0*PI)*r_wheel;
    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;
}

inline void mesure_swing_velocity()
{
    Pulses_swing = (double)Swing_speed_sense.getPulses();
    swing_velocity = Pulses_swing - PrefPulses_swing;
    PrefPulses_swing = Pulses_swing;
}

/***The function is following move speed.***/
float cont_swing=0.0;

inline void velocity_following()
{
    velocity_controller.setSetPoint((float)targ_velocity);
    velocity_controller.setProcessValue((float)velocity);
    x1 = (double)velocity_controller.compute();
}

inline void sita_following()
{
    direction_controller.setSetPoint((float)targ_sita);
    direction_controller.setProcessValue((float)sita);
    //direction_controller.setSetPoint(0.0);  //目標値とのずれをなくす
    //direction_controller.setProcessValue(-y);
    x2 = (double)direction_controller.compute();
}

inline void move_following()
{
    velocity_following();
    sita_following();
    Vr=(x1+x2)/2.0;
    Vl=(x1-x2)/2.0;
    if(abs(Vr)<0.05) Vr=0.0;
    if(abs(Vl)<0.05) Vl=0.0;
    Move_r((float)Vr);
    Move_l((float)Vl);
}

inline void Swing_speed_following()
{
    swing_controller.setSetPoint((float)targ_swing_velocity);
    swing_controller.setProcessValue((float)swing_velocity);
    cont_swing = swing_controller.compute();
    Motor_swing_pwm = cont_swing;
}


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