lee sanghun
test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Diff: button.h
- Revision:
- 0:7cff999a7f5c
- Child:
- 1:7b5469bf5994
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/button.h Mon May 11 08:47:18 2020 +0000
@@ -0,0 +1,713 @@
+#ifndef _BUTTON_H_
+#define _BUTTON_H_
+
+#include "MCP23S17.h"
+#include "7_segment.h"
+
+#define BUTTON_DELAY 50 // 75ms
+#define BUTTON_FAST_LIMIT 45
+#define FLASH_SAVE_DELAY 3000 // 400000, 10min
+void reset_speed_pid();
+void button_pid_info();
+
+bool menual_mode=false;
+bool set_mode = false;
+uint8_t set_command = 0;
+uint16_t axis_button=0x0fff;
+uint32_t axis_cmd_print_delay = 0;
+uint8_t btn1_push_check[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+uint8_t btn2_push_check[4] = {0, 0, 0, 0};
+uint8_t btn2_reset_button_check = 0;
+uint8_t btn2_pgain_data[3] = {0, 0, 0};
+uint8_t btn2_input_data[2] = {0, 0};
+uint8_t btn2_fast_pgain = 0;
+uint8_t btn2_fast_input = 0;
+
+bool flash_save_flag = false;
+uint32_t flash_save_cnt = 0;
+
+double insert_pgain = 0;
+double insert_input = 0;
+
+void Button_Detection();
+void Axis_Button_Detection();
+void Segement_Button_Detection();
+void Axis_Button_Command_Detection();
+bool Front_Board_Detection();
+void Save_Data_To_Flash();
+uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3);
+double Convert_double_Pgain(uint32_t data);
+double Convert_double_Igain(uint32_t data);
+void Button_Erase_Flash();
+uint32_t Button_Read_Flash(uint32_t address);
+void Button_Write_Flash(uint32_t data);
+
+
+void Button_Init()
+{
+ insert_pgain = Convert_double_Pgain(Button_Read_Flash(0));
+ insert_input = Convert_double_Igain(Button_Read_Flash(4));
+
+ //printf("fisrt Gain pgain : %f, input : %f\r\n", insert_pgain, insert_input);
+
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 2] = insert_pgain;
+ position_Pgain[i-1] = insert_pgain;
+ }
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 3] = insert_input;
+ Position_input_filter[i-1] = insert_input;
+ }
+
+ if(insert_pgain != 0xFFFFFFFF)
+ {
+ btn2_pgain_data[0] = (uint8_t)(Button_Read_Flash(0) / 100);
+ btn2_pgain_data[1] = (uint8_t)((Button_Read_Flash(0) % 100) / 10);
+ btn2_pgain_data[2] = (uint8_t)((Button_Read_Flash(0) % 10) / 1);
+ Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
+ }
+ else
+ {
+ Pgain_segment(2, 5, 0, SEG_DELAY_TIME);
+ }
+ if(insert_input != 0xFFFFFFFF)
+ {
+ btn2_input_data[0] = (uint8_t)(Button_Read_Flash(4) / 10);
+ btn2_input_data[1] = (uint8_t)(Button_Read_Flash(4) % 10);
+ Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
+ }
+ {
+ Input_segment(5, 0, SEG_DELAY_TIME);
+ }
+}
+
+void Button_Detection()
+{
+// uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
+ //pc.printf("%x\n",button_data);
+ Axis_Button_Command_Detection();
+// Axis_Button_Detection();
+ Segement_Button_Detection();
+ //if(Front_Board_Detection()) Segement_Button_Detection();
+}
+
+
+void Axis_Button_Detection()
+{
+ uint8_t button_dataA = MCP_Read(MCP_BTN1_CS, MCP_GPIOA);
+ uint8_t button_dataB = MCP_Read(MCP_BTN1_CS, MCP_GPIOB);
+
+ //pc.printf("%x,%x\n",button_dataA,button_dataB);
+
+ axis_button=(((uint16_t)button_dataB & 0x000f )<<8 )+button_dataA;
+
+
+
+ if( (axis_button & 0b000000000001) == 0) //bit1 > AXIS6(-)
+ {
+ button_offset_posion[5]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000000010) == 0) //bit2 > AXIS5(-)
+ {
+ button_offset_posion[4]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000000100) == 0) //bit3 > AXIS4(-)
+ {
+ button_offset_posion[3]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000001000) == 0) //bit4 > AXIS3(-)
+ {
+ button_offset_posion[2]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000010000) == 0) //bit5 > AXIS5(+)
+ {
+ button_offset_posion[4]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000100000) == 0) //bit6 > AXIS6(+)
+ {
+ button_offset_posion[5]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000001000000) == 0) //bit7 > AXIS2(-)
+ {
+ button_offset_posion[1]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000010000000) == 0) //bit8 > AXIS1(-)
+ {
+ button_offset_posion[0]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000100000000) == 0) //bit9 > AXIS1(+)
+ {
+ button_offset_posion[0]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b001000000000) == 0) //bit10 > AXIS2(+)
+ {
+ button_offset_posion[1]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b010000000000) == 0) //bit11 > AXIS3(+)
+ {
+ button_offset_posion[2]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b100000000000) == 0) //bit12 > AXIS4(+)
+ {
+ button_offset_posion[3]++;
+ menual_mode=true;
+
+ }
+ else
+ {
+ for(int i=0;i<6;i++)
+ button_offset_posion[i]=0;
+ }
+
+
+
+ for(int i=0;i<6;i++)
+ {
+ if(button_offset_posion[i]<-200)
+ button_offset_posion[i]=-200;
+
+ if(button_offset_posion[i]>200)
+ button_offset_posion[i]=200;
+ }
+
+/*
+ for(int i=0; i<6;i++)
+ pc.printf("%d,",button_offset_posion[i]);
+ pc.printf("\n");
+ */
+
+
+ uint8_t idx = 0;
+ uint8_t dataA_shift[6] = {7, 6, 3, 2, 1, 0};
+
+ for(int i = 0; i < 6; i++)
+ {
+ if(!(button_dataA & (1 << dataA_shift[i]))) // 1~6Axis up
+ {
+ btn1_push_check[idx]++;
+ }
+ else if(!(button_dataB & (1 << i)) && i < 4) // 1~4Axis down
+ {
+ btn1_push_check[idx + 1]++;
+ }
+ else if(!(button_dataA & (1 << i)) && i > 3) // 5~6Axis down
+ {
+ btn1_push_check[idx + 1]++;
+ }
+ else
+ {
+ btn1_push_check[idx] = 0;
+ btn1_push_check[idx + 1] = 0;
+ }
+ idx += 2;
+ }
+
+ for(int i = 0; i < 12; i++)
+ {
+ if(btn1_push_check[i] > BUTTON_DELAY)
+ {
+ // Axis offset ++ or --
+ btn1_push_check[i] = 0;
+ }
+ }
+}
+
+void Segement_Button_Detection()
+{
+
+ Save_Data_To_Flash();
+
+ uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOA);
+
+ if(!(button_data & (1 << 4))) // push reset button
+ {
+ btn2_reset_button_check++;
+ }
+ else
+ {
+ btn2_reset_button_check = 0;
+ }
+
+ if(btn2_reset_button_check > BUTTON_DELAY)
+ {
+ NVIC_SystemReset();
+ }
+
+ for(int i = 0; i < 3; i += 2)
+ {
+ if(!(button_data & (1 << i))) // Pgain down
+ {
+ btn2_push_check[i + 1]++;
+ }
+ else if(!(button_data & (1 << i + 1))) // Pgain up
+ {
+ btn2_push_check[i]++;
+ }
+ else
+ {
+ btn2_push_check[i] = 0;
+ btn2_push_check[i + 1] = 0;
+ if(i == 0) btn2_fast_pgain = 0;
+ if(i == 2) btn2_fast_input = 0;
+ }
+ }
+
+ if(btn2_push_check[1] > (BUTTON_DELAY - btn2_fast_pgain))
+ {
+ //Pgain down
+
+ if(!(btn2_pgain_data[0] <= 0 && btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0))
+ {
+ if(btn2_pgain_data[1] <= 0 && btn2_pgain_data[2] <= 0)
+ {
+ btn2_pgain_data[0]--;
+ btn2_pgain_data[1] = 9;
+ btn2_pgain_data[2] = 9;
+ }
+ else if(btn2_pgain_data[2] <= 0)
+ {
+ btn2_pgain_data[1]--;
+ btn2_pgain_data[2] = 9;
+ }
+ else
+ {
+ btn2_pgain_data[2]--;
+ }
+ }
+ btn2_push_check[1] = 0;
+ insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 2] = insert_pgain;
+ position_Pgain[i-1] = insert_pgain;
+ }
+ Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
+ btn2_fast_pgain++;
+ if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
+ flash_save_flag = true;
+ flash_save_cnt = 0;
+ button_pid_info();
+ }
+ else if(btn2_push_check[0] > (BUTTON_DELAY - btn2_fast_pgain))
+ {
+ //Pgain up
+
+ if(!(btn2_pgain_data[0] >= 9 && btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9))
+ {
+ if(btn2_pgain_data[1] >= 9 && btn2_pgain_data[2] >= 9)
+ {
+ btn2_pgain_data[0]++;
+ btn2_pgain_data[1] = 0;
+ btn2_pgain_data[2] = 0;
+ }
+ else if(btn2_pgain_data[2] >= 9)
+ {
+ btn2_pgain_data[1]++;
+ btn2_pgain_data[2] = 0;
+ }
+ else
+ {
+ btn2_pgain_data[2]++;
+ }
+ }
+ btn2_push_check[0] = 0;
+ insert_pgain = (double)btn2_pgain_data[0] * 10 + (double)btn2_pgain_data[1] + (double)btn2_pgain_data[2] * 0.1; // inser set_data
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 2] = insert_pgain;
+ position_Pgain[i-1] = insert_pgain;
+ }
+ Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
+ btn2_fast_pgain++;
+ if(btn2_fast_pgain > BUTTON_FAST_LIMIT) btn2_fast_pgain = BUTTON_FAST_LIMIT;
+ flash_save_flag = true;
+ flash_save_cnt = 0;
+ button_pid_info();
+ }
+
+ if(btn2_push_check[3] > (BUTTON_DELAY - btn2_fast_input))
+ {
+ //Input down
+
+ if(!(btn2_input_data[0] <= 0 && btn2_input_data[1] <= 0))
+ {
+ if(btn2_input_data[1] <= 0)
+ {
+ btn2_input_data[0]--;
+ btn2_input_data[1] = 9;
+ }
+ else
+ {
+ btn2_input_data[1]--;
+ }
+ }
+ Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
+ btn2_push_check[3] = 0;
+ insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
+
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 3] = insert_input;
+ Position_input_filter[i-1] = insert_input;
+ }
+ btn2_fast_input++;
+ if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
+ flash_save_flag = true;
+ flash_save_cnt = 0;
+ button_pid_info();
+ }
+ else if(btn2_push_check[2] > (BUTTON_DELAY - btn2_fast_input))
+ {
+ //Input up
+
+ if(!(btn2_input_data[0] >= 9 && btn2_input_data[1] >= 9))
+ {
+ if(btn2_input_data[1] >= 9)
+ {
+ btn2_input_data[0]++;
+ btn2_input_data[1] = 0;
+ }
+ else
+ {
+ btn2_input_data[1]++;
+ }
+ }
+ Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
+ btn2_push_check[2] = 0;
+ insert_input = (double)btn2_input_data[0] * 0.01 + (double)btn2_input_data[1] * 0.001; // inser set_data
+ for(int i = 1; i < 7; i++)
+ {
+ set_data[i * 10 + 3] = insert_input;
+ Position_input_filter[i-1] = insert_input;
+ }
+ btn2_fast_input++;
+ if(btn2_fast_input > BUTTON_FAST_LIMIT) btn2_fast_input = BUTTON_FAST_LIMIT;
+ flash_save_flag = true;
+ flash_save_cnt = 0;
+
+ button_pid_info();
+ }
+}
+
+void Axis_Button_Command_Detection()
+{
+ uint8_t button_dataA = MCP_Read(MCP_BTN1_CS, MCP_GPIOA);
+ uint8_t button_dataB = MCP_Read(MCP_BTN1_CS, MCP_GPIOB);
+
+ axis_button=(((uint16_t)button_dataB & 0x000f )<<8 )+button_dataA;
+ if(!set_mode)
+ {
+ if(((set_command == 0) || (set_command == 1))&& ((axis_button & 0b000000100000) == 0)) //bit6 > AXIS1(+)
+ {
+ set_command = 1;
+ }
+ else if(((set_command == 1) || (set_command == 2)) && ((axis_button & 0b100000000000) == 0)) //bit12 > AXIS1(-)
+ {
+ set_command = 2;
+ }
+ else if(((set_command == 2) || (set_command == 3)) && ((axis_button & 0b000000010000) == 0)) //bit5 > AXIS2(+)
+ {
+ set_command = 3;
+ }
+ else if(((set_command == 3) || (set_command == 4)) && ((axis_button & 0b010000000000) == 0)) //bit11 > AXIS2(-)
+ {
+ set_command = 4;
+ }
+ else if(((set_command == 4) || (set_command == 5)) && ((axis_button & 0b000000001000) == 0)) //bit4 > AXIS3(+)
+ {
+ set_command = 5;
+ }
+ else if(((set_command == 5) || (set_command == 6)) && ((axis_button & 0b001000000000) == 0)) //bit10 > AXIS3(-)
+ {
+ set_command = 6;
+ }
+ else if(((set_command == 6) || (set_command == 7)) && ((axis_button & 0b000000000100) == 0)) //bit3 > AXIS4(+)
+ {
+ set_command = 7;
+ }
+ else if(((set_command == 7) || (set_command == 8)) && ((axis_button & 0b000100000000) == 0)) //bit9 > AXIS4(-)
+ {
+ set_command = 8;
+ }
+ else if(((set_command == 8) || (set_command == 9)) && ((axis_button & 0b000000000010) == 0)) //bit2 > AXIS5(+)
+ {
+ set_command = 9;
+ }
+ else if(((set_command == 9) || (set_command == 10)) && ((axis_button & 0b000010000000) == 0)) //bit8 > AXIS5(-)
+ {
+ set_command = 10;
+ }
+ else if(((set_command == 10) || (set_command == 11)) && ((axis_button & 0b000000000001) == 0)) //bit1 > AXIS6(+)
+ {
+ set_command = 11;
+ }
+ else if((set_command == 11) && ((axis_button & 0b000001000000) == 0)) //bit7 > AXIS6(-)
+ {
+ set_command = 0;
+ set_mode = true;
+ }
+ else if(axis_button == 0b111111111111)
+ {
+ }
+ else
+ {
+ set_command = 0;
+ }
+ }
+ else
+ {
+ if( (axis_button & 0b000001000000) == 0) //bit1 > AXIS6(-)
+ {
+ btn1_push_check[5]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000010000000) == 0) //bit2 > AXIS5(-)
+ {
+ btn1_push_check[4]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000100000000) == 0) //bit3 > AXIS4(-)
+ {
+ btn1_push_check[3]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b001000000000) == 0) //bit4 > AXIS3(-)
+ {
+ btn1_push_check[2]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000000010) == 0) //bit5 > AXIS5(+)
+ {
+ btn1_push_check[4]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000000001) == 0) //bit6 > AXIS6(+)
+ {
+ btn1_push_check[5]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b010000000000) == 0) //bit7 > AXIS2(-)
+ {
+ btn1_push_check[1]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b100000000000) == 0) //bit8 > AXIS1(-)
+ {
+ btn1_push_check[0]--;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000100000) == 0) //bit9 > AXIS1(+)
+ {
+ btn1_push_check[0]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000010000) == 0) //bit10 > AXIS2(+)
+ {
+ btn1_push_check[1]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000001000) == 0) //bit11 > AXIS3(+)
+ {
+ btn1_push_check[2]++;
+ menual_mode=true;
+ }
+ else if( (axis_button & 0b000000000100) == 0) //bit12 > AXIS4(+)
+ {
+ btn1_push_check[3]++;
+ menual_mode=true;
+
+ }
+ else
+ {
+ for(int i=0;i<6;i++)
+ button_offset_posion[i]=0;
+ }
+
+
+
+ for(int i=0;i<6;i++)
+ {
+ if(button_offset_posion[i]<-200)
+ button_offset_posion[i]=-200;
+
+ if(button_offset_posion[i]>200)
+ button_offset_posion[i]=200;
+ }
+
+ Menual_Segment();
+ axis_cmd_print_delay++;
+ if(axis_cmd_print_delay > 666)
+ {
+ printf("Axis menual mode!\r\n");
+ axis_cmd_print_delay = 0;
+ }
+ }
+}
+
+uint16_t front_board_cnt = 0;
+bool front_board_state = false;
+bool Front_Board_Detection()
+{
+ uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB);
+ //pc.printf("%x\n",button_data);
+ if(button_data & (1 << 7)) // front board detection
+ {
+ front_board_cnt++;
+ if(front_board_cnt > 3333)
+ {
+ if(front_board_state == false)
+ {
+ Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
+ Input_segment(btn2_input_data[0], btn2_input_data[1], SEG_DELAY_TIME);
+ front_board_state = true;
+ }
+ front_board_cnt = 3333;
+ return true;
+ }
+ }
+ else
+ {
+ front_board_cnt = 0;
+ front_board_state = false;
+ }
+
+ return false;
+}
+
+void Save_Data_To_Flash()
+{
+ if(flash_save_flag == true)
+ {
+ flash_save_cnt++;
+
+ if(flash_save_cnt > FLASH_SAVE_DELAY)
+ {
+ //printf("pgain : %.2f, input : %.2f\r\n", insert_pgain, insert_input);
+ uint32_t save_pgain = Convert_Uint32(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2]);
+ uint32_t save_input = Convert_Uint32(0, btn2_input_data[0], btn2_input_data[1]);
+
+ //printf("TRY Write pgain : %ld, input : %ld\r\n", save_pgain, save_input);
+
+ if(Button_Read_Flash(0) != save_pgain || Button_Read_Flash(4) != save_input)
+ {
+ Button_Erase_Flash();
+ Button_Write_Flash(save_pgain);
+ Button_Write_Flash(save_input);
+ }
+
+ printf("Flash Save Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
+
+ flash_save_flag = false;
+ }
+ }
+}
+
+uint32_t Convert_Uint32(uint8_t btn_data1, uint8_t btn_data2, uint8_t btn_data3)
+{
+ uint32_t convert_data = 0;
+
+ convert_data = uint32_t(btn_data1 * 100 + btn_data2 * 10 + btn_data3);
+
+ return convert_data;
+}
+
+double Convert_double_Pgain(uint32_t uint32_data)
+{
+ double convert_data = 0;
+
+ convert_data = (double)uint32_data / (double)10.0;
+
+ return convert_data;
+}
+
+double Convert_double_Igain(uint32_t uint32_data)
+{
+ double convert_data = 0;
+
+ convert_data = (double)uint32_data / (double)1000.0;
+
+ return convert_data;
+}
+
+// Page 126
+#define BUTTON_FLASH_START_ADDRESS ((uint32_t)0x0801F800)
+#define BUTTON_FLASH_END_ADDRESS ((uint32_t)0x0801FBFF)
+uint32_t button_flash_address_count = 0;
+
+void Button_Erase_Flash()
+{
+ static FLASH_EraseInitTypeDef button_flash_struct;
+ uint32_t PageError = 0;
+
+ button_flash_struct.TypeErase = FLASH_TYPEERASE_PAGES;
+ button_flash_struct.PageAddress = BUTTON_FLASH_START_ADDRESS;
+ button_flash_struct.NbPages = 1;
+
+ HAL_FLASH_Unlock();
+ if(HAL_FLASHEx_Erase(&button_flash_struct, &PageError) != HAL_OK)
+ {
+ //pc.printf("Button Flash erase error!\n\r");
+ }
+ HAL_FLASH_Lock();
+ button_flash_address_count = 0;
+}
+
+uint32_t Button_Read_Flash(uint32_t address)
+{
+ uint32_t val = 0;
+ address = address + BUTTON_FLASH_START_ADDRESS;
+ val = *(__IO uint32_t*)address;
+
+ return val;
+}
+
+void Button_Write_Flash(uint32_t data)
+{
+ if(data != Button_Read_Flash(button_flash_address_count * 4))
+ {
+ HAL_FLASH_Unlock();
+ HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, BUTTON_FLASH_START_ADDRESS + (button_flash_address_count * 4), data);
+ HAL_FLASH_Lock();
+ }
+ button_flash_address_count++;
+}
+
+void button_pid_info()
+{
+ pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
+ for(int i = 0; i < 6; i++)
+ {
+ 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]);
+ }
+ pc.printf("\n\n\n");
+
+}
+void reset_speed_pid()
+{
+ for(int i=0; i<6; i++)
+ {
+ taget_speed[i]=0;
+ error_speed[i]=0;
+ Speed_PID_OUTPUT[i]=0;
+ result_i[i]=0;
+ filter_result_i[i]=0;
+ output_i[i]=0;
+ output_p[i]=0;
+ }
+}
+
+#endif