File content as of revision 256:7c8cc8b56b88:

#include "mbed.h"
#include "FastPWM.h"

// pwm 
#define PIN_V       PB_7
#define PIN_W       PB_6
//#define PWM_ARR 0x465         // loop 80k, pwm 40k 
//#define PWM_ARR     0x8CA       // loop 40k, pwm 20k
#define PWM_ARR     0x1194      // loop 20k, pwm 10k
//#define PWM_ARR     0x2328    // loop 10k, pwm 5k
#define TMR3_COUNT  0x4650      // loop 5k
#define TMR2_COUNT  0x2710      // loop 500hz with prescale 18

#define FREQ_500    500.0f
#define FREQ_1k     1000.0f
#define FREQ_5k     5000.0f
#define FREQ_10k    10000.0f
#define FREQ_20k    20000.0f
#define FREQ_40k    40000.0f
#define DT_500      0.002f
#define DT_1k       0.0005f
#define DT_5k       0.0002f
#define DT_10k      0.0001f
#define DT_20k      0.00005f
#define DT_40k      0.000025f

extern DigitalOut check;
extern DigitalOut check_2;
extern AnalogOut dac_1;
extern AnalogOut dac_2;

extern float dtc_v;
extern float dtc_w;

// I2C
extern I2C i2c; // SDA, SCL (for K22F)
extern const int i2c_slave_addr1; 
extern unsigned int value; // 10bit output of reading sensor AS5510

// SPI
extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
extern DigitalOut eeprom_cs;
extern SPI enc;
extern DigitalOut enc_cs;

// UART 
extern Serial pc; //Serial pc(PA_9,PA_10); _ UART

// CAN
extern CAN can;
extern CANMessage msg;

// Board Information 


// User Function 

void CurrentControl();
void ValveControl(unsigned int ControlMode);


/*******************************************************************************
 * COMMON Settings
 ******************************************************************************/
#define             LATEST_VERSION      19032



/*******************************************************************************
 * COMMON CONSTANTS
 ******************************************************************************/
#define             RAD_30              0.523598775598299f
#define             RAD_60              1.047197551196598f
#define             RAD_120             2.094395102393195f
#define             RAD_180             3.141592653589793f
#define             RAD_240             4.188790204786391f
#define             RAD_300             5.235987755982989f
#define             RAD_360             6.283185307179586f

#define             SYSFREQ             200000000
#define             PBCLK               100000000

//#define             TMR_FREQ_10k       10000
#define             TMR_FREQ_5k       5000

#define             FALSE               0
#define             TRUE                1
#define             OUTPUT              0
#define             INPUT               1
#define             LOW                 0
#define             HIGH                1
#define             DIGITAL             0
#define             ANALOG              1

#define             PI                  3.141592653589793f
#define             D2R                 0.017453292519943f
#define             R2D                 57.295779513082323f


/*******************************************************************************
 * HEADER INCLUDE
 ******************************************************************************/

/*******************************************************************************
 * VARIABLE
 ******************************************************************************/

// Board Information 
extern uint8_t BNO;
extern uint8_t CONTROL_MODE;
extern uint8_t OPERATING_MODE;
extern uint8_t SENSING_MODE;
extern uint8_t CONTROL_UTILITY_MODE;
extern uint8_t CURRENT_CONTROL_MODE;
extern uint8_t FLAG_VALVE_DEADZONE;
extern uint8_t REFERENCE_MODE;
extern int16_t CAN_FREQ;
extern int16_t DIR_JOINT_ENC;
extern int16_t DIR_VALVE;
extern int16_t DIR_VALVE_ENC;

extern float SUPPLY_VOLTAGE;
extern float VALVE_VOLTAGE_LIMIT;

extern float P_GAIN_VALVE_POSITION;
extern float I_GAIN_VALVE_POSITION;
extern float D_GAIN_VALVE_POSITION;
extern float P_GAIN_JOINT_POSITION;
extern float I_GAIN_JOINT_POSITION;
extern float D_GAIN_JOINT_POSITION;
extern float P_GAIN_JOINT_TORQUE;
extern float I_GAIN_JOINT_TORQUE;
extern float D_GAIN_JOINT_TORQUE;
extern float P_GAIN_JOINT_TORQUE_FF;
extern float I_GAIN_JOINT_TORQUE_FF;
extern float D_GAIN_JOINT_TORQUE_FF;

