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