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 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00066 
00067 #ifdef USE_SUBPROCESS
00068 #if USE_PROCESS_NUM>0
00069 static void Process0(void);
00070 #endif
00071 #if USE_PROCESS_NUM>1
00072 static void Process1(void);
00073 #endif
00074 #if USE_PROCESS_NUM>2
00075 static void Process2(void);
00076 #endif
00077 #if USE_PROCESS_NUM>3
00078 static void Process3(void);
00079 #endif
00080 #if USE_PROCESS_NUM>4
00081 static void Process4(void);
00082 #endif
00083 #if USE_PROCESS_NUM>5
00084 static void Process5(void);
00085 #endif
00086 #if USE_PROCESS_NUM>6
00087 static void Process6(void);
00088 #endif
00089 #if USE_PROCESS_NUM>7
00090 static void Process7(void);
00091 #endif
00092 #if USE_PROCESS_NUM>8
00093 static void Process8(void);
00094 #endif
00095 #if USE_PROCESS_NUM>9
00096 static void Process9(void);
00097 #endif
00098 #endif
00099 
00100 void SystemProcessInitialize()
00101 {
00102     #pragma region USER-DEFINED_VARIABLE_INIT
00103     /*Replace here with the initialization code of your variables.*/    
00104     #pragma endregion USER-DEFINED_VARIABLE_INIT
00105 
00106     lock = true;
00107     processChangeComp = true;
00108     current = DEFAULT_PROCESS;
00109 
00110     #ifdef USE_SUBPROCESS
00111     #if USE_PROCESS_NUM>0
00112     Process[0] = Process0;
00113     #endif
00114     #if USE_PROCESS_NUM>1
00115     Process[1] = Process1;
00116     #endif
00117     #if USE_PROCESS_NUM>2
00118     Process[2] = Process2;
00119     #endif
00120     #if USE_PROCESS_NUM>3
00121     Process[3] = Process3;
00122     #endif
00123     #if USE_PROCESS_NUM>4
00124     Process[4] = Process4;
00125     #endif
00126     #if USE_PROCESS_NUM>5
00127     Process[5] = Process5;
00128     #endif
00129     #if USE_PROCESS_NUM>6
00130     Process[6] = Process6;
00131     #endif
00132     #if USE_PROCESS_NUM>7
00133     Process[7] = Process7;
00134     #endif
00135     #if USE_PROCESS_NUM>8
00136     Process[8] = Process8;
00137     #endif
00138     #if USE_PROCESS_NUM>9
00139     Process[9] = Process9;
00140     #endif
00141     #endif
00142 }
00143 
00144 static void SystemProcessUpdate()
00145 {
00146     #ifdef USE_SUBPROCESS
00147     if(controller->Button.HOME) lock = false;
00148     
00149     if(controller->Button.START && processChangeComp)
00150     {
00151         current++;
00152         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00153         processChangeComp = false;
00154     }
00155     else if(controller->Button.SELECT && processChangeComp)
00156     {
00157         current--;
00158         if (current < 0) current = 0;
00159         processChangeComp = false;
00160     }
00161     else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00162     #endif
00163     
00164     #ifdef USE_MOTOR
00165     ACTUATORHUB::MOTOR::Motor::Update(motor);
00166     #endif
00167     
00168     #ifdef USE_SOLENOID
00169     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00170     #endif
00171 
00172     #ifdef USE_RS485
00173     ACTUATORHUB::ActuatorHub::Update();
00174     #endif
00175     
00176 }
00177 
00178 
00179 
00180 void SystemProcess()
00181 {
00182     SystemProcessInitialize();
00183 
00184     while(1)
00185     {
00186         //LED_DEBUG0 = !LED_DEBUG0;
00187         //LED_DEBUG1 = !LED_DEBUG1;
00188         //printf("%d\r\n",ECD_0.getPulses());
00189         buzzer.period(1.0/800);
00190         
00191         #ifdef USE_MU
00192         controller = CONTROLLER::Controller::GetData();
00193         #endif
00194 
00195         #ifdef USE_ERRORCHECK
00196         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
00197         {
00198             CONTROLLER::Controller::DataReset();
00199             AllActuatorReset();
00200             lock = true;
00201             //buzzer = 1;
00202             //BuzzerTimer.attach(BuzzerTimer_func, 0.5);
00203             //buzzer = 1;
00204             //BuzzerTimer.attach(&BuzzerTimer_func, 1.2);
00205             //BuzzerTimer.attach(BuzzerTimer_func, 1);
00206         }
00207         else
00208         #endif
00209         {
00210 
00211             #ifdef USE_SUBPROCESS
00212             if(!lock)
00213             {
00214                 Process[current]();
00215             }
00216             else
00217             #endif
00218             {
00219                 //ロック時の処理
00220             }
00221         }
00222         
00223         //Emergency!
00224         if(!EMG_0 && !EMG_1 && !EMGflag){
00225             buzzer = 0;
00226             BuzzerTimer.attach(BuzzerTimer_func, 1);
00227             EMGflag = true;
00228             LED_DEBUG0 = 1;
00229         }
00230         if(EMG_0 && EMG_1 && EMGflag){
00231             buzzer = 1;
00232             BuzzerTimer.detach();
00233             EMGflag = false;
00234         }
00235         SystemProcessUpdate();
00236     }
00237 }
00238 
00239 
00240     
00241 
00242 #pragma region PROCESS
00243 #ifdef USE_SUBPROCESS
00244 #if USE_PROCESS_NUM>0
00245 static void Process0() 
00246 {   
00247     
00248 }
00249 #endif
00250 
00251 #if USE_PROCESS_NUM>1
00252 static void Process1()
00253 {
00254     
00255 }
00256 #endif
00257 
00258 #if USE_PROCESS_NUM>2
00259 static void Process2()
00260 {   
00261     
00262 }
00263 #endif
00264 
00265 #if USE_PROCESS_NUM>3
00266 static void Process3() 
00267 {
00268     
00269 }
00270 #endif
00271 
00272 #if USE_PROCESS_NUM>4
00273 static void Process4() 
00274 {   
00275 
00276 }
00277 #endif
00278 
00279 #if USE_PROCESS_NUM>5
00280 static void Process5() 
00281 {   
00282     
00283 }
00284 #endif
00285 
00286 #if USE_PROCESS_NUM>6
00287 static void Process6() 
00288 {
00289     
00290 }
00291 #endif
00292 
00293 #if USE_PROCESS_NUM>7
00294 static void Process7()
00295 {
00296     
00297 }
00298 #endif
00299 
00300 #if USE_PROCESS_NUM>8 
00301 static void Process8()
00302 {
00303     
00304 }
00305 #endif
00306 
00307 #if USE_PROCESS_NUM>9
00308 static void Process9()
00309 {
00310     
00311 }
00312 #endif
00313 #endif
00314 #pragma endregion PROCESS
00315 
00316 static void AllActuatorReset()
00317 {
00318 
00319     #ifdef USE_SOLENOID
00320     solenoid.all = ALL_SOLENOID_OFF;
00321     #endif
00322 
00323     #ifdef USE_MOTOR
00324     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
00325     {
00326         motor[i].dir = FREE;
00327         motor[i].pwm = 0;
00328     }
00329     #endif
00330 }
00331 
00332 void BuzzerTimer_func(){
00333     buzzer = !buzzer;
00334     //LED_DEBUG0 = !LED_DEBUG0;
00335 }
00336 
00337 void TapeLedEms_func() {
00338     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
00339 }
00340 
00341 #pragma region USER-DEFINED-FUNCTIONS
00342 
00343 
00344 #pragma endregion