Process.cpp

00001 
00002 #include "mbed.h"
00003 #include "Process.h"
00004 #include "QEI.h"
00005 
00006 #include "../../CommonLibraries/PID/PID.h"
00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00008 #include "../../Communication/Controller/Controller.h"
00009 #include "../../Input/ExternalInt/ExternalInt.h"
00010 #include "../../Input/Switch/Switch.h"
00011 #include "../../Input/Potentiometer/Potentiometer.h"
00012 #include "../../Input/Encoder/Encoder.h"
00013 #include "../../LED/LED.h"
00014 #include "../../Safty/Safty.h"
00015 #include "../Using.h"
00016 
00017 using namespace SWITCH;
00018 using namespace PID_SPACE;
00019 using namespace ENCODER;
00020 
00021 static CONTROLLER::ControllerData *controller;
00022 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00023 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00024 
00025 static bool lock;
00026 static bool processChangeComp;
00027 static int current;
00028 
00029 static void AllActuatorReset();
00030 
00031 #ifdef USE_SUBPROCESS
00032 static void (*Process[USE_PROCESS_NUM])(void);
00033 #endif
00034 
00035 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00036 
00037 /*Replace here with the definition code of your variables.*/
00038 
00039 Serial pc(USBTX, USBRX);
00040 
00041 //**************Encoder***************
00042 const int PerRev = 256;
00043 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
00044 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
00045 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
00046 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
00047 //**************Encoder***************
00048 
00049 //**************Buzzer****************
00050 //DigitalOut buzzer(BUZZER_PIN);
00051 void BuzzerTimer_func();
00052 Ticker BuzzerTimer;
00053 bool EMGflag = false;
00054 PwmOut buzzer(BUZZER_PIN);
00055 //**************Buzzer****************
00056 
00057 //************TapeLed*****************
00058 void TapeLedEms_func();
00059 TapeLedData tapeLED;
00060 TapeLedData sendLedData;
00061 TapeLED_Mode ledMode = Normal;
00062 Ticker tapeLedTimer;
00063 //************TapaLed*****************
00064 
00065 float tireProRPM[4];
00066 float tireTarRPM[4];
00067 float tirePWM[4];
00068 
00069 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00070 
00071 #ifdef USE_SUBPROCESS
00072 #if USE_PROCESS_NUM>0
00073 static void Process0(void);
00074 #endif
00075 #if USE_PROCESS_NUM>1
00076 static void Process1(void);
00077 #endif
00078 #if USE_PROCESS_NUM>2
00079 static void Process2(void);
00080 #endif
00081 #if USE_PROCESS_NUM>3
00082 static void Process3(void);
00083 #endif
00084 #if USE_PROCESS_NUM>4
00085 static void Process4(void);
00086 #endif
00087 #if USE_PROCESS_NUM>5
00088 static void Process5(void);
00089 #endif
00090 #if USE_PROCESS_NUM>6
00091 static void Process6(void);
00092 #endif
00093 #if USE_PROCESS_NUM>7
00094 static void Process7(void);
00095 #endif
00096 #if USE_PROCESS_NUM>8
00097 static void Process8(void);
00098 #endif
00099 #if USE_PROCESS_NUM>9
00100 static void Process9(void);
00101 #endif
00102 #endif
00103 
00104 void SystemProcessInitialize()
00105 {
00106     #pragma region USER-DEFINED_VARIABLE_INIT
00107     /*Replace here with the initialization code of your variables.*/    
00108     #pragma endregion USER-DEFINED_VARIABLE_INIT
00109 
00110     lock = true;
00111     processChangeComp = true;
00112     current = DEFAULT_PROCESS;
00113 
00114     #ifdef USE_SUBPROCESS
00115     #if USE_PROCESS_NUM>0
00116     Process[0] = Process0;
00117     #endif
00118     #if USE_PROCESS_NUM>1
00119     Process[1] = Process1;
00120     #endif
00121     #if USE_PROCESS_NUM>2
00122     Process[2] = Process2;
00123     #endif
00124     #if USE_PROCESS_NUM>3
00125     Process[3] = Process3;
00126     #endif
00127     #if USE_PROCESS_NUM>4
00128     Process[4] = Process4;
00129     #endif
00130     #if USE_PROCESS_NUM>5
00131     Process[5] = Process5;
00132     #endif
00133     #if USE_PROCESS_NUM>6
00134     Process[6] = Process6;
00135     #endif
00136     #if USE_PROCESS_NUM>7
00137     Process[7] = Process7;
00138     #endif
00139     #if USE_PROCESS_NUM>8
00140     Process[8] = Process8;
00141     #endif
00142     #if USE_PROCESS_NUM>9
00143     Process[9] = Process9;
00144     #endif
00145     #endif
00146 }
00147 
00148 static void SystemProcessUpdate()
00149 {
00150     #ifdef USE_SUBPROCESS
00151     if(controller->Button.HOME) lock = false;
00152     
00153     if(controller->Button.START && processChangeComp)
00154     {
00155         current++;
00156         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00157         processChangeComp = false;
00158     }
00159     else if(controller->Button.SELECT && processChangeComp)
00160     {
00161         current--;
00162         if (current < 0) current = 0;
00163         processChangeComp = false;
00164     }
00165     else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00166     #endif
00167     
00168     #ifdef USE_MOTOR
00169     ACTUATORHUB::MOTOR::Motor::Update(motor);
00170     #endif
00171     
00172     #ifdef USE_SOLENOID
00173     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00174     #endif
00175 
00176     #ifdef USE_RS485
00177     ACTUATORHUB::ActuatorHub::Update();
00178     #endif
00179     
00180 }
00181 
00182 
00183 
00184 void SystemProcess()
00185 {
00186     SystemProcessInitialize();
00187 
00188     while(1)
00189     {
00190         if(LimitSw::IsPressed(10)) {
00191             LED_DEBUG0 = LED_ON;
00192         } else {
00193             LED_DEBUG0 = LED_OFF;
00194         }
00195         //printf("%d\r\n",ECD_0.getPulses());
00196         buzzer.period(1.0/800);
00197         
00198         #ifdef USE_MU
00199         controller = CONTROLLER::Controller::GetData();
00200         #endif
00201 
00202         #ifdef USE_ERRORCHECK
00203         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
00204         {
00205             CONTROLLER::Controller::DataReset();
00206             AllActuatorReset();
00207             lock = true;
00208         }
00209         else
00210         #endif
00211         {
00212 
00213             #ifdef USE_SUBPROCESS
00214             if(!lock)
00215             {
00216                 Process[current]();
00217             }
00218             else
00219             #endif
00220             {
00221                 //ロック時の処理
00222             }
00223         }
00224         
00225         //Emergency!
00226         if(!EMG_0 && !EMG_1 && !EMGflag){
00227             buzzer = 0;
00228             BuzzerTimer.attach(BuzzerTimer_func, 1);
00229             EMGflag = true;
00230             LED_DEBUG0 = 1;
00231         }
00232         if(EMG_0 && EMG_1 && EMGflag){
00233             buzzer = 1;
00234             BuzzerTimer.detach();
00235             EMGflag = false;
00236         }
00237         SystemProcessUpdate();
00238     }
00239 }
00240 
00241 
00242     
00243 
00244 #pragma region PROCESS
00245 #ifdef USE_SUBPROCESS
00246 #if USE_PROCESS_NUM>0
00247 static void Process0() 
00248 {   
00249     
00250 }
00251 #endif
00252 
00253 #if USE_PROCESS_NUM>1
00254 static void Process1()
00255 {
00256     
00257 }
00258 #endif
00259 
00260 #if USE_PROCESS_NUM>2
00261 static void Process2()
00262 {   
00263     
00264 }
00265 #endif
00266 
00267 #if USE_PROCESS_NUM>3
00268 static void Process3() 
00269 {
00270     
00271 }
00272 #endif
00273 
00274 #if USE_PROCESS_NUM>4
00275 static void Process4() 
00276 {   
00277 
00278 }
00279 #endif
00280 
00281 #if USE_PROCESS_NUM>5
00282 static void Process5() 
00283 {   
00284     
00285 }
00286 #endif
00287 
00288 #if USE_PROCESS_NUM>6
00289 static void Process6() 
00290 {
00291     
00292 }
00293 #endif
00294 
00295 #if USE_PROCESS_NUM>7
00296 static void Process7()
00297 {
00298     
00299 }
00300 #endif
00301 
00302 #if USE_PROCESS_NUM>8 
00303 static void Process8()
00304 {
00305     
00306 }
00307 #endif
00308 
00309 #if USE_PROCESS_NUM>9
00310 static void Process9()
00311 {
00312     
00313 }
00314 #endif
00315 #endif
00316 #pragma endregion PROCESS
00317 
00318 static void AllActuatorReset()
00319 {
00320 
00321     #ifdef USE_SOLENOID
00322     solenoid.all = ALL_SOLENOID_OFF;
00323     #endif
00324 
00325     #ifdef USE_MOTOR
00326     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
00327     {
00328         motor[i].dir = FREE;
00329         motor[i].pwm = 0;
00330     }
00331     #endif
00332 }
00333 
00334 void BuzzerTimer_func(){
00335     buzzer = !buzzer;
00336     //LED_DEBUG0 = !LED_DEBUG0;
00337 }
00338 
00339 void TapeLedEms_func() {
00340     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
00341 }
00342 
00343 #pragma region USER-DEFINED-FUNCTIONS
00344 
00345 
00346 #pragma endregion