extern int16_t K_SPRING;
extern int16_t D_DAMPER;

extern int16_t flag_delay_test;

//extern float P_GAIN_VALVE_POSITION_OPP;
//extern float I_GAIN_VALVE_POSITION_OPP;
//extern float D_GAIN_VALVE_POSITION_OPP;
//extern float P_GAIN_JOINT_POSITION_OPP;
//extern float I_GAIN_JOINT_POSITION_OPP;
//extern float D_GAIN_JOINT_POSITION_OPP;
//extern float P_GAIN_JOINT_TORQUE_OPP;
//extern float I_GAIN_JOINT_TORQUE_OPP;
//extern float D_GAIN_JOINT_TORQUE_OPP;

extern int16_t VELOCITY_COMP_GAIN;
extern int16_t COMPLIANCE_GAIN;

extern int16_t VALVE_CENTER;

extern int16_t VALVE_FF;

extern int16_t BULK_MODULUS;

extern int16_t CHAMBER_VOLUME_A;
extern int16_t CHAMBER_VOLUME_B;

extern int16_t PISTON_AREA_A;
extern int16_t PISTON_AREA_B;
extern float PISTON_AREA_alpha;
extern float alpha3;


extern int16_t PRES_SUPPLY;
extern int16_t PRES_RETURN;

extern int16_t ENC_LIMIT_PLUS;
extern int16_t ENC_LIMIT_MINUS;

extern int16_t STROKE;


extern float Amm;
extern float beta;
extern float Ps;
extern float Pt;
extern float gamma_hat;
extern float a_hat;
extern float V_adapt;
extern float x_4_des_old;

//extern int16_t VALVE_LIMIT_PLUS;
//extern int16_t VALVE_LIMIT_MINUS;

extern float ENC_PULSE_PER_POSITION;
extern float TORQUE_SENSOR_PULSE_PER_TORQUE;

extern float PRES_SENSOR_A_PULSE_PER_BAR;
extern float PRES_SENSOR_B_PULSE_PER_BAR;

extern int HOMEPOS_OFFSET;
extern int HOMEPOS_VALVE_OPENING;

extern float FRICTION;
extern float REF_PERIOD;
extern float REF_MAG;
extern int REF_NUM;


extern float DAC_REF;
extern float DAC_RESOL;

extern int REF_POSITION;
extern int REF_VELOCITY;
extern int16_t REF_TORQUE;
extern int16_t REF_PRES_DIFF;
extern int16_t REF_PWM;
extern int16_t REF_VALVE_POSITION;
extern int16_t REF_CURRENT;

extern int REF_MOVE_TIME_5k;
extern int INIT_REF_PWM;
extern int INIT_REF_VALVE_POS;
extern int INIT_REF_POS;
extern int INIT_REF_VEL;
extern int INIT_REF_TORQUE;
extern int INIT_REF_PRES_DIFF;
extern int INIT_REF_CURRENT;

extern int CUR_POSITION;
extern int CUR_VELOCITY;
extern float CUR_TORQUE;
extern float CUR_PRES_A;
extern float CUR_PRES_B;
extern int CUR_VALVE_POSITION;

extern unsigned int    TMR2_COUNT_LED1;
extern unsigned int    TMR2_COUNT_LED2;
extern unsigned int    TMR2_COUNT_CAN_TX;
extern unsigned int    TMR3_COUNT_TEST;

extern int num_err;
extern int flag_err[8];
extern int flag_err_old[8];
extern int flag_err_rt;

extern int flag_ref_enable;

extern int flag_data_request[5];

extern int MODE_POS_FT_TRANS;
extern int NN_Control_Flag;

extern int cnt_buffer;

extern float CUR_CURRENT_mA;
extern float CUR_PRES_A_BAR;
extern float CUR_PRES_B_BAR;
extern float CUR_TORQUE_NM;
extern float CUR_TORQUE_NM_PRESS;

extern float PRES_A_VREF;
extern float PRES_B_VREF;
extern float TORQUE_VREF;

extern float VALVE_PWM_RAW_FB;
extern float VALVE_PWM_RAW_FF;
extern float VALVE_PWM_RAW;
extern int VALVE_PWM_VALVE_DZ;

