Controller.cpp

00001 #include "Controller.h"
00002 #include <chrono>
00003 
00004 using namespace std::chrono;
00005 
00006 #define  pi 3.14159265358979323846
00007 
00008 Controller::Controller(float Ts) : user_button(PC_13), encoderCounter(PB_6, PB_7),
00009     u_accx(PA_0), u_accy(PA_1), u_gyro(PA_4), u_i(PA_5),
00010     bufferedSerial(USBTX, USBRX), thread(osPriorityHigh, 4096)
00011 {
00012     this->Ts = Ts;
00013 
00014     executeMainTask = false;
00015     
00016     user_button.fall(callback(this, &Controller::button_fall));
00017     user_button.rise(callback(this, &Controller::button_rise));
00018 
00019     /* setup LinearCharacteristics objects for calibration */
00020     /* measurements for modell D
00021         - accx: +g: 0.70 V, -g: 0.31 V
00022         - accy: +g: 0.70 V, -g: 0.31 V
00023         - gyro: gain: -4.6517f*3.3f, offset: 0.4542 V  */
00024     u2accx.setup(0.7f, 0.31f, 9.81f, -9.81f);
00025     u2accy.setup(0.7f, 0.31f, 9.81f, -9.81f);
00026     u2gyro.setup(-4.6517f*3.3f, 0.4542f);
00027     /* output maps -15...15 -> 0...1 */
00028     i2u.setup(-15.0f, 15.0f, 0.0f, 1.0f);
00029 
00030     /* setup variables */
00031     time = accx = accy = gyro = phi1 = phi2 = dphi2 = M = 0.0f;
00032 
00033     /* setup all IIR_filter objects */
00034     float tau = 1.0f;
00035     Gacc .setup(tau, Ts, 1.0f);
00036     Ggyro.setup(tau, Ts, tau);
00037     Gdiff.setup(0.01f, Ts);
00038     Gf   .setup(0.02f, Ts, 1.0f);
00039 
00040     /* setup I-controller object */
00041     // Ki = -6.8523   -0.8210   -0.0167   -0.0158
00042     Ci.setup(-0.0158f, Ts, -0.217f*15.0f, 0.217f*15.0f);
00043     
00044     bufferedSerial.set_baud(2000000);
00045     bufferedSerial.set_blocking(false);
00046     
00047     /* write_counter_write_val = 5 means every 5th measurement is send via serial */
00048     write_counter_write_val = 5;
00049 
00050     reset();
00051     u_i.write( i2u( 0.0f ) );
00052 }
00053 
00054 Controller::~Controller()
00055 {
00056     ticker.detach();
00057 }
00058 
00059 void Controller::reset()
00060 {
00061     loop_timer.reset();
00062     loop_timer.start();
00063     time_offset = 0.0f;
00064     isFirstTimeMeas = true;
00065     write_counter = write_counter_write_val - 1;
00066     count_past = encoderCounter.read();
00067     count = 0;
00068     Gacc .reset();
00069     Ggyro.reset();
00070     Gdiff.reset();
00071     Gf   .reset();
00072     Ci.reset();
00073 }
00074 
00075 void Controller::start_loop()
00076 {
00077     thread.start(callback(this, &Controller::loop));
00078     ticker.attach(callback(this, &Controller::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * Ts)});
00079 }
00080 
00081 void Controller::loop()
00082 {
00083     while(true) {
00084         ThisThread::flags_wait_any(threadFlag);
00085         mutex.lock();
00086         // ----------------------------------------------------------- LOOP START
00087 
00088         /* update time, only used as time reference */
00089         time = static_cast<float>( duration_cast<microseconds>(loop_timer.elapsed_time()).count() ) * 0.000001f;
00090         if(isFirstTimeMeas & executeMainTask) {
00091             time_offset = time;
00092             isFirstTimeMeas = false;
00093         }
00094         time = time - time_offset;
00095 
00096         /* read analog sensors, use LinearCharacteristics for calibration */
00097         accx = u2accx( u_accx.read() );
00098         accy = u2accy( u_accy.read() );
00099         gyro = u2gyro( u_gyro.read() );
00100 
00101         /* complementary filter, use atan2f and IIR_filter for filtering, do not forget the offset */
00102         phi1 = Gacc(atan2f(accx, accy)) + Ggyro(gyro) - pi/4.0f;
00103 
00104         /* read encoder, short prevents overflow */
00105         short count_act = encoderCounter.read();
00106         short delta_count = count_act - count_past;
00107         count_past = count_act;
00108         count += delta_count;
00109 
00110         /* calculate angle and angle velocity, use IIR_filter for filtering */
00111         phi2  = static_cast<float>( count )*2.0f*pi/25600.0f;
00112         dphi2 = Gdiff(phi2);
00113 
00114         if (executeMainTask) {
00115 
00116             /* controller, use PID_Cntrl for I-controller and IIR_filter for filtering */
00117             // Ki = -6.8523   -0.8210   -0.0167   -0.0158
00118             M = Ci(0.0f - dphi2) - (-6.8523*phi1 - 0.8210*Gf( gyro ) - 0.0167*dphi2);
00119             u_i.write( i2u( M/0.217f ) );
00120 
00121             /* send data via serial */
00122             if(++write_counter == write_counter_write_val) {
00123                 write_counter = 0;
00124                 /* write output via serial buffer */
00125                 int act_buffer_length = snprintf(buffer, 500,
00126                                                  "%3.3f, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e\r\n",
00127                                                  time, accx, accy, gyro, phi1, phi2, dphi2, M);
00128                 bufferedSerial.write(buffer, act_buffer_length);
00129             }
00130 
00131         } else {
00132             u_i.write( i2u( 0.0f ) );
00133         }
00134         
00135         // ----------------------------------------------------------- LOOP END
00136         mutex.unlock();
00137     }
00138 }
00139 
00140 void Controller::sendThreadFlag()
00141 {
00142     thread.flags_set(threadFlag);
00143 }
00144 
00145 void Controller::button_fall()
00146 {
00147     user_button_timer.reset();
00148     user_button_timer.start();
00149 }
00150 
00151 void Controller::button_rise()
00152 {
00153     int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
00154     user_button_timer.stop();
00155     if (t_button_ms > 200) {
00156         if(executeMainTask) {
00157             reset();
00158         }
00159         executeMainTask = !executeMainTask;
00160     }
00161 }