File content as of revision 4:bf278ddb8504:

#ifndef _BUTTON_H_
#define _BUTTON_H_

#include "MCP23S17.h"
#include "7_segment.h"

#define BUTTON_DELAY            60      // 75ms
#define BUTTON_FAST_LIMIT       50
#define FLASH_SAVE_DELAY        3333  // 3333, 5 sec
#define PGAIN_CORRETION_FACTOR  0.4

void reset_speed_pid();
void button_pid_info();

bool menual_mode=false;
bool set_mode = false;
bool set_cmd_togle = false;
uint8_t set_command = 0;
uint16_t set_cmd_delay = 200;
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()
{
    if (Button_Read_Flash(0) != 0xFFFFFFFF)
    {
        insert_pgain = Convert_double_Pgain(Button_Read_Flash(0));
        insert_pgain *= PGAIN_CORRETION_FACTOR;
        
        for (int i = 1; i < 7; i++)
        {
            set_data[i * 10 + 2] = insert_pgain;
            position_Pgain[i - 1] = insert_pgain;
        }
        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);
        Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
    }
    else
    {
        insert_pgain = 25.0 * PGAIN_CORRETION_FACTOR;
        for (int i = 1; i < 7; i++)
        {
            set_data[i * 10 + 2] = insert_pgain;
            position_Pgain[i - 1] = insert_pgain;
        }
        btn2_pgain_data[0] = (uint8_t)2;
        btn2_pgain_data[1] = (uint8_t)5;
        btn2_pgain_data[2] = (uint8_t)0;
        Pgain_segment(btn2_pgain_data[0], btn2_pgain_data[1], btn2_pgain_data[2], SEG_DELAY_TIME);
        flash_save_flag = true;
    }


    if (Button_Read_Flash(4) != 0xFFFFFFFF)
    {
        insert_input = Convert_double_Igain(Button_Read_Flash(4));
        
        for (int i = 1; i < 7; i++)
        {
            set_data[i * 10 + 3] = insert_input;
            Position_input_filter[i - 1] = insert_input;
        }
        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);
    }
    else
    {
        insert_input = 0.05;

        for (int i = 1; i < 7; i++)
        {
            set_data[i * 10 + 3] = insert_input;
            Position_input_filter[i - 1] = insert_input;
        }
        btn2_input_data[0] = (uint8_t)5;
        btn2_input_data[1] = (uint8_t)0;
        Input_segment(5, 0, SEG_DELAY_TIME);
        flash_save_flag = true;
    }
}

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
        insert_pgain *= PGAIN_CORRETION_FACTOR;
        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
        insert_pgain *= PGAIN_CORRETION_FACTOR;
        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)
    {
        set_cmd_delay++;
        if(set_cmd_delay > 200) set_cmd_delay = 200;
        
        if((axis_button & 0b000000100000) == 0) //bit6 > AXIS1(+)
        {
            if(set_cmd_delay > 100)
            {
                set_cmd_togle = true;
                set_cmd_delay = 0;
            }
        }
        else if((set_cmd_togle == true) && (axis_button  == 0b111111111111))
        {
            set_cmd_togle = false;
            set_command++;
        
            if(set_command >= 5)
            {
                set_mode = true;
            }
        }
        else if(axis_button == 0b111111111111)
        {
        }
        else
        {
            set_command = 0;
        }
    }        
    else
    {                
        if( (axis_button & 0b000001000000) == 0) //bit1 > AXIS6(-)
        {
            button_offset_posion[5]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b000010000000) == 0) //bit2 > AXIS5(-)
        {
            button_offset_posion[4]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b000100000000) == 0) //bit3 > AXIS4(-)
        {
            button_offset_posion[3]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b001000000000) == 0) //bit4 > AXIS3(-)
        {
            button_offset_posion[2]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000000010) == 0) //bit5 > AXIS5(+)
        {
            button_offset_posion[4]++;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000000001) == 0) //bit6 > AXIS6(+)
        {
            button_offset_posion[5]++;
            menual_mode=true;
        }
        else if( (axis_button & 0b010000000000) == 0) //bit7 > AXIS2(-)
        {
            button_offset_posion[1]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b100000000000) == 0) //bit8 > AXIS1(-)
        {
            button_offset_posion[0]--;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000100000) == 0) //bit9 > AXIS1(+)
        {
            button_offset_posion[0]++;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000010000) == 0) //bit10 > AXIS2(+)
        {
            button_offset_posion[1]++;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000001000) == 0) //bit11 > AXIS3(+)
        {
            button_offset_posion[2]++;
            menual_mode=true;
        }
        else if( (axis_button & 0b000000000100) == 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;
        }
        
        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