extern float VALVE_GAIN_LPM_PER_V[10];
extern float VALVE_POS_VS_PWM[25];
extern long JOINT_VEL[100];

extern int VALVE_MAX_POS;
extern int VALVE_MIN_POS;
extern int VALVE_POS_NUM;
extern float VALVE_CENTER_OFFSET;
extern float VALVE_DZ_MINUS_OFFSET;
extern float VALVE_DZ_PLUS_OFFSET;
extern float VALVE_DEADZONE_MINUS;
extern float VALVE_DEADZONE_PLUS;

extern int TMR3_COUNT_FINDHOME;
extern int TMR3_COUNT_FLOWRATE;
extern int TMR3_COUNT_DEADZONE;
extern int TMR3_COUNT_PRES_NULL;
extern int TMR3_COUNT_TORQUE_NULL;
extern int TMR3_COUNT_PRES_CALIB;
extern int TMR3_COUNT_REFERENCE;
extern int TMR3_COUNT_JOINT;
extern int TMR3_COUNT_ROTARY_FRIC_TUNE;

extern float TUNING_TIME;

extern float REFERENCE_FREQ;
extern float REFERENCE_MAG;

extern bool FLAG_FIND_HOME;

extern int MODE_JUMP_STATUS;

extern float CUR_PRES_DIFF_BAR;
extern float CUR_PRES_A_sum;
extern float CUR_PRES_B_sum;
extern float CUR_PRES_A_mean;
extern float CUR_PRES_B_mean;
extern float CUR_TORQUE_sum;
extern float CUR_TORQUE_mean;
extern float PRES_A_NULL;
extern float PRES_B_NULL;
extern float TORQUE_NULL;

extern float Ref_Valve_Pos_Old;

extern int VALVE_ID_timer;
extern int VALVE_DZ_timer;
extern int VALVE_FR_timer;
extern int VALVE_HPL_timer;
extern int VALVE_POS_TMP;
extern int JOINT_VEL_TMP;
extern int DDV_POS_AVG ;
extern int VALVE_POS_AVG[50];
extern int VALVE_POS_AVG_OLD;
extern int data_num;
extern int ID_index;
extern int DZ_index;
extern int ID_index_array[50];
extern int first_check;
extern float init_time;
extern int DZ_case;
extern int START_POS;
extern int FINAL_POS;
extern int DZ_DIRECTION;
extern int FIRST_DZ;
extern int SECOND_DZ;
extern int DZ_NUM;
extern int one_period_end;
extern float Ref_Vel_Test;
extern long TMR2_FOR_SLOW_LOGGING;
//extern int velcount;
extern char max_check;
extern char min_check; 

extern float valve_pos_err, valve_pos_err_old, valve_pos_err_diff, valve_pos_err_sum;
extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum;
extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum;
extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ;

extern float CUR_FLOWRATE;
extern float VALVE_FF_VOLTAGE;

extern int pos_plus_end;
extern int pos_minus_end;

extern bool need_enc_init;

extern int temp_time;

extern float CUR_VELOCITY_sum;
extern float temp_vel_sum;

extern int DZ_dir;
extern int DZ_temp_cnt;
extern int DZ_temp_cnt2;
extern int DZ_end;
extern int flag_flowrate;
extern int fl_temp_cnt;
extern int fl_temp_cnt2;
extern int cur_vel_sum;

extern float Cur_Valve_Open_pulse;

// find home
extern int CUR_VELOCITY_OLD;
extern int cnt_findhome;
extern int cnt_vel_findhome;
extern int FINDHOME_VELOCITY;
extern int FINDHOME_VELOCITY_OLD;
extern int FINDHOME_POSITION;
extern int FINDHOME_POSITION_OLD;

// valve gain
extern int check_vel_pos_init;
extern int check_vel_pos_fin;
extern int check_vel_pos_interv;
extern int valve_gain_repeat_cnt;
extern float VALVE_VOLTAGE;

extern float freq_fric_tune;

extern uint32_t TMR3_COUNT_CAN_TX;

// Current Control Variables
extern double I_REF;
extern double I_REF_fil;
extern double I_ERR;
extern double I_ERR_INT;
extern double I_REF_fil_old;
extern double I_REF_fil_diff;
extern double I_REF_fil_DZ;

