lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.h@0:e12eb40b9fef, 2020-04-23 (annotated)

Committer:
lsh2205
Date:
Thu Apr 23 00:38:16 2020 +0000
Revision:
0:e12eb40b9fef
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:e12eb40b9fef 1 #ifndef _MAIN_H_
lsh2205 0:e12eb40b9fef 2 #define _MAIN_H_
lsh2205 0:e12eb40b9fef 3 int button_offset_posion[6]={0,};
lsh2205 0:e12eb40b9fef 4 bool encoder_check[6]={false,false,false,false,false,false};
lsh2205 0:e12eb40b9fef 5 bool motor_onoff[6]={true,true,true,true,true,true};
lsh2205 0:e12eb40b9fef 6 int encoder_data[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 7
lsh2205 0:e12eb40b9fef 8 double Speed_Pgain[6]={ 3,
lsh2205 0:e12eb40b9fef 9 3,
lsh2205 0:e12eb40b9fef 10 3,
lsh2205 0:e12eb40b9fef 11 3,
lsh2205 0:e12eb40b9fef 12 3,
lsh2205 0:e12eb40b9fef 13 3};
lsh2205 0:e12eb40b9fef 14
lsh2205 0:e12eb40b9fef 15 double Speed_Igain[6]={ 0.01,
lsh2205 0:e12eb40b9fef 16 0.01,
lsh2205 0:e12eb40b9fef 17 0.01,
lsh2205 0:e12eb40b9fef 18 0.01,
lsh2205 0:e12eb40b9fef 19 0.01,
lsh2205 0:e12eb40b9fef 20 0.01};
lsh2205 0:e12eb40b9fef 21
lsh2205 0:e12eb40b9fef 22 double position_Pgain[6]
lsh2205 0:e12eb40b9fef 23 ={ 10,
lsh2205 0:e12eb40b9fef 24 10,
lsh2205 0:e12eb40b9fef 25 10,
lsh2205 0:e12eb40b9fef 26 10,
lsh2205 0:e12eb40b9fef 27 10,
lsh2205 0:e12eb40b9fef 28 10};
lsh2205 0:e12eb40b9fef 29
lsh2205 0:e12eb40b9fef 30 double Position_input_filter[6]
lsh2205 0:e12eb40b9fef 31 ={ 0.05,
lsh2205 0:e12eb40b9fef 32 0.05,
lsh2205 0:e12eb40b9fef 33 0.05,
lsh2205 0:e12eb40b9fef 34 0.05,
lsh2205 0:e12eb40b9fef 35 0.05,
lsh2205 0:e12eb40b9fef 36 0.05};
lsh2205 0:e12eb40b9fef 37 double Speed_I_input_filter[6]
lsh2205 0:e12eb40b9fef 38 ={ 0.1,
lsh2205 0:e12eb40b9fef 39 0.1,
lsh2205 0:e12eb40b9fef 40 0.1,
lsh2205 0:e12eb40b9fef 41 0.1,
lsh2205 0:e12eb40b9fef 42 0.1,
lsh2205 0:e12eb40b9fef 43 0.1};
lsh2205 0:e12eb40b9fef 44
lsh2205 0:e12eb40b9fef 45 double offset[6]={ 1500,
lsh2205 0:e12eb40b9fef 46 1500,
lsh2205 0:e12eb40b9fef 47 1500,
lsh2205 0:e12eb40b9fef 48 1500,
lsh2205 0:e12eb40b9fef 49 1500,
lsh2205 0:e12eb40b9fef 50 1500};
lsh2205 0:e12eb40b9fef 51
lsh2205 0:e12eb40b9fef 52
lsh2205 0:e12eb40b9fef 53 // SPI
lsh2205 0:e12eb40b9fef 54 #define SPI_MOSI1 PA_7
lsh2205 0:e12eb40b9fef 55 #define SPI_MISO1 PA_6
lsh2205 0:e12eb40b9fef 56 #define SPI_SCK1 PA_5
lsh2205 0:e12eb40b9fef 57
lsh2205 0:e12eb40b9fef 58 // Encoder
lsh2205 0:e12eb40b9fef 59 #define ENCODER_CS1 PB_0
lsh2205 0:e12eb40b9fef 60 #define ENCODER_CS2 PC_1
lsh2205 0:e12eb40b9fef 61 #define ENCODER_CS3 PC_0
lsh2205 0:e12eb40b9fef 62 #define ENCODER_CS4 PB_15
lsh2205 0:e12eb40b9fef 63 #define ENCODER_CS5 PB_14
lsh2205 0:e12eb40b9fef 64 #define ENCODER_CS6 PB_13
lsh2205 0:e12eb40b9fef 65
lsh2205 0:e12eb40b9fef 66 // Limit switch
lsh2205 0:e12eb40b9fef 67 #define LIMIT_SW1 PC_3
lsh2205 0:e12eb40b9fef 68 #define LIMIT_SW2 PC_2
lsh2205 0:e12eb40b9fef 69 #define LIMIT_SW3 PC_5
lsh2205 0:e12eb40b9fef 70 #define LIMIT_SW4 PC_9
lsh2205 0:e12eb40b9fef 71 #define LIMIT_SW5 PC_6
lsh2205 0:e12eb40b9fef 72 #define LIMIT_SW6 PC_8
lsh2205 0:e12eb40b9fef 73
lsh2205 0:e12eb40b9fef 74 // Motor
lsh2205 0:e12eb40b9fef 75 #define MOTOR_PWM1 PA_15
lsh2205 0:e12eb40b9fef 76 #define MOTOR_PWM2 PA_9
lsh2205 0:e12eb40b9fef 77 #define MOTOR_PWM3 PA_8
lsh2205 0:e12eb40b9fef 78 #define MOTOR_PWM4 PB_10
lsh2205 0:e12eb40b9fef 79 #define MOTOR_PWM5 PB_3
lsh2205 0:e12eb40b9fef 80 #define MOTOR_PWM6 PA_10
lsh2205 0:e12eb40b9fef 81 #define MOTOR_PWM7 PB_11
lsh2205 0:e12eb40b9fef 82
lsh2205 0:e12eb40b9fef 83 #include "flash.h"
lsh2205 0:e12eb40b9fef 84 #include "spi_setup.h"
lsh2205 0:e12eb40b9fef 85 #include "pc_serial.h"
lsh2205 0:e12eb40b9fef 86 #include "motor.h"
lsh2205 0:e12eb40b9fef 87 #include "encoder.h"
lsh2205 0:e12eb40b9fef 88 #include "speed_pid.h"
lsh2205 0:e12eb40b9fef 89 #include "target_position_cal.h"
lsh2205 0:e12eb40b9fef 90 #include "Position.h"
lsh2205 0:e12eb40b9fef 91 #include "limit.h"
lsh2205 0:e12eb40b9fef 92 #include "button.h"
lsh2205 0:e12eb40b9fef 93
lsh2205 0:e12eb40b9fef 94
lsh2205 0:e12eb40b9fef 95 void board_init()
lsh2205 0:e12eb40b9fef 96 {
lsh2205 0:e12eb40b9fef 97 Serial_Init();
lsh2205 0:e12eb40b9fef 98 if(Flash_Head_Check() == true)
lsh2205 0:e12eb40b9fef 99 {
lsh2205 0:e12eb40b9fef 100 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 101 {
lsh2205 0:e12eb40b9fef 102 set_data[i * 10] = (double)Read_Flash(i * 40);
lsh2205 0:e12eb40b9fef 103 set_data[i * 10 + 1] = (double)Read_Flash(i * 40 + 4);
lsh2205 0:e12eb40b9fef 104 set_data[i * 10 + 2] = (double)Read_Flash(i * 40 + 8);
lsh2205 0:e12eb40b9fef 105 set_data[i * 10 + 3] = (double)Read_Flash(i * 40 + 12);
lsh2205 0:e12eb40b9fef 106 set_data[i * 10 + 4] = (double)Read_Flash(i * 40 + 16);
lsh2205 0:e12eb40b9fef 107 set_data[i * 10 + 5] = (double)Read_Flash(i * 40 + 20);
lsh2205 0:e12eb40b9fef 108 }
lsh2205 0:e12eb40b9fef 109 }
lsh2205 0:e12eb40b9fef 110 else
lsh2205 0:e12eb40b9fef 111 {
lsh2205 0:e12eb40b9fef 112 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 113 {
lsh2205 0:e12eb40b9fef 114 set_data[(i+1)*10+0]=Speed_Pgain[i];
lsh2205 0:e12eb40b9fef 115 set_data[(i+1)*10+1]=Speed_Igain[i];
lsh2205 0:e12eb40b9fef 116 set_data[(i+1)*10+2]=position_Pgain[i];
lsh2205 0:e12eb40b9fef 117 set_data[(i+1)*10+3]=Position_input_filter[i];
lsh2205 0:e12eb40b9fef 118 set_data[(i+1)*10+4]=Speed_I_input_filter[i];
lsh2205 0:e12eb40b9fef 119 set_data[(i+1)*10+5]=offset[i];
lsh2205 0:e12eb40b9fef 120 }
lsh2205 0:e12eb40b9fef 121 }
lsh2205 0:e12eb40b9fef 122
lsh2205 0:e12eb40b9fef 123 limit_init();
lsh2205 0:e12eb40b9fef 124 spi_init();
lsh2205 0:e12eb40b9fef 125 motor_init();
lsh2205 0:e12eb40b9fef 126 Serial_Init();
lsh2205 0:e12eb40b9fef 127
lsh2205 0:e12eb40b9fef 128 }
lsh2205 0:e12eb40b9fef 129
lsh2205 0:e12eb40b9fef 130 #endif