Miya Miyagawa
lknds
Dependencies: mbed TrapezoidControl Pulse QEI
Embed:
(wiki syntax)
Show/hide line numbers
Process.cpp
00001 /////////////////////////////////////////////////////////////////////// 00002 00003 /* 00004 三浦先輩へ 00005 主に変えたところ 00006 ・超音波センサーの値取得の関数化 00007 ・PulseInOutライブラリを導入 00008 ・LedOut()関数を追加 00009 00010 の3つです。他にも変えたところがあるかもしれないので、確認お願いします。 00011 また、ソレノイドハブとの通信は書いていません。洗濯ばさみの電磁弁はNRPの時の例のアレで動かしています。 00012 普通にピン数が足りないため、ソレノイドハブでプログラムを動かすことを強く望みます。 00013 Process.hの確認もよくお願いします。 00014 LedOutですが、現在は 00015 LedOut(0)=金色光りっぱなし 00016 LedOut(1)=旧式の光り方 00017 LedOut(2)=ゾーン色で光りっぱなし 00018 LedOut(3)=ゾーン色でふわんふわん 00019 となっています。希望があれば追加します。無ければ適当に引数が増えるごとに光り方をうざくしていきます。 00020 温度センサーですが、調子が悪いため、直で温度の変数を設定してください。 00021 //////////////////////////////////////////////////////////////////////////////////////// 00022 */ 00023 //////////////////////////////// 00024 /* 00025 2019/09/24 00026 Process.cpp 00027 Auther:Miyagawa Ryosei 00028 00029 */ 00030 //////////////////////////////// 00031 #include "mbed.h" 00032 #include "Process.h" 00033 00034 #include "Pulse.h"////////////こいつ 00035 00036 00037 #include "../../CommonLibraries/PID/PID.h" 00038 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" 00039 #include "../../Communication/RS485/LineHub/LineHub.h" 00040 #include "../../Communication/Controller/Controller.h" 00041 #include "../../Input/ExternalInt/ExternalInt.h" 00042 #include "../../Input/Switch/Switch.h" 00043 #include "../../Input/Encoder/Encoder.h" 00044 //#include "../../Input/Ultrasonic/Ultrasonic.h" 00045 #include "../../LED/LED.h" 00046 #include "../../Safty/Safty.h" 00047 #include "../Using.h" 00048 00049 using namespace SWITCH; 00050 using namespace PID_SPACE; 00051 using namespace ENCODER; 00052 //using namespace ULTRSONIC; 00053 using namespace LINEHUB; 00054 00055 00056 static CONTROLLER::ControllerData *controller; 00057 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00058 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; 00059 00060 static bool lock; 00061 static bool processChangeComp; 00062 static int current; 00063 00064 static void AllActuatorReset(); 00065 00066 #ifdef USE_SUBPROCESS 00067 static void (*Process[USE_PROCESS_NUM])(void); 00068 #endif 00069 00070 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE 00071 00072 /*Replace here with the definition code of your variables.*/ 00073 /* 00074 USS ultrasonic[] = { 00075 USS(ECHO_0,TRIG_0,TEMP), 00076 USS(ECHO_1,TRIG_1,TEMP), 00077 }; 00078 */ 00079 Serial pc(USBTX, USBRX); 00080 00081 //**************Buzzer**************** 00082 //DigitalOut buzzer(BUZZER_PIN); 00083 void BuzzerTimer_func(); 00084 Ticker BuzzerTimer; 00085 bool EMGflag = false; 00086 PwmOut buzzer(BUZZER_PIN); 00087 //**************Buzzer**************** 00088 00089 //************TapeLed***************** 00090 void TapeLedEms_func(); 00091 TapeLedData tapeLED; 00092 TapeLedData sendLedData; 00093 TapeLED_Mode ledMode = Normal; 00094 Ticker tapeLedTimer; 00095 //************TapaLed***************** 00096 00097 //*************** lift *************** 00098 #define LOWER 1 00099 #define MIDDLRE 2 00100 #define UPPER 3 00101 uint8_t liftState = LOWER; 00102 bool moving = false; 00103 bool switchFlag_LB = false; 00104 bool switchFlag_RB = false; 00105 00106 //*************** lift *************** 00107 00108 //*************tire************* 00109 PID rotaconPID[] = { 00110 PID(0.0001,-1,1,0.05,0,0), //LF 00111 PID(0.0001,-1,1,0.05,0,0), //LB 00112 PID(0.0001,-1,1,0.05,0,0), //RB 00113 PID(0.0001,-1,1,0.05,0,0), //RF 00114 }; 00115 00116 #define FL 0 00117 #define BL 1 00118 #define BR 2 00119 #define FR 3 00120 00121 #define PI 3.141592 00122 00123 const float tireR = 101.6; //タイヤの半径 00124 const float ucR = 420.0; //中心からのタイヤの距離 00125 00126 typedef struct { 00127 float Vx; //X方向の速度 00128 float Vy; //Y方向の速度 00129 float Va; //角速度 00130 } Vvector; 00131 00132 Vvector move; //進む速度 00133 Vvector correction_LT; //ライントレースの補正速度 00134 Vvector synthetic; //合成速度 00135 00136 float sita = 0; 00137 00138 bool PIDflag = false; 00139 00140 int linePara[8]; 00141 int linePara_U; 00142 int linePara_B; 00143 int linePara_L; 00144 int linePara_R; 00145 00146 #define FL 0 00147 #define BL 1 00148 #define BR 2 00149 #define FR 3 00150 00151 float tireProcessRPM[4]; 00152 float tireTargetMaxRPM[4]; 00153 float tireTargetRPM[4]; 00154 00155 float tirePWM[4]; 00156 00157 float timePV[4]; 00158 float timeCV[4]; 00159 float pulsePV[4]; 00160 float pulseCV[4]; 00161 00162 void tirePID(); 00163 int lineCast(char k); 00164 00165 Timer rotaconSampling; 00166 Ticker rotaconPIDtimer; 00167 00168 bool countFlag; 00169 //*************tire**************// 00170 00171 // ************* Line ************** // 00172 00173 float pw = 0; 00174 int lineFase = 0; 00175 bool lineCheck = false; 00176 int linePWM; 00177 int adj_F; 00178 int adj_B; 00179 00180 int mode = 0; 00181 00182 int lineCount = 0; 00183 00184 // ************* Line ************** // 00185 00186 const int omni[15][15] = { 00187 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, 00188 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, 00189 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, 00190 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, 00191 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, 00192 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, 00193 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, 00194 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, 00195 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, 00196 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, 00197 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, 00198 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, 00199 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, 00200 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, 00201 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } 00202 }; 00203 00204 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 }; 00205 00206 uint8_t SetStatus(int); 00207 uint8_t SetStatus(int pwmVal) 00208 { 00209 if (pwmVal < 0) return BACK; 00210 else if (pwmVal > 0) return FOR; 00211 else if (pwmVal == 0) return BRAKE; 00212 else return BRAKE; 00213 } 00214 uint8_t SetPWM(int); 00215 uint8_t SetPWM(int pwmVal) 00216 { 00217 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; 00218 else return abs(pwmVal); 00219 } 00220 //*********Ultra-Sound sensor*************** 00221 AnalogIn Temp(TEMP); 00222 //double temp=( Temp.read()* 3.3 - 0.6) / 0.01; 00223 PulseInOut Echo0(ECHO_0); 00224 PulseInOut Echo1(ECHO_1); 00225 PulseInOut Trig0(TRIG_0); 00226 PulseInOut Trig1(TRIG_1); 00227 double UltraRead(int num) 00228 { 00229 double Distance=0; 00230 double Duration=0; 00231 double temp=21;////////////////////////////////////////////////温度 00232 if(num==0) { 00233 Trig0.write_us(1,10); 00234 Duration=Echo0.read_high_us(5000); 00235 } else if(num==1) { 00236 Trig1.write_us(1,10); 00237 Duration=Echo1.read_high_us(5000); 00238 } 00239 if(Duration>0) { 00240 Duration=Duration/2; 00241 double sspead=331.5+0.6*temp; 00242 Distance=Duration*sspead*100/1000000; 00243 } else { 00244 return 0; 00245 } 00246 return Distance; 00247 } 00248 //*********Ultra-Sound sensor*************** 00249 00250 //*********Tape LED************************** 00251 DigitalOut select1(SELECT_1); 00252 DigitalOut select2(SELECT_2); 00253 DigitalOut select3(SELECT_3); 00254 void LedOut(int num) 00255 { 00256 int selectnum[8][3]= { 00257 {0,0,0}, 00258 {0,0,1}, 00259 {0,1,0}, 00260 {0,1,1}, 00261 {1,0,0}, 00262 {1,0,1}, 00263 {1,1,0}, 00264 {1,1,1} 00265 }; 00266 select1=selectnum[num][0]; 00267 select2=selectnum[num][1]; 00268 select3=selectnum[num][2]; 00269 } 00270 //*********Tape LED************************** 00271 00272 00273 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00274 00275 #ifdef USE_SUBPROCESS 00276 #if USE_PROCESS_NUM>0 00277 static void Process0(void); 00278 #endif 00279 #if USE_PROCESS_NUM>1 00280 static void Process1(void); 00281 #endif 00282 #if USE_PROCESS_NUM>2 00283 static void Process2(void); 00284 #endif 00285 #if USE_PROCESS_NUM>3 00286 static void Process3(void); 00287 #endif 00288 #if USE_PROCESS_NUM>4 00289 static void Process4(void); 00290 #endif 00291 #if USE_PROCESS_NUM>5 00292 static void Process5(void); 00293 #endif 00294 #if USE_PROCESS_NUM>6 00295 static void Process6(void); 00296 #endif 00297 #if USE_PROCESS_NUM>7 00298 static void Process7(void); 00299 #endif 00300 #if USE_PROCESS_NUM>8 00301 static void Process8(void); 00302 #endif 00303 #if USE_PROCESS_NUM>9 00304 static void Process9(void); 00305 #endif 00306 #endif 00307 00308 void SystemProcessInitialize() 00309 { 00310 #pragma region USER-DEFINED_VARIABLE_INIT 00311 /*Replace here with the initialization code of your variables.*/ 00312 rotaconPIDtimer.attach(tirePID,0.1); 00313 00314 #pragma endregion USER-DEFINED_VARIABLE_INIT 00315 00316 lock = true; 00317 processChangeComp = true; 00318 current = DEFAULT_PROCESS; 00319 00320 #ifdef USE_SUBPROCESS 00321 #if USE_PROCESS_NUM>0 00322 Process[0] = Process0; 00323 #endif 00324 #if USE_PROCESS_NUM>1 00325 Process[1] = Process1; 00326 #endif 00327 #if USE_PROCESS_NUM>2 00328 Process[2] = Process2; 00329 #endif 00330 #if USE_PROCESS_NUM>3 00331 Process[3] = Process3; 00332 #endif 00333 #if USE_PROCESS_NUM>4 00334 Process[4] = Process4; 00335 #endif 00336 #if USE_PROCESS_NUM>5 00337 Process[5] = Process5; 00338 #endif 00339 #if USE_PROCESS_NUM>6 00340 Process[6] = Process6; 00341 #endif 00342 #if USE_PROCESS_NUM>7 00343 Process[7] = Process7; 00344 #endif 00345 #if USE_PROCESS_NUM>8 00346 Process[8] = Process8; 00347 #endif 00348 #if USE_PROCESS_NUM>9 00349 Process[9] = Process9; 00350 #endif 00351 #endif 00352 } 00353 00354 static void SystemProcessUpdate() 00355 { 00356 #ifdef USE_SUBPROCESS 00357 if(controller->Button.HOME) lock = false; 00358 00359 if(controller->Button.START && processChangeComp) { 00360 current++; 00361 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00362 processChangeComp = false; 00363 } else if(controller->Button.SELECT && processChangeComp) { 00364 current--; 00365 if (current < 0) current = 0; 00366 processChangeComp = false; 00367 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00368 #endif 00369 00370 #ifdef USE_MOTOR 00371 ACTUATORHUB::MOTOR::Motor::Update(motor); 00372 #endif 00373 00374 #ifdef USE_SOLENOID 00375 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00376 #endif 00377 00378 #ifdef USE_RS485 00379 ACTUATORHUB::ActuatorHub::Update(); 00380 //LINEHUB::LineHub::Update(); 00381 #endif 00382 00383 } 00384 00385 00386 double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor 00387 double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor 00388 00389 00390 void SystemProcess() 00391 { 00392 SystemProcessInitialize(); 00393 00394 while(1) { 00395 int g[8]; 00396 for(int i = 0; i < 8; i++) { 00397 g[i] = lineCast(LineHub::GetPara(i)); 00398 } 00399 Ult_left=UltraRead(0);//////////////////////////////////////////left sensor 00400 Ult_right=UltraRead(1);//////////////////////////////////////////right sensor 00401 00402 00403 // pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d ULTRA0:%lf ULTRA1:%lf 1-2:%lf:\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7],Ult_left,Ult_right,Ult_left-Ult_right); 00404 00405 //float a = ultrasonic[0].ReadDis(); 00406 //pc.printf("%f\n\r",a); 00407 00408 //int ppap = encoder[0].getPulses(); 00409 //pc.printf("%d\n\r",ppap); 00410 00411 00412 buzzer.period(1.0/800); 00413 00414 #ifdef USE_MU 00415 controller = CONTROLLER::Controller::GetData(); 00416 #endif 00417 00418 #ifdef USE_ERRORCHECK 00419 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { 00420 CONTROLLER::Controller::DataReset(); 00421 AllActuatorReset(); 00422 lock = true; 00423 } else 00424 #endif 00425 { 00426 00427 #ifdef USE_SUBPROCESS 00428 if(!lock) { 00429 Process[current](); 00430 } else 00431 #endif 00432 { 00433 //ロック時の処理 00434 } 00435 } 00436 00437 00438 //Emergency! 00439 /* 00440 if(!EMG_0 && !EMG_1 && !EMGflag){ 00441 buzzer = 0; 00442 BuzzerTimer.attach(BuzzerTimer_func, 1); 00443 EMGflag = true; 00444 LED_DEBUG0 = 1; 00445 } 00446 if(EMG_0 && EMG_1 && EMGflag){ 00447 buzzer = 1; 00448 BuzzerTimer.detach(); 00449 EMGflag = false; 00450 } 00451 */ 00452 SystemProcessUpdate(); 00453 } 00454 } 00455 00456 00457 00458 00459 #pragma region PROCESS 00460 #ifdef USE_SUBPROCESS 00461 #if USE_PROCESS_NUM>0 00462 static void Process0() 00463 { 00464 AllActuatorReset(); 00465 } 00466 #endif 00467 DigitalOut CLOTHESPIN(PB_9); 00468 #if USE_PROCESS_NUM>1 00469 static void Process1() 00470 { 00471 LedOut(1); 00472 PIDflag = false; 00473 if(controller->Button.ZR) { 00474 CLOTHESPIN = 1; 00475 } else { 00476 CLOTHESPIN=0; 00477 } 00478 00479 if(controller->Button.UP) { 00480 motor[LIFT_LB].dir = FOR; 00481 motor[LIFT_LB].pwm = 190; 00482 motor[LIFT_RB].dir = BACK; 00483 motor[LIFT_RB].pwm = 180; 00484 } else if(controller->Button.DOWN) { 00485 motor[LIFT_LB].dir = BACK; 00486 motor[LIFT_LB].pwm = 180; 00487 motor[LIFT_RB].dir = FOR; 00488 motor[LIFT_RB].pwm = 190; 00489 } else if(controller->Button.LEFT) { 00490 motor[LIFT_LB].dir = FOR; 00491 motor[LIFT_LB].pwm = 180; 00492 } else if(controller->Button.RIGHT) { 00493 motor[LIFT_RB].dir = BACK; 00494 motor[LIFT_RB].pwm = 180; 00495 } else { 00496 motor[LIFT_LB].dir = FREE; 00497 motor[LIFT_LB].pwm = 255; 00498 motor[LIFT_RB].dir = BACK; 00499 motor[LIFT_RB].pwm = 10; 00500 } 00501 00502 00503 if(controller->Button.X) { 00504 motor[LIFT_U].dir = FOR; 00505 motor[LIFT_U].pwm = 180; 00506 } else if(controller->Button.Y) { 00507 motor[LIFT_U].dir = BACK; 00508 motor[LIFT_U].pwm = 180; 00509 } else { 00510 motor[LIFT_U].dir = BRAKE; 00511 motor[LIFT_U].pwm = 180; 00512 } 00513 00514 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) { 00515 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] ); 00516 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] ); 00517 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] ); 00518 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] ); 00519 00520 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ; 00521 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ; 00522 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; 00523 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; 00524 } else { 00525 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]); 00526 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]); 00527 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]); 00528 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]); 00529 00530 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]); 00531 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]); 00532 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]); 00533 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]); 00534 } 00535 00536 } 00537 #endif 00538 00539 #if USE_PROCESS_NUM>2 00540 static void Process2() 00541 { 00542 00543 static int phase=0;//system phase 00544 static int StartFlag=0;//if start switch is pressed,the valiable sets "1". 00545 pc.printf("phase:%d,air:%d\r\n",phase,CLOTHESPIN); 00546 Timer FlagTimer;//調整中に超音波センサーが変な動きをした時のためのタイマー 00547 Timer AirTimer;//エアーのタイマー 00548 int RedOrBlue=LimitSw::IsPressed(RED_OR_BLUE); 00549 // solenoid.solenoid0=1; 00550 if(LimitSw::IsPressed(STARTSW)) { 00551 CLOTHESPIN=0; 00552 StartFlag=1; 00553 } 00554 if(controller->Button.ZL) { 00555 CLOTHESPIN=0; 00556 motor[TIRE_FL].dir=FOR; 00557 motor[TIRE_FR].dir=FOR; 00558 motor[TIRE_BL].dir=FOR; 00559 motor[TIRE_BR].dir=FOR; 00560 motor[TIRE_FL].pwm=15; 00561 motor[TIRE_FR].pwm=15; 00562 motor[TIRE_BL].pwm=15; 00563 motor[TIRE_BR].pwm=15; 00564 } else { 00565 CLOTHESPIN=0; 00566 } 00567 if((controller->Button.A)||(StartFlag==1)) { 00568 if(phase==0) { 00569 //位置調整 00570 //(P制御) 00571 CLOTHESPIN=0; 00572 LedOut(4); 00573 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき 00574 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合 00575 motor[TIRE_FL].dir=BRAKE; 00576 motor[TIRE_FR].dir=BRAKE; 00577 motor[TIRE_BL].dir=BRAKE; 00578 motor[TIRE_BR].dir=BRAKE; 00579 motor[TIRE_FL].pwm=100; 00580 motor[TIRE_FR].pwm=100; 00581 motor[TIRE_BL].pwm=100; 00582 motor[TIRE_BR].pwm=100; 00583 phase=1;//system phase increasing 00584 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合 00585 if(Ult_right>16) { //Ult_rightが遠い場合 00586 motor[TIRE_FL].dir=FOR; 00587 motor[TIRE_FR].dir=BACK; 00588 motor[TIRE_BL].dir=FOR; 00589 motor[TIRE_BR].dir=BACK; 00590 motor[TIRE_FL].pwm=0; 00591 motor[TIRE_FR].pwm=15; 00592 motor[TIRE_BL].pwm=0; 00593 motor[TIRE_BR].pwm=15; 00594 } else if(Ult_right<14) { //Ult_rightが近い場合 00595 motor[TIRE_FL].dir=BACK; 00596 motor[TIRE_FR].dir=FOR; 00597 motor[TIRE_BL].dir=BACK; 00598 motor[TIRE_BR].dir=FOR; 00599 motor[TIRE_FL].pwm=0; 00600 motor[TIRE_FR].pwm=15; 00601 motor[TIRE_BL].pwm=0; 00602 motor[TIRE_BR].pwm=15; 00603 } 00604 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合 00605 if(Ult_left>16) { //Ult_leftが遠い場合 00606 motor[TIRE_FL].dir=FOR; 00607 motor[TIRE_FR].dir=BACK; 00608 motor[TIRE_BL].dir=FOR; 00609 motor[TIRE_BR].dir=BACK; 00610 motor[TIRE_FL].pwm=15; 00611 motor[TIRE_FR].pwm=0; 00612 motor[TIRE_BL].pwm=15; 00613 motor[TIRE_BR].pwm=0; 00614 } else if(Ult_left<14) { //Ult_leftが近い場合 00615 motor[TIRE_FL].dir=BACK; 00616 motor[TIRE_FR].dir=FOR; 00617 motor[TIRE_BL].dir=BACK; 00618 motor[TIRE_BR].dir=FOR; 00619 motor[TIRE_FL].pwm=15; 00620 motor[TIRE_FR].pwm=0; 00621 motor[TIRE_BL].pwm=15; 00622 motor[TIRE_BR].pwm=0; 00623 } 00624 } else { //どっちもあってない場合 00625 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき 00626 if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき 00627 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき 00628 motor[TIRE_FL].dir=FOR; 00629 motor[TIRE_FR].dir=FOR; 00630 motor[TIRE_BL].dir=FOR; 00631 motor[TIRE_BR].dir=FOR; 00632 motor[TIRE_FL].pwm=15; 00633 motor[TIRE_FR].pwm=15; 00634 motor[TIRE_BL].pwm=15; 00635 motor[TIRE_BR].pwm=15; 00636 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき 00637 motor[TIRE_FL].dir=BACK; 00638 motor[TIRE_FR].dir=BACK; 00639 motor[TIRE_BL].dir=BACK; 00640 motor[TIRE_BR].dir=BACK; 00641 motor[TIRE_FL].pwm=15; 00642 motor[TIRE_FR].pwm=15; 00643 motor[TIRE_BL].pwm=15; 00644 motor[TIRE_BR].pwm=15; 00645 } 00646 } else { //傾きが大きくなくて離れているとき 00647 if((Ult_right+Ult_left)<=25) { //近すぎるとき 00648 motor[TIRE_FL].dir=BACK; 00649 motor[TIRE_FR].dir=FOR; 00650 motor[TIRE_BL].dir=BACK; 00651 motor[TIRE_BR].dir=FOR; 00652 motor[TIRE_FL].pwm=20; 00653 motor[TIRE_FR].pwm=20; 00654 motor[TIRE_BL].pwm=20; 00655 motor[TIRE_BR].pwm=20; 00656 } else if((Ult_right+Ult_left)>=35) { //離れているとき 00657 motor[TIRE_FL].dir=FOR; 00658 motor[TIRE_FR].dir=BACK; 00659 motor[TIRE_BL].dir=FOR; 00660 motor[TIRE_BR].dir=BACK; 00661 motor[TIRE_FL].pwm=20; 00662 motor[TIRE_FR].pwm=20; 00663 motor[TIRE_BL].pwm=20; 00664 motor[TIRE_BR].pwm=20; 00665 } 00666 } 00667 00668 } else { //さほど離れてはいないが傾きが大きいとき 00669 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき 00670 motor[TIRE_FL].dir=FOR; 00671 motor[TIRE_FR].dir=FOR; 00672 motor[TIRE_BL].dir=FOR; 00673 motor[TIRE_BR].dir=FOR; 00674 motor[TIRE_FL].pwm=15; 00675 motor[TIRE_FR].pwm=15; 00676 motor[TIRE_BL].pwm=15; 00677 motor[TIRE_BR].pwm=15; 00678 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき 00679 motor[TIRE_FL].dir=BACK; 00680 motor[TIRE_FR].dir=BACK; 00681 motor[TIRE_BL].dir=BACK; 00682 motor[TIRE_BR].dir=BACK; 00683 motor[TIRE_FL].pwm=15; 00684 motor[TIRE_FR].pwm=15; 00685 motor[TIRE_BL].pwm=15; 00686 motor[TIRE_BR].pwm=15; 00687 } 00688 } 00689 } 00690 00691 } else {//データを受け取ってないとき 00692 motor[TIRE_FL].dir=BRAKE; 00693 motor[TIRE_FR].dir=BRAKE; 00694 motor[TIRE_BL].dir=BRAKE; 00695 motor[TIRE_BR].dir=BRAKE; 00696 motor[TIRE_FL].pwm=100; 00697 motor[TIRE_FR].pwm=100; 00698 motor[TIRE_BL].pwm=100; 00699 motor[TIRE_BR].pwm=100; 00700 } 00701 } else if(phase==1) { 00702 phase=2; 00703 00704 00705 //待機＆phase1に戻る 00706 00707 } else if(phase==2) { 00708 CLOTHESPIN=SOLENOID_OFF; 00709 LedOut(1); 00710 //リミットスイッチに当てる 00711 static int count2=0; 00712 if(count2==0) { 00713 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき 00714 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) { 00715 if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) { 00716 if(Ult_left-Ult_right<=0) { 00717 motor[TIRE_FL].dir=BACK; 00718 motor[TIRE_FR].dir=BACK; 00719 motor[TIRE_BL].dir=FOR; 00720 motor[TIRE_BR].dir=FOR; 00721 00722 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right)); 00723 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right)); 00724 00725 motor[TIRE_BL].pwm=25; 00726 motor[TIRE_BR].pwm=25; 00727 } else if(Ult_left-Ult_right>0) { 00728 motor[TIRE_FL].dir=BACK; 00729 motor[TIRE_FR].dir=BACK; 00730 motor[TIRE_BL].dir=FOR; 00731 motor[TIRE_BR].dir=FOR; 00732 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right)); 00733 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right)); 00734 motor[TIRE_FL].pwm=25; 00735 motor[TIRE_FR].pwm=25; 00736 } 00737 } else if((Ult_left+Ult_right)<=25) { 00738 motor[TIRE_FL].dir=BACK; 00739 motor[TIRE_FR].dir=BACK; 00740 motor[TIRE_BL].dir=FOR; 00741 motor[TIRE_BR].dir=FOR; 00742 motor[TIRE_FL].pwm=25; 00743 motor[TIRE_FR].pwm=0; 00744 motor[TIRE_BL].pwm=0; 00745 motor[TIRE_BR].pwm=25; 00746 } else if((Ult_left+Ult_right)>=35) { 00747 motor[TIRE_FL].dir=BACK; 00748 motor[TIRE_FR].dir=BACK; 00749 motor[TIRE_BL].dir=FOR; 00750 motor[TIRE_BR].dir=FOR; 00751 motor[TIRE_FL].pwm=0; 00752 motor[TIRE_FR].pwm=25; 00753 motor[TIRE_BL].pwm=25; 00754 motor[TIRE_BR].pwm=0; 00755 } 00756 } else { 00757 if(Ult_left-Ult_right<=0) { 00758 motor[TIRE_FL].dir=BACK; 00759 motor[TIRE_FR].dir=BACK; 00760 motor[TIRE_BL].dir=FOR; 00761 motor[TIRE_BR].dir=FOR; 00762 00763 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right)); 00764 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right)); 00765 00766 motor[TIRE_BL].pwm=25; 00767 motor[TIRE_BR].pwm=25; 00768 } else if(Ult_left-Ult_right>0) { 00769 motor[TIRE_FL].dir=BACK; 00770 motor[TIRE_FR].dir=BACK; 00771 motor[TIRE_BL].dir=FOR; 00772 motor[TIRE_BR].dir=FOR; 00773 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right)); 00774 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right)); 00775 motor[TIRE_FL].pwm=25; 00776 motor[TIRE_FR].pwm=25; 00777 } 00778 } 00779 00780 } else { 00781 motor[TIRE_FL].dir=BRAKE; 00782 motor[TIRE_FR].dir=BRAKE; 00783 motor[TIRE_BL].dir=BRAKE; 00784 motor[TIRE_BR].dir=BRAKE; 00785 motor[TIRE_FL].pwm=100; 00786 motor[TIRE_FR].pwm=100; 00787 motor[TIRE_BL].pwm=100; 00788 motor[TIRE_BR].pwm=100; 00789 00790 } 00791 if(LimitSw::IsPressed(LEFTlim)) { 00792 count2=1; 00793 } 00794 00795 } else if(count2==1) { 00796 motor[TIRE_FL].dir=BRAKE; 00797 motor[TIRE_FR].dir=BRAKE; 00798 motor[TIRE_BL].dir=BRAKE; 00799 motor[TIRE_BR].dir=BRAKE; 00800 motor[TIRE_FL].pwm=255; 00801 motor[TIRE_FR].pwm=255; 00802 motor[TIRE_BL].pwm=255; 00803 motor[TIRE_BR].pwm=255; 00804 phase=10; 00805 } 00806 } else if(phase==3) { 00807 //洗濯ばさみを止める&戻す 00808 static int count3=0; 00809 static int loop=0; 00810 pc.printf("%d\r\n",loop); 00811 CLOTHESPIN=1; 00812 if(count3==0) { 00813 loop++; 00814 if(loop==60) { 00815 count3=1; 00816 } 00817 } else if(count3==1) { 00818 CLOTHESPIN=0; 00819 phase=4; 00820 00821 } 00822 } else if(phase==4) { 00823 //シーツをかける 00824 LedOut(3); 00825 CLOTHESPIN=0; 00826 if(!(LimitSw::IsPressed(RIGHTlim))) { 00827 00828 //超音波 00829 if(Ult_right>0) { 00830 if(Ult_right<15) { 00831 motor[TIRE_FL].dir=FOR; 00832 motor[TIRE_FR].dir=FOR; 00833 motor[TIRE_BL].dir=BACK; 00834 motor[TIRE_BR].dir=BACK; 00835 motor[TIRE_FL].pwm=25; 00836 motor[TIRE_FR].pwm=25+(15-Ult_right); 00837 motor[TIRE_BL].pwm=25+(15-Ult_right); 00838 motor[TIRE_BR].pwm=25; 00839 } else if(Ult_right>=15) { 00840 motor[TIRE_FL].dir=FOR; 00841 motor[TIRE_FR].dir=FOR; 00842 motor[TIRE_BL].dir=BACK; 00843 motor[TIRE_BR].dir=BACK; 00844 motor[TIRE_FL].pwm=25+(Ult_right-15); 00845 motor[TIRE_FR].pwm=25; 00846 motor[TIRE_BL].pwm=25; 00847 motor[TIRE_BR].pwm=25+(Ult_right-15); 00848 } 00849 } else { 00850 motor[TIRE_FL].dir=FOR; 00851 motor[TIRE_FR].dir=FOR; 00852 motor[TIRE_BL].dir=BACK; 00853 motor[TIRE_BR].dir=BACK; 00854 motor[TIRE_FL].pwm=25; 00855 motor[TIRE_FR].pwm=25; 00856 motor[TIRE_BL].pwm=25; 00857 motor[TIRE_BR].pwm=25; 00858 } 00859 00860 /* 00861 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき 00862 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) { 00863 if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) { 00864 if(Ult_left-Ult_right<=0) { 00865 motor[TIRE_FL].dir=FOR; 00866 motor[TIRE_FR].dir=FOR; 00867 motor[TIRE_BL].dir=BACK; 00868 motor[TIRE_BR].dir=BACK; 00869 00870 motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right)); 00871 motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right)); 00872 00873 motor[TIRE_FL].pwm=25; 00874 motor[TIRE_FR].pwm=25; 00875 } else if(Ult_left-Ult_right>0) { 00876 motor[TIRE_FL].dir=FOR; 00877 motor[TIRE_FR].dir=FOR; 00878 motor[TIRE_BL].dir=BACK; 00879 motor[TIRE_BR].dir=BACK; 00880 motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right)); 00881 motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right)); 00882 motor[TIRE_BL].pwm=25; 00883 motor[TIRE_BR].pwm=25; 00884 } 00885 } else if((Ult_left+Ult_right)<=25) { 00886 motor[TIRE_FL].dir=FOR; 00887 motor[TIRE_FR].dir=FOR; 00888 motor[TIRE_BL].dir=BACK; 00889 motor[TIRE_BR].dir=BACK; 00890 motor[TIRE_FL].pwm=0; 00891 motor[TIRE_FR].pwm=25; 00892 motor[TIRE_BL].pwm=25; 00893 motor[TIRE_BR].pwm=0; 00894 } else if((Ult_left+Ult_right)>=35) { 00895 motor[TIRE_FL].dir=FOR; 00896 motor[TIRE_FR].dir=FOR; 00897 motor[TIRE_BL].dir=BACK; 00898 motor[TIRE_BR].dir=BACK; 00899 motor[TIRE_FL].pwm=25; 00900 motor[TIRE_FR].pwm=0; 00901 motor[TIRE_BL].pwm=0; 00902 motor[TIRE_BR].pwm=25; 00903 } 00904 } else { 00905 if(Ult_left-Ult_right<=0) { 00906 motor[TIRE_FL].dir=FOR; 00907 motor[TIRE_FR].dir=FOR; 00908 motor[TIRE_BL].dir=BACK; 00909 motor[TIRE_BR].dir=BACK; 00910 00911 motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right)); 00912 motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right)); 00913 00914 motor[TIRE_FL].pwm=25; 00915 motor[TIRE_FR].pwm=25; 00916 00917 } else if(Ult_left-Ult_right>0) { 00918 motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right)); 00919 motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right)); 00920 motor[TIRE_BL].pwm=25; 00921 motor[TIRE_BR].pwm=25; 00922 } 00923 } 00924 } else { 00925 motor[TIRE_FL].dir=BRAKE; 00926 motor[TIRE_FR].dir=BRAKE; 00927 motor[TIRE_BL].dir=BRAKE; 00928 motor[TIRE_BR].dir=BRAKE; 00929 motor[TIRE_FL].pwm=100; 00930 motor[TIRE_FR].pwm=100; 00931 motor[TIRE_BL].pwm=100; 00932 motor[TIRE_BR].pwm=100; 00933 00934 } 00935 */ 00936 } else { 00937 motor[TIRE_FL].dir=BRAKE; 00938 motor[TIRE_FR].dir=BRAKE; 00939 motor[TIRE_BL].dir=BRAKE; 00940 motor[TIRE_BR].dir=BRAKE; 00941 motor[TIRE_FL].pwm=100; 00942 motor[TIRE_FR].pwm=100; 00943 motor[TIRE_BL].pwm=100; 00944 motor[TIRE_BR].pwm=100; 00945 phase=5; 00946 } 00947 } else if(phase==5) { 00948 motor[TIRE_FL].dir=BACK; 00949 motor[TIRE_FR].dir=BRAKE; 00950 motor[TIRE_BL].dir=BRAKE; 00951 motor[TIRE_BR].dir=BACK; 00952 motor[TIRE_FL].pwm=30; 00953 motor[TIRE_FR].pwm=100; 00954 motor[TIRE_BL].pwm=100; 00955 motor[TIRE_BR].pwm=30; 00956 //斜め後ろに下がる 00957 } else if(phase==10) { 00958 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき 00959 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合 00960 motor[TIRE_FL].dir=BRAKE; 00961 motor[TIRE_FR].dir=BRAKE; 00962 motor[TIRE_BL].dir=BRAKE; 00963 motor[TIRE_BR].dir=BRAKE; 00964 motor[TIRE_FL].pwm=255; 00965 motor[TIRE_FR].pwm=255; 00966 motor[TIRE_BL].pwm=255; 00967 motor[TIRE_BR].pwm=255; 00968 phase=3;//system phase increasing 00969 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合 00970 if(Ult_right>16) { //Ult_rightが遠い場合 00971 motor[TIRE_FL].dir=FOR; 00972 motor[TIRE_FR].dir=BACK; 00973 motor[TIRE_BL].dir=FOR; 00974 motor[TIRE_BR].dir=BACK; 00975 motor[TIRE_FL].pwm=0; 00976 motor[TIRE_FR].pwm=13; 00977 motor[TIRE_BL].pwm=0; 00978 motor[TIRE_BR].pwm=13; 00979 } else if(Ult_right<14) { //Ult_rightが近い場合 00980 motor[TIRE_FL].dir=BACK; 00981 motor[TIRE_FR].dir=FOR; 00982 motor[TIRE_BL].dir=BACK; 00983 motor[TIRE_BR].dir=FOR; 00984 motor[TIRE_FL].pwm=0; 00985 motor[TIRE_FR].pwm=13; 00986 motor[TIRE_BL].pwm=0; 00987 motor[TIRE_BR].pwm=13; 00988 } 00989 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合 00990 if(Ult_left>16) { //Ult_leftが遠い場合 00991 motor[TIRE_FL].dir=FOR; 00992 motor[TIRE_FR].dir=BACK; 00993 motor[TIRE_BL].dir=FOR; 00994 motor[TIRE_BR].dir=BACK; 00995 motor[TIRE_FL].pwm=13; 00996 motor[TIRE_FR].pwm=0; 00997 motor[TIRE_BL].pwm=13; 00998 motor[TIRE_BR].pwm=0; 00999 } else if(Ult_left<14) { //Ult_leftが近い場合 01000 motor[TIRE_FL].dir=BACK; 01001 motor[TIRE_FR].dir=FOR; 01002 motor[TIRE_BL].dir=BACK; 01003 motor[TIRE_BR].dir=FOR; 01004 motor[TIRE_FL].pwm=13; 01005 motor[TIRE_FR].pwm=0; 01006 motor[TIRE_BL].pwm=13; 01007 motor[TIRE_BR].pwm=0; 01008 } 01009 } else { //どっちもあってない場合 01010 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき 01011 if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき 01012 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき 01013 motor[TIRE_FL].dir=FOR; 01014 motor[TIRE_FR].dir=FOR; 01015 motor[TIRE_BL].dir=FOR; 01016 motor[TIRE_BR].dir=FOR; 01017 motor[TIRE_FL].pwm=10*Ult_left-Ult_right; 01018 motor[TIRE_FR].pwm=10*Ult_left-Ult_right; 01019 motor[TIRE_BL].pwm=10*Ult_left-Ult_right; 01020 motor[TIRE_BR].pwm=10*Ult_left-Ult_right; 01021 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき 01022 motor[TIRE_FL].dir=BACK; 01023 motor[TIRE_FR].dir=BACK; 01024 motor[TIRE_BL].dir=BACK; 01025 motor[TIRE_BR].dir=BACK; 01026 motor[TIRE_FL].pwm=10*Ult_left-Ult_right; 01027 motor[TIRE_FR].pwm=10*Ult_left-Ult_right; 01028 motor[TIRE_BL].pwm=10*Ult_left-Ult_right; 01029 motor[TIRE_BR].pwm=10*Ult_left-Ult_right; 01030 } 01031 } 01032 } else { //さほど離れてはいないが傾きが大きいとき 01033 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき 01034 motor[TIRE_FL].dir=FOR; 01035 motor[TIRE_FR].dir=FOR; 01036 motor[TIRE_BL].dir=FOR; 01037 motor[TIRE_BR].dir=FOR; 01038 motor[TIRE_FL].pwm=10*Ult_left-Ult_right; 01039 motor[TIRE_FR].pwm=10*Ult_left-Ult_right; 01040 motor[TIRE_BL].pwm=10*Ult_left-Ult_right; 01041 motor[TIRE_BR].pwm=10*Ult_left-Ult_right; 01042 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき 01043 motor[TIRE_FL].dir=BACK; 01044 motor[TIRE_FR].dir=BACK; 01045 motor[TIRE_BL].dir=BACK; 01046 motor[TIRE_BR].dir=BACK; 01047 motor[TIRE_FL].pwm=10*Ult_left-Ult_right; 01048 motor[TIRE_FR].pwm=10*Ult_left-Ult_right; 01049 motor[TIRE_BL].pwm=10*Ult_left-Ult_right; 01050 motor[TIRE_BR].pwm=10*Ult_left-Ult_right; 01051 } 01052 } 01053 } 01054 01055 } else {//データを受け取ってないとき 01056 motor[TIRE_FL].dir=BRAKE; 01057 motor[TIRE_FR].dir=BRAKE; 01058 motor[TIRE_BL].dir=BRAKE; 01059 motor[TIRE_BR].dir=BRAKE; 01060 motor[TIRE_FL].pwm=100; 01061 motor[TIRE_FR].pwm=100; 01062 motor[TIRE_BL].pwm=100; 01063 motor[TIRE_BR].pwm=100; 01064 } 01065 } else { 01066 //一応 01067 motor[TIRE_FL].dir=BRAKE; 01068 motor[TIRE_FR].dir=BRAKE; 01069 motor[TIRE_BL].dir=BRAKE; 01070 motor[TIRE_BR].dir=BRAKE; 01071 motor[TIRE_FL].pwm=100; 01072 motor[TIRE_FR].pwm=100; 01073 motor[TIRE_BL].pwm=100; 01074 motor[TIRE_BR].pwm=100; 01075 } 01076 } else { 01077 motor[TIRE_FL].dir=BRAKE; 01078 motor[TIRE_FR].dir=BRAKE; 01079 motor[TIRE_BL].dir=BRAKE; 01080 motor[TIRE_BR].dir=BRAKE; 01081 motor[TIRE_FL].pwm=100; 01082 motor[TIRE_FR].pwm=100; 01083 motor[TIRE_BL].pwm=100; 01084 motor[TIRE_BR].pwm=100; 01085 } 01086 } 01087 #endif 01088 01089 #if USE_PROCESS_NUM>3 01090 static void Process3() 01091 { 01092 AllActuatorReset(); 01093 lineFase = 0; 01094 } 01095 #endif 01096 01097 #if USE_PROCESS_NUM>4 01098 static void Process4() 01099 { 01100 01101 static int x,y; 01102 static int count = 0; 01103 01104 linePara_U = lineCast(LineHub::GetPara(0)); 01105 linePara_B = lineCast(LineHub::GetPara(2)); 01106 linePara_L = lineCast(LineHub::GetPara(3)); 01107 linePara_R = lineCast(LineHub::GetPara(4)); 01108 01109 if(linePara_B == 'A' && count == 0) { 01110 lineFase = 1; 01111 } 01112 01113 if(lineFase == 0) { 01114 pw = 0.5; 01115 switch(linePara_U) { 01116 case -2: 01117 x = 5; 01118 y = 3; 01119 break; 01120 case -3: 01121 x = 5; 01122 y = 3; 01123 break; 01124 case -1: 01125 x = 6; 01126 y = 3; 01127 break; 01128 case 0: 01129 x = 7; 01130 y = 3; 01131 break; 01132 case 1: 01133 x = 8; 01134 y = 3; 01135 break; 01136 case 3: 01137 x = 9; 01138 y = 3; 01139 break; 01140 case 2: 01141 x = 9; 01142 y = 3; 01143 break; 01144 case 'A': 01145 lineCheck = true; 01146 x = x; 01147 y = y; 01148 break; 01149 case 'N': 01150 x = 7; 01151 y = 7; 01152 break; 01153 x = 7; 01154 y = 7; 01155 default: 01156 x = 9; 01157 y = 9; 01158 } 01159 if(lineCheck == true && (!(linePara_U) == 'A')) { 01160 count++; 01161 } 01162 } else if(lineFase == 1) { 01163 pw = 0.3; 01164 x = 7; 01165 y = 9; 01166 if(linePara_R == 0) { 01167 lineFase = 2; 01168 x = 7; 01169 y = 7; 01170 } 01171 } else if(lineFase == 2) { 01172 x = 7; 01173 y = 7; 01174 } else { 01175 x = 7; 01176 y = 7; 01177 } 01178 01179 int t = 0; 01180 if((linePara_U + linePara_B) > 3) t = 1; 01181 01182 if(controller->Button.A) { 01183 motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] ); 01184 motor[TIRE_FL].dir = SetStatus(omni[y][x]); 01185 motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]); 01186 motor[TIRE_FR].dir = SetStatus(omni[x][14-y]); 01187 01188 motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw; 01189 motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw; 01190 motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw; 01191 motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw; 01192 } else { 01193 motor[TIRE_BL].dir = SetStatus(0); 01194 motor[TIRE_FL].dir = SetStatus(0); 01195 motor[TIRE_BR].dir = SetStatus(0); 01196 motor[TIRE_FR].dir = SetStatus(0); 01197 01198 motor[TIRE_FR].pwm = SetPWM(0); 01199 motor[TIRE_FL].pwm = SetPWM(0); 01200 motor[TIRE_BR].pwm = SetPWM(0); 01201 motor[TIRE_BL].pwm = SetPWM(0); 01202 } 01203 01204 } 01205 #endif 01206 01207 #if USE_PROCESS_NUM>5 01208 static void Process5() 01209 { 01210 lineFase = 0; 01211 lineCheck = true; 01212 } 01213 #endif 01214 01215 #if USE_PROCESS_NUM>6 01216 static void Process6() 01217 { 01218 01219 for(int i = 0; i < 8; i++) { 01220 linePara[i] = lineCast(LineHub::GetPara(i)); 01221 } 01222 01223 static int count = 100000; 01224 count++; 01225 01226 if(count < 10000) { 01227 lineCheck = false; 01228 } else { 01229 lineCheck = true; 01230 } 01231 01232 if(lineFase == 0) { // 前進 01233 switch(linePara[0]) { 01234 motor[TIRE_FL].dir = FOR; 01235 motor[TIRE_BL].dir = BRAKE; 01236 motor[TIRE_BR].dir = BACK; 01237 motor[TIRE_FR].dir = BRAKE; 01238 motor[TIRE_FL].pwm = 30; 01239 motor[TIRE_FR].pwm = 0; 01240 motor[TIRE_BR].pwm = 30; 01241 motor[TIRE_BL].pwm = 0; 01242 break; 01243 case -3: 01244 motor[TIRE_FL].dir = FOR; 01245 motor[TIRE_BL].dir = BACK; 01246 motor[TIRE_BR].dir = BACK; 01247 motor[TIRE_FR].dir = FOR; 01248 motor[TIRE_FL].pwm = 30; 01249 motor[TIRE_FR].pwm = 10; 01250 motor[TIRE_BR].pwm = 30; 01251 motor[TIRE_BL].pwm = 10; 01252 break; 01253 case -1: 01254 motor[TIRE_FL].dir = FOR; 01255 motor[TIRE_BL].dir = BACK; 01256 motor[TIRE_BR].dir = BACK; 01257 motor[TIRE_FR].dir = FOR; 01258 motor[TIRE_FL].pwm = 30; 01259 motor[TIRE_FR].pwm = 20; 01260 motor[TIRE_BR].pwm = 30; 01261 motor[TIRE_BL].pwm = 20; 01262 break; 01263 case 0: 01264 motor[TIRE_FL].dir = FOR; 01265 motor[TIRE_BL].dir = BACK; 01266 motor[TIRE_BR].dir = BACK; 01267 motor[TIRE_FR].dir = FOR; 01268 motor[TIRE_FL].pwm = 30; 01269 motor[TIRE_FR].pwm = 30; 01270 motor[TIRE_BR].pwm = 30; 01271 motor[TIRE_BL].pwm = 30; 01272 break; 01273 case 1: 01274 motor[TIRE_FL].dir = FOR; 01275 motor[TIRE_BL].dir = BACK; 01276 motor[TIRE_BR].dir = BACK; 01277 motor[TIRE_FR].dir = FOR; 01278 motor[TIRE_FL].pwm = 20; 01279 motor[TIRE_FR].pwm = 30; 01280 motor[TIRE_BR].pwm = 20; 01281 motor[TIRE_BL].pwm = 30; 01282 break; 01283 case 3: 01284 motor[TIRE_FL].dir = FOR; 01285 motor[TIRE_BL].dir = FOR; 01286 motor[TIRE_BR].dir = BACK; 01287 motor[TIRE_FR].dir = BACK; 01288 motor[TIRE_FL].pwm = 10; 01289 motor[TIRE_FR].pwm = 30; 01290 motor[TIRE_BR].pwm = 10; 01291 motor[TIRE_BL].pwm = 30; 01292 break; 01293 case 2: 01294 motor[TIRE_FL].dir = BRAKE; 01295 motor[TIRE_BL].dir = FOR; 01296 motor[TIRE_BR].dir = BRAKE; 01297 motor[TIRE_FR].dir = BACK; 01298 motor[TIRE_FL].pwm = 0; 01299 motor[TIRE_FR].pwm = 30; 01300 motor[TIRE_BR].pwm = 0; 01301 motor[TIRE_BL].pwm = 30; 01302 break; 01303 case 'A': 01304 motor[TIRE_FL].dir = FOR; 01305 motor[TIRE_BL].dir = BACK; 01306 motor[TIRE_BR].dir = BACK; 01307 motor[TIRE_FR].dir = FOR; 01308 motor[TIRE_FL].pwm = 30; 01309 motor[TIRE_FR].pwm = 30; 01310 motor[TIRE_BR].pwm = 30; 01311 motor[TIRE_BL].pwm = 30; 01312 if(lineCheck == true) { 01313 lineCount++; 01314 count = 0; 01315 } 01316 default: 01317 motor[TIRE_FL].dir = BRAKE; 01318 motor[TIRE_BL].dir = BRAKE; 01319 motor[TIRE_BR].dir = BRAKE; 01320 motor[TIRE_FR].dir = BRAKE; 01321 motor[TIRE_FL].pwm = 30; 01322 motor[TIRE_FR].pwm = 30; 01323 motor[TIRE_BR].pwm = 30; 01324 motor[TIRE_BL].pwm = 30; 01325 } 01326 if(lineCount == 1) { 01327 lineFase = 1; 01328 } 01329 } else if(lineFase == 1) { // 前進 低速 01330 motor[TIRE_FL].dir = FOR; 01331 motor[TIRE_BL].dir = FOR; 01332 motor[TIRE_BR].dir = BACK; 01333 motor[TIRE_FR].dir = BACK; 01334 motor[TIRE_FL].pwm = 15; 01335 motor[TIRE_FR].pwm = 15; 01336 motor[TIRE_BR].pwm = 15; 01337 motor[TIRE_BL].pwm = 15; 01338 if(linePara[4] == 0) { 01339 lineFase = 2; 01340 motor[TIRE_FL].dir = BRAKE; 01341 motor[TIRE_BL].dir = BRAKE; 01342 motor[TIRE_BR].dir = BRAKE; 01343 motor[TIRE_FR].dir = BRAKE; 01344 motor[TIRE_FL].pwm = 30; 01345 motor[TIRE_FR].pwm = 30; 01346 motor[TIRE_BR].pwm = 30; 01347 motor[TIRE_BL].pwm = 30; 01348 } 01349 } else if(lineFase == 2) { // 位置調整 01350 lineFase = 3; 01351 } else if(lineFase == 3) { // 右 直進 01352 motor[TIRE_FL].dir = BRAKE; 01353 motor[TIRE_BL].dir = BRAKE; 01354 motor[TIRE_BR].dir = BRAKE; 01355 motor[TIRE_FR].dir = BRAKE; 01356 motor[TIRE_FL].pwm = 30; 01357 motor[TIRE_FR].pwm = 30; 01358 motor[TIRE_BR].pwm = 30; 01359 motor[TIRE_BL].pwm = 30; 01360 } 01361 01362 } 01363 #endif 01364 Timer samp; 01365 #if USE_PROCESS_NUM>7 01366 static void Process7() 01367 { 01368 ; 01369 static int flag=0; 01370 static int FL_PWM=0; 01371 static int FR_PWM=0; 01372 static int BL_PWM=0; 01373 static int BR_PWM=0; 01374 int targetpulse=1000; 01375 01376 if(flag==0) { 01377 01378 samp.start(); 01379 01380 flag=1; 01381 } else { 01382 int pulseFL = -encoder[FL].getPulses()/samp.read(); 01383 int pulseBL = -encoder[BL].getPulses()/samp.read(); 01384 int pulseBR = -encoder[BR].getPulses()/samp.read(); 01385 int pulseFR = -encoder[FR].getPulses()/samp.read(); 01386 pc.printf("%f ,pulseFL:%d\r\n",samp.read(),pulseFL); 01387 01388 FL_PWM=FL_PWM+=0.01*(targetpulse-pulseFL); 01389 FR_PWM=FR_PWM+=0.01*(targetpulse-pulseFR); 01390 BL_PWM=BL_PWM+=0.01*(targetpulse-pulseBL); 01391 BR_PWM=BR_PWM+=0.01*(targetpulse-pulseBR); 01392 motor[TIRE_FL].dir = FOR; 01393 motor[TIRE_BL].dir = FOR; 01394 motor[TIRE_BR].dir = FOR; 01395 motor[TIRE_FR].dir = FOR; 01396 motor[TIRE_FL].pwm = FL_PWM; 01397 motor[TIRE_FR].pwm = FR_PWM; 01398 motor[TIRE_BR].pwm = BR_PWM; 01399 motor[TIRE_BL].pwm = BL_PWM; 01400 } 01401 } 01402 #endif 01403 01404 #if USE_PROCESS_NUM>8 01405 static void Process8() 01406 { 01407 if(controller->Button.A) { 01408 rotaconSampling.start(); 01409 PIDflag = true; 01410 01411 //linePara_U = LineHub::GetPara(0); 01412 //linePara_B = LineHub::GetPara(3); 01413 01414 01415 pulsePV[FL] = encoder[FL].getPulses(); 01416 pulsePV[BL] = encoder[BL].getPulses(); 01417 pulsePV[BR] = encoder[BR].getPulses(); 01418 pulsePV[FR] = encoder[FR].getPulses(); 01419 01420 01421 for (int i = 0; i < 4; i++) { 01422 timeCV[i] = timePV[i]; 01423 timePV[i] = rotaconSampling.read(); 01424 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60; 01425 pulseCV[i] = pulsePV[i]; 01426 } 01427 01428 move.Vx = 0.5; 01429 move.Vy = 0.5; 01430 move.Va = 0; 01431 01432 correction_LT.Vx = 0; //0.1 * linePara_U; 01433 correction_LT.Vy = 0; 01434 correction_LT.Va = 0; 01435 01436 synthetic.Vx = move.Vx + correction_LT.Vx; 01437 synthetic.Vy = move.Vy + correction_LT.Vy; 01438 synthetic.Va = move.Va + correction_LT.Va; 01439 01440 sita = 0; 01441 01442 //タイヤの目標速度算出 01443 float sinR = 0.7071f * (float)sin(sita); 01444 float cosR = 0.7071f * (float)cos(sita); 01445 float nv = (60 * 1000) / ( 2.00 * PI * tireR); 01446 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; 01447 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; 01448 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; 01449 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; 01450 01451 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]); 01452 01453 //PIDによるPWM算出 01454 01455 //モータの駆動 01456 for (int i = 0; i < 4; i++) { 01457 if (tirePWM[i] > 255) { 01458 tirePWM[i] = 255; 01459 } else if (tirePWM[i] < -255) { 01460 tirePWM[i] = -255; 01461 } 01462 } 01463 01464 for(int i = 0; i < 4; i++) { 01465 motor[i].dir = SetStatus(tirePWM[i]); 01466 motor[i].pwm = SetPWM(tirePWM[i]); 01467 } 01468 } else { 01469 PIDflag = false; 01470 rotaconSampling.stop(); 01471 rotaconSampling.reset(); 01472 for(int i = 0; i < 4; i++) { 01473 encoder[i].reset(); 01474 pulsePV[i] = 0; 01475 pulseCV[i] = 0; 01476 timePV[i] = 0; 01477 timeCV[i] = 0; 01478 tirePWM[i] = 0; 01479 motor[i].dir = SetStatus(tirePWM[i]); 01480 motor[i].pwm = SetPWM(tirePWM[i]); 01481 } 01482 } 01483 } 01484 #endif 01485 01486 #if USE_PROCESS_NUM>9 01487 static void Process9() 01488 { 01489 01490 } 01491 #endif 01492 #endif 01493 #pragma endregion PROCESS 01494 01495 static void AllActuatorReset() 01496 { 01497 01498 #ifdef USE_SOLENOID 01499 solenoid.all = ALL_SOLENOID_OFF; 01500 #endif 01501 01502 #ifdef USE_MOTOR 01503 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 01504 motor[i].dir = FREE; 01505 motor[i].pwm = 0; 01506 } 01507 #endif 01508 } 01509 01510 void BuzzerTimer_func() 01511 { 01512 buzzer = !buzzer; 01513 //LED_DEBUG0 = !LED_DEBUG0; 01514 } 01515 01516 void TapeLedEms_func() 01517 { 01518 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 01519 } 01520 01521 #pragma region USER-DEFINED-FUNCTIONS 01522 void tirePID() 01523 { 01524 if(PIDflag == true) { 01525 //加算するPID値の算出 01526 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]); 01527 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]); 01528 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]); 01529 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]); 01530 //PID値の加算 01531 tirePWM[FL] += rotaconPID[0].GetMV(); 01532 tirePWM[BL] += rotaconPID[1].GetMV(); 01533 tirePWM[FR] += rotaconPID[2].GetMV(); 01534 tirePWM[BR] += rotaconPID[3].GetMV(); 01535 } 01536 } 01537 01538 int lineCast(char k) 01539 { 01540 int l; 01541 switch(k) { 01542 case 255: 01543 l = -1; 01544 break; 01545 case 254: 01546 l = -2; 01547 break; 01548 case 253: 01549 l = -3; 01550 break; 01551 default: 01552 l = k; 01553 } 01554 return l; 01555 } 01556 01557 #pragma endregion
Generated on Sun Jul 17 2022 15:33:13 by
1.7.2