// system id
extern int cnt_sysid;
extern double freq_sysid_Iref;

extern int cnt_freq_test;
extern int cnt_step_test;
extern int buffer_data_size;
extern int cnt_send_buffer;
extern float freq_test_valve_ref;
extern float ref_array[10000];
extern int pos_array[10000];
extern int flag_every_reference;

extern int TMR3_COUNT_IREF;
extern float CUR_CURRENT;
extern float u_CUR[3];

extern int FINDHOME_STAGE;
extern int FINDHOME_INIT;
extern int FINDHOME_GOTOLIMIT;
extern int FINDHOME_ZEROPOSE;

extern float alpha_trans;


extern float V_out;
extern float V_rem;
extern float V_MAX;

extern float PWM_out;

extern double K_v;
extern double mV_PER_mA;
extern double mV_PER_pulse;
extern double mA_PER_pulse;

extern int timer_while;
extern int while_index;
extern int RL_timer;

extern float K_LPF;
extern float D_LPF;

extern float totq_sen_past;
extern float torq_ref_past;
extern float output_normalized;


#define     numpast_u           0
#define     numpast_x           5
#define     numfuture_x         0
#define     numpast_f           0
#define     numfuture_f         8
#define     num_input           15  //numpast_x + 1 + numfuture_x + numpast_f + 1 + numfuture_f
//#define     num_input           17  //numpast_x + 1 + numfuture_x + numpast_f + 1 + numfuture_f
//#define     num_input           16  //numpast_x + 1 + numfuture_x + numpast_f + numfuture_f
//#define     num_input           15

#define     num_array_u_past        1  // numpast_u * time_interval + 1
#define     num_array_f_future      9  // numfuture_f * time_interval + 1
#define     num_array_f_past        1  // numpast_f * time_interval + 1        
#define     num_array_x_past        6  // numpast_x * time_interval + 1      
#define     num_array_x_future      1  // numfuture_x * time_interval + 1

#define     time_interval           1



/////////////RL
#define num_batch           10
#define num_epoch           1
#define batch_size          100
#define num_input_RL        2
#define num_hidden_unit1    10
#define num_hidden_unit2    10

extern int batch;
extern float train_set_x[batch_size];
extern float train_set_error[batch_size];
extern float train_set_count[batch_size];
extern float state_array[batch_size][num_input_RL];
extern float V[batch_size];
extern float r[batch_size];
extern float td_target[batch_size];
extern float delta[batch_size];
extern float advantage[batch_size];
extern float return_G[batch_size];

extern float mean;
extern float deviation;
extern float mean_old;
extern float deviation_old;
extern float mean_before_SP;
extern float deviation_before_SP;
extern float mean_before_SP_array[batch_size];
extern float deviation_before_SP_array[batch_size];
extern float mean_array[batch_size];
extern float mean_array_old[batch_size];
extern float deviation_array[batch_size];
extern float deviation_array_old[batch_size];

extern float hx_c_sum[num_hidden_unit1];
extern float hx_c_sum_array[batch_size][num_hidden_unit1];
extern float hxh_c_sum[num_hidden_unit2];
extern float hxh_c_sum_array[batch_size][num_hidden_unit2];
extern float hxhh_c_sum;
extern float hxhh_c_sum_array[batch_size];

extern float hx_a_sum[num_hidden_unit1];
extern float hx_a_sum_array[batch_size][num_hidden_unit1];
extern float hxh_a_sum[num_hidden_unit2];
extern float hxh_a_sum_array[batch_size][num_hidden_unit2];
extern float hxhh_a_sum[2];
extern float hxhh_a_sum_array[batch_size][2];

extern float action;
extern float action_array[batch_size];
extern float ratio[batch_size];
extern float pi[batch_size];
extern float pi_old[batch_size];
extern float epsilon;
extern float surr1[batch_size];
extern float surr2[batch_size];
extern float loss[batch_size];
extern float loss_batch;
extern float gamma;
extern float lmbda;
extern char Update_Done_Flag;
extern char Update_Case;
extern float reward_sum; 

extern float virt_pos;
extern float logging1;
extern float logging2;
extern float logging3;
extern float logging4;
extern float logging5;