Process.cpp

00001 #include "mbed.h"
00002 #include "Process.h"
00003 
00004 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00005 #include "../../Communication/Controller/Controller.h"
00006 #include "../../Input/ExternalInt/ExternalInt.h"
00007 #include "../../Input/Switch/Switch.h"
00008 #include "../../Input/ColorSensor/ColorSensor.h"
00009 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
00010 #include "../../Input/Potentiometer/Potentiometer.h"
00011 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
00012 #include "../../LED/LED.h"
00013 #include "../../Safty/Safty.h"
00014 #include "../Using.h"
00015 #include "../../Communication/PID/PID.h"
00016 
00017 
00018 using namespace SWITCH;
00019 using namespace COLORSENSOR;
00020 using namespace ACCELERATIONSENSOR;
00021 using namespace PID_SPACE;
00022 using namespace ROTARYENCODER;
00023 
00024 static CONTROLLER::ControllerData *controller;
00025 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00026 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00027 
00028 static bool lock;
00029 static bool processChangeComp;
00030 static int current;
00031 
00032 static void AllActuatorReset();
00033 
00034 #ifdef USE_SUBPROCESS
00035 static void (*Process[USE_PROCESS_NUM])(void);
00036 #endif
00037 
00038 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00039 
00040 /*Replace here with the definition code of your variables.*/
00041 
00042 Serial pc(USBTX, USBRX);
00043 
00044 unsigned long ColorIn(int index)
00045 {
00046     int result = 0;
00047     bool rtn = false;
00048     for(int i=0; i<12; i++)
00049     {
00050         CK[index] = 1;
00051         rtn = DOUT[index];
00052         CK[index] = 0;
00053         if(rtn)
00054         {
00055            result|=(1 << i);
00056         }
00057     }
00058     return result;
00059 }
00060 
00061 #define TIRE_FR 0 //足回り前右
00062 #define TIRE_FL 1 //足回り前左
00063 #define TIRE_BR 2 //足回り後右
00064 #define TIRE_BL 3 //足回り後左
00065 
00066 #define Angle_R 4 //角度調節右
00067 #define Angle_L 5 //角度調節左
00068 
00069 #define Lim_AR 3 //角度調節右
00070 #define Lim_AL 4 //角度調節左
00071 #define Lim_R  0 //センター右
00072 #define Lim_L  1 //センター左
00073 
00074 //************メカナム********************
00075 
00076 const int mecanum[15][15]=
00077 {
00078     {   0,    5,     21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255},
00079     {  -5,    0,      5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255},
00080     { -21,   -5,      0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255},
00081     { -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255},
00082     { -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255},
00083     {-130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255},
00084     {-187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255},
00085     {-255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255},
00086     {-255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187},
00087     {-255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130},
00088     {-255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83},
00089     {-255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47},
00090     {-255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21},
00091     {-255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5},
00092     {-255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0}
00093 };
00094 
00095 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
00096 uint8_t SetStatus(int);
00097 uint8_t SetStatus(int pwmVal){
00098     if(pwmVal < 0) return BACK;
00099     else if(pwmVal > 0) return FOR;
00100     else if(pwmVal == 0) return BRAKE;
00101     else return BRAKE;
00102 }
00103 uint8_t SetPWM(int);
00104 uint8_t SetPWM(int pwmVal){
00105     if(pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
00106     else return abs(pwmVal);
00107 }
00108 
00109 //************メカナム********************
00110 
00111 //************カラーセンサ変数********************
00112 int Color_A[3]; //[赤,緑,青]
00113 int Color_B[3];
00114 int Color_C[3];
00115 int Color_D[3];         
00116 int intergration = 50;
00117 /*int averageR_A;
00118 int averageG_A;
00119 int averageB_A;
00120 int averageR_B;
00121 int averageG_B;
00122 int averageB_B;
00123 int averageR_C;
00124 int averageG_C;
00125 int averageB_C;
00126 int averageR_D;
00127 int averageG_D;
00128 int averageB_D;*/
00129 
00130 void ColorDetection();
00131 
00132 /*DigitalIn www(RT11_PIN);
00133 DigitalIn mmm(RT12_PIN);
00134 
00135 int pp = 0;
00136 int bb = 0;
00137 
00138 */
00139 
00140 
00141 //************カラーセンサ変数********************
00142 
00143 //************ライントレース変数*******************
00144 int Point[3] = {234, 466, 590};//赤，緑，青
00145 int colorSN;
00146 int startP = 35;
00147 int downP = 5;
00148 
00149 bool Goal_flag = false;
00150 PID goal = PID(0.03,-255,255,0,0,0);    
00151 void GoalArrival();
00152 //************ライントレース変数*******************
00153 
00154 //************ジャイロ*******************
00155 //void AngleDetection();
00156 //void AngleControl();
00157 float AngleY;
00158 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
00159 bool Angle_flag = false;
00160 float rotateY;
00161 int AngletargetX = 50;
00162 int AngletargetY = -50;
00163 int Angle_I;
00164 //************ジャイロ*******************
00165 
00166 //************ロタコン*******************
00167 int MemoRt;
00168 int Rt0;
00169 int Rt1;
00170 int Rt_A;
00171 int Rt_B;
00172 int PresentRt;
00173 //************ロタコン*******************
00174 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00175 
00176 #ifdef USE_SUBPROCESS
00177 #if USE_PROCESS_NUM>0
00178 static void Process0(void);
00179 #endif
00180 #if USE_PROCESS_NUM>1
00181 static void Process1(void);
00182 #endif
00183 #if USE_PROCESS_NUM>2
00184 static void Process2(void);
00185 #endif
00186 #if USE_PROCESS_NUM>3
00187 static void Process3(void);
00188 #endif
00189 #if USE_PROCESS_NUM>4
00190 static void Process4(void);
00191 #endif
00192 #if USE_PROCESS_NUM>5
00193 static void Process5(void);
00194 #endif
00195 #if USE_PROCESS_NUM>6
00196 static void Process6(void);
00197 #endif
00198 #if USE_PROCESS_NUM>7
00199 static void Process7(void);
00200 #endif
00201 #if USE_PROCESS_NUM>8
00202 static void Process8(void);
00203 #endif
00204 #if USE_PROCESS_NUM>9
00205 static void Process9(void);
00206 #endif
00207 #endif
00208 
00209 void SystemProcessInitialize()
00210 {
00211     #pragma region USER-DEFINED_VARIABLE_INIT
00212     
00213     /*Replace here with the initialization code of your variables.*/
00214     #pragma endregion USER-DEFINED_VARIABLE_INIT
00215 
00216     lock = true;
00217     processChangeComp = true;
00218     current = DEFAULT_PROCESS;
00219 
00220     #ifdef USE_SUBPROCESS
00221     #if USE_PROCESS_NUM>0
00222     Process[0] = Process0;
00223     #endif
00224     #if USE_PROCESS_NUM>1
00225     Process[1] = Process1;
00226     #endif
00227     #if USE_PROCESS_NUM>2
00228     Process[2] = Process2;
00229     #endif
00230     #if USE_PROCESS_NUM>3
00231     Process[3] = Process3;
00232     #endif
00233     #if USE_PROCESS_NUM>4
00234     Process[4] = Process4;
00235     #endif
00236     #if USE_PROCESS_NUM>5
00237     Process[5] = Process5;
00238     #endif
00239     #if USE_PROCESS_NUM>6
00240     Process[6] = Process6;
00241     #endif
00242     #if USE_PROCESS_NUM>7
00243     Process[7] = Process7;
00244     #endif
00245     #if USE_PROCESS_NUM>8
00246     Process[8] = Process8;
00247     #endif
00248     #if USE_PROCESS_NUM>9
00249     Process[9] = Process9;
00250     #endif
00251     #endif
00252 }
00253 
00254 static void SystemProcessUpdate()
00255 {
00256     #ifdef USE_SUBPROCESS
00257     if(controller->Button.HOME) lock = false;
00258     
00259     if(controller->Button.START && processChangeComp)
00260     {
00261         current++;
00262         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00263         processChangeComp = false;
00264     }
00265     else if(controller->Button.SELECT && processChangeComp)
00266     {
00267         current--;
00268         if (current < 0) current = 0;
00269         processChangeComp = false;
00270     }
00271     else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00272     #endif
00273     
00274     #ifdef USE_MOTOR
00275     ACTUATORHUB::MOTOR::Motor::Update(motor);
00276     #endif
00277     
00278     #ifdef USE_SOLENOID
00279     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00280     #endif
00281 
00282     #ifdef USE_RS485
00283     ACTUATORHUB::ActuatorHub::Update();
00284     #endif
00285     
00286 }
00287 
00288 
00289 
00290 void SystemProcess()
00291 {
00292     SystemProcessInitialize();
00293 
00294     while(1)
00295     {
00296         
00297         #ifdef USE_MU
00298         controller = CONTROLLER::Controller::GetData();
00299         #endif
00300 
00301         #ifdef USE_ERRORCHECK
00302         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
00303         {
00304             CONTROLLER::Controller::DataReset();
00305             AllActuatorReset();
00306             lock = true;
00307         }
00308         else
00309         #endif
00310         {
00311 
00312             #ifdef USE_SUBPROCESS
00313             if(!lock)
00314             {
00315                 Process[current]();
00316             }
00317             else
00318             #endif
00319             {
00320                 //ロック時の処理
00321             }
00322         }
00323         
00324         SystemProcessUpdate();
00325     }
00326 }
00327 
00328 
00329     
00330 
00331 #pragma region PROCESS
00332 #ifdef USE_SUBPROCESS
00333 #if USE_PROCESS_NUM>0
00334 static void Process0()
00335 {
00336     
00337     if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
00338         motor[Angle_R].dir = BRAKE;
00339         motor[Angle_L].dir = BRAKE;
00340         motor[Angle_R].pwm = 255;
00341         motor[Angle_L].pwm = 255;
00342     }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
00343         motor[Angle_R].dir = BRAKE;
00344         motor[Angle_L].dir = BRAKE;
00345         motor[Angle_R].pwm = 255;
00346         motor[Angle_L].pwm = 255;
00347     }   
00348     for(int i = 0;i<20;i++){
00349         float y = 0;
00350         y = acc[1]*1000;
00351         float rotateY = (y - 305)/2.21 - 90;
00352         AngleY += rotateY;
00353     }
00354     AngleY = AngleY /20;
00355     int gyropwm = gyro.SetPV(AngleY,Angle_I);
00356     
00357     if(controller->Button.A){
00358         Angle_flag = true;
00359     }/*
00360     if(controller->Button.Y){
00361         Angle_flag = true;
00362     }*/
00363     if (Angle_flag){
00364         motor[Angle_R].dir = SetStatus(gyropwm);
00365         motor[Angle_L].dir = SetStatus(-gyropwm);
00366         motor[Angle_R].pwm = SetPWM(gyropwm);
00367         motor[Angle_L].pwm = SetPWM(gyropwm);
00368         if(Angle_I - 2 < AngleY && AngleY < Angle_I + 2){
00369         motor[Angle_R].dir = BRAKE;
00370         motor[Angle_L].dir = BRAKE;
00371         Angle_flag = false;
00372         }
00373     }
00374 }
00375 #endif
00376 
00377 #if USE_PROCESS_NUM>1
00378 static void Process1()
00379 {
00380     motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
00381     motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
00382     motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]); 
00383     motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
00384          
00385     motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
00386     motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
00387     motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
00388     motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
00389                   
00390     if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
00391     motor[TIRE_FR].pwm = motor[0].pwm * 1.3;
00392     motor[TIRE_FL].pwm = motor[1].pwm * 1.3;
00393     }
00394 }
00395 #endif
00396 
00397 #if USE_PROCESS_NUM>2
00398 static void Process2()
00399 {
00400     static bool color_flag = false;
00401     static bool traceon = false;
00402     
00403     static bool yokofla = false;
00404     
00405     static bool compA = false;
00406     static bool compB = false;
00407     static bool compC = false;
00408     static bool compD = false;
00409     
00410     static bool invationA = false;
00411     static bool invationB = false;
00412     static bool invationC = false;
00413     static bool invationD = false;
00414     
00415     ColorDetection();
00416     //
00417     if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
00418     {
00419     invationA ^= 1;//start false,over true
00420     compA = true;//on true,noon false
00421     }   
00422     else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
00423     
00424     if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
00425     {
00426     invationB ^= 1;//start false,over true
00427     compB = true;//on true,noon false
00428     }   
00429     else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
00430     //
00431     
00432     if(controller->Button.B && !color_flag)
00433     {
00434         traceon ^= 1;
00435         color_flag = true;
00436     }
00437     else if(!controller->Button.B)
00438     {
00439         color_flag = false;
00440     }
00441     
00442     if(traceon)
00443     {
00444         if(!invationA && !compA && !invationB && !compB){
00445             motor[TIRE_FR].dir = FOR;
00446             motor[TIRE_FL].dir = FOR;
00447             motor[TIRE_BR].dir = BACK;
00448             motor[TIRE_BL].dir = BACK;
00449         
00450             motor[TIRE_FR].pwm = startP;
00451             motor[TIRE_FL].pwm = startP;
00452             motor[TIRE_BR].pwm = startP;
00453             motor[TIRE_BL].pwm = startP;
00454         }
00455         else if(invationA && compA && !invationB && !compB){
00456             motor[TIRE_FR].dir = FOR;
00457             motor[TIRE_FL].dir = FOR;
00458             motor[TIRE_BR].dir = BACK;
00459             motor[TIRE_BL].dir = BACK;
00460         
00461             motor[TIRE_FR].pwm = startP - downP;
00462             motor[TIRE_FL].pwm = startP - downP;
00463             motor[TIRE_BR].pwm = startP - downP;
00464             motor[TIRE_BL].pwm = startP - downP;
00465         }
00466         else if(!invationA && !compA && !invationB && !compB){
00467             motor[TIRE_FR].dir = BRAKE;
00468             motor[TIRE_FL].dir = BRAKE;
00469             motor[TIRE_BR].dir = BRAKE;
00470             motor[TIRE_BL].dir = BRAKE;
00471         
00472             motor[0].pwm = 255;
00473             motor[1].pwm = 255;
00474             motor[2].pwm = 255;
00475             motor[3].pwm = 255;
00476         
00477             wait(5);
00478             yokofla = true;
00479         }
00480         else if(invationA && !compA && invationB && !compB && yokofla){
00481             motor[TIRE_FR].dir = BACK;
00482             motor[TIRE_FL].dir = BACK;
00483             motor[TIRE_BR].dir = FOR;
00484             motor[TIRE_BL].dir = FOR;
00485         
00486             motor[TIRE_FR].pwm = startP - downP;
00487             motor[TIRE_FL].pwm = startP - downP;
00488             motor[TIRE_BR].pwm = startP - downP; 
00489             motor[TIRE_BL].pwm = startP - downP;
00490         } 
00491         else if(invationA && !compA && !invationB && !compB && yokofla){
00492             motor[TIRE_FR].dir = FOR;
00493             motor[TIRE_FL].dir = BACK;
00494             motor[TIRE_BR].dir = FOR;
00495             motor[TIRE_BL].dir = BACK;
00496         
00497             motor[TIRE_FR].pwm = startP;
00498             motor[TIRE_FL].pwm = startP;
00499             motor[TIRE_BR].pwm = startP;
00500             motor[TIRE_BL].pwm = startP;
00501         }
00502         else if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4) && invationA && !compA && !invationB && !compB && yokofla && traceon){
00503             motor[TIRE_FR].dir = BRAKE;
00504             motor[TIRE_FL].dir = BRAKE;
00505             motor[TIRE_BR].dir = BRAKE;
00506             motor[TIRE_BL].dir = BRAKE;
00507         
00508             motor[TIRE_FR].pwm = 255;
00509             motor[TIRE_FL].pwm = 255;
00510             motor[TIRE_BR].pwm = 255;
00511             motor[TIRE_BL].pwm = 255;
00512         }
00513         /*////
00514         motor[0].dir = BACK;
00515         motor[1].dir = BACK;
00516         motor[2].dir = FOR;
00517         motor[3].dir = FOR;
00518         
00519         motor[0].pwm = startP;
00520         motor[1].pwm = startP;
00521         motor[2].pwm = startP;
00522         motor[3].pwm = startP;
00523         else if()
00524         {
00525         motor[0].dir = BRAKE;
00526         motor[1].dir = BRAKE;
00527         motor[2].dir = BRAKE;
00528         motor[3].dir = BRAKE;
00529         
00530         motor[0].pwm = 255;
00531         motor[1].pwm = 255;
00532         motor[2].pwm = 255;
00533         motor[3].pwm = 255;
00534         }*//////
00535         else if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)){
00536             motor[TIRE_FR].dir = FOR;
00537             motor[TIRE_FL].dir = FOR;
00538             motor[TIRE_BR].dir = BACK;
00539             motor[TIRE_BL].dir = BACK;
00540             motor[TIRE_FR].pwm = 100;
00541             motor[TIRE_FL].pwm = 100;
00542             motor[TIRE_BR].pwm = 100;
00543             motor[TIRE_BL].pwm = 100;
00544         }else if(!LimitSw::IsPressed(3) && LimitSw::IsPressed(4)){
00545             motor[TIRE_FR].dir = FOR;
00546             motor[TIRE_BR].dir = FOR;
00547             motor[TIRE_FR].pwm = 20;
00548             motor[TIRE_BR].pwm = 20;
00549         }else if(LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
00550             motor[TIRE_FL].dir = BACK;
00551             motor[TIRE_BL].dir = BACK;
00552             motor[TIRE_FL].pwm = 20;
00553             motor[TIRE_BL].pwm = 20;
00554         }
00555          else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
00556             Goal_flag = true;
00557             GoalArrival();          
00558         }else{
00559             motor[TIRE_FR].dir = BRAKE;
00560             motor[TIRE_FL].dir = BRAKE;
00561             motor[TIRE_BR].dir = BRAKE;
00562             motor[TIRE_BL].dir = BRAKE;
00563         }
00564     }
00565 }
00566 #endif
00567 
00568 #if USE_PROCESS_NUM>3
00569 static void Process3()
00570 {
00571     if(controller->Button.R){
00572         motor[Angle_R].dir = FOR;
00573         motor[Angle_L].dir = BACK;
00574         motor[Angle_R].pwm = 150;
00575         motor[Angle_L].pwm = 150;
00576     }else if(controller->Button.L){
00577         motor[Angle_R].dir = BACK;
00578         motor[Angle_L].dir = FOR;
00579         motor[Angle_R].pwm = 150;
00580         motor[Angle_L].pwm = 150;
00581     }else{
00582         motor[Angle_R].dir = BRAKE;
00583         motor[Angle_L].dir = BRAKE;
00584     }
00585 
00586     if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
00587         motor[Angle_R].dir = BRAKE;
00588         motor[Angle_L].dir = BRAKE;
00589         
00590         motor[Angle_R].pwm = 255;
00591         motor[Angle_L].pwm = 255;
00592     }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
00593         motor[Angle_R].dir = BRAKE;
00594         motor[Angle_L].dir = BRAKE;
00595         
00596         motor[Angle_R].pwm = 255;
00597         motor[Angle_L].pwm = 255;
00598     }
00599 }
00600 #endif
00601 
00602 #if USE_PROCESS_NUM>4
00603 static void Process4()
00604 {
00605  /*   for(int i=0;i<10;i++)
00606     {
00607         ColorDetection();
00608         
00609         averageR_A += Color_A[0];
00610         averageG_A += Color_A[1];
00611         averageB_A += Color_A[2];
00612         averageR_B += Color_B[0];
00613         averageG_B += Color_B[1];
00614         averageB_B += Color_B[2];
00615         averageR_C += Color_C[0];
00616         averageG_C += Color_C[1];
00617         averageB_C += Color_C[2];
00618         averageR_D += Color_D[0];
00619         averageG_D += Color_D[1];
00620         averageB_D += Color_D[2];
00621     }
00622     pc.printf("AR_A:%d, AG_A:%d ,AB_A:%d \r\n",averageR_A / 10 ,averageG_A / 10, averageB_A / 10);
00623     pc.printf("AR_B:%d, AG_B:%d ,AB_B:%d \r\n",averageR_B / 10 ,averageG_B / 10, averageB_B / 10);
00624     pc.printf("AR_C:%d, AG_C:%d ,AB_C:%d \r\n",averageR_C / 10 ,averageG_C / 10, averageB_C / 10);
00625     pc.printf("AR_D:%d, AG_D:%d ,AB_D:%d \r\n",averageR_D / 10 ,averageG_D / 10, averageB_D / 10);
00626     
00627     averageR_A = 0;
00628     averageG_A = 0;
00629     averageB_A = 0;
00630     averageR_B = 0;
00631     averageG_B = 0;
00632     averageB_B = 0;
00633     averageR_C = 0;
00634     averageG_C = 0;
00635     averageB_C = 0;
00636     averageR_D = 0;
00637     averageG_D = 0;
00638     averageB_D = 0;*/
00639 }
00640 #endif
00641 
00642 #if USE_PROCESS_NUM>5
00643 static void Process5()
00644 {   
00645     if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].dir == BACK){
00646         motor[Angle_R].dir = BRAKE;
00647         motor[Angle_L].dir = BRAKE;
00648         motor[Angle_R].pwm = 255;
00649         motor[Angle_L].pwm = 255;
00650     }else if(LimitSw::IsPressed(Lim_AL) && motor[4].dir == BACK && motor[5].dir == FOR){
00651         motor[Angle_R].dir = BRAKE;
00652         motor[Angle_L].dir = BRAKE;
00653         motor[Angle_R].pwm = 255;
00654         motor[Angle_L].pwm = 255;
00655     }
00656     for(int i = 0;i<20;i++){
00657         float y = 0;
00658         y = acc[1]*1000;
00659         float rotateY = (y - 305)/2.21 - 90;
00660         AngleY += rotateY;
00661     }
00662     AngleY = AngleY /20;
00663     int gyropwm = gyro.SetPV(AngleY,AngletargetY);
00664     
00665     if(controller->Button.X){
00666         Angle_flag = true;
00667     }/*
00668     if(controller->Button.Y){
00669         Angle_flag = true;
00670     }*/
00671     if (Angle_flag){
00672         motor[Angle_R].dir = SetStatus(gyropwm);
00673         motor[Angle_L].dir = SetStatus(-gyropwm);
00674         motor[Angle_R].pwm = SetPWM(gyropwm);
00675         motor[Angle_L].pwm = SetPWM(gyropwm);
00676         if(AngletargetY - 2 < AngleY && AngleY < AngletargetY + 2){
00677         motor[Angle_R].dir = BRAKE;
00678         motor[Angle_L].dir = BRAKE;
00679         Angle_flag = false;
00680         }
00681     }
00682     /*float y = 0;
00683     y = acc[1]*1000;
00684     float rotateY = (y - 305)/2.21 - 90;    
00685     int gyropwm = gyro.SetPV(rotateY , Angletarget);
00686     
00687     if(controller->Button.X){
00688         Angle_flag = true;
00689     }
00690     if (Angle_flag){
00691         motor[Angle_R].dir = SetStatus(gyropwm);
00692         motor[Angle_L].dir = SetStatus(-gyropwm);
00693         motor[Angle_R].pwm = SetPWM(gyropwm);
00694         motor[Angle_L].pwm = SetPWM(gyropwm);
00695         if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
00696         motor[Angle_R].dir = BRAKE;
00697         motor[Angle_L].dir = BRAKE;
00698         Angle_flag = false;
00699         }
00700     }*/
00701     else{
00702         motor[Angle_R].dir = BRAKE;
00703         motor[Angle_L].dir = BRAKE;
00704     }
00705 }
00706 #endif
00707 
00708 #if USE_PROCESS_NUM>6
00709 static void Process6()
00710 {       
00711  //void Int::Initialize(BoardRtInt[0].fall(int2));
00712 }
00713 #endif
00714 
00715 #if USE_PROCESS_NUM>7
00716 static void Process7()
00717 {
00718 
00719 }
00720 #endif
00721 
00722 #if USE_PROCESS_NUM>8
00723 static void Process8()
00724 {
00725 
00726 }
00727 #endif
00728 
00729 #if USE_PROCESS_NUM>9
00730 static void Process9()
00731 {
00732 
00733 }
00734 #endif
00735 #endif
00736 #pragma endregion PROCESS
00737 
00738 static void AllActuatorReset()
00739 {
00740 
00741     #ifdef USE_SOLENOID
00742     solenoid.all = ALL_SOLENOID_OFF;
00743     #endif
00744 
00745     #ifdef USE_MOTOR
00746     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
00747     {
00748         motor[i].dir = FREE;
00749         motor[i].pwm = 0;
00750     }
00751     #endif
00752 }
00753 
00754 #pragma region USER-DEFINED-FUNCTIONS
00755 void ColorDetection(){
00756     GATE = 0;
00757            
00758     CK[0] = 0;
00759     CK[1] = 0;
00760     CK[2] = 0;
00761     CK[3] = 0;
00762         
00763     RANGE = 1;
00764         
00765     GATE = 1;
00766     wait_ms(intergration);
00767     GATE = 0;
00768     wait_us(4);
00769         
00770     Color_A[0] = ColorIn(0); //赤
00771     wait_us(3);
00772     Color_A[1] = ColorIn(0); //青
00773     wait_us(3);
00774     Color_A[2] = ColorIn(0); //緑
00775         
00776     Color_B[0] = ColorIn(1);
00777     wait_us(3);
00778     Color_B[1] = ColorIn(1);
00779     wait_us(3);
00780     Color_B[2] = ColorIn(1);
00781          
00782     Color_C[0] = ColorIn(2);
00783     wait_us(3);
00784     Color_C[1] = ColorIn(2);
00785     wait_us(3);
00786     Color_C[2] = ColorIn(2);
00787         
00788     Color_D[0] = ColorIn(3);
00789     wait_us(3);
00790     Color_D[1] = ColorIn(3);
00791     wait_us(3);
00792     Color_D[2] = ColorIn(3);
00793 }
00794 /*void AngleDetection(){
00795     float x = 0, y= 0, z = 0;
00796         
00797     //x = acc[0]*1000;
00798     y = acc[1]*1000;
00799     //z = acc[2]*1000;
00800     //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
00801     //float rotateX = (x - 306)/2.22 - 90;
00802     float rotateY = (y - 305)/2.21 - 90;
00803     //float rotateZ = (z - 300)/1.18 - 90;
00804     //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
00805 }*/
00806 /*void AngleControl(){  
00807     int Angletarget = -50;
00808     float AnglevalueY = rotateY;  
00809     int gyropwm = gyro.SetPV(AnglevalueY , Angletarget);
00810             
00811     if (Angle_flag){
00812         motor[Angle_R].dir = SetStatus(gyropwm);
00813         motor[Angle_L].dir = SetStatus(-gyropwm);
00814         motor[Angle_R].pwm = SetPWM(gyropwm);
00815         motor[Angle_L].pwm = SetPWM(gyropwm);
00816         if(Angletarget - 2 < AnglevalueY && AnglevalueY < Angletarget + 2){
00817         motor[Angle_R].dir = BRAKE;
00818         motor[Angle_L].dir = BRAKE;
00819         Angle_flag = false;
00820         }
00821     }else{
00822         motor[Angle_R].dir = BRAKE;
00823         motor[Angle_L].dir = BRAKE;
00824     }    
00825     //pc.printf("PWM:%d \r\n" , gyropwm);
00826 }*/
00827 void GoalArrival(){ 
00828     int Goaltarget = 0;
00829     float Goalvalue = 0;  
00830     int goalpwm = goal.SetPV(Goalvalue,Goaltarget);
00831     
00832     if (Goal_flag){
00833         motor[TIRE_FR].dir = SetStatus(goalpwm);
00834         motor[TIRE_FL].dir = SetStatus(-goalpwm);
00835         motor[TIRE_BR].dir = SetStatus(goalpwm);
00836         motor[TIRE_BL].dir = SetStatus(-goalpwm);
00837         motor[TIRE_FR].pwm = SetPWM(goalpwm);
00838         motor[TIRE_FL].pwm = SetPWM(goalpwm);
00839         motor[TIRE_BR].pwm = SetPWM(goalpwm);
00840         motor[TIRE_BL].pwm = SetPWM(goalpwm);
00841         if(Goaltarget > Goalvalue - 10 && Goaltarget < Goalvalue+ 10){
00842         motor[TIRE_FR].dir = BRAKE;
00843         motor[TIRE_FL].dir = BRAKE;
00844         motor[TIRE_BR].dir = BRAKE;
00845         motor[TIRE_BL].dir = BRAKE;
00846         Goal_flag = false;
00847         }
00848     }else{
00849         motor[Angle_R].dir = BRAKE;
00850         motor[Angle_L].dir = BRAKE;
00851     }    
00852 }
00853 /*
00854 void RotaryD(uint8_t, uint8_t){
00855     uint8_t D;
00856     MemoRt = Rt1;
00857     Rt0 = Rt0 << 1;
00858     Rt0 = Rt0 & 3;
00859     Rt1 = Rt1 & 3;
00860     D = (Rt0 + Rt1) & 3;
00861     //return (D);
00862 }
00863 void GetRt(){
00864     uint8_t DJ;
00865     double rad = 0;
00866     PresentRt = 0x00;
00867     if(LimitSw::IsPressed(Rt_A))
00868         PresentRt = 0x02;
00869     if(LimitSw::IsPressed(Rt_B))
00870         PresentRt = 0x01;
00871     if(MemoRt != PresentRt)
00872         DJ = RotaryD(MemoRt,PresentRt);
00873     if(D>=2)rad = 360.0 /50.0;
00874     else rad = -(360.0/50.0);
00875     Rt = Rt + rad /360.0*20;
00876 }*/
00877 
00878 #pragma endregion