lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

button.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 _BUTTON_H_
lsh2205 0:e12eb40b9fef 2 #define _BUTTON_H_
lsh2205 0:e12eb40b9fef 3
lsh2205 0:e12eb40b9fef 4 #include "MCP23S17.h"
lsh2205 0:e12eb40b9fef 5 #include "7_segment.h"
lsh2205 0:e12eb40b9fef 6
lsh2205 0:e12eb40b9fef 7 #define BUTTON_DELAY 50 // 75ms
lsh2205 0:e12eb40b9fef 8 #define BUTTON_FAST_LIMIT 45
lsh2205 0:e12eb40b9fef 9 #define FLASH_SAVE_DELAY 3000 // 400000, 10min
lsh2205 0:e12eb40b9fef 10 void reset_speed_pid();
lsh2205 0:e12eb40b9fef 11 void button_pid_info();
lsh2205 0:e12eb40b9fef 12
lsh2205 0:e12eb40b9fef 13 bool menual_mode=false;
lsh2205 0:e12eb40b9fef 14 uint16_t axis_button=0x0fff;
lsh2205 0:e12eb40b9fef 15
lsh2205 0:e12eb40b9fef 16 uint8_t btn1_push_check[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
lsh2205 0:e12eb40b9fef 17
lsh2205 0:e12eb40b9fef 18 uint8_t btn2_push_check[4] = {0, 0, 0, 0};
lsh2205 0:e12eb40b9fef 19 uint8_t btn2_reset_button_check = 0;
lsh2205 0:e12eb40b9fef 20 uint8_t btn2_pgain_data[3] = {0, 0, 0};
lsh2205 0:e12eb40b9fef 21 uint8_t btn2_input_data[2] = {0, 0};
lsh2205 0:e12eb40b9fef 22 uint8_t btn2_fast_pgain = 0;
lsh2205 0:e12eb40b9fef 23 uint8_t btn2_fast_input = 0;
lsh2205 0:e12eb40b9fef 24
lsh2205 0:e12eb40b9fef 25 bool flash_save_flag = false;
lsh2205 0:e12eb40b9fef 26 uint32_t flash_save_cnt = 0;
lsh2205 0:e12eb40b9fef 27
lsh2205 0:e12eb40b9fef 28 double insert_pgain = 0;
lsh2205 0:e12eb40b9fef 29 double insert_input = 0;
lsh2205 0:e12eb40b9fef 30
lsh2205 0:e12eb40b9fef 31 void Button_Detection();
lsh2205 0:e12eb40b9fef 32 void Axis_Button_Detection();
lsh2205 0:e12eb40b9fef 33 void Segement_Button_Detection();
lsh2205 0:e12eb40b9fef 34 bool Front_Board_Detection();
lsh2205 0:e12eb40b9fef 35 void Save_Data_To_Flash();
lsh2205 0:e12eb40b9fef 36 uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3);
lsh2205 0:e12eb40b9fef 37 double Convert_double_Pgain(uint32_t data);
lsh2205 0:e12eb40b9fef 38 double Convert_double_Igain(uint32_t data);
lsh2205 0:e12eb40b9fef 39 void Button_Erase_Flash();
lsh2205 0:e12eb40b9fef 40 uint32_t Button_Read_Flash(uint32_t address);
lsh2205 0:e12eb40b9fef 41 void Button_Write_Flash(uint32_t data);
lsh2205 0:e12eb40b9fef 42
lsh2205 0:e12eb40b9fef 43
lsh2205 0:e12eb40b9fef 44 void Button_Init()
lsh2205 0:e12eb40b9fef 45 {
lsh2205 0:e12eb40b9fef 46 insert_pgain = Convert_double_Pgain(Button_Read_Flash(0));
lsh2205 0:e12eb40b9fef 47 insert_input = Convert_double_Igain(Button_Read_Flash(4));
lsh2205 0:e12eb40b9fef 48
lsh2205 0:e12eb40b9fef 49 //printf("fisrt Gain pgain : %f, input : %f\r\n", insert_pgain, insert_input);
lsh2205 0:e12eb40b9fef 50
lsh2205 0:e12eb40b9fef 51 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 52 {
lsh2205 0:e12eb40b9fef 53 set_data[i * 10 + 2] = insert_pgain;
lsh2205 0:e12eb40b9fef 54 position_Pgain[i-1] = insert_pgain;
lsh2205 0:e12eb40b9fef 55 }
lsh2205 0:e12eb40b9fef 56 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 57 {
lsh2205 0:e12eb40b9fef 58 set_data[i * 10 + 3] = insert_input;
lsh2205 0:e12eb40b9fef 59 Position_input_filter[i-1] = insert_input;
lsh2205 0:e12eb40b9fef 60 }
lsh2205 0:e12eb40b9fef 61
lsh2205 0:e12eb40b9fef 62
lsh2205 0:e12eb40b9fef 63
lsh2205 0:e12eb40b9fef 64 if(insert_pgain != 0xFFFFFFFF)
lsh2205 0:e12eb40b9fef 65 {
lsh2205 0:e12eb40b9fef 66 btn2_pgain_data[0] = (uint8_t)(Button_Read_Flash(0) / 100);
lsh2205 0:e12eb40b9fef 67 btn2_pgain_data[1] = (uint8_t)((Button_Read_Flash(0) % 100) / 10);
lsh2205 0:e12eb40b9fef 68 btn2_pgain_data[2] = (uint8_t)((Button_Read_Flash(0) % 10) / 1);
lsh2205 0:e12eb40b9fef 69 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 70
lsh2205 0:e12eb40b9fef 71 }
lsh2205 0:e12eb40b9fef 72 if(insert_input != 0xFFFFFFFF)
lsh2205 0:e12eb40b9fef 73 {
lsh2205 0:e12eb40b9fef 74 btn2_input_data[0] = (uint8_t)(Button_Read_Flash(4) / 10);
lsh2205 0:e12eb40b9fef 75 btn2_input_data[1] = (uint8_t)(Button_Read_Flash(4) % 10);
lsh2205 0:e12eb40b9fef 76 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 77 }
lsh2205 0:e12eb40b9fef 78 }
lsh2205 0:e12eb40b9fef 79
lsh2205 0:e12eb40b9fef 80 void Button_Detection()
lsh2205 0:e12eb40b9fef 81 {
lsh2205 0:e12eb40b9fef 82 uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
lsh2205 0:e12eb40b9fef 83 //pc.printf("%x\n",button_data);
lsh2205 0:e12eb40b9fef 84
lsh2205 0:e12eb40b9fef 85 Axis_Button_Detection();
lsh2205 0:e12eb40b9fef 86 Segement_Button_Detection();
lsh2205 0:e12eb40b9fef 87 //if(Front_Board_Detection()) Segement_Button_Detection();
lsh2205 0:e12eb40b9fef 88 }
lsh2205 0:e12eb40b9fef 89
lsh2205 0:e12eb40b9fef 90
lsh2205 0:e12eb40b9fef 91 void Axis_Button_Detection()
lsh2205 0:e12eb40b9fef 92 {
lsh2205 0:e12eb40b9fef 93 uint8_t button_dataA = MCP_Read(MCP_BTN1_CS, MCP_GPIOA);
lsh2205 0:e12eb40b9fef 94 uint8_t button_dataB = MCP_Read(MCP_BTN1_CS, MCP_GPIOB);
lsh2205 0:e12eb40b9fef 95
lsh2205 0:e12eb40b9fef 96 //pc.printf("%x,%x\n",button_dataA,button_dataB);
lsh2205 0:e12eb40b9fef 97
lsh2205 0:e12eb40b9fef 98 axis_button=(((uint16_t)button_dataB & 0x000f )<<8 )+button_dataA;
lsh2205 0:e12eb40b9fef 99
lsh2205 0:e12eb40b9fef 100
lsh2205 0:e12eb40b9fef 101
lsh2205 0:e12eb40b9fef 102 if( (axis_button & 0b000000000001) == 0) //bit1 > AXIS6(-)
lsh2205 0:e12eb40b9fef 103 {
lsh2205 0:e12eb40b9fef 104 button_offset_posion[5]--;
lsh2205 0:e12eb40b9fef 105 menual_mode=true;
lsh2205 0:e12eb40b9fef 106 }
lsh2205 0:e12eb40b9fef 107 else if( (axis_button & 0b000000000010) == 0) //bit2 > AXIS5(-)
lsh2205 0:e12eb40b9fef 108 {
lsh2205 0:e12eb40b9fef 109 button_offset_posion[4]--;
lsh2205 0:e12eb40b9fef 110 menual_mode=true;
lsh2205 0:e12eb40b9fef 111 }
lsh2205 0:e12eb40b9fef 112 else if( (axis_button & 0b000000000100) == 0) //bit3 > AXIS4(-)
lsh2205 0:e12eb40b9fef 113 {
lsh2205 0:e12eb40b9fef 114 button_offset_posion[3]--;
lsh2205 0:e12eb40b9fef 115 menual_mode=true;
lsh2205 0:e12eb40b9fef 116 }
lsh2205 0:e12eb40b9fef 117 else if( (axis_button & 0b000000001000) == 0) //bit4 > AXIS3(-)
lsh2205 0:e12eb40b9fef 118 {
lsh2205 0:e12eb40b9fef 119 button_offset_posion[2]--;
lsh2205 0:e12eb40b9fef 120 menual_mode=true;
lsh2205 0:e12eb40b9fef 121 }
lsh2205 0:e12eb40b9fef 122 else if( (axis_button & 0b000000010000) == 0) //bit5 > AXIS5(+)
lsh2205 0:e12eb40b9fef 123 {
lsh2205 0:e12eb40b9fef 124 button_offset_posion[4]++;
lsh2205 0:e12eb40b9fef 125 menual_mode=true;
lsh2205 0:e12eb40b9fef 126 }
lsh2205 0:e12eb40b9fef 127 else if( (axis_button & 0b000000100000) == 0) //bit6 > AXIS6(+)
lsh2205 0:e12eb40b9fef 128 {
lsh2205 0:e12eb40b9fef 129 button_offset_posion[5]++;
lsh2205 0:e12eb40b9fef 130 menual_mode=true;
lsh2205 0:e12eb40b9fef 131 }
lsh2205 0:e12eb40b9fef 132 else if( (axis_button & 0b000001000000) == 0) //bit7 > AXIS2(-)
lsh2205 0:e12eb40b9fef 133 {
lsh2205 0:e12eb40b9fef 134 button_offset_posion[1]--;
lsh2205 0:e12eb40b9fef 135 menual_mode=true;
lsh2205 0:e12eb40b9fef 136 }
lsh2205 0:e12eb40b9fef 137 else if( (axis_button & 0b000010000000) == 0) //bit8 > AXIS1(-)
lsh2205 0:e12eb40b9fef 138 {
lsh2205 0:e12eb40b9fef 139 button_offset_posion[0]--;
lsh2205 0:e12eb40b9fef 140 menual_mode=true;
lsh2205 0:e12eb40b9fef 141 }
lsh2205 0:e12eb40b9fef 142 else if( (axis_button & 0b000100000000) == 0) //bit9 > AXIS1(+)
lsh2205 0:e12eb40b9fef 143 {
lsh2205 0:e12eb40b9fef 144 button_offset_posion[0]++;
lsh2205 0:e12eb40b9fef 145 menual_mode=true;
lsh2205 0:e12eb40b9fef 146 }
lsh2205 0:e12eb40b9fef 147 else if( (axis_button & 0b001000000000) == 0) //bit10 > AXIS2(+)
lsh2205 0:e12eb40b9fef 148 {
lsh2205 0:e12eb40b9fef 149 button_offset_posion[1]++;
lsh2205 0:e12eb40b9fef 150 menual_mode=true;
lsh2205 0:e12eb40b9fef 151 }
lsh2205 0:e12eb40b9fef 152 else if( (axis_button & 0b010000000000) == 0) //bit11 > AXIS3(+)
lsh2205 0:e12eb40b9fef 153 {
lsh2205 0:e12eb40b9fef 154 button_offset_posion[2]++;
lsh2205 0:e12eb40b9fef 155 menual_mode=true;
lsh2205 0:e12eb40b9fef 156 }
lsh2205 0:e12eb40b9fef 157 else if( (axis_button & 0b100000000000) == 0) //bit12 > AXIS4(+)
lsh2205 0:e12eb40b9fef 158 {
lsh2205 0:e12eb40b9fef 159 button_offset_posion[3]++;
lsh2205 0:e12eb40b9fef 160 menual_mode=true;
lsh2205 0:e12eb40b9fef 161
lsh2205 0:e12eb40b9fef 162 }
lsh2205 0:e12eb40b9fef 163 else
lsh2205 0:e12eb40b9fef 164 {
lsh2205 0:e12eb40b9fef 165 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 166 button_offset_posion[i]=0;
lsh2205 0:e12eb40b9fef 167 }
lsh2205 0:e12eb40b9fef 168
lsh2205 0:e12eb40b9fef 169
lsh2205 0:e12eb40b9fef 170
lsh2205 0:e12eb40b9fef 171 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 172 {
lsh2205 0:e12eb40b9fef 173 if(button_offset_posion[i]<-200)
lsh2205 0:e12eb40b9fef 174 button_offset_posion[i]=-200;
lsh2205 0:e12eb40b9fef 175
lsh2205 0:e12eb40b9fef 176 if(button_offset_posion[i]>200)
lsh2205 0:e12eb40b9fef 177 button_offset_posion[i]=200;
lsh2205 0:e12eb40b9fef 178 }
lsh2205 0:e12eb40b9fef 179
lsh2205 0:e12eb40b9fef 180 /*
lsh2205 0:e12eb40b9fef 181 for(int i=0; i<6;i++)
lsh2205 0:e12eb40b9fef 182 pc.printf("%d,",button_offset_posion[i]);
lsh2205 0:e12eb40b9fef 183 pc.printf("\n");
lsh2205 0:e12eb40b9fef 184 */
lsh2205 0:e12eb40b9fef 185
lsh2205 0:e12eb40b9fef 186
lsh2205 0:e12eb40b9fef 187 uint8_t idx = 0;
lsh2205 0:e12eb40b9fef 188 uint8_t dataA_shift[6] = {7, 6, 3, 2, 1, 0};
lsh2205 0:e12eb40b9fef 189
lsh2205 0:e12eb40b9fef 190 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 191 {
lsh2205 0:e12eb40b9fef 192 if(!(button_dataA & (1 << dataA_shift[i]))) // 1~6Axis up
lsh2205 0:e12eb40b9fef 193 {
lsh2205 0:e12eb40b9fef 194 btn1_push_check[idx]++;
lsh2205 0:e12eb40b9fef 195 }
lsh2205 0:e12eb40b9fef 196 else if(!(button_dataB & (1 << i)) && i < 4) // 1~4Axis down
lsh2205 0:e12eb40b9fef 197 {
lsh2205 0:e12eb40b9fef 198 btn1_push_check[idx + 1]++;
lsh2205 0:e12eb40b9fef 199 }
lsh2205 0:e12eb40b9fef 200 else if(!(button_dataA & (1 << i)) && i > 3) // 5~6Axis down
lsh2205 0:e12eb40b9fef 201 {
lsh2205 0:e12eb40b9fef 202 btn1_push_check[idx + 1]++;
lsh2205 0:e12eb40b9fef 203 }
lsh2205 0:e12eb40b9fef 204 else
lsh2205 0:e12eb40b9fef 205 {
lsh2205 0:e12eb40b9fef 206 btn1_push_check[idx] = 0;
lsh2205 0:e12eb40b9fef 207 btn1_push_check[idx + 1] = 0;
lsh2205 0:e12eb40b9fef 208 }
lsh2205 0:e12eb40b9fef 209 idx += 2;
lsh2205 0:e12eb40b9fef 210 }
lsh2205 0:e12eb40b9fef 211
lsh2205 0:e12eb40b9fef 212 for(int i = 0; i < 12; i++)
lsh2205 0:e12eb40b9fef 213 {
lsh2205 0:e12eb40b9fef 214 if(btn1_push_check[i] > BUTTON_DELAY)
lsh2205 0:e12eb40b9fef 215 {
lsh2205 0:e12eb40b9fef 216 // Axis offset ++ or --
lsh2205 0:e12eb40b9fef 217 btn1_push_check[i] = 0;
lsh2205 0:e12eb40b9fef 218 }
lsh2205 0:e12eb40b9fef 219 }
lsh2205 0:e12eb40b9fef 220 }
lsh2205 0:e12eb40b9fef 221
lsh2205 0:e12eb40b9fef 222 void Segement_Button_Detection()
lsh2205 0:e12eb40b9fef 223 {
lsh2205 0:e12eb40b9fef 224
lsh2205 0:e12eb40b9fef 225 Save_Data_To_Flash();
lsh2205 0:e12eb40b9fef 226
lsh2205 0:e12eb40b9fef 227 uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOA);
lsh2205 0:e12eb40b9fef 228
lsh2205 0:e12eb40b9fef 229 if(!(button_data & (1 << 4))) // push reset button
lsh2205 0:e12eb40b9fef 230 {
lsh2205 0:e12eb40b9fef 231 btn2_reset_button_check++;
lsh2205 0:e12eb40b9fef 232 }
lsh2205 0:e12eb40b9fef 233 else
lsh2205 0:e12eb40b9fef 234 {
lsh2205 0:e12eb40b9fef 235 btn2_reset_button_check = 0;
lsh2205 0:e12eb40b9fef 236 }
lsh2205 0:e12eb40b9fef 237
lsh2205 0:e12eb40b9fef 238 if(btn2_reset_button_check > BUTTON_DELAY)
lsh2205 0:e12eb40b9fef 239 {
lsh2205 0:e12eb40b9fef 240 NVIC_SystemReset();
lsh2205 0:e12eb40b9fef 241 }
lsh2205 0:e12eb40b9fef 242
lsh2205 0:e12eb40b9fef 243 for(int i = 0; i < 3; i += 2)
lsh2205 0:e12eb40b9fef 244 {
lsh2205 0:e12eb40b9fef 245 if(!(button_data & (1 << i))) // Pgain down
lsh2205 0:e12eb40b9fef 246 {
lsh2205 0:e12eb40b9fef 247 btn2_push_check[i + 1]++;
lsh2205 0:e12eb40b9fef 248 }
lsh2205 0:e12eb40b9fef 249 else if(!(button_data & (1 << i + 1))) // Pgain up
lsh2205 0:e12eb40b9fef 250 {
lsh2205 0:e12eb40b9fef 251 btn2_push_check[i]++;
lsh2205 0:e12eb40b9fef 252 }
lsh2205 0:e12eb40b9fef 253 else
lsh2205 0:e12eb40b9fef 254 {
lsh2205 0:e12eb40b9fef 255 btn2_push_check[i] = 0;
lsh2205 0:e12eb40b9fef 256 btn2_push_check[i + 1] = 0;
lsh2205 0:e12eb40b9fef 257 if(i == 0) btn2_fast_pgain = 0;
lsh2205 0:e12eb40b9fef 258 if(i == 2) btn2_fast_input = 0;
lsh2205 0:e12eb40b9fef 259 }
lsh2205 0:e12eb40b9fef 260 }
lsh2205 0:e12eb40b9fef 261
lsh2205 0:e12eb40b9fef 262 if(btn2_push_check[1] > (BUTTON_DELAY - btn2_fast_pgain))
lsh2205 0:e12eb40b9fef 263 {
lsh2205 0:e12eb40b9fef 264 //Pgain down
lsh2205 0:e12eb40b9fef 265
lsh2205 0:e12eb40b9fef 266 if(!(btn2_pgain_data[0] <= 0 && btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0))
lsh2205 0:e12eb40b9fef 267 {
lsh2205 0:e12eb40b9fef 268 if(btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0)
lsh2205 0:e12eb40b9fef 269 {
lsh2205 0:e12eb40b9fef 270 btn2_pgain_data[0]--;
lsh2205 0:e12eb40b9fef 271 btn2_pgain_data[1] = 9;
lsh2205 0:e12eb40b9fef 272 btn2_pgain_data[2] = 9;
lsh2205 0:e12eb40b9fef 273 }
lsh2205 0:e12eb40b9fef 274 else if(btn2_pgain_data[2] <= 0)
lsh2205 0:e12eb40b9fef 275 {
lsh2205 0:e12eb40b9fef 276 btn2_pgain_data[1]--;
lsh2205 0:e12eb40b9fef 277 btn2_pgain_data[2] = 9;
lsh2205 0:e12eb40b9fef 278 }
lsh2205 0:e12eb40b9fef 279 else
lsh2205 0:e12eb40b9fef 280 {
lsh2205 0:e12eb40b9fef 281 btn2_pgain_data[2]--;
lsh2205 0:e12eb40b9fef 282 }
lsh2205 0:e12eb40b9fef 283 }
lsh2205 0:e12eb40b9fef 284 btn2_push_check[1] = 0;
lsh2205 0:e12eb40b9fef 285 insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
lsh2205 0:e12eb40b9fef 286 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 287 {
lsh2205 0:e12eb40b9fef 288 set_data[i * 10 + 2] = insert_pgain;
lsh2205 0:e12eb40b9fef 289 position_Pgain[i-1] = insert_pgain;
lsh2205 0:e12eb40b9fef 290 }
lsh2205 0:e12eb40b9fef 291 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 292 btn2_fast_pgain++;
lsh2205 0:e12eb40b9fef 293 if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
lsh2205 0:e12eb40b9fef 294 flash_save_flag = true;
lsh2205 0:e12eb40b9fef 295 flash_save_cnt = 0;
lsh2205 0:e12eb40b9fef 296 button_pid_info();
lsh2205 0:e12eb40b9fef 297 }
lsh2205 0:e12eb40b9fef 298 else if(btn2_push_check[0] > (BUTTON_DELAY - btn2_fast_pgain))
lsh2205 0:e12eb40b9fef 299 {
lsh2205 0:e12eb40b9fef 300 //Pgain up
lsh2205 0:e12eb40b9fef 301
lsh2205 0:e12eb40b9fef 302 if(!(btn2_pgain_data[0] >= 9 && btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9))
lsh2205 0:e12eb40b9fef 303 {
lsh2205 0:e12eb40b9fef 304 if(btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9)
lsh2205 0:e12eb40b9fef 305 {
lsh2205 0:e12eb40b9fef 306 btn2_pgain_data[0]++;
lsh2205 0:e12eb40b9fef 307 btn2_pgain_data[1] = 0;
lsh2205 0:e12eb40b9fef 308 btn2_pgain_data[2] = 0;
lsh2205 0:e12eb40b9fef 309 }
lsh2205 0:e12eb40b9fef 310 else if(btn2_pgain_data[2] >= 9)
lsh2205 0:e12eb40b9fef 311 {
lsh2205 0:e12eb40b9fef 312 btn2_pgain_data[1]++;
lsh2205 0:e12eb40b9fef 313 btn2_pgain_data[2] = 0;
lsh2205 0:e12eb40b9fef 314 }
lsh2205 0:e12eb40b9fef 315 else
lsh2205 0:e12eb40b9fef 316 {
lsh2205 0:e12eb40b9fef 317 btn2_pgain_data[2]++;
lsh2205 0:e12eb40b9fef 318 }
lsh2205 0:e12eb40b9fef 319 }
lsh2205 0:e12eb40b9fef 320 btn2_push_check[0] = 0;
lsh2205 0:e12eb40b9fef 321 insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
lsh2205 0:e12eb40b9fef 322 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 323 {
lsh2205 0:e12eb40b9fef 324 set_data[i * 10 + 2] = insert_pgain;
lsh2205 0:e12eb40b9fef 325 position_Pgain[i-1] = insert_pgain;
lsh2205 0:e12eb40b9fef 326 }
lsh2205 0:e12eb40b9fef 327 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 328 btn2_fast_pgain++;
lsh2205 0:e12eb40b9fef 329 if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
lsh2205 0:e12eb40b9fef 330 flash_save_flag = true;
lsh2205 0:e12eb40b9fef 331 flash_save_cnt = 0;
lsh2205 0:e12eb40b9fef 332 button_pid_info();
lsh2205 0:e12eb40b9fef 333 }
lsh2205 0:e12eb40b9fef 334
lsh2205 0:e12eb40b9fef 335 if(btn2_push_check[3] > (BUTTON_DELAY - btn2_fast_input))
lsh2205 0:e12eb40b9fef 336 {
lsh2205 0:e12eb40b9fef 337 //Input down
lsh2205 0:e12eb40b9fef 338
lsh2205 0:e12eb40b9fef 339 if(!(btn2_input_data[0] <= 0 && btn2_input_data[1] <= 0))
lsh2205 0:e12eb40b9fef 340 {
lsh2205 0:e12eb40b9fef 341 if(btn2_input_data[1] <= 0)
lsh2205 0:e12eb40b9fef 342 {
lsh2205 0:e12eb40b9fef 343 btn2_input_data[0]--;
lsh2205 0:e12eb40b9fef 344 btn2_input_data[1] = 9;
lsh2205 0:e12eb40b9fef 345 }
lsh2205 0:e12eb40b9fef 346 else
lsh2205 0:e12eb40b9fef 347 {
lsh2205 0:e12eb40b9fef 348 btn2_input_data[1]--;
lsh2205 0:e12eb40b9fef 349 }
lsh2205 0:e12eb40b9fef 350 }
lsh2205 0:e12eb40b9fef 351 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 352 btn2_push_check[3] = 0;
lsh2205 0:e12eb40b9fef 353 insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
lsh2205 0:e12eb40b9fef 354
lsh2205 0:e12eb40b9fef 355 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 356 {
lsh2205 0:e12eb40b9fef 357 set_data[i * 10 + 3] = insert_input;
lsh2205 0:e12eb40b9fef 358 Position_input_filter[i-1] = insert_input;
lsh2205 0:e12eb40b9fef 359 }
lsh2205 0:e12eb40b9fef 360 btn2_fast_input++;
lsh2205 0:e12eb40b9fef 361 if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
lsh2205 0:e12eb40b9fef 362 flash_save_flag = true;
lsh2205 0:e12eb40b9fef 363 flash_save_cnt = 0;
lsh2205 0:e12eb40b9fef 364 button_pid_info();
lsh2205 0:e12eb40b9fef 365 }
lsh2205 0:e12eb40b9fef 366 else if(btn2_push_check[2] > (BUTTON_DELAY - btn2_fast_input))
lsh2205 0:e12eb40b9fef 367 {
lsh2205 0:e12eb40b9fef 368 //Input up
lsh2205 0:e12eb40b9fef 369
lsh2205 0:e12eb40b9fef 370 if(!(btn2_input_data[0] >= 9 && btn2_input_data[1] >= 9))
lsh2205 0:e12eb40b9fef 371 {
lsh2205 0:e12eb40b9fef 372 if(btn2_input_data[1] >= 9)
lsh2205 0:e12eb40b9fef 373 {
lsh2205 0:e12eb40b9fef 374 btn2_input_data[0]++;
lsh2205 0:e12eb40b9fef 375 btn2_input_data[1] = 0;
lsh2205 0:e12eb40b9fef 376 }
lsh2205 0:e12eb40b9fef 377 else
lsh2205 0:e12eb40b9fef 378 {
lsh2205 0:e12eb40b9fef 379 btn2_input_data[1]++;
lsh2205 0:e12eb40b9fef 380 }
lsh2205 0:e12eb40b9fef 381 }
lsh2205 0:e12eb40b9fef 382 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 383 btn2_push_check[2] = 0;
lsh2205 0:e12eb40b9fef 384 insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
lsh2205 0:e12eb40b9fef 385 for(int i = 1; i < 7; i++)
lsh2205 0:e12eb40b9fef 386 {
lsh2205 0:e12eb40b9fef 387 set_data[i * 10 + 3] = insert_input;
lsh2205 0:e12eb40b9fef 388 Position_input_filter[i-1] = insert_input;
lsh2205 0:e12eb40b9fef 389 }
lsh2205 0:e12eb40b9fef 390 btn2_fast_input++;
lsh2205 0:e12eb40b9fef 391 if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
lsh2205 0:e12eb40b9fef 392 flash_save_flag = true;
lsh2205 0:e12eb40b9fef 393 flash_save_cnt = 0;
lsh2205 0:e12eb40b9fef 394
lsh2205 0:e12eb40b9fef 395 button_pid_info();
lsh2205 0:e12eb40b9fef 396 }
lsh2205 0:e12eb40b9fef 397 }
lsh2205 0:e12eb40b9fef 398
lsh2205 0:e12eb40b9fef 399 uint16_t front_board_cnt = 0;
lsh2205 0:e12eb40b9fef 400 bool front_board_state = false;
lsh2205 0:e12eb40b9fef 401 bool Front_Board_Detection()
lsh2205 0:e12eb40b9fef 402 {
lsh2205 0:e12eb40b9fef 403 uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
lsh2205 0:e12eb40b9fef 404 //pc.printf("%x\n",button_data);
lsh2205 0:e12eb40b9fef 405 if(button_data & (1 << 7)) // front board detection
lsh2205 0:e12eb40b9fef 406 {
lsh2205 0:e12eb40b9fef 407 front_board_cnt++;
lsh2205 0:e12eb40b9fef 408 if(front_board_cnt > 3333)
lsh2205 0:e12eb40b9fef 409 {
lsh2205 0:e12eb40b9fef 410 if(front_board_state == false)
lsh2205 0:e12eb40b9fef 411 {
lsh2205 0:e12eb40b9fef 412 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 413 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
lsh2205 0:e12eb40b9fef 414 front_board_state = true;
lsh2205 0:e12eb40b9fef 415 }
lsh2205 0:e12eb40b9fef 416 front_board_cnt = 3333;
lsh2205 0:e12eb40b9fef 417 return true;
lsh2205 0:e12eb40b9fef 418 }
lsh2205 0:e12eb40b9fef 419 }
lsh2205 0:e12eb40b9fef 420 else
lsh2205 0:e12eb40b9fef 421 {
lsh2205 0:e12eb40b9fef 422 front_board_cnt = 0;
lsh2205 0:e12eb40b9fef 423 front_board_state = false;
lsh2205 0:e12eb40b9fef 424 }
lsh2205 0:e12eb40b9fef 425
lsh2205 0:e12eb40b9fef 426 return false;
lsh2205 0:e12eb40b9fef 427 }
lsh2205 0:e12eb40b9fef 428
lsh2205 0:e12eb40b9fef 429 void Save_Data_To_Flash()
lsh2205 0:e12eb40b9fef 430 {
lsh2205 0:e12eb40b9fef 431 if(flash_save_flag == true)
lsh2205 0:e12eb40b9fef 432 {
lsh2205 0:e12eb40b9fef 433 flash_save_cnt++;
lsh2205 0:e12eb40b9fef 434
lsh2205 0:e12eb40b9fef 435 if(flash_save_cnt > FLASH_SAVE_DELAY)
lsh2205 0:e12eb40b9fef 436 {
lsh2205 0:e12eb40b9fef 437 //printf("pgain : %.2f, input : %.2f\r\n", insert_pgain, insert_input);
lsh2205 0:e12eb40b9fef 438 uint32_t save_pgain = Convert_Uint32(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2]);
lsh2205 0:e12eb40b9fef 439 uint32_t save_input = Convert_Uint32(0, btn2_input_data[0], btn2_input_data[1]);
lsh2205 0:e12eb40b9fef 440
lsh2205 0:e12eb40b9fef 441 //printf("TRY Write pgain : %ld, input : %ld\r\n", save_pgain, save_input);
lsh2205 0:e12eb40b9fef 442
lsh2205 0:e12eb40b9fef 443 if(Button_Read_Flash(0) != save_pgain || Button_Read_Flash(4) != save_input)
lsh2205 0:e12eb40b9fef 444 {
lsh2205 0:e12eb40b9fef 445 Button_Erase_Flash();
lsh2205 0:e12eb40b9fef 446 Button_Write_Flash(save_pgain);
lsh2205 0:e12eb40b9fef 447 Button_Write_Flash(save_input);
lsh2205 0:e12eb40b9fef 448 }
lsh2205 0:e12eb40b9fef 449
lsh2205 0:e12eb40b9fef 450 printf("Flash Save Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
lsh2205 0:e12eb40b9fef 451
lsh2205 0:e12eb40b9fef 452 flash_save_flag = false;
lsh2205 0:e12eb40b9fef 453 }
lsh2205 0:e12eb40b9fef 454 }
lsh2205 0:e12eb40b9fef 455 }
lsh2205 0:e12eb40b9fef 456
lsh2205 0:e12eb40b9fef 457 uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3)
lsh2205 0:e12eb40b9fef 458 {
lsh2205 0:e12eb40b9fef 459 uint32_t convert_data = 0;
lsh2205 0:e12eb40b9fef 460
lsh2205 0:e12eb40b9fef 461 convert_data = uint32_t(btn_data1 * 100 + btn_data2 * 10 + btn_data3);
lsh2205 0:e12eb40b9fef 462
lsh2205 0:e12eb40b9fef 463 return convert_data;
lsh2205 0:e12eb40b9fef 464 }
lsh2205 0:e12eb40b9fef 465
lsh2205 0:e12eb40b9fef 466 double Convert_double_Pgain(uint32_t uint32_data)
lsh2205 0:e12eb40b9fef 467 {
lsh2205 0:e12eb40b9fef 468 double convert_data = 0;
lsh2205 0:e12eb40b9fef 469
lsh2205 0:e12eb40b9fef 470 convert_data = (double)uint32_data / (double)10.0;
lsh2205 0:e12eb40b9fef 471
lsh2205 0:e12eb40b9fef 472 return convert_data;
lsh2205 0:e12eb40b9fef 473 }
lsh2205 0:e12eb40b9fef 474
lsh2205 0:e12eb40b9fef 475 double Convert_double_Igain(uint32_t uint32_data)
lsh2205 0:e12eb40b9fef 476 {
lsh2205 0:e12eb40b9fef 477 double convert_data = 0;
lsh2205 0:e12eb40b9fef 478
lsh2205 0:e12eb40b9fef 479 convert_data = (double)uint32_data / (double)1000.0;
lsh2205 0:e12eb40b9fef 480
lsh2205 0:e12eb40b9fef 481 return convert_data;
lsh2205 0:e12eb40b9fef 482 }
lsh2205 0:e12eb40b9fef 483
lsh2205 0:e12eb40b9fef 484 // Page 126
lsh2205 0:e12eb40b9fef 485 #define BUTTON_FLASH_START_ADDRESS ((uint32_t)0x0801F800)
lsh2205 0:e12eb40b9fef 486 #define BUTTON_FLASH_END_ADDRESS ((uint32_t)0x0801FBFF)
lsh2205 0:e12eb40b9fef 487 uint32_t button_flash_address_count = 0;
lsh2205 0:e12eb40b9fef 488
lsh2205 0:e12eb40b9fef 489 void Button_Erase_Flash()
lsh2205 0:e12eb40b9fef 490 {
lsh2205 0:e12eb40b9fef 491 static FLASH_EraseInitTypeDef button_flash_struct;
lsh2205 0:e12eb40b9fef 492 uint32_t PageError = 0;
lsh2205 0:e12eb40b9fef 493
lsh2205 0:e12eb40b9fef 494 button_flash_struct.TypeErase = FLASH_TYPEERASE_PAGES;
lsh2205 0:e12eb40b9fef 495 button_flash_struct.PageAddress = BUTTON_FLASH_START_ADDRESS;
lsh2205 0:e12eb40b9fef 496 button_flash_struct.NbPages = 1;
lsh2205 0:e12eb40b9fef 497
lsh2205 0:e12eb40b9fef 498 HAL_FLASH_Unlock();
lsh2205 0:e12eb40b9fef 499 if(HAL_FLASHEx_Erase(&button_flash_struct, &PageError) != HAL_OK)
lsh2205 0:e12eb40b9fef 500 {
lsh2205 0:e12eb40b9fef 501 //pc.printf("Button Flash erase error!\n\r");
lsh2205 0:e12eb40b9fef 502 }
lsh2205 0:e12eb40b9fef 503 HAL_FLASH_Lock();
lsh2205 0:e12eb40b9fef 504 button_flash_address_count = 0;
lsh2205 0:e12eb40b9fef 505 }
lsh2205 0:e12eb40b9fef 506
lsh2205 0:e12eb40b9fef 507 uint32_t Button_Read_Flash(uint32_t address)
lsh2205 0:e12eb40b9fef 508 {
lsh2205 0:e12eb40b9fef 509 uint32_t val = 0;
lsh2205 0:e12eb40b9fef 510 address = address + BUTTON_FLASH_START_ADDRESS;
lsh2205 0:e12eb40b9fef 511 val = *(__IO uint32_t*)address;
lsh2205 0:e12eb40b9fef 512
lsh2205 0:e12eb40b9fef 513 return val;
lsh2205 0:e12eb40b9fef 514 }
lsh2205 0:e12eb40b9fef 515
lsh2205 0:e12eb40b9fef 516 void Button_Write_Flash(uint32_t data)
lsh2205 0:e12eb40b9fef 517 {
lsh2205 0:e12eb40b9fef 518 if(data != Button_Read_Flash(button_flash_address_count * 4))
lsh2205 0:e12eb40b9fef 519 {
lsh2205 0:e12eb40b9fef 520 HAL_FLASH_Unlock();
lsh2205 0:e12eb40b9fef 521 HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, BUTTON_FLASH_START_ADDRESS + (button_flash_address_count * 4), data);
lsh2205 0:e12eb40b9fef 522 HAL_FLASH_Lock();
lsh2205 0:e12eb40b9fef 523 }
lsh2205 0:e12eb40b9fef 524 button_flash_address_count++;
lsh2205 0:e12eb40b9fef 525 }
lsh2205 0:e12eb40b9fef 526
lsh2205 0:e12eb40b9fef 527 void button_pid_info()
lsh2205 0:e12eb40b9fef 528 {
lsh2205 0:e12eb40b9fef 529 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
lsh2205 0:e12eb40b9fef 530 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 531 {
lsh2205 0:e12eb40b9fef 532 pc.printf("%2d. %6.3f %6.3f %6.3f %6.3f %6.3f %4d\r\n", i + 1, Speed_Pgain[i], Speed_Igain[i], position_Pgain[i], Position_input_filter[i], Speed_I_input_filter[i], (int)offset[i]);
lsh2205 0:e12eb40b9fef 533 }
lsh2205 0:e12eb40b9fef 534 pc.printf("\n\n\n");
lsh2205 0:e12eb40b9fef 535
lsh2205 0:e12eb40b9fef 536 }
lsh2205 0:e12eb40b9fef 537 void reset_speed_pid()
lsh2205 0:e12eb40b9fef 538 {
lsh2205 0:e12eb40b9fef 539 for(int i=0; i<6; i++)
lsh2205 0:e12eb40b9fef 540 {
lsh2205 0:e12eb40b9fef 541 taget_speed[i]=0;
lsh2205 0:e12eb40b9fef 542 error_speed[i]=0;
lsh2205 0:e12eb40b9fef 543 Speed_PID_OUTPUT[i]=0;
lsh2205 0:e12eb40b9fef 544 result_i[i]=0;
lsh2205 0:e12eb40b9fef 545 filter_result_i[i]=0;
lsh2205 0:e12eb40b9fef 546 output_i[i]=0;
lsh2205 0:e12eb40b9fef 547 output_p[i]=0;
lsh2205 0:e12eb40b9fef 548 }
lsh2205 0:e12eb40b9fef 549 }
lsh2205 0:e12eb40b9fef 550
lsh2205 0:e12eb40b9fef 551 #endif