lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.cpp@0:e12eb40b9fef, 2020-04-23 (annotated)

Committer:
lsh2205
Date:
Thu Apr 23 00:38:16 2020 +0000
Revision:
0:e12eb40b9fef
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:e12eb40b9fef 1 #include "mbed.h"
lsh2205 0:e12eb40b9fef 2 #include "main.h"
lsh2205 0:e12eb40b9fef 3
lsh2205 0:e12eb40b9fef 4 void initial_position();
lsh2205 0:e12eb40b9fef 5 void begin_pid();
lsh2205 0:e12eb40b9fef 6 void command_init();
lsh2205 0:e12eb40b9fef 7 void pid_info();
lsh2205 0:e12eb40b9fef 8 void test();
lsh2205 0:e12eb40b9fef 9
lsh2205 0:e12eb40b9fef 10 int cnt=0;
lsh2205 0:e12eb40b9fef 11
lsh2205 0:e12eb40b9fef 12 int m_cnt=0;
lsh2205 0:e12eb40b9fef 13 int menual=false;
lsh2205 0:e12eb40b9fef 14 int main()
lsh2205 0:e12eb40b9fef 15 {
lsh2205 0:e12eb40b9fef 16
lsh2205 0:e12eb40b9fef 17 board_init();
lsh2205 0:e12eb40b9fef 18 MCP23S17_Init();
lsh2205 0:e12eb40b9fef 19 Segment_Init();
lsh2205 0:e12eb40b9fef 20 Button_Init();
lsh2205 0:e12eb40b9fef 21
lsh2205 0:e12eb40b9fef 22 printf("READ Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
lsh2205 0:e12eb40b9fef 23
lsh2205 0:e12eb40b9fef 24
lsh2205 0:e12eb40b9fef 25 encoder_check2();
lsh2205 0:e12eb40b9fef 26
lsh2205 0:e12eb40b9fef 27 find_limit();
lsh2205 0:e12eb40b9fef 28
lsh2205 0:e12eb40b9fef 29 command_init();
lsh2205 0:e12eb40b9fef 30
lsh2205 0:e12eb40b9fef 31 Button_Init();
lsh2205 0:e12eb40b9fef 32
lsh2205 0:e12eb40b9fef 33 pc.printf("PID LOOP_START\r\n");
lsh2205 0:e12eb40b9fef 34 pid_info();
lsh2205 0:e12eb40b9fef 35
lsh2205 0:e12eb40b9fef 36
lsh2205 0:e12eb40b9fef 37 while(1) {
lsh2205 0:e12eb40b9fef 38
lsh2205 0:e12eb40b9fef 39 reset_check();
lsh2205 0:e12eb40b9fef 40
lsh2205 0:e12eb40b9fef 41 m_cnt++;
lsh2205 0:e12eb40b9fef 42 if(m_cnt>1000)
lsh2205 0:e12eb40b9fef 43 {
lsh2205 0:e12eb40b9fef 44 m_cnt=0;
lsh2205 0:e12eb40b9fef 45 //pid_info();
lsh2205 0:e12eb40b9fef 46 }
lsh2205 0:e12eb40b9fef 47
lsh2205 0:e12eb40b9fef 48 Button_Detection();
lsh2205 0:e12eb40b9fef 49
lsh2205 0:e12eb40b9fef 50 comunication(); // 모터의 위치 값을 받음
lsh2205 0:e12eb40b9fef 51
lsh2205 0:e12eb40b9fef 52 taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw);
lsh2205 0:e12eb40b9fef 53 encoder_read();
lsh2205 0:e12eb40b9fef 54
lsh2205 0:e12eb40b9fef 55 Position_PID();
lsh2205 0:e12eb40b9fef 56 Speed_PID();
lsh2205 0:e12eb40b9fef 57
lsh2205 0:e12eb40b9fef 58 for(int i=0; i<6; i++)
lsh2205 0:e12eb40b9fef 59 motor_power(i,Speed_PID_OUTPUT[i]);
lsh2205 0:e12eb40b9fef 60
lsh2205 0:e12eb40b9fef 61 }
lsh2205 0:e12eb40b9fef 62 }
lsh2205 0:e12eb40b9fef 63
lsh2205 0:e12eb40b9fef 64
lsh2205 0:e12eb40b9fef 65
lsh2205 0:e12eb40b9fef 66 void command_init()
lsh2205 0:e12eb40b9fef 67 {
lsh2205 0:e12eb40b9fef 68 cmd_roll = 32768;
lsh2205 0:e12eb40b9fef 69 cmd_pitch = 32768;
lsh2205 0:e12eb40b9fef 70 cmd_heave = 32768;
lsh2205 0:e12eb40b9fef 71 cmd_sway = 32768;
lsh2205 0:e12eb40b9fef 72 cmd_surge = 32768;
lsh2205 0:e12eb40b9fef 73 cmd_yaw = 32768;
lsh2205 0:e12eb40b9fef 74 }
lsh2205 0:e12eb40b9fef 75
lsh2205 0:e12eb40b9fef 76
lsh2205 0:e12eb40b9fef 77 void pid_info()
lsh2205 0:e12eb40b9fef 78 {
lsh2205 0:e12eb40b9fef 79 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
lsh2205 0:e12eb40b9fef 80 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 81 {
lsh2205 0:e12eb40b9fef 82 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]);
lsh2205 0:e12eb40b9fef 83 }
lsh2205 0:e12eb40b9fef 84 pc.printf("\n\n\n");
lsh2205 0:e12eb40b9fef 85
lsh2205 0:e12eb40b9fef 86 }
lsh2205 0:e12eb40b9fef 87
lsh2205 0:e12eb40b9fef 88 /*
lsh2205 0:e12eb40b9fef 89 Position_PID();
lsh2205 0:e12eb40b9fef 90 if(menual_mode && (menual == false))
lsh2205 0:e12eb40b9fef 91 {
lsh2205 0:e12eb40b9fef 92 menual=true;
lsh2205 0:e12eb40b9fef 93 reset_speed_pid();
lsh2205 0:e12eb40b9fef 94 }
lsh2205 0:e12eb40b9fef 95
lsh2205 0:e12eb40b9fef 96 if(!menual)
lsh2205 0:e12eb40b9fef 97 Position_PID();
lsh2205 0:e12eb40b9fef 98 else
lsh2205 0:e12eb40b9fef 99 {
lsh2205 0:e12eb40b9fef 100 for(int i=0; i<6; i++)
lsh2205 0:e12eb40b9fef 101 {
lsh2205 0:e12eb40b9fef 102 taget_speed[i]=button_offset_posion[i]/5;
lsh2205 0:e12eb40b9fef 103 }
lsh2205 0:e12eb40b9fef 104 }
lsh2205 0:e12eb40b9fef 105
lsh2205 0:e12eb40b9fef 106 Speed_PID();
lsh2205 0:e12eb40b9fef 107 */
lsh2205 0:e12eb40b9fef 108
lsh2205 0:e12eb40b9fef 109