lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.cpp@1:7b5469bf5994, 2020-07-27 (annotated)

Committer:
gohgwaja
Date:
Mon Jul 27 23:29:30 2020 +0000
Revision:
1:7b5469bf5994
Parent:
0:7cff999a7f5c
Child:
2:14b52dee1c35
2222

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 1:7b5469bf5994 8 void Menual_Position_PID();
gohgwaja 0:7cff999a7f5c 9 void test();
gohgwaja 0:7cff999a7f5c 10
gohgwaja 0:7cff999a7f5c 11 int cnt=0;
gohgwaja 0:7cff999a7f5c 12
gohgwaja 0:7cff999a7f5c 13 int m_cnt=0;
gohgwaja 0:7cff999a7f5c 14 int menual=false;
gohgwaja 1:7b5469bf5994 15
gohgwaja 0:7cff999a7f5c 16 int main()
gohgwaja 0:7cff999a7f5c 17 {
gohgwaja 0:7cff999a7f5c 18
gohgwaja 0:7cff999a7f5c 19 board_init();
gohgwaja 0:7cff999a7f5c 20 MCP23S17_Init();
gohgwaja 0:7cff999a7f5c 21 Segment_Init();
gohgwaja 0:7cff999a7f5c 22 Button_Init();
gohgwaja 0:7cff999a7f5c 23
gohgwaja 0:7cff999a7f5c 24 printf("READ Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
gohgwaja 0:7cff999a7f5c 25
gohgwaja 0:7cff999a7f5c 26
gohgwaja 0:7cff999a7f5c 27 encoder_check2();
gohgwaja 0:7cff999a7f5c 28
gohgwaja 0:7cff999a7f5c 29 find_limit();
gohgwaja 0:7cff999a7f5c 30
gohgwaja 0:7cff999a7f5c 31 command_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 1:7b5469bf5994 37 while(1)
gohgwaja 1:7b5469bf5994 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 }
gohgwaja 0:7cff999a7f5c 46
gohgwaja 0:7cff999a7f5c 47 Button_Detection();
gohgwaja 0:7cff999a7f5c 48
gohgwaja 0:7cff999a7f5c 49 comunication(); // 모터의 위치 값을 받음
gohgwaja 0:7cff999a7f5c 50 taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw);
gohgwaja 0:7cff999a7f5c 51 encoder_read();
gohgwaja 0:7cff999a7f5c 52
gohgwaja 1:7b5469bf5994 53 // if(Idle_Check() == false)
gohgwaja 1:7b5469bf5994 54 // {
gohgwaja 1:7b5469bf5994 55 // if(idle_state == true)
gohgwaja 1:7b5469bf5994 56 // {
gohgwaja 1:7b5469bf5994 57 // idle_state = false;
gohgwaja 1:7b5469bf5994 58 // }
gohgwaja 1:7b5469bf5994 59
gohgwaja 1:7b5469bf5994 60
gohgwaja 1:7b5469bf5994 61
gohgwaja 1:7b5469bf5994 62
gohgwaja 1:7b5469bf5994 63 if(!menual_mode)
gohgwaja 1:7b5469bf5994 64 Position_PID();
gohgwaja 1:7b5469bf5994 65 else
gohgwaja 1:7b5469bf5994 66 Menual_Position_PID();
gohgwaja 1:7b5469bf5994 67
gohgwaja 1:7b5469bf5994 68 Speed_PID();
gohgwaja 1:7b5469bf5994 69
gohgwaja 1:7b5469bf5994 70 for(int i=0; i<6; i++)
gohgwaja 1:7b5469bf5994 71 motor_power(i,Speed_PID_OUTPUT[i]);
gohgwaja 1:7b5469bf5994 72
gohgwaja 1:7b5469bf5994 73
gohgwaja 1:7b5469bf5994 74
gohgwaja 1:7b5469bf5994 75
gohgwaja 1:7b5469bf5994 76 // }
gohgwaja 1:7b5469bf5994 77 // else
gohgwaja 1:7b5469bf5994 78 // {
gohgwaja 1:7b5469bf5994 79 // if(menual_mode)
gohgwaja 1:7b5469bf5994 80 // {
gohgwaja 1:7b5469bf5994 81 // Menual_Position_PID();
gohgwaja 1:7b5469bf5994 82 // for(int i=0; i<6; i++)
gohgwaja 1:7b5469bf5994 83 // motor_power(i,Speed_PID_OUTPUT[i]);
gohgwaja 1:7b5469bf5994 84 // }
gohgwaja 1:7b5469bf5994 85 // else
gohgwaja 1:7b5469bf5994 86 // {
gohgwaja 1:7b5469bf5994 87 // if(idle_state == false)
gohgwaja 1:7b5469bf5994 88 // {
gohgwaja 1:7b5469bf5994 89 // Idle_Enc_Check();
gohgwaja 1:7b5469bf5994 90 // idle_state = true;
gohgwaja 1:7b5469bf5994 91 // pc.printf("idle state..\r\n");
gohgwaja 1:7b5469bf5994 92 // }
gohgwaja 1:7b5469bf5994 93 // for(int i = 0; i < 6; i++)
gohgwaja 1:7b5469bf5994 94 // motor_power(i, 0);
gohgwaja 1:7b5469bf5994 95 // }
gohgwaja 1:7b5469bf5994 96 // }
gohgwaja 0:7cff999a7f5c 97
gohgwaja 0:7cff999a7f5c 98 }
gohgwaja 0:7cff999a7f5c 99 }
gohgwaja 0:7cff999a7f5c 100
gohgwaja 0:7cff999a7f5c 101 void command_init()
gohgwaja 0:7cff999a7f5c 102 {
gohgwaja 0:7cff999a7f5c 103 cmd_roll = 32768;
gohgwaja 0:7cff999a7f5c 104 cmd_pitch = 32768;
gohgwaja 0:7cff999a7f5c 105 cmd_heave = 32768;
gohgwaja 0:7cff999a7f5c 106 cmd_sway = 32768;
gohgwaja 0:7cff999a7f5c 107 cmd_surge = 32768;
gohgwaja 0:7cff999a7f5c 108 cmd_yaw = 32768;
gohgwaja 0:7cff999a7f5c 109 }
gohgwaja 0:7cff999a7f5c 110
gohgwaja 0:7cff999a7f5c 111
gohgwaja 0:7cff999a7f5c 112 void pid_info()
gohgwaja 0:7cff999a7f5c 113 {
gohgwaja 0:7cff999a7f5c 114 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
gohgwaja 0:7cff999a7f5c 115 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 116 {
gohgwaja 0:7cff999a7f5c 117 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 118 }
gohgwaja 0:7cff999a7f5c 119 pc.printf("\n\n\n");
gohgwaja 0:7cff999a7f5c 120
gohgwaja 0:7cff999a7f5c 121 }
gohgwaja 0:7cff999a7f5c 122
gohgwaja 0:7cff999a7f5c 123 void Menual_Position_PID()
gohgwaja 0:7cff999a7f5c 124 {
gohgwaja 0:7cff999a7f5c 125 for(int i=0; i<6; i++)
gohgwaja 0:7cff999a7f5c 126 {
gohgwaja 0:7cff999a7f5c 127 taget_speed[i]=button_offset_posion[i]/5;
gohgwaja 0:7cff999a7f5c 128 }
gohgwaja 0:7cff999a7f5c 129 }
gohgwaja 0:7cff999a7f5c 130
gohgwaja 0:7cff999a7f5c 131