lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

button.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?

UserRevisionLine numberNew contents of line
gohgwaja 0:7cff999a7f5c 1 #ifndef _BUTTON_H_
gohgwaja 0:7cff999a7f5c 2 #define _BUTTON_H_
gohgwaja 0:7cff999a7f5c 3
gohgwaja 0:7cff999a7f5c 4 #include "MCP23S17.h"
gohgwaja 0:7cff999a7f5c 5 #include "7_segment.h"
gohgwaja 0:7cff999a7f5c 6
gohgwaja 1:7b5469bf5994 7 #define BUTTON_DELAY 60 // 75ms
gohgwaja 1:7b5469bf5994 8 #define BUTTON_FAST_LIMIT 50
gohgwaja 1:7b5469bf5994 9 #define FLASH_SAVE_DELAY 3333 // 3333, 5 sec
gohgwaja 1:7b5469bf5994 10 #define PGAIN_CORRETION_FACTOR 0.4
gohgwaja 1:7b5469bf5994 11
gohgwaja 0:7cff999a7f5c 12 void reset_speed_pid();
gohgwaja 0:7cff999a7f5c 13 void button_pid_info();
gohgwaja 0:7cff999a7f5c 14
gohgwaja 0:7cff999a7f5c 15 bool menual_mode=false;
gohgwaja 0:7cff999a7f5c 16 bool set_mode = false;
gohgwaja 1:7b5469bf5994 17 bool set_cmd_togle = false;
gohgwaja 0:7cff999a7f5c 18 uint8_t set_command = 0;
gohgwaja 1:7b5469bf5994 19 uint16_t set_cmd_delay = 200;
gohgwaja 0:7cff999a7f5c 20 uint16_t axis_button=0x0fff;
gohgwaja 0:7cff999a7f5c 21 uint32_t axis_cmd_print_delay = 0;
gohgwaja 0:7cff999a7f5c 22 uint8_t btn1_push_check[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
gohgwaja 0:7cff999a7f5c 23
gohgwaja 0:7cff999a7f5c 24 uint8_t btn2_push_check[4] = {0, 0, 0, 0};
gohgwaja 0:7cff999a7f5c 25 uint8_t btn2_reset_button_check = 0;
gohgwaja 0:7cff999a7f5c 26 uint8_t btn2_pgain_data[3] = {0, 0, 0};
gohgwaja 0:7cff999a7f5c 27 uint8_t btn2_input_data[2] = {0, 0};
gohgwaja 0:7cff999a7f5c 28 uint8_t btn2_fast_pgain = 0;
gohgwaja 0:7cff999a7f5c 29 uint8_t btn2_fast_input = 0;
gohgwaja 0:7cff999a7f5c 30
gohgwaja 0:7cff999a7f5c 31 bool flash_save_flag = false;
gohgwaja 0:7cff999a7f5c 32 uint32_t flash_save_cnt = 0;
gohgwaja 0:7cff999a7f5c 33
gohgwaja 0:7cff999a7f5c 34 double insert_pgain = 0;
gohgwaja 0:7cff999a7f5c 35 double insert_input = 0;
gohgwaja 0:7cff999a7f5c 36
gohgwaja 0:7cff999a7f5c 37 void Button_Detection();
gohgwaja 0:7cff999a7f5c 38 void Axis_Button_Detection();
gohgwaja 0:7cff999a7f5c 39 void Segement_Button_Detection();
gohgwaja 0:7cff999a7f5c 40 void Axis_Button_Command_Detection();
gohgwaja 0:7cff999a7f5c 41 bool Front_Board_Detection();
gohgwaja 0:7cff999a7f5c 42 void Save_Data_To_Flash();
gohgwaja 0:7cff999a7f5c 43 uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3);
gohgwaja 0:7cff999a7f5c 44 double Convert_double_Pgain(uint32_t data);
gohgwaja 0:7cff999a7f5c 45 double Convert_double_Igain(uint32_t data);
gohgwaja 0:7cff999a7f5c 46 void Button_Erase_Flash();
gohgwaja 0:7cff999a7f5c 47 uint32_t Button_Read_Flash(uint32_t address);
gohgwaja 0:7cff999a7f5c 48 void Button_Write_Flash(uint32_t data);
gohgwaja 0:7cff999a7f5c 49
gohgwaja 0:7cff999a7f5c 50
gohgwaja 0:7cff999a7f5c 51 void Button_Init()
gohgwaja 0:7cff999a7f5c 52 {
gohgwaja 1:7b5469bf5994 53 if (Button_Read_Flash(0) != 0xFFFFFFFF)
gohgwaja 0:7cff999a7f5c 54 {
gohgwaja 1:7b5469bf5994 55 insert_pgain = Convert_double_Pgain(Button_Read_Flash(0));
gohgwaja 1:7b5469bf5994 56 insert_pgain *= PGAIN_CORRETION_FACTOR;
gohgwaja 1:7b5469bf5994 57
gohgwaja 1:7b5469bf5994 58 for (int i = 1; i < 7; i++)
gohgwaja 1:7b5469bf5994 59 {
gohgwaja 1:7b5469bf5994 60 set_data[i * 10 + 2] = insert_pgain;
gohgwaja 1:7b5469bf5994 61 position_Pgain[i - 1] = insert_pgain;
gohgwaja 1:7b5469bf5994 62 }
gohgwaja 0:7cff999a7f5c 63 btn2_pgain_data[0] = (uint8_t)(Button_Read_Flash(0) / 100);
gohgwaja 0:7cff999a7f5c 64 btn2_pgain_data[1] = (uint8_t)((Button_Read_Flash(0) % 100) / 10);
gohgwaja 1:7b5469bf5994 65 btn2_pgain_data[2] = (uint8_t)(Button_Read_Flash(0) % 10);
gohgwaja 0:7cff999a7f5c 66 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 67 }
gohgwaja 0:7cff999a7f5c 68 else
gohgwaja 0:7cff999a7f5c 69 {
gohgwaja 1:7b5469bf5994 70 insert_pgain = 25.0 * PGAIN_CORRETION_FACTOR;
gohgwaja 1:7b5469bf5994 71 for (int i = 1; i < 7; i++)
gohgwaja 1:7b5469bf5994 72 {
gohgwaja 1:7b5469bf5994 73 set_data[i * 10 + 2] = insert_pgain;
gohgwaja 1:7b5469bf5994 74 position_Pgain[i - 1] = insert_pgain;
gohgwaja 1:7b5469bf5994 75 }
gohgwaja 1:7b5469bf5994 76 btn2_pgain_data[0] = (uint8_t)2;
gohgwaja 1:7b5469bf5994 77 btn2_pgain_data[1] = (uint8_t)5;
gohgwaja 1:7b5469bf5994 78 btn2_pgain_data[2] = (uint8_t)0;
gohgwaja 1:7b5469bf5994 79 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
gohgwaja 1:7b5469bf5994 80 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 81 }
gohgwaja 1:7b5469bf5994 82
gohgwaja 1:7b5469bf5994 83
gohgwaja 1:7b5469bf5994 84 if (Button_Read_Flash(4) != 0xFFFFFFFF)
gohgwaja 0:7cff999a7f5c 85 {
gohgwaja 1:7b5469bf5994 86 insert_input = Convert_double_Igain(Button_Read_Flash(4));
gohgwaja 1:7b5469bf5994 87
gohgwaja 1:7b5469bf5994 88 for (int i = 1; i < 7; i++)
gohgwaja 1:7b5469bf5994 89 {
gohgwaja 1:7b5469bf5994 90 set_data[i * 10 + 3] = insert_input;
gohgwaja 1:7b5469bf5994 91 Position_input_filter[i - 1] = insert_input;
gohgwaja 1:7b5469bf5994 92 }
gohgwaja 0:7cff999a7f5c 93 btn2_input_data[0] = (uint8_t)(Button_Read_Flash(4) / 10);
gohgwaja 0:7cff999a7f5c 94 btn2_input_data[1] = (uint8_t)(Button_Read_Flash(4) % 10);
gohgwaja 0:7cff999a7f5c 95 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 96 }
gohgwaja 1:7b5469bf5994 97 else
gohgwaja 0:7cff999a7f5c 98 {
gohgwaja 1:7b5469bf5994 99 insert_input = 0.05;
gohgwaja 1:7b5469bf5994 100
gohgwaja 1:7b5469bf5994 101 for (int i = 1; i < 7; i++)
gohgwaja 1:7b5469bf5994 102 {
gohgwaja 1:7b5469bf5994 103 set_data[i * 10 + 3] = insert_input;
gohgwaja 1:7b5469bf5994 104 Position_input_filter[i - 1] = insert_input;
gohgwaja 1:7b5469bf5994 105 }
gohgwaja 1:7b5469bf5994 106 btn2_input_data[0] = (uint8_t)5;
gohgwaja 1:7b5469bf5994 107 btn2_input_data[1] = (uint8_t)0;
gohgwaja 0:7cff999a7f5c 108 Input_segment(5, 0, SEG_DELAY_TIME);
gohgwaja 1:7b5469bf5994 109 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 110 }
gohgwaja 0:7cff999a7f5c 111 }
gohgwaja 0:7cff999a7f5c 112
gohgwaja 0:7cff999a7f5c 113 void Button_Detection()
gohgwaja 0:7cff999a7f5c 114 {
gohgwaja 0:7cff999a7f5c 115 // uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
gohgwaja 0:7cff999a7f5c 116 //pc.printf("%x\n",button_data);
gohgwaja 0:7cff999a7f5c 117 Axis_Button_Command_Detection();
gohgwaja 0:7cff999a7f5c 118 // Axis_Button_Detection();
gohgwaja 0:7cff999a7f5c 119 Segement_Button_Detection();
gohgwaja 0:7cff999a7f5c 120 //if(Front_Board_Detection()) Segement_Button_Detection();
gohgwaja 0:7cff999a7f5c 121 }
gohgwaja 0:7cff999a7f5c 122
gohgwaja 0:7cff999a7f5c 123
gohgwaja 0:7cff999a7f5c 124 void Axis_Button_Detection()
gohgwaja 0:7cff999a7f5c 125 {
gohgwaja 0:7cff999a7f5c 126 uint8_t button_dataA = MCP_Read(MCP_BTN1_CS, MCP_GPIOA);
gohgwaja 0:7cff999a7f5c 127 uint8_t button_dataB = MCP_Read(MCP_BTN1_CS, MCP_GPIOB);
gohgwaja 0:7cff999a7f5c 128
gohgwaja 0:7cff999a7f5c 129 //pc.printf("%x,%x\n",button_dataA,button_dataB);
gohgwaja 0:7cff999a7f5c 130
gohgwaja 0:7cff999a7f5c 131 axis_button=(((uint16_t)button_dataB & 0x000f )<<8 )+button_dataA;
gohgwaja 0:7cff999a7f5c 132
gohgwaja 0:7cff999a7f5c 133
gohgwaja 0:7cff999a7f5c 134
gohgwaja 0:7cff999a7f5c 135 if( (axis_button & 0b000000000001) == 0) //bit1 > AXIS6(-)
gohgwaja 0:7cff999a7f5c 136 {
gohgwaja 0:7cff999a7f5c 137 button_offset_posion[5]--;
gohgwaja 0:7cff999a7f5c 138 menual_mode=true;
gohgwaja 0:7cff999a7f5c 139 }
gohgwaja 0:7cff999a7f5c 140 else if( (axis_button & 0b000000000010) == 0) //bit2 > AXIS5(-)
gohgwaja 0:7cff999a7f5c 141 {
gohgwaja 0:7cff999a7f5c 142 button_offset_posion[4]--;
gohgwaja 0:7cff999a7f5c 143 menual_mode=true;
gohgwaja 0:7cff999a7f5c 144 }
gohgwaja 0:7cff999a7f5c 145 else if( (axis_button & 0b000000000100) == 0) //bit3 > AXIS4(-)
gohgwaja 0:7cff999a7f5c 146 {
gohgwaja 0:7cff999a7f5c 147 button_offset_posion[3]--;
gohgwaja 0:7cff999a7f5c 148 menual_mode=true;
gohgwaja 0:7cff999a7f5c 149 }
gohgwaja 0:7cff999a7f5c 150 else if( (axis_button & 0b000000001000) == 0) //bit4 > AXIS3(-)
gohgwaja 0:7cff999a7f5c 151 {
gohgwaja 0:7cff999a7f5c 152 button_offset_posion[2]--;
gohgwaja 0:7cff999a7f5c 153 menual_mode=true;
gohgwaja 0:7cff999a7f5c 154 }
gohgwaja 0:7cff999a7f5c 155 else if( (axis_button & 0b000000010000) == 0) //bit5 > AXIS5(+)
gohgwaja 0:7cff999a7f5c 156 {
gohgwaja 0:7cff999a7f5c 157 button_offset_posion[4]++;
gohgwaja 0:7cff999a7f5c 158 menual_mode=true;
gohgwaja 0:7cff999a7f5c 159 }
gohgwaja 0:7cff999a7f5c 160 else if( (axis_button & 0b000000100000) == 0) //bit6 > AXIS6(+)
gohgwaja 0:7cff999a7f5c 161 {
gohgwaja 0:7cff999a7f5c 162 button_offset_posion[5]++;
gohgwaja 0:7cff999a7f5c 163 menual_mode=true;
gohgwaja 0:7cff999a7f5c 164 }
gohgwaja 0:7cff999a7f5c 165 else if( (axis_button & 0b000001000000) == 0) //bit7 > AXIS2(-)
gohgwaja 0:7cff999a7f5c 166 {
gohgwaja 0:7cff999a7f5c 167 button_offset_posion[1]--;
gohgwaja 0:7cff999a7f5c 168 menual_mode=true;
gohgwaja 0:7cff999a7f5c 169 }
gohgwaja 0:7cff999a7f5c 170 else if( (axis_button & 0b000010000000) == 0) //bit8 > AXIS1(-)
gohgwaja 0:7cff999a7f5c 171 {
gohgwaja 0:7cff999a7f5c 172 button_offset_posion[0]--;
gohgwaja 0:7cff999a7f5c 173 menual_mode=true;
gohgwaja 0:7cff999a7f5c 174 }
gohgwaja 0:7cff999a7f5c 175 else if( (axis_button & 0b000100000000) == 0) //bit9 > AXIS1(+)
gohgwaja 0:7cff999a7f5c 176 {
gohgwaja 0:7cff999a7f5c 177 button_offset_posion[0]++;
gohgwaja 0:7cff999a7f5c 178 menual_mode=true;
gohgwaja 0:7cff999a7f5c 179 }
gohgwaja 0:7cff999a7f5c 180 else if( (axis_button & 0b001000000000) == 0) //bit10 > AXIS2(+)
gohgwaja 0:7cff999a7f5c 181 {
gohgwaja 0:7cff999a7f5c 182 button_offset_posion[1]++;
gohgwaja 0:7cff999a7f5c 183 menual_mode=true;
gohgwaja 0:7cff999a7f5c 184 }
gohgwaja 0:7cff999a7f5c 185 else if( (axis_button & 0b010000000000) == 0) //bit11 > AXIS3(+)
gohgwaja 0:7cff999a7f5c 186 {
gohgwaja 0:7cff999a7f5c 187 button_offset_posion[2]++;
gohgwaja 0:7cff999a7f5c 188 menual_mode=true;
gohgwaja 0:7cff999a7f5c 189 }
gohgwaja 0:7cff999a7f5c 190 else if( (axis_button & 0b100000000000) == 0) //bit12 > AXIS4(+)
gohgwaja 0:7cff999a7f5c 191 {
gohgwaja 0:7cff999a7f5c 192 button_offset_posion[3]++;
gohgwaja 0:7cff999a7f5c 193 menual_mode=true;
gohgwaja 0:7cff999a7f5c 194
gohgwaja 0:7cff999a7f5c 195 }
gohgwaja 0:7cff999a7f5c 196 else
gohgwaja 0:7cff999a7f5c 197 {
gohgwaja 0:7cff999a7f5c 198 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 199 button_offset_posion[i]=0;
gohgwaja 0:7cff999a7f5c 200 }
gohgwaja 0:7cff999a7f5c 201
gohgwaja 0:7cff999a7f5c 202
gohgwaja 0:7cff999a7f5c 203
gohgwaja 0:7cff999a7f5c 204 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 205 {
gohgwaja 0:7cff999a7f5c 206 if(button_offset_posion[i]<-200)
gohgwaja 0:7cff999a7f5c 207 button_offset_posion[i]=-200;
gohgwaja 0:7cff999a7f5c 208
gohgwaja 0:7cff999a7f5c 209 if(button_offset_posion[i]>200)
gohgwaja 0:7cff999a7f5c 210 button_offset_posion[i]=200;
gohgwaja 0:7cff999a7f5c 211 }
gohgwaja 0:7cff999a7f5c 212
gohgwaja 0:7cff999a7f5c 213 /*
gohgwaja 0:7cff999a7f5c 214 for(int i=0; i<6;i++)
gohgwaja 0:7cff999a7f5c 215 pc.printf("%d,",button_offset_posion[i]);
gohgwaja 0:7cff999a7f5c 216 pc.printf("\n");
gohgwaja 0:7cff999a7f5c 217 */
gohgwaja 0:7cff999a7f5c 218
gohgwaja 0:7cff999a7f5c 219
gohgwaja 0:7cff999a7f5c 220 uint8_t idx = 0;
gohgwaja 0:7cff999a7f5c 221 uint8_t dataA_shift[6] = {7, 6, 3, 2, 1, 0};
gohgwaja 0:7cff999a7f5c 222
gohgwaja 0:7cff999a7f5c 223 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 224 {
gohgwaja 0:7cff999a7f5c 225 if(!(button_dataA & (1 << dataA_shift[i]))) // 1~6Axis up
gohgwaja 0:7cff999a7f5c 226 {
gohgwaja 0:7cff999a7f5c 227 btn1_push_check[idx]++;
gohgwaja 0:7cff999a7f5c 228 }
gohgwaja 0:7cff999a7f5c 229 else if(!(button_dataB & (1 << i)) && i < 4) // 1~4Axis down
gohgwaja 0:7cff999a7f5c 230 {
gohgwaja 0:7cff999a7f5c 231 btn1_push_check[idx + 1]++;
gohgwaja 0:7cff999a7f5c 232 }
gohgwaja 0:7cff999a7f5c 233 else if(!(button_dataA & (1 << i)) && i > 3) // 5~6Axis down
gohgwaja 0:7cff999a7f5c 234 {
gohgwaja 0:7cff999a7f5c 235 btn1_push_check[idx + 1]++;
gohgwaja 0:7cff999a7f5c 236 }
gohgwaja 0:7cff999a7f5c 237 else
gohgwaja 0:7cff999a7f5c 238 {
gohgwaja 0:7cff999a7f5c 239 btn1_push_check[idx] = 0;
gohgwaja 0:7cff999a7f5c 240 btn1_push_check[idx + 1] = 0;
gohgwaja 0:7cff999a7f5c 241 }
gohgwaja 0:7cff999a7f5c 242 idx += 2;
gohgwaja 0:7cff999a7f5c 243 }
gohgwaja 0:7cff999a7f5c 244
gohgwaja 0:7cff999a7f5c 245 for(int i = 0; i < 12; i++)
gohgwaja 0:7cff999a7f5c 246 {
gohgwaja 0:7cff999a7f5c 247 if(btn1_push_check[i] > BUTTON_DELAY)
gohgwaja 0:7cff999a7f5c 248 {
gohgwaja 0:7cff999a7f5c 249 // Axis offset ++ or --
gohgwaja 0:7cff999a7f5c 250 btn1_push_check[i] = 0;
gohgwaja 0:7cff999a7f5c 251 }
gohgwaja 0:7cff999a7f5c 252 }
gohgwaja 0:7cff999a7f5c 253 }
gohgwaja 0:7cff999a7f5c 254
gohgwaja 0:7cff999a7f5c 255 void Segement_Button_Detection()
gohgwaja 0:7cff999a7f5c 256 {
gohgwaja 0:7cff999a7f5c 257
gohgwaja 0:7cff999a7f5c 258 Save_Data_To_Flash();
gohgwaja 0:7cff999a7f5c 259
gohgwaja 0:7cff999a7f5c 260 uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOA);
gohgwaja 0:7cff999a7f5c 261
gohgwaja 0:7cff999a7f5c 262 if(!(button_data & (1 << 4))) // push reset button
gohgwaja 0:7cff999a7f5c 263 {
gohgwaja 0:7cff999a7f5c 264 btn2_reset_button_check++;
gohgwaja 0:7cff999a7f5c 265 }
gohgwaja 0:7cff999a7f5c 266 else
gohgwaja 0:7cff999a7f5c 267 {
gohgwaja 0:7cff999a7f5c 268 btn2_reset_button_check = 0;
gohgwaja 0:7cff999a7f5c 269 }
gohgwaja 0:7cff999a7f5c 270
gohgwaja 0:7cff999a7f5c 271 if(btn2_reset_button_check > BUTTON_DELAY)
gohgwaja 0:7cff999a7f5c 272 {
gohgwaja 0:7cff999a7f5c 273 NVIC_SystemReset();
gohgwaja 0:7cff999a7f5c 274 }
gohgwaja 0:7cff999a7f5c 275
gohgwaja 0:7cff999a7f5c 276 for(int i = 0; i < 3; i += 2)
gohgwaja 0:7cff999a7f5c 277 {
gohgwaja 0:7cff999a7f5c 278 if(!(button_data & (1 << i))) // Pgain down
gohgwaja 0:7cff999a7f5c 279 {
gohgwaja 0:7cff999a7f5c 280 btn2_push_check[i + 1]++;
gohgwaja 0:7cff999a7f5c 281 }
gohgwaja 0:7cff999a7f5c 282 else if(!(button_data & (1 << i + 1))) // Pgain up
gohgwaja 0:7cff999a7f5c 283 {
gohgwaja 0:7cff999a7f5c 284 btn2_push_check[i]++;
gohgwaja 0:7cff999a7f5c 285 }
gohgwaja 0:7cff999a7f5c 286 else
gohgwaja 0:7cff999a7f5c 287 {
gohgwaja 0:7cff999a7f5c 288 btn2_push_check[i] = 0;
gohgwaja 0:7cff999a7f5c 289 btn2_push_check[i + 1] = 0;
gohgwaja 0:7cff999a7f5c 290 if(i == 0) btn2_fast_pgain = 0;
gohgwaja 0:7cff999a7f5c 291 if(i == 2) btn2_fast_input = 0;
gohgwaja 0:7cff999a7f5c 292 }
gohgwaja 0:7cff999a7f5c 293 }
gohgwaja 0:7cff999a7f5c 294
gohgwaja 0:7cff999a7f5c 295 if(btn2_push_check[1] > (BUTTON_DELAY - btn2_fast_pgain))
gohgwaja 0:7cff999a7f5c 296 {
gohgwaja 0:7cff999a7f5c 297 //Pgain down
gohgwaja 0:7cff999a7f5c 298
gohgwaja 0:7cff999a7f5c 299 if(!(btn2_pgain_data[0] <= 0 && btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0))
gohgwaja 0:7cff999a7f5c 300 {
gohgwaja 0:7cff999a7f5c 301 if(btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0)
gohgwaja 0:7cff999a7f5c 302 {
gohgwaja 0:7cff999a7f5c 303 btn2_pgain_data[0]--;
gohgwaja 0:7cff999a7f5c 304 btn2_pgain_data[1] = 9;
gohgwaja 0:7cff999a7f5c 305 btn2_pgain_data[2] = 9;
gohgwaja 0:7cff999a7f5c 306 }
gohgwaja 0:7cff999a7f5c 307 else if(btn2_pgain_data[2] <= 0)
gohgwaja 0:7cff999a7f5c 308 {
gohgwaja 0:7cff999a7f5c 309 btn2_pgain_data[1]--;
gohgwaja 0:7cff999a7f5c 310 btn2_pgain_data[2] = 9;
gohgwaja 0:7cff999a7f5c 311 }
gohgwaja 0:7cff999a7f5c 312 else
gohgwaja 0:7cff999a7f5c 313 {
gohgwaja 0:7cff999a7f5c 314 btn2_pgain_data[2]--;
gohgwaja 0:7cff999a7f5c 315 }
gohgwaja 0:7cff999a7f5c 316 }
gohgwaja 0:7cff999a7f5c 317 btn2_push_check[1] = 0;
gohgwaja 0:7cff999a7f5c 318 insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
gohgwaja 1:7b5469bf5994 319 insert_pgain *= PGAIN_CORRETION_FACTOR;
gohgwaja 0:7cff999a7f5c 320 for(int i = 1; i < 7; i++)
gohgwaja 0:7cff999a7f5c 321 {
gohgwaja 0:7cff999a7f5c 322 set_data[i * 10 + 2] = insert_pgain;
gohgwaja 0:7cff999a7f5c 323 position_Pgain[i-1] = insert_pgain;
gohgwaja 0:7cff999a7f5c 324 }
gohgwaja 0:7cff999a7f5c 325 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 326 btn2_fast_pgain++;
gohgwaja 0:7cff999a7f5c 327 if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
gohgwaja 0:7cff999a7f5c 328 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 329 flash_save_cnt = 0;
gohgwaja 0:7cff999a7f5c 330 button_pid_info();
gohgwaja 0:7cff999a7f5c 331 }
gohgwaja 0:7cff999a7f5c 332 else if(btn2_push_check[0] > (BUTTON_DELAY - btn2_fast_pgain))
gohgwaja 0:7cff999a7f5c 333 {
gohgwaja 0:7cff999a7f5c 334 //Pgain up
gohgwaja 0:7cff999a7f5c 335
gohgwaja 0:7cff999a7f5c 336 if(!(btn2_pgain_data[0] >= 9 && btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9))
gohgwaja 0:7cff999a7f5c 337 {
gohgwaja 0:7cff999a7f5c 338 if(btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9)
gohgwaja 0:7cff999a7f5c 339 {
gohgwaja 0:7cff999a7f5c 340 btn2_pgain_data[0]++;
gohgwaja 0:7cff999a7f5c 341 btn2_pgain_data[1] = 0;
gohgwaja 0:7cff999a7f5c 342 btn2_pgain_data[2] = 0;
gohgwaja 0:7cff999a7f5c 343 }
gohgwaja 0:7cff999a7f5c 344 else if(btn2_pgain_data[2] >= 9)
gohgwaja 0:7cff999a7f5c 345 {
gohgwaja 0:7cff999a7f5c 346 btn2_pgain_data[1]++;
gohgwaja 0:7cff999a7f5c 347 btn2_pgain_data[2] = 0;
gohgwaja 0:7cff999a7f5c 348 }
gohgwaja 0:7cff999a7f5c 349 else
gohgwaja 0:7cff999a7f5c 350 {
gohgwaja 0:7cff999a7f5c 351 btn2_pgain_data[2]++;
gohgwaja 0:7cff999a7f5c 352 }
gohgwaja 0:7cff999a7f5c 353 }
gohgwaja 0:7cff999a7f5c 354 btn2_push_check[0] = 0;
gohgwaja 0:7cff999a7f5c 355 insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
gohgwaja 1:7b5469bf5994 356 insert_pgain *= PGAIN_CORRETION_FACTOR;
gohgwaja 0:7cff999a7f5c 357 for(int i = 1; i < 7; i++)
gohgwaja 0:7cff999a7f5c 358 {
gohgwaja 0:7cff999a7f5c 359 set_data[i * 10 + 2] = insert_pgain;
gohgwaja 0:7cff999a7f5c 360 position_Pgain[i-1] = insert_pgain;
gohgwaja 0:7cff999a7f5c 361 }
gohgwaja 0:7cff999a7f5c 362 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 363 btn2_fast_pgain++;
gohgwaja 0:7cff999a7f5c 364 if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
gohgwaja 0:7cff999a7f5c 365 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 366 flash_save_cnt = 0;
gohgwaja 0:7cff999a7f5c 367 button_pid_info();
gohgwaja 0:7cff999a7f5c 368 }
gohgwaja 0:7cff999a7f5c 369
gohgwaja 0:7cff999a7f5c 370 if(btn2_push_check[3] > (BUTTON_DELAY - btn2_fast_input))
gohgwaja 0:7cff999a7f5c 371 {
gohgwaja 0:7cff999a7f5c 372 //Input down
gohgwaja 0:7cff999a7f5c 373
gohgwaja 0:7cff999a7f5c 374 if(!(btn2_input_data[0] <= 0 && btn2_input_data[1] <= 0))
gohgwaja 0:7cff999a7f5c 375 {
gohgwaja 0:7cff999a7f5c 376 if(btn2_input_data[1] <= 0)
gohgwaja 0:7cff999a7f5c 377 {
gohgwaja 0:7cff999a7f5c 378 btn2_input_data[0]--;
gohgwaja 0:7cff999a7f5c 379 btn2_input_data[1] = 9;
gohgwaja 0:7cff999a7f5c 380 }
gohgwaja 0:7cff999a7f5c 381 else
gohgwaja 0:7cff999a7f5c 382 {
gohgwaja 0:7cff999a7f5c 383 btn2_input_data[1]--;
gohgwaja 0:7cff999a7f5c 384 }
gohgwaja 0:7cff999a7f5c 385 }
gohgwaja 0:7cff999a7f5c 386 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 387 btn2_push_check[3] = 0;
gohgwaja 0:7cff999a7f5c 388 insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
gohgwaja 0:7cff999a7f5c 389
gohgwaja 0:7cff999a7f5c 390 for(int i = 1; i < 7; i++)
gohgwaja 0:7cff999a7f5c 391 {
gohgwaja 0:7cff999a7f5c 392 set_data[i * 10 + 3] = insert_input;
gohgwaja 0:7cff999a7f5c 393 Position_input_filter[i-1] = insert_input;
gohgwaja 0:7cff999a7f5c 394 }
gohgwaja 0:7cff999a7f5c 395 btn2_fast_input++;
gohgwaja 0:7cff999a7f5c 396 if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
gohgwaja 0:7cff999a7f5c 397 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 398 flash_save_cnt = 0;
gohgwaja 0:7cff999a7f5c 399 button_pid_info();
gohgwaja 0:7cff999a7f5c 400 }
gohgwaja 0:7cff999a7f5c 401 else if(btn2_push_check[2] > (BUTTON_DELAY - btn2_fast_input))
gohgwaja 0:7cff999a7f5c 402 {
gohgwaja 0:7cff999a7f5c 403 //Input up
gohgwaja 0:7cff999a7f5c 404
gohgwaja 0:7cff999a7f5c 405 if(!(btn2_input_data[0] >= 9 && btn2_input_data[1] >= 9))
gohgwaja 0:7cff999a7f5c 406 {
gohgwaja 0:7cff999a7f5c 407 if(btn2_input_data[1] >= 9)
gohgwaja 0:7cff999a7f5c 408 {
gohgwaja 0:7cff999a7f5c 409 btn2_input_data[0]++;
gohgwaja 0:7cff999a7f5c 410 btn2_input_data[1] = 0;
gohgwaja 0:7cff999a7f5c 411 }
gohgwaja 0:7cff999a7f5c 412 else
gohgwaja 0:7cff999a7f5c 413 {
gohgwaja 0:7cff999a7f5c 414 btn2_input_data[1]++;
gohgwaja 0:7cff999a7f5c 415 }
gohgwaja 0:7cff999a7f5c 416 }
gohgwaja 0:7cff999a7f5c 417 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 418 btn2_push_check[2] = 0;
gohgwaja 0:7cff999a7f5c 419 insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
gohgwaja 0:7cff999a7f5c 420 for(int i = 1; i < 7; i++)
gohgwaja 0:7cff999a7f5c 421 {
gohgwaja 0:7cff999a7f5c 422 set_data[i * 10 + 3] = insert_input;
gohgwaja 0:7cff999a7f5c 423 Position_input_filter[i-1] = insert_input;
gohgwaja 0:7cff999a7f5c 424 }
gohgwaja 0:7cff999a7f5c 425 btn2_fast_input++;
gohgwaja 0:7cff999a7f5c 426 if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
gohgwaja 0:7cff999a7f5c 427 flash_save_flag = true;
gohgwaja 0:7cff999a7f5c 428 flash_save_cnt = 0;
gohgwaja 0:7cff999a7f5c 429
gohgwaja 0:7cff999a7f5c 430 button_pid_info();
gohgwaja 0:7cff999a7f5c 431 }
gohgwaja 0:7cff999a7f5c 432 }
gohgwaja 0:7cff999a7f5c 433
gohgwaja 0:7cff999a7f5c 434 void Axis_Button_Command_Detection()
gohgwaja 0:7cff999a7f5c 435 {
gohgwaja 0:7cff999a7f5c 436 uint8_t button_dataA = MCP_Read(MCP_BTN1_CS, MCP_GPIOA);
gohgwaja 0:7cff999a7f5c 437 uint8_t button_dataB = MCP_Read(MCP_BTN1_CS, MCP_GPIOB);
gohgwaja 1:7b5469bf5994 438
gohgwaja 0:7cff999a7f5c 439 axis_button=(((uint16_t)button_dataB & 0x000f )<<8 )+button_dataA;
gohgwaja 1:7b5469bf5994 440
gohgwaja 0:7cff999a7f5c 441 if(!set_mode)
gohgwaja 1:7b5469bf5994 442 {
gohgwaja 1:7b5469bf5994 443 set_cmd_delay++;
gohgwaja 1:7b5469bf5994 444 if(set_cmd_delay > 200) set_cmd_delay = 200;
gohgwaja 1:7b5469bf5994 445
gohgwaja 1:7b5469bf5994 446 if((axis_button & 0b000000100000) == 0) //bit6 > AXIS1(+)
gohgwaja 0:7cff999a7f5c 447 {
gohgwaja 1:7b5469bf5994 448 if(set_cmd_delay > 100)
gohgwaja 1:7b5469bf5994 449 {
gohgwaja 1:7b5469bf5994 450 set_cmd_togle = true;
gohgwaja 1:7b5469bf5994 451 set_cmd_delay = 0;
gohgwaja 1:7b5469bf5994 452 }
gohgwaja 0:7cff999a7f5c 453 }
gohgwaja 1:7b5469bf5994 454 else if((set_cmd_togle == true) && (axis_button == 0b111111111111))
gohgwaja 0:7cff999a7f5c 455 {
gohgwaja 1:7b5469bf5994 456 set_cmd_togle = false;
gohgwaja 1:7b5469bf5994 457 set_command++;
gohgwaja 1:7b5469bf5994 458
gohgwaja 1:7b5469bf5994 459 if(set_command >= 5)
gohgwaja 1:7b5469bf5994 460 {
gohgwaja 1:7b5469bf5994 461 set_mode = true;
gohgwaja 1:7b5469bf5994 462 }
gohgwaja 0:7cff999a7f5c 463 }
gohgwaja 1:7b5469bf5994 464 else if(axis_button == 0b111111111111)
gohgwaja 0:7cff999a7f5c 465 {
gohgwaja 0:7cff999a7f5c 466 }
gohgwaja 0:7cff999a7f5c 467 else
gohgwaja 0:7cff999a7f5c 468 {
gohgwaja 0:7cff999a7f5c 469 set_command = 0;
gohgwaja 0:7cff999a7f5c 470 }
gohgwaja 0:7cff999a7f5c 471 }
gohgwaja 0:7cff999a7f5c 472 else
gohgwaja 0:7cff999a7f5c 473 {
gohgwaja 0:7cff999a7f5c 474 if( (axis_button & 0b000001000000) == 0) //bit1 > AXIS6(-)
gohgwaja 0:7cff999a7f5c 475 {
gohgwaja 1:7b5469bf5994 476 button_offset_posion[5]--;
gohgwaja 0:7cff999a7f5c 477 menual_mode=true;
gohgwaja 0:7cff999a7f5c 478 }
gohgwaja 0:7cff999a7f5c 479 else if( (axis_button & 0b000010000000) == 0) //bit2 > AXIS5(-)
gohgwaja 0:7cff999a7f5c 480 {
gohgwaja 1:7b5469bf5994 481 button_offset_posion[4]--;
gohgwaja 0:7cff999a7f5c 482 menual_mode=true;
gohgwaja 0:7cff999a7f5c 483 }
gohgwaja 0:7cff999a7f5c 484 else if( (axis_button & 0b000100000000) == 0) //bit3 > AXIS4(-)
gohgwaja 0:7cff999a7f5c 485 {
gohgwaja 1:7b5469bf5994 486 button_offset_posion[3]--;
gohgwaja 0:7cff999a7f5c 487 menual_mode=true;
gohgwaja 0:7cff999a7f5c 488 }
gohgwaja 0:7cff999a7f5c 489 else if( (axis_button & 0b001000000000) == 0) //bit4 > AXIS3(-)
gohgwaja 0:7cff999a7f5c 490 {
gohgwaja 1:7b5469bf5994 491 button_offset_posion[2]--;
gohgwaja 0:7cff999a7f5c 492 menual_mode=true;
gohgwaja 0:7cff999a7f5c 493 }
gohgwaja 0:7cff999a7f5c 494 else if( (axis_button & 0b000000000010) == 0) //bit5 > AXIS5(+)
gohgwaja 0:7cff999a7f5c 495 {
gohgwaja 1:7b5469bf5994 496 button_offset_posion[4]++;
gohgwaja 0:7cff999a7f5c 497 menual_mode=true;
gohgwaja 0:7cff999a7f5c 498 }
gohgwaja 0:7cff999a7f5c 499 else if( (axis_button & 0b000000000001) == 0) //bit6 > AXIS6(+)
gohgwaja 0:7cff999a7f5c 500 {
gohgwaja 1:7b5469bf5994 501 button_offset_posion[5]++;
gohgwaja 0:7cff999a7f5c 502 menual_mode=true;
gohgwaja 0:7cff999a7f5c 503 }
gohgwaja 0:7cff999a7f5c 504 else if( (axis_button & 0b010000000000) == 0) //bit7 > AXIS2(-)
gohgwaja 0:7cff999a7f5c 505 {
gohgwaja 1:7b5469bf5994 506 button_offset_posion[1]--;
gohgwaja 0:7cff999a7f5c 507 menual_mode=true;
gohgwaja 0:7cff999a7f5c 508 }
gohgwaja 0:7cff999a7f5c 509 else if( (axis_button & 0b100000000000) == 0) //bit8 > AXIS1(-)
gohgwaja 0:7cff999a7f5c 510 {
gohgwaja 1:7b5469bf5994 511 button_offset_posion[0]--;
gohgwaja 0:7cff999a7f5c 512 menual_mode=true;
gohgwaja 0:7cff999a7f5c 513 }
gohgwaja 0:7cff999a7f5c 514 else if( (axis_button & 0b000000100000) == 0) //bit9 > AXIS1(+)
gohgwaja 0:7cff999a7f5c 515 {
gohgwaja 1:7b5469bf5994 516 button_offset_posion[0]++;
gohgwaja 0:7cff999a7f5c 517 menual_mode=true;
gohgwaja 0:7cff999a7f5c 518 }
gohgwaja 0:7cff999a7f5c 519 else if( (axis_button & 0b000000010000) == 0) //bit10 > AXIS2(+)
gohgwaja 0:7cff999a7f5c 520 {
gohgwaja 1:7b5469bf5994 521 button_offset_posion[1]++;
gohgwaja 0:7cff999a7f5c 522 menual_mode=true;
gohgwaja 0:7cff999a7f5c 523 }
gohgwaja 0:7cff999a7f5c 524 else if( (axis_button & 0b000000001000) == 0) //bit11 > AXIS3(+)
gohgwaja 0:7cff999a7f5c 525 {
gohgwaja 1:7b5469bf5994 526 button_offset_posion[2]++;
gohgwaja 0:7cff999a7f5c 527 menual_mode=true;
gohgwaja 0:7cff999a7f5c 528 }
gohgwaja 0:7cff999a7f5c 529 else if( (axis_button & 0b000000000100) == 0) //bit12 > AXIS4(+)
gohgwaja 0:7cff999a7f5c 530 {
gohgwaja 1:7b5469bf5994 531 button_offset_posion[3]++;
gohgwaja 0:7cff999a7f5c 532 menual_mode=true;
gohgwaja 0:7cff999a7f5c 533
gohgwaja 0:7cff999a7f5c 534 }
gohgwaja 0:7cff999a7f5c 535 else
gohgwaja 0:7cff999a7f5c 536 {
gohgwaja 0:7cff999a7f5c 537 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 538 button_offset_posion[i]=0;
gohgwaja 0:7cff999a7f5c 539 }
gohgwaja 0:7cff999a7f5c 540
gohgwaja 0:7cff999a7f5c 541
gohgwaja 0:7cff999a7f5c 542
gohgwaja 0:7cff999a7f5c 543 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 544 {
gohgwaja 0:7cff999a7f5c 545 if(button_offset_posion[i]<-200)
gohgwaja 0:7cff999a7f5c 546 button_offset_posion[i]=-200;
gohgwaja 0:7cff999a7f5c 547
gohgwaja 0:7cff999a7f5c 548 if(button_offset_posion[i]>200)
gohgwaja 0:7cff999a7f5c 549 button_offset_posion[i]=200;
gohgwaja 0:7cff999a7f5c 550 }
gohgwaja 0:7cff999a7f5c 551
gohgwaja 0:7cff999a7f5c 552 Menual_Segment();
gohgwaja 0:7cff999a7f5c 553 axis_cmd_print_delay++;
gohgwaja 0:7cff999a7f5c 554 if(axis_cmd_print_delay > 666)
gohgwaja 0:7cff999a7f5c 555 {
gohgwaja 0:7cff999a7f5c 556 printf("Axis menual mode!\r\n");
gohgwaja 0:7cff999a7f5c 557 axis_cmd_print_delay = 0;
gohgwaja 0:7cff999a7f5c 558 }
gohgwaja 0:7cff999a7f5c 559 }
gohgwaja 0:7cff999a7f5c 560 }
gohgwaja 0:7cff999a7f5c 561
gohgwaja 0:7cff999a7f5c 562 uint16_t front_board_cnt = 0;
gohgwaja 0:7cff999a7f5c 563 bool front_board_state = false;
gohgwaja 0:7cff999a7f5c 564 bool Front_Board_Detection()
gohgwaja 0:7cff999a7f5c 565 {
gohgwaja 0:7cff999a7f5c 566 uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
gohgwaja 0:7cff999a7f5c 567 //pc.printf("%x\n",button_data);
gohgwaja 0:7cff999a7f5c 568 if(button_data & (1 << 7)) // front board detection
gohgwaja 0:7cff999a7f5c 569 {
gohgwaja 0:7cff999a7f5c 570 front_board_cnt++;
gohgwaja 0:7cff999a7f5c 571 if(front_board_cnt > 3333)
gohgwaja 0:7cff999a7f5c 572 {
gohgwaja 0:7cff999a7f5c 573 if(front_board_state == false)
gohgwaja 0:7cff999a7f5c 574 {
gohgwaja 0:7cff999a7f5c 575 Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 576 Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
gohgwaja 0:7cff999a7f5c 577 front_board_state = true;
gohgwaja 0:7cff999a7f5c 578 }
gohgwaja 0:7cff999a7f5c 579 front_board_cnt = 3333;
gohgwaja 0:7cff999a7f5c 580 return true;
gohgwaja 0:7cff999a7f5c 581 }
gohgwaja 0:7cff999a7f5c 582 }
gohgwaja 0:7cff999a7f5c 583 else
gohgwaja 0:7cff999a7f5c 584 {
gohgwaja 0:7cff999a7f5c 585 front_board_cnt = 0;
gohgwaja 0:7cff999a7f5c 586 front_board_state = false;
gohgwaja 0:7cff999a7f5c 587 }
gohgwaja 0:7cff999a7f5c 588
gohgwaja 0:7cff999a7f5c 589 return false;
gohgwaja 0:7cff999a7f5c 590 }
gohgwaja 0:7cff999a7f5c 591
gohgwaja 0:7cff999a7f5c 592 void Save_Data_To_Flash()
gohgwaja 0:7cff999a7f5c 593 {
gohgwaja 0:7cff999a7f5c 594 if(flash_save_flag == true)
gohgwaja 0:7cff999a7f5c 595 {
gohgwaja 0:7cff999a7f5c 596 flash_save_cnt++;
gohgwaja 0:7cff999a7f5c 597
gohgwaja 0:7cff999a7f5c 598 if(flash_save_cnt > FLASH_SAVE_DELAY)
gohgwaja 0:7cff999a7f5c 599 {
gohgwaja 0:7cff999a7f5c 600 //printf("pgain : %.2f, input : %.2f\r\n", insert_pgain, insert_input);
gohgwaja 0:7cff999a7f5c 601 uint32_t save_pgain = Convert_Uint32(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2]);
gohgwaja 0:7cff999a7f5c 602 uint32_t save_input = Convert_Uint32(0, btn2_input_data[0], btn2_input_data[1]);
gohgwaja 0:7cff999a7f5c 603
gohgwaja 0:7cff999a7f5c 604 //printf("TRY Write pgain : %ld, input : %ld\r\n", save_pgain, save_input);
gohgwaja 0:7cff999a7f5c 605
gohgwaja 0:7cff999a7f5c 606 if(Button_Read_Flash(0) != save_pgain || Button_Read_Flash(4) != save_input)
gohgwaja 0:7cff999a7f5c 607 {
gohgwaja 0:7cff999a7f5c 608 Button_Erase_Flash();
gohgwaja 0:7cff999a7f5c 609 Button_Write_Flash(save_pgain);
gohgwaja 0:7cff999a7f5c 610 Button_Write_Flash(save_input);
gohgwaja 0:7cff999a7f5c 611 }
gohgwaja 0:7cff999a7f5c 612
gohgwaja 0:7cff999a7f5c 613 printf("Flash Save Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
gohgwaja 0:7cff999a7f5c 614
gohgwaja 0:7cff999a7f5c 615 flash_save_flag = false;
gohgwaja 0:7cff999a7f5c 616 }
gohgwaja 0:7cff999a7f5c 617 }
gohgwaja 0:7cff999a7f5c 618 }
gohgwaja 0:7cff999a7f5c 619
gohgwaja 0:7cff999a7f5c 620 uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3)
gohgwaja 0:7cff999a7f5c 621 {
gohgwaja 0:7cff999a7f5c 622 uint32_t convert_data = 0;
gohgwaja 0:7cff999a7f5c 623
gohgwaja 0:7cff999a7f5c 624 convert_data = uint32_t(btn_data1 * 100 + btn_data2 * 10 + btn_data3);
gohgwaja 0:7cff999a7f5c 625
gohgwaja 0:7cff999a7f5c 626 return convert_data;
gohgwaja 0:7cff999a7f5c 627 }
gohgwaja 0:7cff999a7f5c 628
gohgwaja 0:7cff999a7f5c 629 double Convert_double_Pgain(uint32_t uint32_data)
gohgwaja 0:7cff999a7f5c 630 {
gohgwaja 0:7cff999a7f5c 631 double convert_data = 0;
gohgwaja 0:7cff999a7f5c 632
gohgwaja 0:7cff999a7f5c 633 convert_data = (double)uint32_data / (double)10.0;
gohgwaja 0:7cff999a7f5c 634
gohgwaja 0:7cff999a7f5c 635 return convert_data;
gohgwaja 0:7cff999a7f5c 636 }
gohgwaja 0:7cff999a7f5c 637
gohgwaja 0:7cff999a7f5c 638 double Convert_double_Igain(uint32_t uint32_data)
gohgwaja 0:7cff999a7f5c 639 {
gohgwaja 0:7cff999a7f5c 640 double convert_data = 0;
gohgwaja 0:7cff999a7f5c 641
gohgwaja 0:7cff999a7f5c 642 convert_data = (double)uint32_data / (double)1000.0;
gohgwaja 0:7cff999a7f5c 643
gohgwaja 0:7cff999a7f5c 644 return convert_data;
gohgwaja 0:7cff999a7f5c 645 }
gohgwaja 0:7cff999a7f5c 646
gohgwaja 0:7cff999a7f5c 647 // Page 126
gohgwaja 0:7cff999a7f5c 648 #define BUTTON_FLASH_START_ADDRESS ((uint32_t)0x0801F800)
gohgwaja 0:7cff999a7f5c 649 #define BUTTON_FLASH_END_ADDRESS ((uint32_t)0x0801FBFF)
gohgwaja 0:7cff999a7f5c 650 uint32_t button_flash_address_count = 0;
gohgwaja 0:7cff999a7f5c 651
gohgwaja 0:7cff999a7f5c 652 void Button_Erase_Flash()
gohgwaja 0:7cff999a7f5c 653 {
gohgwaja 0:7cff999a7f5c 654 static FLASH_EraseInitTypeDef button_flash_struct;
gohgwaja 0:7cff999a7f5c 655 uint32_t PageError = 0;
gohgwaja 0:7cff999a7f5c 656
gohgwaja 0:7cff999a7f5c 657 button_flash_struct.TypeErase = FLASH_TYPEERASE_PAGES;
gohgwaja 0:7cff999a7f5c 658 button_flash_struct.PageAddress = BUTTON_FLASH_START_ADDRESS;
gohgwaja 0:7cff999a7f5c 659 button_flash_struct.NbPages = 1;
gohgwaja 0:7cff999a7f5c 660
gohgwaja 0:7cff999a7f5c 661 HAL_FLASH_Unlock();
gohgwaja 0:7cff999a7f5c 662 if(HAL_FLASHEx_Erase(&button_flash_struct, &PageError) != HAL_OK)
gohgwaja 0:7cff999a7f5c 663 {
gohgwaja 0:7cff999a7f5c 664 //pc.printf("Button Flash erase error!\n\r");
gohgwaja 0:7cff999a7f5c 665 }
gohgwaja 0:7cff999a7f5c 666 HAL_FLASH_Lock();
gohgwaja 0:7cff999a7f5c 667 button_flash_address_count = 0;
gohgwaja 0:7cff999a7f5c 668 }
gohgwaja 0:7cff999a7f5c 669
gohgwaja 0:7cff999a7f5c 670 uint32_t Button_Read_Flash(uint32_t address)
gohgwaja 0:7cff999a7f5c 671 {
gohgwaja 0:7cff999a7f5c 672 uint32_t val = 0;
gohgwaja 0:7cff999a7f5c 673 address = address + BUTTON_FLASH_START_ADDRESS;
gohgwaja 0:7cff999a7f5c 674 val = *(__IO uint32_t*)address;
gohgwaja 0:7cff999a7f5c 675
gohgwaja 0:7cff999a7f5c 676 return val;
gohgwaja 0:7cff999a7f5c 677 }
gohgwaja 0:7cff999a7f5c 678
gohgwaja 0:7cff999a7f5c 679 void Button_Write_Flash(uint32_t data)
gohgwaja 0:7cff999a7f5c 680 {
gohgwaja 0:7cff999a7f5c 681 if(data != Button_Read_Flash(button_flash_address_count * 4))
gohgwaja 0:7cff999a7f5c 682 {
gohgwaja 0:7cff999a7f5c 683 HAL_FLASH_Unlock();
gohgwaja 0:7cff999a7f5c 684 HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, BUTTON_FLASH_START_ADDRESS + (button_flash_address_count * 4), data);
gohgwaja 0:7cff999a7f5c 685 HAL_FLASH_Lock();
gohgwaja 0:7cff999a7f5c 686 }
gohgwaja 0:7cff999a7f5c 687 button_flash_address_count++;
gohgwaja 0:7cff999a7f5c 688 }
gohgwaja 0:7cff999a7f5c 689
gohgwaja 0:7cff999a7f5c 690 void button_pid_info()
gohgwaja 0:7cff999a7f5c 691 {
gohgwaja 0:7cff999a7f5c 692 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
gohgwaja 0:7cff999a7f5c 693 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 694 {
gohgwaja 0:7cff999a7f5c 695 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]);
gohgwaja 0:7cff999a7f5c 696 }
gohgwaja 0:7cff999a7f5c 697 pc.printf("\n\n\n");
gohgwaja 0:7cff999a7f5c 698
gohgwaja 0:7cff999a7f5c 699 }
gohgwaja 0:7cff999a7f5c 700 void reset_speed_pid()
gohgwaja 0:7cff999a7f5c 701 {
gohgwaja 0:7cff999a7f5c 702 for(int i=0; i<6; i++)
gohgwaja 0:7cff999a7f5c 703 {
gohgwaja 0:7cff999a7f5c 704 taget_speed[i]=0;
gohgwaja 0:7cff999a7f5c 705 error_speed[i]=0;
gohgwaja 0:7cff999a7f5c 706 Speed_PID_OUTPUT[i]=0;
gohgwaja 0:7cff999a7f5c 707 result_i[i]=0;
gohgwaja 0:7cff999a7f5c 708 filter_result_i[i]=0;
gohgwaja 0:7cff999a7f5c 709 output_i[i]=0;
gohgwaja 0:7cff999a7f5c 710 output_p[i]=0;
gohgwaja 0:7cff999a7f5c 711 }
gohgwaja 0:7cff999a7f5c 712 }
gohgwaja 0:7cff999a7f5c 713
gohgwaja 0:7cff999a7f5c 714 #endif