lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.cpp@0:7cff999a7f5c, 2020-05-11 (annotated)

Committer:
gohgwaja
Date:
Mon May 11 08:47:18 2020 +0000
Revision:
0:7cff999a7f5c
Child:
1:7b5469bf5994
hahaha

Who changed what in which revision?

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