test
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Revision 0:e12eb40b9fef, committed 2020-04-23
- Comitter:
- lsh2205
- Date:
- Thu Apr 23 00:38:16 2020 +0000
- Commit message:
- test;
Changed in this revision
diff -r 000000000000 -r e12eb40b9fef 7_segment.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/7_segment.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,208 @@ +#ifndef _7_SEGMENT_H_ +#define _7_SEGMENT_H_ + +#include "MCP23S17.h" + +#define SEG_DELAY_TIME (int)3 +#define SEG_OE_PERIOD (int)1500 // 1.5 ms + +#define SEG_PGAIN 0 +#define SEG_INPUT 0 + + +//FastPWM OE_pin(PB_11, -1); +//PwmOut OE_pin(PB_11); +//DigitalOut output_enable_pin(PB_11); +bool OE_state = false; + +const bool num_data[10][8] = { + {1,0,0,0,0,0,0,1}, + {1,1,1,1,0,0,1,1}, + {0,1,0,0,1,0,0,1}, + {0,1,1,0,0,0,0,1}, + {0,0,1,1,0,0,1,1}, + {0,0,1,0,0,1,0,1}, + {0,0,0,0,0,1,0,1}, + {1,0,1,1,0,0,0,1}, + {0,0,0,0,0,0,0,1}, + {0,0,1,1,0,0,0,1}, + }; + +int Gobal_GPIOA = 0x00; + +void Segment_Init(); +void Segment_PWM_Control(); +void pgain_Latch_pin(bool logic); +void pgain_Clock_pin(bool logic); +void pgain_data_pin(bool logic); +void input_Latch_pin(bool logic); +void input_Clock_pin(bool logic); +void input_data_pin(bool logic); +void Pgain_segment(int num1,int num2,int num3,int delay_time_us); +void Input_segment(int num1,int num2,int delay_time_us); + +void Segment_Init() +{ + Pgain_segment(0, 0, 0, SEG_DELAY_TIME); + Input_segment(0, 0, SEG_DELAY_TIME); + //output_enable_pin = 1; +// OE_pin.period(0.015); +// OE_pin.write(0.5f); +} +/* +void Segment_PWM_Control() +{ + output_enable_pin = OE_state; + OE_state = !OE_state; +} +*/ +void pgain_Latch_pin(bool logic) //GPA2 +{ + if(logic) + Gobal_GPIOA |= (1<<2);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<2);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void pgain_Clock_pin(bool logic) //GPA3 +{ + if(logic) + Gobal_GPIOA |= (1<<3);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<3);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void pgain_data_pin(bool logic) //GPA0 +{ + if(logic) + Gobal_GPIOA |= (1<<0);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<0);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void input_Latch_pin(bool logic) //GPA6 +{ + if(logic) + Gobal_GPIOA |= (1<<6);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<6);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void input_Clock_pin(bool logic) //GPA7 +{ + if(logic) + Gobal_GPIOA |= (1<<7);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<7);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void input_data_pin(bool logic) //GPA4 +{ + if(logic) + Gobal_GPIOA |= (1<<4);//LED를 켠다. + else + Gobal_GPIOA &= ~(1<<4);//LED를 끈다. + + MCP_Write(MCP_SEG_CS, MCP_GPIOA, Gobal_GPIOA); +} + +void Pgain_segment(int num1,int num2,int num3,int delay_time_us) +{ + pgain_Latch_pin(0); + wait_us(delay_time_us); + + if(num1>9) + num1=9; + if(num1<0) + num1=0; + + if(num2>9) + num2=9; + if(num2<0) + num2=0; + + if(num3>9) + num3=9; + if(num3<0) + num3=0; + + + for(int i=0;i<8;i++) + { + pgain_Clock_pin(1); + wait_us(delay_time_us); + pgain_data_pin(num_data[num3][i]); + wait_us(delay_time_us); + pgain_Clock_pin(0); + wait_us(delay_time_us); + } + for(int i=0;i<8;i++) + { + pgain_Clock_pin(1); + wait_us(delay_time_us); + pgain_data_pin(num_data[num2][i]); + wait_us(delay_time_us); + pgain_Clock_pin(0); + wait_us(delay_time_us); + } + for(int i=0;i<8;i++) + { + pgain_Clock_pin(1); + wait_us(delay_time_us); + pgain_data_pin(num_data[num1][i]); + wait_us(delay_time_us); + pgain_Clock_pin(0); + wait_us(delay_time_us); + } + wait_us(delay_time_us); + pgain_Latch_pin(1); +} + +void Input_segment(int num1,int num2,int delay_time_us) +{ + input_Latch_pin(0); + wait_us(delay_time_us); + + if(num1>9) + num1=9; + if(num1<0) + num1=0; + + if(num2>9) + num2=9; + if(num2<0) + num2=0; + + for(int i=0;i<8;i++) + { + input_Clock_pin(1); + wait_us(delay_time_us); + input_data_pin(num_data[num2][i]); + wait_us(delay_time_us); + input_Clock_pin(0); + wait_us(delay_time_us); + } + for(int i=0;i<8;i++) + { + input_Clock_pin(1); + wait_us(delay_time_us); + input_data_pin(num_data[num1][i]); + wait_us(delay_time_us); + input_Clock_pin(0); + wait_us(delay_time_us); + } + wait_us(delay_time_us); + input_Latch_pin(1); +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef BufferedSerial.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sam_grove/code/BufferedSerial/#7e5e866edd3d
diff -r 000000000000 -r e12eb40b9fef FastPWM.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastPWM.lib Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Sissors/code/FastPWM/#d6c2b73d71f5
diff -r 000000000000 -r e12eb40b9fef LS7366LIB.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LS7366LIB.lib Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/lsh2205/code/LS7366LIB2/#6063c8171873
diff -r 000000000000 -r e12eb40b9fef MCP23S17.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MCP23S17.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,75 @@ +#ifndef _MCP23S17_H_ +#define _MCP23S17_H_ + +#include "spi_setup.h" + +#define MCP_GPPUA 0x0C // PULL_UP Port A +#define MCP_GPPUB 0x0D // PULL_UP Port B + +#define MCP_IODIRA 0x00 // DIR Port A +#define MCP_IODIRB 0x01 // DIR Port B +#define MCP_GPPUA 0x0C // Pull up Port A +#define MCP_GPPUB 0x0D // Pull up Port B +#define MCP_GPIOA 0x12 // Address Port A +#define MCP_GPIOB 0x13 // Address Port B +#define MCP_WRITE_BYTE 0x40 +#define MCP_READ_BYTE 0x41 + +#define MCP_SEG_CS (uint8_t)0 +#define MCP_BTN1_CS (uint8_t)1 +#define MCP_BTN2_CS (uint8_t)2 + +void MCP23S17_Init(); +void MCP_Write(uint8_t cs_select, uint8_t reg_addr, uint8_t data); +uint8_t MCP_Read(uint8_t cs_select, uint8_t reg_addr); + +void MCP23S17_Init() +{ + MCP_Write(MCP_SEG_CS, MCP_IODIRA, 0x00); //GPIOA as output A + MCP_Write(MCP_SEG_CS, MCP_IODIRB, 0x00); //GPIOB as output B + + MCP_Write(MCP_BTN1_CS, MCP_IODIRA, 0xff); //GPIOA as input A + MCP_Write(MCP_BTN1_CS, MCP_IODIRB, 0xff); //GPIOB as input B + + MCP_Write(MCP_BTN2_CS, MCP_IODIRA, 0xff); //GPIOA as input A + MCP_Write(MCP_BTN2_CS, MCP_IODIRB, 0xff); //GPIOB as input B + + MCP_Write(MCP_BTN2_CS, MCP_GPPUB, 0xff); //GPIOB as input B +} + +void MCP_Write(uint8_t cs_select, uint8_t reg_addr, uint8_t data) +{ + if(cs_select == MCP_SEG_CS) seg_cs = 1; + else if(cs_select == MCP_BTN1_CS) btn1_cs = 1; + else if(cs_select == MCP_BTN2_CS) btn2_cs = 1; + + spi1.write(MCP_WRITE_BYTE); + spi1.write(reg_addr); + spi1.write(data); + + if(cs_select == MCP_SEG_CS) seg_cs = 0; + else if(cs_select == MCP_BTN1_CS) btn1_cs = 0; + else if(cs_select == MCP_BTN2_CS) btn2_cs = 0; +} + +uint8_t MCP_Read(uint8_t cs_select, uint8_t reg_addr) +{ + uint8_t receive_data = 0; + + if(cs_select == MCP_SEG_CS) seg_cs = 1; + else if(cs_select == MCP_BTN1_CS) btn1_cs = 1; + else if(cs_select == MCP_BTN2_CS) btn2_cs = 1; + + spi1.write(MCP_READ_BYTE); + spi1.write(reg_addr); + receive_data = spi1.write(0x00); + + if(cs_select == MCP_SEG_CS) seg_cs = 0; + else if(cs_select == MCP_BTN1_CS) btn1_cs = 0; + else if(cs_select == MCP_BTN2_CS) btn2_cs = 0; + + return receive_data; +} + + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef Position.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Position.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,98 @@ +#ifndef _POSITION_H_ +#define _POSITION_H_ + +double taget_position[6]={0,}; +double oder_position[6]={0,}; + +double now_degree[6]={0,}; +double error_degree[6]={0,}; + +double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5}; + +double boot_cnt=0; +int boot_cnt_max=15000; + +void taget_position_read() +{ + + taget_position[0]=first_degree; + taget_position[1]=second_degree; + taget_position[2]=third_degree; + taget_position[3]=-four_degree; + taget_position[4]=five_degree; + taget_position[5]=six_degree; + + + for(int i = 0; i < 3; i++) + { + if(taget_position[i] > 160) + taget_position[i] = 160; + if(taget_position[i] < 0) + taget_position[i] = 0; + } + + + if(taget_position[3] > 0) + taget_position[3] = 0; + if(taget_position[3] < -152) + taget_position[3] = -152; + + + for(int i = 4; i < 6; i++) + { + if(taget_position[i] > 110) + taget_position[i] = 110; + if(taget_position[i] < 5) + taget_position[i] = 5; + } + +} + +void input_filter() +{ + + +for(int i=0;i<6;i++) + oder_position[i] = oder_position[i]*(1-Position_input_filter[i]*(boot_cnt/boot_cnt_max)) + taget_position[i]*Position_input_filter[i]*(boot_cnt/boot_cnt_max); +} + +void cal_error() +{ + for(int i=0;i<6;i++) + error_degree[i] = oder_position[i]-now_degree[i]; +} + +void cal_degree() +{ + for(int i=0;i<6;i++) + now_degree[i] = -(((double)encoder_data[i]/100)/1024)*90 * gear[i]; +} + +void Position_P() +{ + for(int i=0;i<6;i++) + taget_speed[i]=-error_degree[i]*position_Pgain[i]; + +} + + + +void Position_PID() +{ + boot_cnt++; + if(boot_cnt>boot_cnt_max) + boot_cnt=boot_cnt_max; + + taget_position_read(); + + input_filter(); + + cal_degree(); + + cal_error(); + + Position_P(); + +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef button.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/button.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,551 @@ +#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; +uint16_t axis_button=0x0fff; + +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(); +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); + + } + 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); + } +} + +void Button_Detection() +{ + uint8_t button_data = MCP_Read(MCP_BTN2_CS, MCP_GPIOB); + //pc.printf("%x\n",button_data); + + 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(); + } +} + +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 \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef encoder.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/encoder.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,287 @@ +#ifndef _ENCODER_H_ +#define _ENCODER_H_ + +int ex_encoder_data[6]={0,}; +int dif_encoder_data[6]={0,}; +double filter_dif_encoder_data[6]={0,}; +double filter_dif_encoder_co[6] = {0.01,0.01,0.01,0.01,0.01,0.01}; + +void filter_encoder_data() +{ + for(int i=0; i<6;i++) + { + dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i]; + + filter_dif_encoder_data[i] = filter_dif_encoder_data[i]*(1-filter_dif_encoder_co[i]) + (double)dif_encoder_data[i]*filter_dif_encoder_co[i]; + + ex_encoder_data[i] = encoder_data[i]; + } +} + +void encoder_read_raw() +{ + encoder_data[0] = encoder1.read(); + encoder_data[1] = encoder2.read(); + encoder_data[2] = encoder3.read(); + encoder_data[3] = encoder4.read(); + encoder_data[4] = -encoder5.read(); + encoder_data[5] = -encoder6.read(); + +} + + +void encoder_read() +{ + encoder_data[0] = encoder1.read(); + encoder_data[1] = encoder2.read(); + encoder_data[2] = encoder3.read(); + encoder_data[3] = encoder4.read(); + encoder_data[4] = -encoder5.read(); + encoder_data[5] = -encoder6.read(); + + /* + for(int i=0;i<6;i++) + { + pc.printf("%8d", encoder_data[i]); + }pc.printf("\r\n"); + */ + filter_encoder_data(); +} + +/* +void reset_check() +{ + } +*/ +void reset_check() +{ + if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1) + { + motor_onoff[0]=false; + motor_onoff[1]=false; + motor_onoff[2]=false; + motor_onoff[3]=false; + motor_onoff[4]=false; + motor_onoff[5]=false; + + while(1) + { + + pwm1.write(0.4); + pwm2.write(0.4); + pwm3.write(0.4); + pwm4.write(0.4); + pwm5.write(0.4); + pwm6.write(0.4); + + wait_ms(50); + + encoder_data[0] = encoder1.read(); + if(encoder_data[0]!=-1) + { + NVIC_SystemReset(); + } + wait_ms(200); + } + + } +} + +void encoder_reset_cnt() +{ + encoder1.LS7366_reset_counter(); + encoder2.LS7366_reset_counter(); + encoder3.LS7366_reset_counter(); + encoder4.LS7366_reset_counter(); + encoder5.LS7366_reset_counter(); + encoder6.LS7366_reset_counter(); +} + + +void Time_To_Motor_Drive(int dir, double time_ms) +{ + const int motor_percent[6] = {400, 400, 400, 350, -200, -200}; + int motor_4_dir = 1; + + for(int i = 0; i < 6; i++) + { + if(i == 3) + { + motor_4_dir = -1; + } + else + { + motor_4_dir = 1; + } + motor_power(i, motor_percent[i] * dir * motor_4_dir); + } + wait_ms(time_ms); + + if(dir == -1) + { + for(int i = 4; i < 6; i++) + { + motor_power(i, 0); + } + wait_ms(time_ms); + } +} + + +void encoder_check0() +{ + + int forward_enc[6] = {0,}; + int reverse_enc[6] = {0,}; + bool encoder_state = false; + + Time_To_Motor_Drive(0, 200); + encoder_reset_cnt(); + encoder_read(); + + for(int i = 0; i < 4; i++) + { + if(encoder_data[i] == -1) + { + encoder_state = true; + pc.printf("%d : -1 \n\r", i); + } + } + for(int i = 4; i < 6; i++) + { + if(encoder_data[i] == 1) + { + encoder_state = true; + pc.printf("%d : -1 \n\r", i); + } + } + + if(encoder_state != true) + { + Time_To_Motor_Drive(1, 200); + Time_To_Motor_Drive(0, 1000); + encoder_read(); + + for(int i = 0; i < 6; i++) + { + forward_enc[i] = encoder_data[i];// * -1; + } + Time_To_Motor_Drive(-1, 250); + Time_To_Motor_Drive(0, 1000); + encoder_read(); + + for(int i = 0; i < 6; i++) + { + reverse_enc[i] = encoder_data[i];// * -1; + if((forward_enc[i] > 100) || (forward_enc[i] > reverse_enc[i]) || (forward_enc[i] == 0 && reverse_enc[i] == 0)) + { + encoder_state = true; + pc.printf("%d ", i); + } + } + } + + //error + if(encoder_state == true) + { + pc.printf("Encoder error!\n\r"); + wait_ms(1000); + while(1) + { + } + } +} + +void encoder_check2() +{ + encoder_reset_cnt(); + + + encoder_read(); + + for(int i=0;i<6;i++) + { + pc.printf("%8d", encoder_data[i]); + }pc.printf("\r\n"); + + int encoder_check_move[6]={0,0,0,0,0,0}; + + for(int i=0; i<200;i++) + { + encoder_read(); + + for(int k=0;k<6;k++) + { + if(encoder_check[k]==false) + { + if(encoder_data[k]<-3) + encoder_check[k]=true; + if(encoder_data[k]>3) + encoder_check[k]=true; + encoder_check_move[k]=i; + } + + + if(encoder_check[k]) + { + motor_power(k, 0); + } + else + { + motor_power(k, i); + } + } + } + + + for(int i=0;i<6;i++) + motor_power(i, 0); + + wait_ms(100); + + for(int i=0;i<6;i++) + { + pc.printf("%8d", encoder_data[i]); + }pc.printf("\r\n"); + + for(int i=0; i<200;i++) + { + encoder_read(); + for(int k=0;k<6;k++) + { + if(encoder_check[k]==false) + motor_power(k, -i); + } + } + + for(int i=0;i<6;i++) + motor_power(i, 0); + + + wait_ms(100); + + encoder_read(); + for(int i=0;i<6;i++) + { + pc.printf("%8d", encoder_data[i]); + }pc.printf("\r\n"); + for(int i=0;i<6;i++) + { + pc.printf("%8d", encoder_check_move[i]); + }pc.printf("\r\n"); + + for(int i=0;i<6;i++) + { + if(encoder_check[i]) + pc.printf(" OK"); + else + pc.printf(" ER"); + + motor_onoff[i]=encoder_check[i]; + }pc.printf("\r\n"); + + + +} + +#endif
diff -r 000000000000 -r e12eb40b9fef flash.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/flash.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,84 @@ +#ifndef _FLASH_H_ +#define _FLASH_H_ + +// ST32F103RBT6 - FLASH : 128KB, SRAM : 20KB (Medium-density model) +// flash memory page : 127 (last page) +// flash memory size : 1KB, unit : Word(4Byte) +// 0x0801 FC00 - 0x0801 FFFF +#define FLASH_START_ADDRESS ((uint32_t)0x0801FC00) +#define FLASH_END_ADDRESS ((uint32_t)0x0801FFFF) + +#define FLASH_START_ADDRESS2 ((uint32_t)0x0801F800) +#define FLASH_END_ADDRESS2 ((uint32_t)0x0801FBFF) + +uint32_t flash_address_count = 0; + +bool Flash_Head_Check(); +void Erase_Flash(); +void Write_Flash(uint32_t data); +uint32_t Read_Flash(uint32_t address); + +bool Flash_Head_Check() +{ + uint32_t flash_data[5]; + for(uint32_t i = 0; i < 5; i++) + { + flash_data[i] = Read_Flash(i * 4); + } + + if ( (char)flash_data[0] == 'F' + && (char)flash_data[1] == 'L' + && (char)flash_data[2] == 'A' + && (char)flash_data[3] == 'S' + && (char)flash_data[4] == 'H') + { + return true; + } + else + { + return false; + } +} + + +void Erase_Flash() +{ + static FLASH_EraseInitTypeDef flash_struct; // Flash erase struct + uint32_t PageError = 0; // Page error state + + flash_struct.TypeErase = FLASH_TYPEERASE_PAGES; // Erase type : Page + flash_struct.PageAddress = FLASH_START_ADDRESS; // Flash start address + flash_struct.NbPages = 1; // Number of page + + HAL_FLASH_Unlock(); // Flash control unlock + if(HAL_FLASHEx_Erase(&flash_struct, &PageError) != HAL_OK) // Flash erase function & result, HAL_OK = true + { + //pc.printf("Flash erase error!\n\r"); + } + HAL_FLASH_Lock(); // Flash control lock + flash_address_count = 0; // Flash address count variable reset +} + + +void Write_Flash(uint32_t data) +{ + if(Read_Flash(flash_address_count * 4)) + { + HAL_FLASH_Unlock(); + HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, FLASH_START_ADDRESS + (flash_address_count * 4), data); + HAL_FLASH_Lock(); + } + flash_address_count++; +} + +uint32_t Read_Flash(uint32_t address) +{ + uint32_t val = 0; + address = address + FLASH_START_ADDRESS; + val = *(__IO uint32_t*)address; + + return val; +} + + +#endif
diff -r 000000000000 -r e12eb40b9fef limit.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/limit.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,313 @@ +#ifndef _LIMIT_H_ +#define _LIMIT_H_ + +#define limit_debug_print false +#define start_duty 200 + +bool limit_find[6]={false,false,false,false,false,false}; + +DigitalIn limit_sw1(LIMIT_SW1); +DigitalIn limit_sw2(LIMIT_SW2); +DigitalIn limit_sw3(LIMIT_SW3); +DigitalIn limit_sw4(LIMIT_SW4); +DigitalIn limit_sw5(LIMIT_SW5); +DigitalIn limit_sw6(LIMIT_SW6); + +#define limit_time 10000 + +void limit_init() +{ + limit_sw1.mode(PullUp); + limit_sw2.mode(PullUp); + limit_sw3.mode(PullUp); + limit_sw4.mode(PullUp); + limit_sw5.mode(PullUp); + limit_sw6.mode(PullUp); +} + + +double limit_ex_encoder_data[6]={0,}; +double diff_encoder_data[6]={0,0,0,0,0,0}; +double sum_error[6]={0,0,0,0,0,0}; +double motor_duty[6]={-60,0,0,0,0,0}; +double limit_taget_speed[6]={0,0,0,0,0,0}; +double limit_result[6]={0,0,0,0,0,0}; +double limit_output[6]={0,0,0,0,0,0}; +double limit_error[6]={0,0,0,0,0,0}; + +double axis123_taget_speed=30; +double axis4_taget_speed=-56; +double axis56_taget_speed=30; + +int ccnt=0; +bool limit_check() +{ + + int sw1 = limit_sw1; + int sw2 = limit_sw2; + int sw3 = limit_sw3; + int sw4 = limit_sw4; + int sw5 = limit_sw5; + int sw6 = limit_sw6; + + encoder_read_raw(); + + + if(limit_find[0]==false) + { + if(limit_taget_speed[0]<axis123_taget_speed) + limit_taget_speed[0]+=0.3; + + motor_power(0,limit_output[0]); + }else + { + if(limit_taget_speed[0]>0) + limit_taget_speed[0]-=1; + + if(limit_taget_speed[0]<0) + { + limit_taget_speed[0]=0; + } + motor_power(0,limit_output[0]); + } + + if(limit_find[1]==false) + { + if(limit_taget_speed[1]<axis123_taget_speed) + limit_taget_speed[1]+=0.3; + + motor_power(1,limit_output[1]); + }else + { + if(limit_taget_speed[1]>0) + limit_taget_speed[1]-=1; + + if(limit_taget_speed[1]<0) + { + limit_taget_speed[1]=0; + } + motor_power(1,limit_output[1]); + } + + if(limit_find[2]==false) + { + if(limit_taget_speed[2]<axis123_taget_speed) + limit_taget_speed[2]+=0.3; + + motor_power(2,limit_output[2]); + }else + { + if(limit_taget_speed[2]>0) + limit_taget_speed[2]-=1; + + if(limit_taget_speed[2]<0) + { + limit_taget_speed[2]=0; + } + motor_power(2,limit_output[2]); + } + + if(limit_find[3]==false) + { + if(limit_taget_speed[3]>axis4_taget_speed) + limit_taget_speed[3]-=0.3; + + motor_power(3,limit_output[3]); + }else + { + if(limit_taget_speed[3]<0) + limit_taget_speed[3]+=1; + + if(limit_taget_speed[3]>0) + { + limit_taget_speed[3]=0; + } + motor_power(3,limit_output[3]); + } + + if(ccnt<450) + { + ccnt++; + limit_taget_speed[0]=0; + limit_taget_speed[1]=0; + limit_taget_speed[2]=0; + limit_taget_speed[3]=0; + limit_taget_speed[4]-=0.4; + limit_taget_speed[5]-=0.4; + } + + if(limit_find[4]==false) + { + if(limit_taget_speed[4]<axis56_taget_speed) + limit_taget_speed[4]+=0.3; + + motor_power(4,-limit_output[4]); + }else + { + if(limit_taget_speed[4]>0) + limit_taget_speed[4]-=1; + + if(limit_taget_speed[4]<0) + { + limit_taget_speed[4]=0; + } + motor_power(4,-limit_output[4]); + } + if(limit_find[5]==false) + { + if(limit_taget_speed[5]<axis56_taget_speed) + limit_taget_speed[5]+=0.3; + + motor_power(5,-limit_output[5]); + }else + { + if(limit_taget_speed[5]>0) + limit_taget_speed[5]-=1; + + if(limit_taget_speed[5]<0) + { + limit_taget_speed[5]=0; + } + motor_power(5,-limit_output[5]); + } + + + for(int i=0;i<3;i++) + { + + diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i]; + limit_ex_encoder_data[i]=encoder_data[i]; + limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]); + sum_error[i]+=limit_error[i]*0.2; + limit_result[i]=limit_error[i]*2+sum_error[i]*1; + + if(limit_result[i]>200) + limit_result[i]=200; + if(limit_result[i]<-200) + limit_result[i]=-200; + + limit_output[i]=motor_duty[i]+limit_result[i]; + + if(limit_debug_print) + pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]); + } + + + for(int i=3;i<4;i++) + { + + diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i]; + limit_ex_encoder_data[i]=encoder_data[i]; + limit_error[i]=-(diff_encoder_data[i]-limit_taget_speed[i]); + sum_error[i]+=limit_error[i]*0.2; + limit_result[i]=limit_error[i]*2+sum_error[i]*1; + + if(limit_result[i]>200) + limit_result[i]=200; + if(limit_result[i]<-200) + limit_result[i]=-200; + + limit_output[i]=motor_duty[i]+limit_result[i]; + + if(limit_debug_print) + pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]); + } + + + for(int i=4;i<6;i++) + { + + diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i]; + limit_ex_encoder_data[i]=encoder_data[i]; + limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]); + sum_error[i]+=limit_error[i]*0.2; + limit_result[i]=limit_error[i]*2+sum_error[i]*1; + + if(limit_result[i]>200) + limit_result[i]=200; + if(limit_result[i]<-200) + limit_result[i]=-200; + + limit_output[i]=motor_duty[i]+limit_result[i]; + + if(limit_debug_print) + pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]); + } + + + if(limit_debug_print) + pc.printf("\n\r"); + + + + if(sw1==0 && !limit_find[0]) + { + limit_find[0]=true; + encoder1.LS7366_reset_counter(); + limit_ex_encoder_data[0]=0; + } + + if(sw2==0 && !limit_find[1]) + { + limit_find[1]=true; + encoder2.LS7366_reset_counter(); + limit_ex_encoder_data[1]=0; + } + + if(sw3==0 && !limit_find[2]) + { + limit_find[2]=true; + encoder3.LS7366_reset_counter(); + limit_ex_encoder_data[2]=0; + } + + if(sw4==0 && !limit_find[3]) + { + limit_find[3]=true; + encoder4.LS7366_reset_counter(); + limit_ex_encoder_data[3]=0; + } + if(sw5==0 && !limit_find[4]) + { + limit_find[4]=true; + encoder5.LS7366_reset_counter(); + limit_ex_encoder_data[4]=0; + } + if(sw6==0 && !limit_find[5]) + { + limit_find[5]=true; + encoder6.LS7366_reset_counter(); + limit_ex_encoder_data[5]=0; + } + + return (limit_find[0] || !motor_onoff[0]) && (limit_find[1] || !motor_onoff[1]) && (limit_find[2] || !motor_onoff[2]) && (limit_find[3] || !motor_onoff[3]) && (limit_find[4] || !motor_onoff[4]) && (limit_find[5] || !motor_onoff[5]); +} + +bool limit_check_done=false; +int limit_check_done_cnt=0; +void find_limit() +{ + wait_ms(0); + + encoder_reset_cnt(); + + for(int i=0;i<limit_time;i++) + { + if(limit_check()) + { + break; + } + reset_check(); + wait_ms(1); + + } + encoder_read_raw(); + motor_power(0,0); + motor_power(1,0); + motor_power(2,0); + motor_power(3,0); + motor_power(4,0); + motor_power(5,0); +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,109 @@ +#include "mbed.h" +#include "main.h" + +void initial_position(); +void begin_pid(); +void command_init(); +void pid_info(); +void test(); + +int cnt=0; + +int m_cnt=0; +int menual=false; +int main() +{ + + board_init(); + MCP23S17_Init(); + Segment_Init(); + Button_Init(); + + printf("READ Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4)); + + + encoder_check2(); + + find_limit(); + + command_init(); + + Button_Init(); + + pc.printf("PID LOOP_START\r\n"); + pid_info(); + + + while(1) { + + reset_check(); + + m_cnt++; + if(m_cnt>1000) + { + m_cnt=0; + //pid_info(); + } + + Button_Detection(); + + comunication(); // 모터의 위치 값을 받음 + + taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw); + encoder_read(); + + Position_PID(); + Speed_PID(); + + for(int i=0; i<6; i++) + motor_power(i,Speed_PID_OUTPUT[i]); + + } +} + + + +void command_init() +{ + cmd_roll = 32768; + cmd_pitch = 32768; + cmd_heave = 32768; + cmd_sway = 32768; + cmd_surge = 32768; + cmd_yaw = 32768; +} + + +void 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"); + +} + +/* +Position_PID(); + if(menual_mode && (menual == false)) + { + menual=true; + reset_speed_pid(); + } + + if(!menual) + Position_PID(); + else + { + for(int i=0; i<6; i++) + { + taget_speed[i]=button_offset_posion[i]/5; + } + } + + Speed_PID(); +*/ + +
diff -r 000000000000 -r e12eb40b9fef main.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,130 @@ +#ifndef _MAIN_H_ +#define _MAIN_H_ +int button_offset_posion[6]={0,}; +bool encoder_check[6]={false,false,false,false,false,false}; +bool motor_onoff[6]={true,true,true,true,true,true}; +int encoder_data[6]={0,0,0,0,0,0}; + +double Speed_Pgain[6]={ 3, + 3, + 3, + 3, + 3, + 3}; + +double Speed_Igain[6]={ 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01}; + +double position_Pgain[6] + ={ 10, + 10, + 10, + 10, + 10, + 10}; + +double Position_input_filter[6] + ={ 0.05, + 0.05, + 0.05, + 0.05, + 0.05, + 0.05}; +double Speed_I_input_filter[6] + ={ 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1}; + +double offset[6]={ 1500, + 1500, + 1500, + 1500, + 1500, + 1500}; + + +// SPI +#define SPI_MOSI1 PA_7 +#define SPI_MISO1 PA_6 +#define SPI_SCK1 PA_5 + +// Encoder +#define ENCODER_CS1 PB_0 +#define ENCODER_CS2 PC_1 +#define ENCODER_CS3 PC_0 +#define ENCODER_CS4 PB_15 +#define ENCODER_CS5 PB_14 +#define ENCODER_CS6 PB_13 + +// Limit switch +#define LIMIT_SW1 PC_3 +#define LIMIT_SW2 PC_2 +#define LIMIT_SW3 PC_5 +#define LIMIT_SW4 PC_9 +#define LIMIT_SW5 PC_6 +#define LIMIT_SW6 PC_8 + +// Motor +#define MOTOR_PWM1 PA_15 +#define MOTOR_PWM2 PA_9 +#define MOTOR_PWM3 PA_8 +#define MOTOR_PWM4 PB_10 +#define MOTOR_PWM5 PB_3 +#define MOTOR_PWM6 PA_10 +#define MOTOR_PWM7 PB_11 + +#include "flash.h" +#include "spi_setup.h" +#include "pc_serial.h" +#include "motor.h" +#include "encoder.h" +#include "speed_pid.h" +#include "target_position_cal.h" +#include "Position.h" +#include "limit.h" +#include "button.h" + + +void board_init() +{ + Serial_Init(); + if(Flash_Head_Check() == true) + { + for(int i = 1; i < 7; i++) + { + set_data[i * 10] = (double)Read_Flash(i * 40); + set_data[i * 10 + 1] = (double)Read_Flash(i * 40 + 4); + set_data[i * 10 + 2] = (double)Read_Flash(i * 40 + 8); + set_data[i * 10 + 3] = (double)Read_Flash(i * 40 + 12); + set_data[i * 10 + 4] = (double)Read_Flash(i * 40 + 16); + set_data[i * 10 + 5] = (double)Read_Flash(i * 40 + 20); + } + } + else + { + for(int i=0;i<6;i++) + { + set_data[(i+1)*10+0]=Speed_Pgain[i]; + set_data[(i+1)*10+1]=Speed_Igain[i]; + set_data[(i+1)*10+2]=position_Pgain[i]; + set_data[(i+1)*10+3]=Position_input_filter[i]; + set_data[(i+1)*10+4]=Speed_I_input_filter[i]; + set_data[(i+1)*10+5]=offset[i]; + } + } + + limit_init(); + spi_init(); + motor_init(); + Serial_Init(); + +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef motor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,173 @@ +#ifndef _MOTOR_H_ +#define _MOTOR_H_ + +#include "FastPWM.h" + + + +FastPWM pwm1(MOTOR_PWM1, -1); +FastPWM pwm2(MOTOR_PWM2, -1); +FastPWM pwm3(MOTOR_PWM3, -1); +FastPWM pwm4(MOTOR_PWM4, -1); +FastPWM pwm5(MOTOR_PWM5, -1); +FastPWM pwm6(MOTOR_PWM6, -1); +FastPWM pwm7(MOTOR_PWM7, -1); + +void motor_init() +{ + pwm1.period_us(2500); + pwm2.period_us(2500); + pwm3.period_us(2500); + pwm4.period_us(2500); + pwm5.period_us(2500); + pwm6.period_us(2500); + pwm7.period_us(2500); + + pwm7.write(0.2); +} + +int ex_encoder[6]={0,0,0,0,0,0}; +int now_encoder[6]={0,0,0,0,0,0}; +int stop_cnt[6]={0,0,0,0,0,0}; + +double now_motor_duty[6]={0,0,0,0,0,0}; +double after_motor_duty[6]={0,0,0,0,0,0}; +double motor_currnt_cut=1.0; +double sum_duty_123axis=0; +double sum_duty_456axis=0; +double total_duty=0;; +int duty_limit=1000; + +int duty_limit_cnt=0; +bool duty_limit_on=false; + +int error_check_boost_duty[6]={0,}; + +void motor_power(int motor_num,double percent) +{ + + percent=-percent; + double output=offset[motor_num]; + if(percent<-500) + percent=-500; + if(percent>500) + percent=500; + + + if(percent>150 || percent<-150) + { + now_encoder[motor_num]=encoder_data[motor_num]; + + if(ex_encoder[motor_num]==now_encoder[motor_num]) + stop_cnt[motor_num]++; + else + stop_cnt[motor_num]=0; + + ex_encoder[motor_num]=now_encoder[motor_num]; + + if(stop_cnt[motor_num]>300) + { + if(abs(error_check_boost_duty[motor_num])>500) + motor_onoff[motor_num]=false; + else + { + if(percent<0) + error_check_boost_duty[motor_num]--; + else + error_check_boost_duty[motor_num]++; + + percent=percent+error_check_boost_duty[motor_num]; + } + + } + }else + { + stop_cnt[motor_num]=0; + + if(error_check_boost_duty[motor_num]<0) + error_check_boost_duty[motor_num]++; + else if(error_check_boost_duty[motor_num]>0) + error_check_boost_duty[motor_num]--; + + } + + + + now_motor_duty[motor_num]=abs(percent); + + sum_duty_123axis = now_motor_duty[0]+now_motor_duty[1]+now_motor_duty[2]+now_motor_duty[3]; + sum_duty_456axis = now_motor_duty[4]+now_motor_duty[5]; + + total_duty=sum_duty_123axis; + + + if(total_duty>duty_limit) + motor_currnt_cut=(duty_limit/total_duty); + else + motor_currnt_cut=1.0; + + if(duty_limit_on) + { + if( (motor_num==1) || (motor_num==2) || (motor_num==3) || (motor_num==4) ) + percent=percent*motor_currnt_cut; + + duty_limit_cnt++; + if(duty_limit_cnt>200) + { + duty_limit_cnt=0; + pc.printf("%0.1f,%4.0f,%4.0f,%4.0f,%4.0f\r\n",motor_currnt_cut,now_motor_duty[0],now_motor_duty[1],now_motor_duty[2],now_motor_duty[3]); + } + } + + if(motor_num==0) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm1.write(output); + } + else if(motor_num==1) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm2.write(output); + } + else if(motor_num==2) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm3.write(output); + } + else if(motor_num==3) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm4.write(output); + } + else if(motor_num==4) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm5.write(output); + } + else if(motor_num==5) + { + if(motor_onoff[motor_num]==false) + percent=1000; + + output = 1-(offset[motor_num] + percent)/2500; + pwm6.write(output); + } +} + + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef noustf.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/noustf.txt Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,110 @@ +/* +void initial_position() +{ + cmd_roll = 32768; + cmd_pitch = 32768; + cmd_heave = 32768; + cmd_sway = 32768; + cmd_surge = 32768; + cmd_yaw = 32768; + + double base_position[6]= {0,0,0,0,0,0}; + double motor_duty=200; + + base_position[0]=-(origin_degree1/(1.5*90))*1024*100; + base_position[1]=-(origin_degree2/(1.5*90))*1024*100; + base_position[2]=-(origin_degree3/(1.5*90))*1024*100; + base_position[3]=(origin_degree4/(1.5*90))*1024*100; + base_position[4]=-(origin_degree5/(1.5*90))*1024*100; + base_position[5]=-(origin_degree6/(1.5*90))*1024*100; + + + bool base_position_check[6]= {false,false,false,false,false,false}; + + int b_cnt=0; + for(int i=0; i<4001; i++) { + encoder_read(); + + b_cnt++; + if(b_cnt>499) { + b_cnt=0; + pc.printf("Base_positioning : %d \r\n",i+1); + + for(int k=0; k<6; k++) { + pc.printf("%8.0f", base_position[k]); + } + pc.printf("\r\n"); + for(int k=0; k<6; k++) { + pc.printf("%8d", encoder_data[k]); + } + pc.printf("\r\n"); + } + + + + for(int u=0; u<3; u++) { + if(encoder_data[u]<base_position[u]) { + motor_power(u, 0); + base_position_check[u]=true; + } else if(encoder_data[u]<base_position[u]*0.9) + motor_power(u, motor_duty*0.5); + else if(encoder_data[u]<base_position[u]*0.8) + motor_power(u, motor_duty*0.7); + else if(encoder_data[u]<base_position[u]*0.7) + motor_power(u, motor_duty*0.9); + else + motor_power(u, motor_duty); + } + + for(int u=4; u<6; u++) { + if(encoder_data[u]<base_position[u]) { + motor_power(u, 0); + base_position_check[u]=true; + } else if(encoder_data[u]<base_position[u]*0.9) + motor_power(u, -motor_duty*0.5); + else if(encoder_data[u]<base_position[u]*0.8) + motor_power(u, -motor_duty*0.7); + else if(encoder_data[u]<base_position[u]*0.7) + motor_power(u, -motor_duty*0.9); + else + motor_power(u, -motor_duty); + } + + for(int u=3; u<4; u++) { + if(encoder_data[u]>base_position[u]) { + motor_power(u, 0); + base_position_check[u]=true; + } else if(encoder_data[u]>base_position[u]*0.9) + motor_power(u, motor_duty*0.5); + else if(encoder_data[u]>base_position[u]*0.8) + motor_power(u, motor_duty*0.7); + else if(encoder_data[u]>base_position[u]*0.7) + motor_power(u, motor_duty*0.9); + else + motor_power(u, motor_duty); + } + + } + + motor_power(0, 0); + motor_power(1, 0); + motor_power(2, 0); + motor_power(3, 0); + motor_power(4, 0); + motor_power(5, 0); +} +*/ + +/* + m_cnt++; + if(m_cnt>3000) + { + m_cnt=0; + 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"); + } + */ \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef pc_serial.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pc_serial.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,341 @@ +#ifndef _PC_SERIAL_H_ +#define _PC_SERIAL_H_ + +#define debug1 0 +#define debug2 0 +#define Start_com 1 + +Serial pc(USBTX, USBRX); + +char check_pitch[6]; +char rx_buffer[100]={0,}; +char command[100]={0,}; +char tx_buffer[100]="txdat32768,32768,32768,32768,32768,32768\n"; +int tx_index = 0; +int rx_index = 0; +bool rx_flag = false; + +long data_recive=0; + +long bool_cnt=0; + +double set_data[100] = {0x00, }; + +unsigned int cmd_roll, cmd_pitch, cmd_heave, cmd_sway, cmd_surge, cmd_yaw; + +void Start_Command(); +void Set_Arrangement(); + +bool data_error_check(); +void char_save_arr(char c); +//void Rx_Interrupt(); +void READ_and_SAVE(); +void Serial_Init(); +void Rx_Buffer_Clear(); + + + +void comunication() +{ + + + for(int i=0;i<6;i++) + { + Speed_Pgain[i]=set_data[(i+1)*10+0]; + Speed_Igain[i]=set_data[(i+1)*10+1]; + position_Pgain[i]=set_data[(i+1)*10+2]; + Position_input_filter[i]=set_data[(i+1)*10+3]; + Speed_I_input_filter[i]=set_data[(i+1)*10+4]; + offset[i]=set_data[(i+1)*10+5]; + } + + + READ_and_SAVE(); + //pc.printf("%s,",command); + Set_Arrangement(); + Start_Command(); + +} + +void Start_Command() +{ + bool com_error=false; + + unsigned int temp=0; + + unsigned int data1=32768; + unsigned int data2=32768; + unsigned int data3=32768; + unsigned int data4=32768; + unsigned int data5=32768; + unsigned int data6=32768; + + com_error=data_error_check(); + + if(com_error==false) + if((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't')) + { + + int num=0; + for(int i=5; i<100;i++) + { + + if(command[i]==',') + { + if(num==0) + data1=temp; + if(num==1) + data2=temp; + if(num==2) + data3=temp; + if(num==3) + data4=temp; + if(num==4) + data5=temp; + + temp=0; + num++; + } + else if(command[i]=='\n') + { + if(num==5) + data6=temp; + }else + temp=temp*10+command[i]-0x30; + } + + cmd_roll = data1; + cmd_pitch = data2; + cmd_heave = data3; + cmd_sway = data4; + cmd_surge = data5; + cmd_yaw = data6; + + /* + pc.printf("%d,",cmd_roll); + pc.printf("%d,",cmd_pitch); + pc.printf("%d,",cmd_heave); + pc.printf("%d,",cmd_sway); + pc.printf("%d,",cmd_surge); + pc.printf("%d,",cmd_yaw); + pc.printf("\n"); + */ + } + +} + +bool data_error_check() // ,아스키코드 44 숫자 아스키코드 0x30, 0x39 +{ + bool error=false; + + + if(!((command[0] == 'S') && (command[1] == 't') && (command[2] == 'a') && (command[3] == 'r') && (command[4] == 't'))) + error=true; + + for(int i=5; i<41;i++) + { + if( command[i]>0x39 || command[i]<0x30 ) + { + if((command[i]=='\n')) + break; + + if(!(command[i]==0x2C)) + { + error=true; + break; + } + } + } + + for(int i=41;i<100;i++) + { + if(command[i]!=0) + error=true; + break; + } + + for(int i=0;i<17;i++) + { + if(command[i]==0) + error=true; + break; + } + + return error; +} + + +// Set arrangement function +void Set_Arrangement() +{ + bool set_error = false; + bool set_point = false; + int set_index = 0; + double set_num = 0; + double square_root = 0.1; + + if((command[0] == 'S') && (command[1] == 'E') && (command[2] == 'T') && (command[5] == '=')) + { + for(int i = 3; i < 5; i++) + { + if(command[i] < '0' || command[i] > '9') + { + set_error = true; + break; + } + } + if(set_error == false) + { + set_index = ((command[3] - 0x30) * 10) + (command[4] - 0x30); + + for(int i = 6; i < 100; i++) + { + // Error check + if(command[i] != '.' && (command[i] < '0' || command[i] > '9')) + { + if(command[i] != '\n') + { + set_error = true; + } + break; + } + + // Operation source + else if(command[i] == '.' && set_point == false) + { + set_point = true; + } + else if(set_point == false) + { + set_num = (set_num * 10) + (command[i] - 0x30); + } + else if(set_point == true) + { + set_num = set_num + ((command[i] - 0x30) * square_root); + square_root = square_root * 0.1; + } + } + } + if((set_error == false) && (set_data[set_index] != set_num)) + { + set_data[set_index] = set_num; + Erase_Flash(); + Write_Flash('F'); + Write_Flash('L'); + Write_Flash('A'); + Write_Flash('S'); + Write_Flash('H'); + for(int i = 0; i < 100; i++) + { + Write_Flash((uint32_t)set_data[i]); + } + pc.printf("SET%02d=%10.5f\n",set_index,set_data[set_index]); + } + } + +} + + +bool tx_done=false; +void READ_and_SAVE() +{ + + at_spi_cs = 1; + char c; + tx_done=false; + for(int i=0; i<64; i++) + { + wait_us(20); + c = spi1.write(255); + + if(c==255) + { + break; + } + else + { + //pc.printf("%d\r\n",c); + char_save_arr(c); + } + } + at_spi_cs = 0; + +} + + +void char_save_arr(char c) +{ + rx_buffer[rx_index]=c; + rx_index++; + if(rx_index>99) + rx_index=99; + if(c=='\n') + { + data_recive++; + for(int i =0; i<100;i++) + command[i]=rx_buffer[i]; + + Rx_Buffer_Clear(); + } +} + + + + +// Interrupt functions +/* +void Rx_Interrupt() +{ + if(pc.readable()) + { + char c = pc.getc(); + rx_buffer[rx_index] = c; + if(rx_buffer[rx_index] == '\n') + { + rx_flag = true; + } + rx_index++; + if(rx_index > 99) + { + rx_index = 99; + } + } + return; +} +*/ +// Basic serial communication functions + +void Serial_Init() +{ + pc.format(8, Serial::None, 1); + pc.baud(500000); + + //pc2.format(8, Serial::None, 1); + //pc2.baud(115200); + //pc.attach(&Rx_Interrupt, Serial::RxIrq); +} + + +void Rx_Buffer_Clear() +{ + for(int i = 0; i < 100; i++) + { + rx_buffer[i] = 0x00; + } + rx_index=0; +} + + +/* + if(com_error==false) + { + cmd_roll = data1; + cmd_pitch = data2; + cmd_heave = data3; + cmd_sway = data4; + cmd_surge = data5; + cmd_yaw = data6; + } + */ + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef speed_pid.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/speed_pid.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,59 @@ +#ifndef _SPEED_PID_H_ +#define _SPEED_PID_H_ + +double taget_speed[6]={0,}; +double error_speed[6]={0,}; +double Speed_PID_OUTPUT[7]={0,}; + +void cal_speed_error() +{ + for(int i=0;i<6;i++) + error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i]; + +} + +double result_i[6]={0,}; +double filter_result_i[6]={0,}; +double output_i[6]={0,}; + +void Speed_I() +{ + for(int i=0;i<6;i++) + { + if(filter_dif_encoder_data[i]<taget_speed[i]) + result_i[i]+=1*Speed_Igain[i]; + else if(filter_dif_encoder_data[i]>taget_speed[i]) + result_i[i]-=1*Speed_Igain[i]; + + filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i]; + output_i[i] = - filter_result_i[i]; + } +} +double output_p[6]={0,}; +void Speed_P() +{ + for(int i=0;i<6;i++) + output_p[i] = error_speed[i] * Speed_Pgain[i]; +} + +void Speed_PID() +{ + cal_speed_error(); + Speed_I(); + Speed_P(); + + for(int i=0;i<6;i++) + { + Speed_PID_OUTPUT[i] = output_i[i] + output_p[i]; + if(i > 2) + { + Speed_PID_OUTPUT[i] *= -1; + } + } + + //pc.printf("%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f",error_speed[0],filter_dif_encoder_data[0],taget_speed[0],Speed_PID_OUTPUT[0],output_i[0],output_p[0]); + //pc.printf("\n"); + +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef spi_setup.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/spi_setup.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,32 @@ +#ifndef _SPI_SETUP_H_ +#define _SPI_SETUP_H_ + +#include "LS7366.h" + +SPI spi1(SPI_MOSI, SPI_MISO, SPI_SCK); + +LS7366 encoder1(spi1, ENCODER_CS1); +LS7366 encoder2(spi1, ENCODER_CS2); +LS7366 encoder3(spi1, ENCODER_CS3); +LS7366 encoder4(spi1, ENCODER_CS4); +LS7366 encoder5(spi1, ENCODER_CS5); +LS7366 encoder6(spi1, ENCODER_CS6); +DigitalOut at_spi_cs(PB_4); +DigitalOut st16_spi_cs(PB_5); + +DigitalOut seg_cs(PA_0); +DigitalOut btn1_cs(PA_11); +DigitalOut btn2_cs(PA_12); + + + +void spi_init() +{ + spi1.format(8,0); + spi1.frequency(2000000); + seg_cs = 0; + btn1_cs = 0; + btn2_cs = 0; +} + +#endif \ No newline at end of file
diff -r 000000000000 -r e12eb40b9fef target_position_cal.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/target_position_cal.h Thu Apr 23 00:38:16 2020 +0000 @@ -0,0 +1,82 @@ +#ifndef _TARGET_POSITION_CAL_H_ +#define _TARGET_POSITION_CAL_H_ + +double first_degree, second_degree, third_degree, four_degree, five_degree, six_degree; + +double motor1_degree=0; +double motor2_degree=0; +double motor3_degree=0; +double motor4_degree=0; +double motor5_degree=0; +double motor6_degree=0; + +#define roll_gain 15 // roll 게인 ( 부호를 바꾸면 방향이 바뀐다.) +#define pitch_gain 15 // pitch 게인 ( 부호를 바꾸면 방향이 바뀐다.) +#define heave_gain 15 // heave_gain 게인 ( 부호를 바꾸면 방향이 바뀐다.) + +double origin_degree1=50; // 모터1 기준위치 지정 +double origin_degree2=50; // 모터2 기준위치 지정 +double origin_degree3=50; // 모터3 기준위치 지정 +double origin_degree4=76; // 모터2 기준위치 지정 +double origin_degree5=50; // 모터3 기준위치 지정 +double origin_degree6=50; // 모터3 기준위치 지정 +double cal_scale=0.7; // 전체 게인 조절 + +double cal_roll=0; +double cal_pitch=0; +double cal_heave=0; +double cal_surge=0; +double cal_yaw=0; +double cal_sway=0; +double cal_senter=0; + +double pitch_motor1, pitch_motor2, pitch_motor3; +double roll_motor1, roll_motor2, roll_motor3; +double heave_motor1, heave_motor2, heave_motor3; + + +void taget_position_cal(double r,double p, double h,double sw,double su,double y) // before, cal(roll_gain*dou_roll,pitch_gain*dou_pitch,heave_gain*dou_heave,0,gain); / add, surge, yaw, sway +{ + + r = ((r - 32768) / 1000) * 0.2746582; + p = ((p - 32768) / 1000) * 0.2746582; + h = ((h - 32768) / 1000) * 0.2746582; + sw = ((sw - 32768) / 1000) * 0.2746582; + su = ((su - 32768) / 1000) * 0.2746582; + y = ((y - 32768) / 1000) * 0.2746582; + + + r=roll_gain*r; + p=pitch_gain*p; + h=heave_gain*h; + + pitch_motor1 = cal_scale * (-p); + pitch_motor2 = cal_scale * (p); + pitch_motor3 = cal_scale * (p); + + roll_motor1 = 0; + roll_motor2 = (cal_scale * r); + roll_motor3 = (cal_scale * -r); + + heave_motor1 = (cal_scale * h); + heave_motor2 = (cal_scale * h); + heave_motor3 = (cal_scale * h); + + motor1_degree = cal_senter + pitch_motor1 + roll_motor1 + heave_motor1; + motor2_degree = cal_senter + pitch_motor2 + roll_motor2 + heave_motor2; + motor3_degree = cal_senter + pitch_motor3 + roll_motor3 + heave_motor3; + + motor4_degree = 18.66 * sw; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) + motor5_degree = 9.33 * su-9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) + motor6_degree = 9.33 * su+9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) + + first_degree = motor1_degree+origin_degree1; + second_degree = motor2_degree+origin_degree2; + third_degree = motor3_degree+origin_degree3; + four_degree = motor4_degree+origin_degree4; + five_degree = motor5_degree+origin_degree5; + six_degree = motor6_degree+origin_degree6; + +} + +#endif \ No newline at end of file