test
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
main.cpp
- Committer:
- lsh2205
- Date:
- 2020-04-23
- Revision:
- 0:e12eb40b9fef
File content as of revision 0:e12eb40b9fef:
#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(); */