lee sanghun
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