00001 /* 00002 * ServoController.cc 00003 * 00004 * Created on: Jul 10, 2009 00005 * Author: mrshah 00006 */ 00007 00008 #include "ServoController.h" 00009 #include <iostream> 00010 #include <time.h> 00011 #include <cstdlib> 00012 #include <jsw.h> 00013 00014 namespace uvsim 00015 { 00016 00017 ServoController::ServoController(float dSmooth, float windupGaurd, float maxCmd, float minCmd) 00018 { 00019 m_dSmooth = dSmooth; 00020 m_windupGaurd = windupGaurd; 00021 m_cmdM = 180 / (maxCmd - minCmd); 00022 m_cmdB = 90; 00023 } 00024 00025 void ServoController::initialize() 00026 { 00027 m_errP = 0.; 00028 m_errI = 0.; 00029 m_errD = 0.; 00030 m_errPPrev = 0.; 00031 gettimeofday(&m_timePrev , NULL); 00032 } 00033 00034 void ServoController::update(float cmd, float feedback, float kP, float kI, float kD) 00035 { 00036 // Time 00037 gettimeofday(&m_timePres , NULL); 00038 m_dt = (m_timePres.tv_sec - m_timePrev.tv_sec) * 1000.0; 00039 // us to ms 00040 m_dt += (m_timePres.tv_usec - m_timePrev.tv_usec) / 1000.0; 00041 std::cout << "dt: " << m_dt << " ms" << std::endl; 00042 00043 // Update P,I, and D of error 00044 m_errP = cmd - feedback; 00045 m_errI = m_errI + m_errP * m_dt; 00046 m_errD = (m_dSmooth * m_errD) + (1.0 - m_dSmooth) * (m_errP - m_errPPrev) / m_dt; 00047 std::cout << "errD: " << m_errD << std::endl; 00048 00049 // Transmission Loss 00050 if (m_dt > 1000) 00051 { 00052 m_errD = 0; 00053 m_errI = 0; 00054 std::cout << "No data transfer!!" << std::endl; 00055 } 00056 00057 // Prevent Windup 00058 if (m_errI > m_windupGaurd) 00059 m_errI = m_windupGaurd; 00060 if (m_errI < m_windupGaurd) 00061 m_errI = m_windupGaurd; 00062 00063 // Output 00064 m_out = -(m_errP * kP + m_errI * kI + m_errD * kI) * m_cmdM + m_cmdB; 00065 std::cout<< "orig. output: " << m_out<<std::endl; 00066 if (m_out > 180) 00067 m_out = 180; 00068 if (m_out < 0) 00069 m_out = 0; 00070 std::cout<< m_out<<std::endl; 00071 00072 // Update previous state 00073 m_errPPrev = m_errP; 00074 m_timePrev = m_timePres; 00075 } 00076 00077 }
1.6.1