main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 //------------------------------------------
00004 #define PI 3.1415927f
00005 //------------------------------------------
00006 #include "EncoderCounter.h"
00007 #include "DiffCounter.h"
00008 #include "IIR_filter.h"
00009 #include "LinearCharacteristics.h"
00010 #include "PI_Cntrl.h"
00011 #include "GPA.h"
00012 /* Cuboid balance on one edge on Nucleo F446RE
00013 
00014  **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
00015                  settings for Maxon ESCON controller (upload via ESCON Studio) ****
00016 hardware Connections:
00017  
00018  CN7                    CN10
00019   :                         :
00020   :                         :
00021  ..                        ..
00022  ..                        ..                  15.
00023  ..    AOUT i_des on (PA_5)o.
00024  ..                        ..
00025  ..                        ..
00026  ..               ENC CH A o.
00027  o. GND                    ..                  10.
00028  o. ENC CH B               ..
00029  ..                        ..
00030  ..                        ..
00031  .o AIN acx (PA_0)         ..
00032  .o AIN acy (PA_1)         ..                  5.
00033  .o AIN Gyro(PA_4)         .o Analog GND
00034  ..                        ..
00035  ..                        ..
00036  ..                        ..                  1.
00037  ----------------------------
00038  CN7               CN10
00039  */
00040 Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
00041 InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
00042 AnalogIn ax(PA_0);                      // Analog IN (acc x) on PA_0
00043 AnalogIn ay(PA_1);                      // Analog IN (acc y) on PA_1
00044 AnalogIn gz(PA_4);                      // Analog IN (gyr z) on PA_4
00045 AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 2A (see ESCON)
00046 float out_value = 1.6f;                 // set voltage on 1.6 V (0 A current)
00047 float w_soll = 10.0f;                   // desired velocity
00048 float Ts = 0.002f;                      // sample time of main loops
00049 int k = 0;
00050 
00051 //------------------------------------------
00052 // ... here define variables like gains etc.
00053 //------------------------------------------
00054 //-------------DEFINE FILTERS----------------
00055 
00056 //------------------------------------------
00057 Ticker  ControllerLoopTimer;            // interrupt for control loop
00058 EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
00059 DiffCounter diff(0.01,Ts);              // discrete differentiate, based on encoder data
00060 //------------------------------------------
00061 // ... here define instantiate classes
00062 //------------------------------------------
00063 
00064 // ----- User defined functions -----------
00065 void updateControllers(void);   // speed controller loop (via interrupt)
00066 // ------ END User defined functions ------
00067 
00068 //******************************************************************************
00069 //---------- main loop -------------
00070 //******************************************************************************
00071 int main()
00072 {
00073     //attach controller loop to timer interrupt
00074     pc.baud(2000000);   // for serial comm.
00075     counter1.reset();   // encoder reset
00076     diff.reset(0.0f,0.0f);
00077     ControllerLoopTimer.attach(&updateControllers, Ts); //Assume Fs = ...;
00078         
00079     
00080 }
00081 //******************************************************************************
00082 //---------- control loop (called via interrupt) -------------
00083 //******************************************************************************
00084 void updateControllers(void){
00085     short counts = counter1;            // get counts from Encoder
00086     float vel = diff(counts);           // motor velocity 
00087 
00088     
00089         if(++k >= 249){
00090         k = 0;
00091         pc.printf("Some Output: %1.5f \r\n",PI);
00092     }
00093 }
00094 //******************************************************************************
00095 //********** User functions like buttens handle etc. **************
00096 
00097 //...