大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_26_7zimae

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@38:391a0c8e4c16, 2019-09-26 (annotated)

Committer:
yabahiro
Date:
Thu Sep 26 09:56:28 2019 +0000
Revision:
38:391a0c8e4c16
Parent:
37:7cfa39c41245 
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
yabahiro 21:1f1e9c585da8 8 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
kishibekairohan 16:3f2c2d89372b 13 #include "../../Input/Encoder/Encoder.h"
t_yamamoto 0:669ef71cba68 14 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 15 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 16 #include "../Using.h"
t_yamamoto 0:669ef71cba68 17
t_yamamoto 0:669ef71cba68 18 using namespace SWITCH;
7ka884 4:ba9df71868df 19 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 20 using namespace ENCODER;
yabahiro 21:1f1e9c585da8 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 24 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static bool lock;
t_yamamoto 0:669ef71cba68 28 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 29 static int current;
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 34 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 35 #endif
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 40
kishibekairohan 2:c015739085d3 41 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 42
kishibekairohan 16:3f2c2d89372b 43 //**************Encoder***************
kishibekairohan 16:3f2c2d89372b 44 const int PerRev = 256;
kishibekairohan 16:3f2c2d89372b 45 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 46 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 47 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 48 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 49 //**************Encoder***************
kishibekairohan 7:e88c5d47a3be 50
kishibekairohan 16:3f2c2d89372b 51 //**************Buzzer****************
kishibekairohan 11:028a150943b5 52 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 53 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 54 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 55 bool EMGflag = false;
yabahiro 21:1f1e9c585da8 56 //PWMOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 57 //**************Buzzer****************
kishibekairohan 11:028a150943b5 58
kishibekairohan 11:028a150943b5 59 //************TapeLed*****************
kishibekairohan 11:028a150943b5 60 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 61 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 62 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 63 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 64 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 65 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 66
M_souta 18:c694bae76e51 67 float tireProRPM[4];
M_souta 18:c694bae76e51 68 float tireTarRPM[4];
yabahiro 21:1f1e9c585da8 69 float tirepwm[4];
yabahiro 21:1f1e9c585da8 70
yabahiro 21:1f1e9c585da8 71 const int omni[15][15] = {
yabahiro 21:1f1e9c585da8 72 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 73 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 74 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 75 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
yabahiro 21:1f1e9c585da8 76 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
yabahiro 21:1f1e9c585da8 77 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
yabahiro 21:1f1e9c585da8 78 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
yabahiro 21:1f1e9c585da8 79 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
yabahiro 21:1f1e9c585da8 80 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
yabahiro 21:1f1e9c585da8 81 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
yabahiro 21:1f1e9c585da8 82 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
yabahiro 21:1f1e9c585da8 83 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
yabahiro 21:1f1e9c585da8 84 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
yabahiro 21:1f1e9c585da8 85 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
yabahiro 21:1f1e9c585da8 86 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
yabahiro 21:1f1e9c585da8 87 };
yabahiro 21:1f1e9c585da8 88
yabahiro 21:1f1e9c585da8 89 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
yabahiro 21:1f1e9c585da8 90 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
yabahiro 21:1f1e9c585da8 91
yabahiro 21:1f1e9c585da8 92 uint8_t SetStatus(int);
yabahiro 21:1f1e9c585da8 93 uint8_t SetStatus(int pwmVal)
yabahiro 21:1f1e9c585da8 94 {
yabahiro 21:1f1e9c585da8 95 if (pwmVal < 0) return BACK;
yabahiro 21:1f1e9c585da8 96 else if (pwmVal > 0) return FOR;
yabahiro 21:1f1e9c585da8 97 else if (pwmVal == 0) return BRAKE;
yabahiro 21:1f1e9c585da8 98 else return BRAKE;
yabahiro 21:1f1e9c585da8 99 }
yabahiro 21:1f1e9c585da8 100 uint8_t Setpwm(int);
yabahiro 21:1f1e9c585da8 101 uint8_t Setpwm(int pwmVal)
yabahiro 21:1f1e9c585da8 102 {
yabahiro 21:1f1e9c585da8 103 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
yabahiro 21:1f1e9c585da8 104 else return abs(pwmVal);
yabahiro 21:1f1e9c585da8 105 }
M_souta 18:c694bae76e51 106
yabahiro 35:94f026ab4d30 107 void LedMode(int led)
yabahiro 35:94f026ab4d30 108 {
yabahiro 36:788274604650 109 switch(led) {
yabahiro 36:788274604650 110 case 1:
yabahiro 36:788274604650 111 POTENTIOMETER::dio[0]= 1;
yabahiro 36:788274604650 112 POTENTIOMETER::dio[1]= 0;
yabahiro 36:788274604650 113 POTENTIOMETER::dio[2]= 0;
yabahiro 36:788274604650 114 POTENTIOMETER::dio[3]= 0;
yabahiro 36:788274604650 115 break;
yabahiro 36:788274604650 116 case 2:
yabahiro 36:788274604650 117 POTENTIOMETER::dio[0]= 0;
yabahiro 36:788274604650 118 POTENTIOMETER::dio[1]= 1;
yabahiro 36:788274604650 119 POTENTIOMETER::dio[2]= 0;
yabahiro 36:788274604650 120 POTENTIOMETER::dio[3]= 0;
yabahiro 36:788274604650 121 break;
yabahiro 36:788274604650 122 case 3:
yabahiro 36:788274604650 123 POTENTIOMETER::dio[0]= 1;
yabahiro 36:788274604650 124 POTENTIOMETER::dio[1]= 1;
yabahiro 36:788274604650 125 POTENTIOMETER::dio[2]= 0;
yabahiro 36:788274604650 126 POTENTIOMETER::dio[3]= 0;
yabahiro 36:788274604650 127 break;
yabahiro 36:788274604650 128 case 4:
yabahiro 36:788274604650 129 POTENTIOMETER::dio[0]= 0;
yabahiro 36:788274604650 130 POTENTIOMETER::dio[1]= 0;
yabahiro 36:788274604650 131 POTENTIOMETER::dio[2]= 1;
yabahiro 36:788274604650 132 POTENTIOMETER::dio[3]= 0;
yabahiro 36:788274604650 133 break;
yabahiro 36:788274604650 134 }
yabahiro 35:94f026ab4d30 135 /*
yabahiro 35:94f026ab4d30 136 if(led/8>=1) {
yabahiro 36:788274604650 137 POTENTIOMETER::dio[3]= 1;
yabahiro 35:94f026ab4d30 138 led=led%8;
yabahiro 35:94f026ab4d30 139 } else {
yabahiro 36:788274604650 140 POTENTIOMETER::dio[3]= 0;
yabahiro 35:94f026ab4d30 141 }
yabahiro 35:94f026ab4d30 142
yabahiro 35:94f026ab4d30 143 if(led/4>=1) {
yabahiro 36:788274604650 144 POTENTIOMETER::dio[2]= 1;
yabahiro 36:788274604650 145 led=led%4;
yabahiro 36:788274604650 146 } else {
yabahiro 36:788274604650 147 POTENTIOMETER::dio[2]= 0;
yabahiro 36:788274604650 148 }
yabahiro 36:788274604650 149
yabahiro 36:788274604650 150 if(led/2>=1) {
yabahiro 35:94f026ab4d30 151 POTENTIOMETER::dio[1]= 1;
yabahiro 36:788274604650 152 led=led%2;
yabahiro 35:94f026ab4d30 153 } else {
yabahiro 35:94f026ab4d30 154 POTENTIOMETER::dio[1]= 0;
yabahiro 35:94f026ab4d30 155 }
yabahiro 35:94f026ab4d30 156
yabahiro 36:788274604650 157 if(led/1>=1) {
yabahiro 36:788274604650 158 POTENTIOMETER::dio[0]= 1;
yabahiro 36:788274604650 159 led=0;
yabahiro 35:94f026ab4d30 160 } else {
yabahiro 36:788274604650 161 POTENTIOMETER::dio[0]= 0;
yabahiro 35:94f026ab4d30 162 }
yabahiro 35:94f026ab4d30 163 */
yabahiro 35:94f026ab4d30 164 }
yabahiro 31:45f97698857b 165 int Twsh;
yabahiro 31:45f97698857b 166 bool UP_flag = false;
yabahiro 35:94f026ab4d30 167 bool SW_flag = false;
yabahiro 31:45f97698857b 168 bool Air_flag = false;
yabahiro 35:94f026ab4d30 169 bool zyouge;
yabahiro 31:45f97698857b 170 bool dz1=true;
yabahiro 31:45f97698857b 171 bool dz1i=false;
yabahiro 31:45f97698857b 172 bool dz2=true;
yabahiro 31:45f97698857b 173 bool dz2i=false;
yabahiro 31:45f97698857b 174 bool dz3=true;
yabahiro 31:45f97698857b 175 bool dz3i=false;
yabahiro 31:45f97698857b 176 bool dz4=true;
yabahiro 31:45f97698857b 177 bool dz4i=false;
yabahiro 31:45f97698857b 178 bool zone;
yabahiro 31:45f97698857b 179 bool through=false;
yabahiro 35:94f026ab4d30 180 bool counts=false;
yabahiro 36:788274604650 181 bool mtc=false;
yabahiro 36:788274604650 182 bool mtc2 = false;
yabahiro 31:45f97698857b 183
yabahiro 31:45f97698857b 184 int mode =1;
yabahiro 31:45f97698857b 185 int cross=0;//十字数える用
yabahiro 35:94f026ab4d30 186 int cros=0;
yabahiro 31:45f97698857b 187 int count=100000;//wait代替え
kishibekairohan 33:656aa14904a5 188
kishibekairohan 33:656aa14904a5 189 ///*********PWM調整用*********///
kishibekairohan 33:656aa14904a5 190 int fast =40;
kishibekairohan 33:656aa14904a5 191 int normal = 30;
kishibekairohan 33:656aa14904a5 192 int slow = 20;
yabahiro 31:45f97698857b 193
t_yamamoto 0:669ef71cba68 194 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 195
t_yamamoto 0:669ef71cba68 196 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 198 static void Process0(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 201 static void Process1(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 204 static void Process2(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 207 static void Process3(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 210 static void Process4(void);
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 213 static void Process5(void);
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 216 static void Process6(void);
t_yamamoto 0:669ef71cba68 217 #endif
t_yamamoto 0:669ef71cba68 218 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 219 static void Process7(void);
t_yamamoto 0:669ef71cba68 220 #endif
t_yamamoto 0:669ef71cba68 221 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 222 static void Process8(void);
t_yamamoto 0:669ef71cba68 223 #endif
t_yamamoto 0:669ef71cba68 224 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 225 static void Process9(void);
t_yamamoto 0:669ef71cba68 226 #endif
t_yamamoto 0:669ef71cba68 227 #endif
t_yamamoto 0:669ef71cba68 228
t_yamamoto 0:669ef71cba68 229 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 230 {
yabahiro 21:1f1e9c585da8 231 #pragma region USER-DEFINED_VARIABLE_INIT
yabahiro 21:1f1e9c585da8 232 /*Replace here with the initialization code of your variables.*/
yabahiro 21:1f1e9c585da8 233 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 234
yabahiro 21:1f1e9c585da8 235 lock = true;
yabahiro 21:1f1e9c585da8 236 processChangeComp = true;
yabahiro 21:1f1e9c585da8 237 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 238
yabahiro 21:1f1e9c585da8 239 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 240 #if USE_PROCESS_NUM>0
yabahiro 21:1f1e9c585da8 241 Process[0] = Process0;
yabahiro 21:1f1e9c585da8 242 #endif
yabahiro 21:1f1e9c585da8 243 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 244 Process[1] = Process1;
yabahiro 21:1f1e9c585da8 245 #endif
yabahiro 21:1f1e9c585da8 246 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 247 Process[2] = Process2;
yabahiro 21:1f1e9c585da8 248 #endif
yabahiro 21:1f1e9c585da8 249 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 250 Process[3] = Process3;
yabahiro 21:1f1e9c585da8 251 #endif
yabahiro 21:1f1e9c585da8 252 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 253 Process[4] = Process4;
yabahiro 21:1f1e9c585da8 254 #endif
yabahiro 21:1f1e9c585da8 255 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 256 Process[5] = Process5;
yabahiro 21:1f1e9c585da8 257 #endif
yabahiro 21:1f1e9c585da8 258 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 259 Process[6] = Process6;
yabahiro 21:1f1e9c585da8 260 #endif
yabahiro 21:1f1e9c585da8 261 #if USE_PROCESS_NUM>7
yabahiro 21:1f1e9c585da8 262 Process[7] = Process7;
yabahiro 21:1f1e9c585da8 263 #endif
yabahiro 21:1f1e9c585da8 264 #if USE_PROCESS_NUM>8
yabahiro 21:1f1e9c585da8 265 Process[8] = Process8;
yabahiro 21:1f1e9c585da8 266 #endif
yabahiro 21:1f1e9c585da8 267 #if USE_PROCESS_NUM>9
yabahiro 21:1f1e9c585da8 268 Process[9] = Process9;
yabahiro 21:1f1e9c585da8 269 #endif
yabahiro 21:1f1e9c585da8 270 #endif
t_yamamoto 0:669ef71cba68 271 }
t_yamamoto 0:669ef71cba68 272
t_yamamoto 0:669ef71cba68 273 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 274 {
yabahiro 21:1f1e9c585da8 275 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 276 if(controller->Button.HOME) lock = false;
yabahiro 21:1f1e9c585da8 277
yabahiro 21:1f1e9c585da8 278 if(controller->Button.START && processChangeComp) {
yabahiro 21:1f1e9c585da8 279 current++;
yabahiro 21:1f1e9c585da8 280 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
yabahiro 21:1f1e9c585da8 281 processChangeComp = false;
yabahiro 21:1f1e9c585da8 282 } else if(controller->Button.SELECT && processChangeComp) {
yabahiro 21:1f1e9c585da8 283 current--;
yabahiro 21:1f1e9c585da8 284 if (current < 0) current = 0;
yabahiro 21:1f1e9c585da8 285 processChangeComp = false;
yabahiro 21:1f1e9c585da8 286 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
yabahiro 21:1f1e9c585da8 287 #endif
yabahiro 21:1f1e9c585da8 288
yabahiro 21:1f1e9c585da8 289 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 290 ACTUATORHUB::MOTOR::Motor::Update(motor);
yabahiro 21:1f1e9c585da8 291 #endif
yabahiro 21:1f1e9c585da8 292
yabahiro 21:1f1e9c585da8 293 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 294 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
yabahiro 21:1f1e9c585da8 295 #endif
yabahiro 21:1f1e9c585da8 296
yabahiro 21:1f1e9c585da8 297 #ifdef USE_RS485
yabahiro 21:1f1e9c585da8 298 ACTUATORHUB::ActuatorHub::Update();
yabahiro 21:1f1e9c585da8 299 #endif
yabahiro 21:1f1e9c585da8 300
yabahiro 21:1f1e9c585da8 301 }
yabahiro 21:1f1e9c585da8 302
yabahiro 21:1f1e9c585da8 303 int g[8];
yabahiro 21:1f1e9c585da8 304
yabahiro 21:1f1e9c585da8 305 void SystemProcess()
yabahiro 21:1f1e9c585da8 306 {
yabahiro 21:1f1e9c585da8 307 SystemProcessInitialize();
yabahiro 21:1f1e9c585da8 308
yabahiro 21:1f1e9c585da8 309 while(1) {
yabahiro 21:1f1e9c585da8 310 for(int i = 0; i < 8; i++) {
yabahiro 21:1f1e9c585da8 311 g[i] = LineHub::GetPara(i);
yabahiro 21:1f1e9c585da8 312 }
yabahiro 35:94f026ab4d30 313
yabahiro 36:788274604650 314 //printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
yabahiro 35:94f026ab4d30 315
yabahiro 21:1f1e9c585da8 316 #ifdef USE_MU
yabahiro 21:1f1e9c585da8 317 controller = CONTROLLER::Controller::GetData();
yabahiro 21:1f1e9c585da8 318 #endif
yabahiro 21:1f1e9c585da8 319
yabahiro 21:1f1e9c585da8 320 #ifdef USE_ERRORCHECK
yabahiro 21:1f1e9c585da8 321 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
yabahiro 21:1f1e9c585da8 322 CONTROLLER::Controller::DataReset();
yabahiro 21:1f1e9c585da8 323 AllActuatorReset();
yabahiro 21:1f1e9c585da8 324 lock = true;
yabahiro 21:1f1e9c585da8 325 } else
yabahiro 21:1f1e9c585da8 326 #endif
yabahiro 21:1f1e9c585da8 327 {
yabahiro 21:1f1e9c585da8 328
yabahiro 21:1f1e9c585da8 329 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 330 if(!lock) {
yabahiro 21:1f1e9c585da8 331 Process[current]();
yabahiro 21:1f1e9c585da8 332 } else
yabahiro 21:1f1e9c585da8 333 #endif
yabahiro 21:1f1e9c585da8 334 {
yabahiro 21:1f1e9c585da8 335 //ロック時の処理
yabahiro 21:1f1e9c585da8 336 }
yabahiro 21:1f1e9c585da8 337 }
yabahiro 21:1f1e9c585da8 338 /*
yabahiro 21:1f1e9c585da8 339 //Emergency!
yabahiro 21:1f1e9c585da8 340 if(!EMG_0 && !EMG_1 && !EMGflag) {
yabahiro 21:1f1e9c585da8 341 buzzer = 0;
yabahiro 21:1f1e9c585da8 342 BuzzerTimer.attach(BuzzerTimer_func, 1);
yabahiro 21:1f1e9c585da8 343 EMGflag = true;
yabahiro 21:1f1e9c585da8 344 LED_DEBUG0 = 1;
yabahiro 21:1f1e9c585da8 345 }
yabahiro 21:1f1e9c585da8 346 if(EMG_0 && EMG_1 && EMGflag) {
yabahiro 21:1f1e9c585da8 347 buzzer = 1;
yabahiro 21:1f1e9c585da8 348 BuzzerTimer.detach();
yabahiro 21:1f1e9c585da8 349 EMGflag = false;
yabahiro 21:1f1e9c585da8 350 }
yabahiro 21:1f1e9c585da8 351 */
yabahiro 21:1f1e9c585da8 352 SystemProcessUpdate();
yabahiro 21:1f1e9c585da8 353 }
t_yamamoto 0:669ef71cba68 354 }
t_yamamoto 0:669ef71cba68 355
t_yamamoto 0:669ef71cba68 356 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 357 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 358 #if USE_PROCESS_NUM>0
yabahiro 21:1f1e9c585da8 359 static void Process0()
yabahiro 21:1f1e9c585da8 360 {
yabahiro 36:788274604650 361 mode=1;
yabahiro 36:788274604650 362 LedMode(1);
yabahiro 35:94f026ab4d30 363 if(ThSW) {
yabahiro 35:94f026ab4d30 364 Twsh=1;
yabahiro 35:94f026ab4d30 365 } else {
yabahiro 35:94f026ab4d30 366 Twsh=2;
yabahiro 31:45f97698857b 367 }
yabahiro 36:788274604650 368
yabahiro 25:38d4964e8aad 369 if(StertSW) {
yabahiro 36:788274604650 370 if(ZoneSW) {
yabahiro 31:45f97698857b 371 mode=1;
yabahiro 31:45f97698857b 372 current = 2;
yabahiro 31:45f97698857b 373 } else {
yabahiro 31:45f97698857b 374 mode=1;
yabahiro 36:788274604650 375 current = 4;
yabahiro 31:45f97698857b 376 }
yabahiro 25:38d4964e8aad 377 }
yabahiro 35:94f026ab4d30 378 if(DWSW) {
yabahiro 35:94f026ab4d30 379 zyouge=true;
yabahiro 35:94f026ab4d30 380 }
yabahiro 35:94f026ab4d30 381 if(AIRSW) {
yabahiro 35:94f026ab4d30 382 if(SW_flag==false) {
yabahiro 35:94f026ab4d30 383 if(Air_flag==false) {
yabahiro 35:94f026ab4d30 384 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 35:94f026ab4d30 385 Air_flag=true;
yabahiro 35:94f026ab4d30 386 SW_flag=true;
yabahiro 35:94f026ab4d30 387 } else {
yabahiro 35:94f026ab4d30 388 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 35:94f026ab4d30 389 Air_flag=false;
yabahiro 35:94f026ab4d30 390 SW_flag=true;
yabahiro 35:94f026ab4d30 391 }
yabahiro 35:94f026ab4d30 392 }
yabahiro 35:94f026ab4d30 393 } else {
yabahiro 35:94f026ab4d30 394 SW_flag=false;
yabahiro 31:45f97698857b 395 }
yabahiro 31:45f97698857b 396
yabahiro 35:94f026ab4d30 397 if(zyouge) {
yabahiro 35:94f026ab4d30 398 if(Twsh==1) {
yabahiro 35:94f026ab4d30 399 motor[MOTOR_0].pwm = 220;
yabahiro 35:94f026ab4d30 400 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 401 if(UPLS) {
yabahiro 35:94f026ab4d30 402 motor[MOTOR_0].pwm = 100;
yabahiro 35:94f026ab4d30 403 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 404 zyouge=false;
yabahiro 35:94f026ab4d30 405 }
yabahiro 35:94f026ab4d30 406 }
yabahiro 35:94f026ab4d30 407 if(Twsh==2) {
yabahiro 37:7cfa39c41245 408 motor[MOTOR_0].pwm = 230;
yabahiro 35:94f026ab4d30 409 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 410 if(UPLS2) {
yabahiro 35:94f026ab4d30 411 motor[MOTOR_0].pwm = 100;
yabahiro 35:94f026ab4d30 412 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 413 zyouge=false;
yabahiro 35:94f026ab4d30 414 }
yabahiro 31:45f97698857b 415 }
yabahiro 37:7cfa39c41245 416 } else {
yabahiro 37:7cfa39c41245 417 motor[MOTOR_0].pwm = 100;
yabahiro 37:7cfa39c41245 418 motor[MOTOR_0].dir = BRAKE;
yabahiro 31:45f97698857b 419 }
yabahiro 35:94f026ab4d30 420 }
yabahiro 31:45f97698857b 421
t_yamamoto 0:669ef71cba68 422 #endif
t_yamamoto 0:669ef71cba68 423
t_yamamoto 0:669ef71cba68 424 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 425 static void Process1()
t_yamamoto 0:669ef71cba68 426 {
yabahiro 36:788274604650 427 mtc=false;
yabahiro 36:788274604650 428 mtc2 = false;
yabahiro 36:788274604650 429 LedMode(2);
yabahiro 31:45f97698857b 430 mode=1;
yabahiro 27:545148741493 431 cross=0;
yabahiro 35:94f026ab4d30 432 Twsh=1;
yabahiro 21:1f1e9c585da8 433 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 434 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 435 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 436 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 437
yabahiro 21:1f1e9c585da8 438 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 439 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 440 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 441 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 442
yabahiro 21:1f1e9c585da8 443 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 444
yabahiro 21:1f1e9c585da8 445 if(controller->Button.UP) {
yabahiro 35:94f026ab4d30 446 motor[MOTOR_0].pwm =200;
yabahiro 21:1f1e9c585da8 447 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 448 if(UPLS) {
yabahiro 35:94f026ab4d30 449 motor[MOTOR_0].pwm = 0;
yabahiro 35:94f026ab4d30 450 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 451 }
yabahiro 21:1f1e9c585da8 452 }
yabahiro 21:1f1e9c585da8 453 if(controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 454 motor[MOTOR_0].pwm = 50;
yabahiro 21:1f1e9c585da8 455 motor[MOTOR_0].dir = BACK;
yabahiro 21:1f1e9c585da8 456 }
yabahiro 21:1f1e9c585da8 457 } else {
yabahiro 21:1f1e9c585da8 458 motor[MOTOR_0].pwm = 0;
yabahiro 21:1f1e9c585da8 459 motor[MOTOR_0].dir = BRAKE;
yabahiro 21:1f1e9c585da8 460 }
yabahiro 21:1f1e9c585da8 461
yabahiro 21:1f1e9c585da8 462 if(controller->Button.Y) {
yabahiro 21:1f1e9c585da8 463 if(dz2==true) {
yabahiro 21:1f1e9c585da8 464 if(dz2i==false) {
yabahiro 21:1f1e9c585da8 465 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 466 dz2i=true;
yabahiro 21:1f1e9c585da8 467 } else {
yabahiro 21:1f1e9c585da8 468 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 469 dz2i=false;
yabahiro 21:1f1e9c585da8 470 }
yabahiro 21:1f1e9c585da8 471 dz2=false;
yabahiro 21:1f1e9c585da8 472 }
yabahiro 21:1f1e9c585da8 473 } else {
yabahiro 21:1f1e9c585da8 474 dz2=true;
yabahiro 21:1f1e9c585da8 475 }
yabahiro 21:1f1e9c585da8 476
yabahiro 21:1f1e9c585da8 477 if(controller->Button.A) {
yabahiro 21:1f1e9c585da8 478 if(dz3==true) {
yabahiro 21:1f1e9c585da8 479 if(dz3i==false) {
yabahiro 30:9450dc1d53cc 480
yabahiro 21:1f1e9c585da8 481 dz3i=true;
yabahiro 21:1f1e9c585da8 482 } else {
yabahiro 21:1f1e9c585da8 483 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 484 dz3i=false;
yabahiro 21:1f1e9c585da8 485 }
yabahiro 21:1f1e9c585da8 486 dz3=false;
yabahiro 21:1f1e9c585da8 487 }
yabahiro 21:1f1e9c585da8 488 } else {
yabahiro 21:1f1e9c585da8 489 dz3=true;
yabahiro 21:1f1e9c585da8 490 }
yabahiro 21:1f1e9c585da8 491
yabahiro 21:1f1e9c585da8 492 if(controller->Button.B) {
yabahiro 21:1f1e9c585da8 493 if(dz4==true) {
yabahiro 21:1f1e9c585da8 494 if(dz4i==false) {
yabahiro 21:1f1e9c585da8 495 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 496 dz4i=true;
yabahiro 21:1f1e9c585da8 497 } else {
yabahiro 21:1f1e9c585da8 498 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 499 dz4i=false;
yabahiro 21:1f1e9c585da8 500 }
yabahiro 21:1f1e9c585da8 501 dz4=false;
yabahiro 21:1f1e9c585da8 502 }
yabahiro 21:1f1e9c585da8 503 } else {
yabahiro 21:1f1e9c585da8 504 dz4=true;
yabahiro 21:1f1e9c585da8 505 }
yabahiro 21:1f1e9c585da8 506 /*
yabahiro 21:1f1e9c585da8 507 if(controller->Button.RIGHT){
yabahiro 21:1f1e9c585da8 508 motor[MOTOR_1].dir = FOR;
yabahiro 21:1f1e9c585da8 509 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 510 if (ARM_1){
yabahiro 21:1f1e9c585da8 511 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 512 }
yabahiro 21:1f1e9c585da8 513 }
yabahiro 21:1f1e9c585da8 514 else if(controller->Button.LEFT){
yabahiro 21:1f1e9c585da8 515 motor[MOTOR_1].dir = BACK;
yabahiro 21:1f1e9c585da8 516 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 517 if (ARM_0){
yabahiro 21:1f1e9c585da8 518 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 519 }
yabahiro 21:1f1e9c585da8 520 }
yabahiro 21:1f1e9c585da8 521 */
t_yamamoto 0:669ef71cba68 522 }
t_yamamoto 0:669ef71cba68 523 #endif
t_yamamoto 0:669ef71cba68 524
t_yamamoto 0:669ef71cba68 525 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 526 static void Process2()
yabahiro 21:1f1e9c585da8 527 {
yabahiro 36:788274604650 528 LedMode(3);
yabahiro 31:45f97698857b 529 // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
yabahiro 31:45f97698857b 530 count++;
yabahiro 31:45f97698857b 531 if(mode==1) {//スタートゾーンから白線検知
kishibekairohan 33:656aa14904a5 532 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 533 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 534 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 535 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 536 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 537 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 538 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 539 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 540 if(g[2]==0) {
yabahiro 31:45f97698857b 541 count=100000;
yabahiro 31:45f97698857b 542 cross=0;
yabahiro 31:45f97698857b 543 mode=3;
yabahiro 31:45f97698857b 544 }
yabahiro 31:45f97698857b 545 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 31:45f97698857b 546 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 547 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 548 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 549 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 550 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 551 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 552 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 553 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 554 if(g[0]==0) {
yabahiro 35:94f026ab4d30 555 mode=10;
yabahiro 35:94f026ab4d30 556 count=100000;
yabahiro 31:45f97698857b 557 }
yabahiro 35:94f026ab4d30 558 } else if(mode==10) { //縦ライントレース
yabahiro 35:94f026ab4d30 559
yabahiro 35:94f026ab4d30 560 if(counts==false&&g[2]==0) {
yabahiro 35:94f026ab4d30 561 cros++;
yabahiro 35:94f026ab4d30 562 counts=true;
yabahiro 31:45f97698857b 563 }
yabahiro 35:94f026ab4d30 564 if(counts==true&&g[2]==99) {
yabahiro 35:94f026ab4d30 565 counts=false;
yabahiro 35:94f026ab4d30 566 }
yabahiro 35:94f026ab4d30 567
yabahiro 35:94f026ab4d30 568 if(cros==Twsh) {
yabahiro 35:94f026ab4d30 569 mode=11;
yabahiro 31:45f97698857b 570 count=0;
yabahiro 35:94f026ab4d30 571 cros=0;
yabahiro 35:94f026ab4d30 572 }//ゆっくりモードに入れなかった時の保険
yabahiro 31:45f97698857b 573 switch(g[0]) {
yabahiro 31:45f97698857b 574 case 98:
kishibekairohan 33:656aa14904a5 575 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 576 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 577 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 578 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 579 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 580 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 581 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 582 motor[TIRE_BL].dir = FOR;
yabahiro 35:94f026ab4d30 583 mtc=true;
yabahiro 35:94f026ab4d30 584 if(count>80000) {
yabahiro 31:45f97698857b 585 cross++;
yabahiro 31:45f97698857b 586 count=0;
yabahiro 31:45f97698857b 587 }
yabahiro 31:45f97698857b 588 if(cross==Twsh) {
yabahiro 31:45f97698857b 589 mode=11;
yabahiro 31:45f97698857b 590 count=0;
yabahiro 35:94f026ab4d30 591 cross=0;
yabahiro 31:45f97698857b 592 }
yabahiro 31:45f97698857b 593 break;
yabahiro 31:45f97698857b 594 case 0:
kishibekairohan 33:656aa14904a5 595 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 596 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 597 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 598 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 599 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 600 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 601 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 602 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 603 mtc=true;
yabahiro 31:45f97698857b 604 break;
yabahiro 31:45f97698857b 605 case 255:
kishibekairohan 33:656aa14904a5 606 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 607 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 608 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 609 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 610 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 611 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 612 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 613 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 614 mtc=true;
yabahiro 31:45f97698857b 615 break;
yabahiro 31:45f97698857b 616 case 253:
kishibekairohan 33:656aa14904a5 617 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 618 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 619 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 620 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 621 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 622 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 623 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 624 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 625 mtc=true;
yabahiro 31:45f97698857b 626 break;
yabahiro 31:45f97698857b 627 case 254:
kishibekairohan 33:656aa14904a5 628 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 629 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 630 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 631 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 632 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 633 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 634 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 635 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 636 mtc=true;
yabahiro 31:45f97698857b 637 break;
yabahiro 31:45f97698857b 638 case 1:
kishibekairohan 33:656aa14904a5 639 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 640 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 641 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 642 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 643 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 644 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 645 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 646 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 647 mtc=true;
yabahiro 31:45f97698857b 648 break;
yabahiro 31:45f97698857b 649 case 3:
yabahiro 31:45f97698857b 650 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 651 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 652 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 653 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 654 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 655 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 656 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 657 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 658 mtc=true;
yabahiro 31:45f97698857b 659 break;
yabahiro 31:45f97698857b 660 case 2:
yabahiro 31:45f97698857b 661 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 662 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 663 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 664 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 665 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 666 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 667 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 668 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 669 mtc=true;
yabahiro 31:45f97698857b 670 break;
yabahiro 31:45f97698857b 671 }
yabahiro 31:45f97698857b 672 if(g[0]!=98&&mtc==true) {
yabahiro 31:45f97698857b 673 switch(g[1]) {
yabahiro 31:45f97698857b 674 case 0:
yabahiro 31:45f97698857b 675 mtc=false;
yabahiro 31:45f97698857b 676 break;
yabahiro 31:45f97698857b 677 case 255:
yabahiro 31:45f97698857b 678 motor[TIRE_BR].pwm += 5;
yabahiro 31:45f97698857b 679 mtc=false;
yabahiro 31:45f97698857b 680 break;
yabahiro 31:45f97698857b 681 case 253:
yabahiro 31:45f97698857b 682 motor[TIRE_BR].pwm += 10;
yabahiro 31:45f97698857b 683 mtc=false;
yabahiro 31:45f97698857b 684 break;
yabahiro 31:45f97698857b 685 case 254:
yabahiro 31:45f97698857b 686 motor[TIRE_BR].pwm += 20;
yabahiro 31:45f97698857b 687 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 688 mtc=false;
yabahiro 31:45f97698857b 689 break;
yabahiro 31:45f97698857b 690 case 1:
yabahiro 31:45f97698857b 691 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 692 mtc=false;
yabahiro 31:45f97698857b 693 break;
yabahiro 31:45f97698857b 694 case 3:
yabahiro 31:45f97698857b 695 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 696 mtc=false;
yabahiro 31:45f97698857b 697 break;
yabahiro 31:45f97698857b 698 case 2:
yabahiro 31:45f97698857b 699 motor[TIRE_BL].pwm += 20;
yabahiro 31:45f97698857b 700 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 701 mtc=false;
yabahiro 31:45f97698857b 702 break;
yabahiro 21:1f1e9c585da8 703 }
yabahiro 31:45f97698857b 704 }
yabahiro 31:45f97698857b 705 } else if(mode==11) {
yabahiro 31:45f97698857b 706 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 707 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 708 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 709 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 710 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 711 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 712 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 713 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 714 if(g[2]==0) {
yabahiro 31:45f97698857b 715 count=0;
yabahiro 31:45f97698857b 716 cross=0;
yabahiro 31:45f97698857b 717 mode=20;
yabahiro 31:45f97698857b 718 }
yabahiro 31:45f97698857b 719 } else if(mode==20) {
yabahiro 35:94f026ab4d30 720
yabahiro 38:391a0c8e4c16 721 if(Twsh==2) {
yabahiro 38:391a0c8e4c16 722 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 38:391a0c8e4c16 723 }
yabahiro 35:94f026ab4d30 724 if(counts==false&&g[0]==0) {
yabahiro 35:94f026ab4d30 725 cros++;
yabahiro 35:94f026ab4d30 726 counts=true;
yabahiro 35:94f026ab4d30 727 }
yabahiro 35:94f026ab4d30 728 if(counts==true&&g[0]==99) {
yabahiro 35:94f026ab4d30 729 counts=false;
yabahiro 35:94f026ab4d30 730 }
yabahiro 35:94f026ab4d30 731
yabahiro 35:94f026ab4d30 732 if(cros==2) {
yabahiro 37:7cfa39c41245 733
yabahiro 37:7cfa39c41245 734 // mode=21;
yabahiro 37:7cfa39c41245 735 // count=0;
yabahiro 37:7cfa39c41245 736 // cros=0;
yabahiro 35:94f026ab4d30 737 }//ゆっくりモードに入れなかった時の保険
yabahiro 35:94f026ab4d30 738
yabahiro 31:45f97698857b 739 switch(g[2]) {
yabahiro 31:45f97698857b 740 case 98:
kishibekairohan 33:656aa14904a5 741 motor[TIRE_FR].pwm = normal;
kishibekairohan 22:c47f29caded6 742 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 743 motor[TIRE_FL].pwm = normal;
kishibekairohan 22:c47f29caded6 744 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 745 motor[TIRE_BR].pwm = normal;
kishibekairohan 22:c47f29caded6 746 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 747 motor[TIRE_BL].pwm = normal;
kishibekairohan 22:c47f29caded6 748 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 749 if(count>80000) {
yabahiro 31:45f97698857b 750 cross++;
yabahiro 31:45f97698857b 751 count=0;
yabahiro 31:45f97698857b 752 }
yabahiro 31:45f97698857b 753 mtc2=true;
yabahiro 35:94f026ab4d30 754 if(cross==2) {
yabahiro 31:45f97698857b 755 mode=21;
yabahiro 31:45f97698857b 756 count=0;
yabahiro 31:45f97698857b 757 cross=0;
yabahiro 31:45f97698857b 758 }
yabahiro 31:45f97698857b 759 break;
yabahiro 31:45f97698857b 760 case 0:
kishibekairohan 33:656aa14904a5 761 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 762 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 763 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 764 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 765 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 766 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 767 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 768 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 769 mtc2=true;
yabahiro 31:45f97698857b 770 break;
yabahiro 31:45f97698857b 771 case 255:
kishibekairohan 33:656aa14904a5 772 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 773 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 774 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 775 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 776 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 777 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 778 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 779 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 780 mtc2=true;
yabahiro 31:45f97698857b 781 break;
yabahiro 31:45f97698857b 782 case 253:
yabahiro 31:45f97698857b 783 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 784 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 785 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 786 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 787 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 788 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 789 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 790 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 791 mtc2=true;
yabahiro 31:45f97698857b 792 break;
yabahiro 31:45f97698857b 793 case 254:
yabahiro 31:45f97698857b 794 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 795 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 796 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 797 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 798 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 799 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 800 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 801 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 802 mtc2=true;
yabahiro 31:45f97698857b 803 break;
yabahiro 31:45f97698857b 804 case 1:
kishibekairohan 33:656aa14904a5 805 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 806 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 807 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 808 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 809 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 810 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 811 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 812 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 813 mtc2=true;
yabahiro 31:45f97698857b 814 break;
yabahiro 31:45f97698857b 815 case 3:
kishibekairohan 33:656aa14904a5 816 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 817 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 818 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 819 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 820 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 821 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 822 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 823 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 824 mtc2=true;
yabahiro 31:45f97698857b 825 break;
yabahiro 31:45f97698857b 826 case 2:
kishibekairohan 33:656aa14904a5 827 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 828 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 829 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 830 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 831 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 832 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 833 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 834 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 835 mtc2=true;
yabahiro 31:45f97698857b 836 break;
yabahiro 31:45f97698857b 837 }
yabahiro 31:45f97698857b 838 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 839 switch(g[3]) {
yabahiro 29:c78a46f51209 840 case 0:
yabahiro 31:45f97698857b 841 mtc2=false;
yabahiro 29:c78a46f51209 842 break;
yabahiro 29:c78a46f51209 843 case 255:
yabahiro 31:45f97698857b 844 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 845 mtc2=false;
yabahiro 29:c78a46f51209 846 break;
yabahiro 29:c78a46f51209 847 case 253:
yabahiro 31:45f97698857b 848 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 849 mtc2=false;
yabahiro 29:c78a46f51209 850 break;
yabahiro 29:c78a46f51209 851 case 254:
yabahiro 31:45f97698857b 852 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 853 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 854 mtc2=false;
yabahiro 29:c78a46f51209 855 break;
yabahiro 29:c78a46f51209 856 case 1:
yabahiro 31:45f97698857b 857 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 858 mtc2=false;
yabahiro 29:c78a46f51209 859 break;
yabahiro 29:c78a46f51209 860 case 3:
yabahiro 31:45f97698857b 861 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 862 mtc2=false;
yabahiro 29:c78a46f51209 863 break;
yabahiro 29:c78a46f51209 864 case 2:
yabahiro 31:45f97698857b 865 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 866 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 867 mtc2=false;
yabahiro 29:c78a46f51209 868 break;
yabahiro 29:c78a46f51209 869 }
yabahiro 31:45f97698857b 870 }
yabahiro 31:45f97698857b 871 } else if(mode==21) {
yabahiro 31:45f97698857b 872 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 873 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 874 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 875 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 876 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 877 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 878 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 879 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 880 if(g[0]==0) {
yabahiro 31:45f97698857b 881 mode=30;
yabahiro 35:94f026ab4d30 882 cros=0;
yabahiro 35:94f026ab4d30 883 zyouge=true;
yabahiro 31:45f97698857b 884 }
yabahiro 31:45f97698857b 885 } else if(mode==30) {
yabahiro 38:391a0c8e4c16 886 motor[TIRE_FR].pwm = 62;
yabahiro 38:391a0c8e4c16 887 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 888 motor[TIRE_FL].pwm = 60;
yabahiro 38:391a0c8e4c16 889 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 890 motor[TIRE_BR].pwm = 60;
yabahiro 38:391a0c8e4c16 891 motor[TIRE_BR].dir = BACK;
yabahiro 38:391a0c8e4c16 892 motor[TIRE_BL].pwm = 62;
yabahiro 38:391a0c8e4c16 893 motor[TIRE_BL].dir = FOR;
yabahiro 38:391a0c8e4c16 894 if(g[0]==98) {
yabahiro 35:94f026ab4d30 895 mode=31;
yabahiro 38:391a0c8e4c16 896 }
yabahiro 38:391a0c8e4c16 897 if(g[3]==0) {
yabahiro 38:391a0c8e4c16 898 mode=32;
yabahiro 35:94f026ab4d30 899 count=0;
yabahiro 35:94f026ab4d30 900 cros=0;
yabahiro 35:94f026ab4d30 901 }//ゆっくりモードに入れなかった時の保険
yabahiro 38:391a0c8e4c16 902 /*
yabahiro 38:391a0c8e4c16 903 if(g[0]==98) {
yabahiro 38:391a0c8e4c16 904 if(Twsh==2) {
yabahiro 38:391a0c8e4c16 905 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 38:391a0c8e4c16 906 }
yabahiro 38:391a0c8e4c16 907 mode=31;
yabahiro 38:391a0c8e4c16 908 count=0;
yabahiro 38:391a0c8e4c16 909 }
yabahiro 31:45f97698857b 910 switch(g[0]) {
yabahiro 31:45f97698857b 911 case 98:
yabahiro 35:94f026ab4d30 912 motor[TIRE_FR].pwm = 30;
yabahiro 31:45f97698857b 913 motor[TIRE_FR].dir = BACK;
yabahiro 35:94f026ab4d30 914 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 915 motor[TIRE_FL].dir = FOR;
yabahiro 35:94f026ab4d30 916 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 917 motor[TIRE_BR].dir = BACK;
yabahiro 35:94f026ab4d30 918 motor[TIRE_BL].pwm = 30;
yabahiro 31:45f97698857b 919 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 920 if(count>100000) {
yabahiro 31:45f97698857b 921 cross++;
yabahiro 31:45f97698857b 922 count=0;
yabahiro 31:45f97698857b 923 }
yabahiro 31:45f97698857b 924 mtc=true;
yabahiro 31:45f97698857b 925 if(cross==1) {
yabahiro 35:94f026ab4d30 926 if(Twsh==2) {
yabahiro 35:94f026ab4d30 927 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 35:94f026ab4d30 928 }
yabahiro 31:45f97698857b 929 mode=31;
yabahiro 31:45f97698857b 930 count=0;
yabahiro 29:c78a46f51209 931 }
yabahiro 31:45f97698857b 932 break;
yabahiro 31:45f97698857b 933 case 0:
yabahiro 38:391a0c8e4c16 934 motor[TIRE_FR].pwm = 80;
yabahiro 31:45f97698857b 935 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 936 motor[TIRE_FL].pwm = 80;
yabahiro 31:45f97698857b 937 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 938 motor[TIRE_BR].pwm = 80;
yabahiro 31:45f97698857b 939 motor[TIRE_BR].dir = BACK;
yabahiro 38:391a0c8e4c16 940 motor[TIRE_BL].pwm = 80;
yabahiro 31:45f97698857b 941 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 942 mtc=true;
yabahiro 31:45f97698857b 943 break;
yabahiro 31:45f97698857b 944 case 255:
yabahiro 38:391a0c8e4c16 945 motor[TIRE_FR].pwm = 80;
yabahiro 31:45f97698857b 946 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 947 motor[TIRE_FL].pwm = 40;
yabahiro 31:45f97698857b 948 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 949 motor[TIRE_BR].pwm = 40;
yabahiro 31:45f97698857b 950 motor[TIRE_BR].dir = BACK;
yabahiro 38:391a0c8e4c16 951 motor[TIRE_BL].pwm = 80;
yabahiro 31:45f97698857b 952 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 953 mtc=true;
yabahiro 31:45f97698857b 954 break;
yabahiro 31:45f97698857b 955 case 253:
yabahiro 38:391a0c8e4c16 956 motor[TIRE_FR].pwm = 80;
yabahiro 31:45f97698857b 957 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 958 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 959 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 960 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 961 motor[TIRE_BR].dir = FREE;
yabahiro 38:391a0c8e4c16 962 motor[TIRE_BL].pwm = 40;
yabahiro 31:45f97698857b 963 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 964 mtc=true;
yabahiro 31:45f97698857b 965 break;
yabahiro 31:45f97698857b 966 case 254:
yabahiro 38:391a0c8e4c16 967 motor[TIRE_FR].pwm = 50;
yabahiro 31:45f97698857b 968 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 969 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 970 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 971 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 972 motor[TIRE_BR].dir = FREE;
yabahiro 38:391a0c8e4c16 973 motor[TIRE_BL].pwm = 50;
yabahiro 31:45f97698857b 974 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 975 mtc=true;
yabahiro 31:45f97698857b 976 break;
yabahiro 31:45f97698857b 977 case 1:
yabahiro 38:391a0c8e4c16 978 motor[TIRE_FR].pwm = 40;
yabahiro 31:45f97698857b 979 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 980 motor[TIRE_FL].pwm = 80;
yabahiro 31:45f97698857b 981 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 982 motor[TIRE_BR].pwm = 80;
yabahiro 31:45f97698857b 983 motor[TIRE_BR].dir = BACK;
yabahiro 38:391a0c8e4c16 984 motor[TIRE_BL].pwm = 40;
yabahiro 31:45f97698857b 985 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 986 mtc=true;
yabahiro 31:45f97698857b 987 break;
yabahiro 31:45f97698857b 988 case 3:
yabahiro 31:45f97698857b 989 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 990 motor[TIRE_FR].dir = FREE;
yabahiro 38:391a0c8e4c16 991 motor[TIRE_FL].pwm = 40;
yabahiro 31:45f97698857b 992 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 993 motor[TIRE_BR].pwm = 40;
yabahiro 31:45f97698857b 994 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 995 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 996 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 997 mtc=true;
yabahiro 31:45f97698857b 998 break;
yabahiro 31:45f97698857b 999 case 2:
yabahiro 31:45f97698857b 1000 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1001 motor[TIRE_FR].dir = FREE;
yabahiro 38:391a0c8e4c16 1002 motor[TIRE_FL].pwm = 50;
yabahiro 31:45f97698857b 1003 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 1004 motor[TIRE_BR].pwm = 50;
yabahiro 31:45f97698857b 1005 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 1006 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1007 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1008 mtc=true;
yabahiro 31:45f97698857b 1009 break;
yabahiro 31:45f97698857b 1010 }
yabahiro 31:45f97698857b 1011 if(g[0]!=98&&mtc==true) {
yabahiro 31:45f97698857b 1012 switch(g[1]) {
yabahiro 29:c78a46f51209 1013 case 0:
yabahiro 31:45f97698857b 1014 mtc=false;
yabahiro 29:c78a46f51209 1015 break;
yabahiro 29:c78a46f51209 1016 case 255:
yabahiro 31:45f97698857b 1017 motor[TIRE_BR].pwm += 5;
yabahiro 31:45f97698857b 1018 mtc=false;
yabahiro 29:c78a46f51209 1019 break;
yabahiro 29:c78a46f51209 1020 case 253:
yabahiro 31:45f97698857b 1021 motor[TIRE_BR].pwm += 10;
yabahiro 31:45f97698857b 1022 mtc=false;
yabahiro 29:c78a46f51209 1023 break;
yabahiro 29:c78a46f51209 1024 case 254:
yabahiro 31:45f97698857b 1025 motor[TIRE_BR].pwm += 20;
yabahiro 31:45f97698857b 1026 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1027 mtc=false;
yabahiro 29:c78a46f51209 1028 break;
yabahiro 29:c78a46f51209 1029 case 1:
yabahiro 31:45f97698857b 1030 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 1031 mtc=false;
yabahiro 29:c78a46f51209 1032 break;
yabahiro 29:c78a46f51209 1033 case 3:
yabahiro 31:45f97698857b 1034 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 1035 mtc=false;
yabahiro 29:c78a46f51209 1036 break;
yabahiro 29:c78a46f51209 1037 case 2:
yabahiro 31:45f97698857b 1038 motor[TIRE_BL].pwm += 20;
yabahiro 31:45f97698857b 1039 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1040 mtc=false;
yabahiro 29:c78a46f51209 1041 break;
yabahiro 29:c78a46f51209 1042 }
yabahiro 38:391a0c8e4c16 1043
yabahiro 31:45f97698857b 1044 }
yabahiro 38:391a0c8e4c16 1045 } else if(mode==31) {
yabahiro 38:391a0c8e4c16 1046 motor[TIRE_FR].pwm = 15;
yabahiro 31:45f97698857b 1047 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 1048 motor[TIRE_FL].pwm = 10;
yabahiro 38:391a0c8e4c16 1049 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1050 motor[TIRE_BR].pwm = 10;
yabahiro 38:391a0c8e4c16 1051 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1052 motor[TIRE_BL].pwm = 15;
yabahiro 31:45f97698857b 1053 motor[TIRE_BL].dir = FOR;
yabahiro 35:94f026ab4d30 1054 if(g[3]==0) {
yabahiro 31:45f97698857b 1055 count=0;
yabahiro 31:45f97698857b 1056 mode=40;
yabahiro 31:45f97698857b 1057 }
yabahiro 38:391a0c8e4c16 1058 */
yabahiro 38:391a0c8e4c16 1059
yabahiro 38:391a0c8e4c16 1060 } else if(mode==31) {
yabahiro 38:391a0c8e4c16 1061 motor[TIRE_FR].pwm = 15;
yabahiro 38:391a0c8e4c16 1062 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 1063 motor[TIRE_FL].pwm = 15;
yabahiro 38:391a0c8e4c16 1064 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 1065 motor[TIRE_BR].pwm = 15;
yabahiro 38:391a0c8e4c16 1066 motor[TIRE_BR].dir = BACK;
yabahiro 38:391a0c8e4c16 1067 motor[TIRE_BL].pwm = 15;
yabahiro 38:391a0c8e4c16 1068 motor[TIRE_BL].dir = FOR;
yabahiro 38:391a0c8e4c16 1069 if(g[3]==0) {
yabahiro 38:391a0c8e4c16 1070 count=0;
yabahiro 38:391a0c8e4c16 1071 mode=32;
yabahiro 38:391a0c8e4c16 1072 }
yabahiro 38:391a0c8e4c16 1073 } else if(mode==32) {
yabahiro 38:391a0c8e4c16 1074 motor[TIRE_FR].pwm = 30;
yabahiro 38:391a0c8e4c16 1075 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1076 motor[TIRE_FL].pwm = 30;
yabahiro 38:391a0c8e4c16 1077 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1078 motor[TIRE_BR].pwm = 30;
yabahiro 38:391a0c8e4c16 1079 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1080 motor[TIRE_BL].pwm = 30;
yabahiro 38:391a0c8e4c16 1081 motor[TIRE_BL].dir = BACK;
yabahiro 38:391a0c8e4c16 1082 if(g[3]==0) {
yabahiro 38:391a0c8e4c16 1083 count=0;
yabahiro 38:391a0c8e4c16 1084 mode=40;
yabahiro 38:391a0c8e4c16 1085 }
yabahiro 31:45f97698857b 1086 } else if(mode==40) {
yabahiro 38:391a0c8e4c16 1087 if(LIB) {
yabahiro 38:391a0c8e4c16 1088 mode=42;
yabahiro 38:391a0c8e4c16 1089 }
yabahiro 35:94f026ab4d30 1090 if(counts==false&&g[0]==0) {
yabahiro 35:94f026ab4d30 1091 cros++;
yabahiro 35:94f026ab4d30 1092 counts=true;
yabahiro 35:94f026ab4d30 1093 }
yabahiro 35:94f026ab4d30 1094 if(counts==true&&g[0]==99) {
yabahiro 35:94f026ab4d30 1095 counts=false;
yabahiro 35:94f026ab4d30 1096 }
yabahiro 35:94f026ab4d30 1097
yabahiro 38:391a0c8e4c16 1098 if(cros==1) {
yabahiro 36:788274604650 1099 // mode=41;
yabahiro 35:94f026ab4d30 1100 //count=0;
yabahiro 35:94f026ab4d30 1101 //cros=0;
yabahiro 35:94f026ab4d30 1102 }//ゆっくりモードに入れなかった時の保険
yabahiro 35:94f026ab4d30 1103
yabahiro 31:45f97698857b 1104 switch(g[3]) {
yabahiro 31:45f97698857b 1105 case 98:
kishibekairohan 33:656aa14904a5 1106 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1107 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1108 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1109 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1110 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1111 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1112 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1113 motor[TIRE_BL].dir = FOR;
yabahiro 35:94f026ab4d30 1114 if(count>20000) {
yabahiro 31:45f97698857b 1115 cross++;
yabahiro 31:45f97698857b 1116 count=0;
yabahiro 31:45f97698857b 1117 }
yabahiro 31:45f97698857b 1118 mtc2=true;
yabahiro 38:391a0c8e4c16 1119 if(cross==2) {
yabahiro 31:45f97698857b 1120 mode=41;
yabahiro 31:45f97698857b 1121 count=0;
yabahiro 31:45f97698857b 1122 cross=0;
kishibekairohan 22:c47f29caded6 1123 }
yabahiro 31:45f97698857b 1124 break;
yabahiro 31:45f97698857b 1125 case 0:
kishibekairohan 33:656aa14904a5 1126 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1127 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1128 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1129 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1130 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1131 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1132 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1133 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1134 mtc2=true;
yabahiro 31:45f97698857b 1135 break;
yabahiro 31:45f97698857b 1136 case 255:
kishibekairohan 33:656aa14904a5 1137 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1138 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1139 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1140 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1141 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1142 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1143 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1144 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1145 mtc2=true;
yabahiro 31:45f97698857b 1146 break;
yabahiro 31:45f97698857b 1147 case 253:
yabahiro 31:45f97698857b 1148 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1149 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1150 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1151 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1152 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1153 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1154 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1155 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1156 mtc2=true;
yabahiro 31:45f97698857b 1157 break;
yabahiro 31:45f97698857b 1158 case 254:
yabahiro 31:45f97698857b 1159 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1160 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1161 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1162 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1163 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1164 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1165 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1166 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1167 mtc2=true;
yabahiro 31:45f97698857b 1168 break;
yabahiro 31:45f97698857b 1169 case 1:
kishibekairohan 33:656aa14904a5 1170 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1171 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1172 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1173 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1174 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1175 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1176 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1177 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1178 mtc2=true;
yabahiro 31:45f97698857b 1179 break;
yabahiro 31:45f97698857b 1180 case 3:
kishibekairohan 33:656aa14904a5 1181 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1182 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 1183 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1184 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1185 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1186 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1187 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1188 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1189 mtc2=true;
yabahiro 31:45f97698857b 1190 break;
yabahiro 31:45f97698857b 1191 case 2:
kishibekairohan 33:656aa14904a5 1192 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1193 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 1194 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1195 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1196 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1197 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1198 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1199 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1200 mtc2=true;
yabahiro 31:45f97698857b 1201 break;
yabahiro 31:45f97698857b 1202 }
yabahiro 31:45f97698857b 1203 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 1204 switch(g[2]) {
yabahiro 29:c78a46f51209 1205 case 0:
yabahiro 31:45f97698857b 1206 mtc2=false;
yabahiro 29:c78a46f51209 1207 break;
yabahiro 29:c78a46f51209 1208 case 255:
yabahiro 31:45f97698857b 1209 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1210 mtc2=false;
yabahiro 29:c78a46f51209 1211 break;
yabahiro 29:c78a46f51209 1212 case 253:
yabahiro 31:45f97698857b 1213 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1214 mtc2=false;
yabahiro 29:c78a46f51209 1215 break;
yabahiro 29:c78a46f51209 1216 case 254:
yabahiro 31:45f97698857b 1217 motor[TIRE_FL].pwm += 20;
yabahiro 29:c78a46f51209 1218 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1219 mtc2=false;
yabahiro 29:c78a46f51209 1220 break;
yabahiro 29:c78a46f51209 1221 case 1:
yabahiro 31:45f97698857b 1222 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 1223 mtc2=false;
yabahiro 29:c78a46f51209 1224 break;
yabahiro 29:c78a46f51209 1225 case 3:
yabahiro 31:45f97698857b 1226 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 1227 mtc2=false;
yabahiro 29:c78a46f51209 1228 break;
yabahiro 29:c78a46f51209 1229 case 2:
yabahiro 31:45f97698857b 1230 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 1231 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1232 mtc2=false;
yabahiro 29:c78a46f51209 1233 break;
yabahiro 29:c78a46f51209 1234 }
yabahiro 31:45f97698857b 1235 }
yabahiro 31:45f97698857b 1236 } else if(mode==41) {
yabahiro 31:45f97698857b 1237 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1238 motor[TIRE_FR].dir = FREE;
yabahiro 35:94f026ab4d30 1239 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1240 motor[TIRE_FL].dir = BACK;
yabahiro 35:94f026ab4d30 1241 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1242 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1243 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1244 motor[TIRE_BL].dir = FREE;
yabahiro 35:94f026ab4d30 1245 if(g[1]==0) {
yabahiro 31:45f97698857b 1246 count=0;
yabahiro 31:45f97698857b 1247 mode=70;
yabahiro 31:45f97698857b 1248 }
yabahiro 38:391a0c8e4c16 1249 } else if(mode==42) {
yabahiro 38:391a0c8e4c16 1250 motor[TIRE_FR].pwm = 20;
yabahiro 38:391a0c8e4c16 1251 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1252 motor[TIRE_FL].pwm = 0;
yabahiro 38:391a0c8e4c16 1253 motor[TIRE_FL].dir = FREE;
yabahiro 38:391a0c8e4c16 1254 motor[TIRE_BR].pwm = 0;
yabahiro 38:391a0c8e4c16 1255 motor[TIRE_BR].dir = FREE;
yabahiro 38:391a0c8e4c16 1256 motor[TIRE_BL].pwm = 20;
yabahiro 38:391a0c8e4c16 1257 motor[TIRE_BL].dir = BACK;
yabahiro 38:391a0c8e4c16 1258 if(g[1]==0) {
yabahiro 38:391a0c8e4c16 1259 count=0;
yabahiro 38:391a0c8e4c16 1260 mode=70;
yabahiro 38:391a0c8e4c16 1261 }
yabahiro 31:45f97698857b 1262 } else if(mode==70) {
yabahiro 31:45f97698857b 1263 switch(g[1]) {
yabahiro 31:45f97698857b 1264 case 99:
yabahiro 31:45f97698857b 1265 if(count>=100000) {
yabahiro 31:45f97698857b 1266 mode=100;
yabahiro 29:c78a46f51209 1267 }
yabahiro 31:45f97698857b 1268 break;
yabahiro 31:45f97698857b 1269 case 98:
kishibekairohan 33:656aa14904a5 1270 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1271 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1272 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1273 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1274 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1275 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1276 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1277 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1278 break;
yabahiro 31:45f97698857b 1279 case 0:
yabahiro 38:391a0c8e4c16 1280 motor[TIRE_FR].pwm = 80;
yabahiro 31:45f97698857b 1281 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1282 motor[TIRE_FL].pwm = 80;
yabahiro 31:45f97698857b 1283 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1284 motor[TIRE_BR].pwm = 80;
yabahiro 31:45f97698857b 1285 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1286 motor[TIRE_BL].pwm = 80;
yabahiro 31:45f97698857b 1287 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1288 mtc=true;
yabahiro 31:45f97698857b 1289 break;
yabahiro 31:45f97698857b 1290 case 255:
kishibekairohan 33:656aa14904a5 1291 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1292 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1293 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1294 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1295 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1296 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1297 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1298 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1299 mtc=true;
yabahiro 31:45f97698857b 1300 break;
yabahiro 31:45f97698857b 1301 case 253:
kishibekairohan 33:656aa14904a5 1302 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1303 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1304 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1305 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1306 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1307 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1308 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1309 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1310 mtc=true;
yabahiro 31:45f97698857b 1311 break;
yabahiro 31:45f97698857b 1312 case 254:
kishibekairohan 33:656aa14904a5 1313 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1314 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1315 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1316 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1317 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1318 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1319 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1320 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1321 mtc=true;
yabahiro 31:45f97698857b 1322 break;
yabahiro 31:45f97698857b 1323 case 1:
kishibekairohan 33:656aa14904a5 1324 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1325 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1326 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1327 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1328 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1329 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1330 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1331 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1332 mtc=true;
yabahiro 31:45f97698857b 1333 break;
yabahiro 31:45f97698857b 1334 case 3:
yabahiro 31:45f97698857b 1335 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1336 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1337 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1338 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1339 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1340 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1341 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1342 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1343 mtc=true;
yabahiro 31:45f97698857b 1344 break;
yabahiro 31:45f97698857b 1345 case 2:
yabahiro 31:45f97698857b 1346 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1347 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1348 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1349 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1350 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1351 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1352 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1353 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1354 mtc=true;
yabahiro 31:45f97698857b 1355 break;
yabahiro 31:45f97698857b 1356 }
yabahiro 31:45f97698857b 1357 if(g[1]!=98&&mtc==true) {
yabahiro 31:45f97698857b 1358 switch(g[0]) {
yabahiro 29:c78a46f51209 1359 case 0:
yabahiro 31:45f97698857b 1360 mtc=false;
yabahiro 29:c78a46f51209 1361 break;
yabahiro 29:c78a46f51209 1362 case 255:
yabahiro 31:45f97698857b 1363 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1364 mtc=false;
yabahiro 29:c78a46f51209 1365 break;
yabahiro 29:c78a46f51209 1366 case 253:
yabahiro 31:45f97698857b 1367 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1368 mtc=false;
yabahiro 29:c78a46f51209 1369 break;
yabahiro 29:c78a46f51209 1370 case 254:
yabahiro 31:45f97698857b 1371 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 1372 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1373 mtc=false;
yabahiro 29:c78a46f51209 1374 break;
yabahiro 29:c78a46f51209 1375 case 1:
yabahiro 31:45f97698857b 1376 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1377 mtc=false;
yabahiro 29:c78a46f51209 1378 break;
yabahiro 29:c78a46f51209 1379 case 3:
yabahiro 31:45f97698857b 1380 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1381 mtc=false;
yabahiro 29:c78a46f51209 1382 break;
yabahiro 29:c78a46f51209 1383 case 2:
yabahiro 31:45f97698857b 1384 motor[TIRE_FR].pwm += 20;
yabahiro 31:45f97698857b 1385 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1386 mtc=false;
yabahiro 29:c78a46f51209 1387 break;
yabahiro 29:c78a46f51209 1388 }
yabahiro 31:45f97698857b 1389 }
yabahiro 31:45f97698857b 1390 } else if(mode==100) {
yabahiro 35:94f026ab4d30 1391 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1392 motor[TIRE_FR].dir = FOR;
yabahiro 35:94f026ab4d30 1393 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1394 motor[TIRE_FL].dir = BACK;
yabahiro 31:45f97698857b 1395 motor[TIRE_BR].pwm = 23;
yabahiro 31:45f97698857b 1396 motor[TIRE_BR].dir = FOR;
yabahiro 35:94f026ab4d30 1397 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1398 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1399 } else {
yabahiro 31:45f97698857b 1400 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1401 motor[TIRE_FR].dir = BRAKE;
yabahiro 31:45f97698857b 1402 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1403 motor[TIRE_FL].dir = BRAKE;
yabahiro 31:45f97698857b 1404 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1405 motor[TIRE_BR].dir = BRAKE;
yabahiro 31:45f97698857b 1406 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1407 motor[TIRE_BL].dir = BRAKE;
yabahiro 31:45f97698857b 1408 }
yabahiro 31:45f97698857b 1409 }
yabahiro 31:45f97698857b 1410 #endif
yabahiro 31:45f97698857b 1411
yabahiro 31:45f97698857b 1412 #if USE_PROCESS_NUM>3
yabahiro 31:45f97698857b 1413 static void Process3()
yabahiro 31:45f97698857b 1414 {
yabahiro 38:391a0c8e4c16 1415
yabahiro 36:788274604650 1416 }
yabahiro 36:788274604650 1417 #endif
yabahiro 36:788274604650 1418
yabahiro 36:788274604650 1419 #if USE_PROCESS_NUM>4
yabahiro 36:788274604650 1420 static void Process4()
yabahiro 36:788274604650 1421 {
yabahiro 36:788274604650 1422 LedMode(4);
yabahiro 36:788274604650 1423 count++;
yabahiro 31:45f97698857b 1424 if(mode==1) {//スタートゾーンから白線検知
kishibekairohan 33:656aa14904a5 1425 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1426 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1427 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1428 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1429 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1430 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1431 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1432 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1433 if(g[2]==0) {
yabahiro 31:45f97698857b 1434 count=100000;
yabahiro 31:45f97698857b 1435 cross=0;
yabahiro 36:788274604650 1436 mode=3;
yabahiro 31:45f97698857b 1437 }
yabahiro 36:788274604650 1438 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 36:788274604650 1439 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 1440 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1441 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1442 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1443 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1444 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1445 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 1446 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1447 if(g[0]==0) {
yabahiro 36:788274604650 1448 mode=10;
yabahiro 36:788274604650 1449 count=100000;
yabahiro 36:788274604650 1450 }
yabahiro 36:788274604650 1451 } else if(mode==10) { //縦ライントレース
yabahiro 36:788274604650 1452
yabahiro 36:788274604650 1453 if(counts==false&&g[2]==0) {
yabahiro 36:788274604650 1454 cros++;
yabahiro 36:788274604650 1455 counts=true;
yabahiro 36:788274604650 1456 }
yabahiro 36:788274604650 1457 if(counts==true&&g[2]==99) {
yabahiro 36:788274604650 1458 counts=false;
yabahiro 36:788274604650 1459 }
yabahiro 36:788274604650 1460
yabahiro 36:788274604650 1461 if(cros==Twsh) {
yabahiro 36:788274604650 1462 mode=11;
yabahiro 31:45f97698857b 1463 count=0;
yabahiro 36:788274604650 1464 cros=0;
yabahiro 36:788274604650 1465 }//ゆっくりモードに入れなかった時の保険
kishibekairohan 32:90f018b10ad6 1466 switch(g[1]) {
yabahiro 31:45f97698857b 1467 case 98:
kishibekairohan 33:656aa14904a5 1468 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1469 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1470 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1471 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1472 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1473 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1474 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1475 motor[TIRE_BL].dir = BACK;
yabahiro 36:788274604650 1476 mtc=true;
yabahiro 36:788274604650 1477 if(count>80000) {
yabahiro 31:45f97698857b 1478 cross++;
yabahiro 31:45f97698857b 1479 count=0;
yabahiro 31:45f97698857b 1480 }
yabahiro 31:45f97698857b 1481 if(cross==Twsh) {
yabahiro 31:45f97698857b 1482 mode=11;
yabahiro 31:45f97698857b 1483 count=0;
yabahiro 36:788274604650 1484 cross=0;
yabahiro 31:45f97698857b 1485 }
yabahiro 31:45f97698857b 1486 break;
yabahiro 31:45f97698857b 1487 case 0:
kishibekairohan 33:656aa14904a5 1488 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1489 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1490 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1491 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1492 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1493 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1494 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1495 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1496 mtc=true;
yabahiro 31:45f97698857b 1497 break;
yabahiro 31:45f97698857b 1498 case 255:
kishibekairohan 33:656aa14904a5 1499 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1500 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1501 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1502 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1503 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1504 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1505 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1506 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1507 mtc=true;
yabahiro 31:45f97698857b 1508 break;
yabahiro 31:45f97698857b 1509 case 253:
kishibekairohan 33:656aa14904a5 1510 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1511 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1512 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1513 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1514 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1515 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1516 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1517 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1518 mtc=true;
yabahiro 31:45f97698857b 1519 break;
yabahiro 31:45f97698857b 1520 case 254:
kishibekairohan 33:656aa14904a5 1521 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1522 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1523 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1524 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1525 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1526 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1527 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1528 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1529 mtc=true;
yabahiro 31:45f97698857b 1530 break;
yabahiro 31:45f97698857b 1531 case 1:
kishibekairohan 33:656aa14904a5 1532 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1533 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1534 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1535 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1536 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1537 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1538 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1539 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1540 mtc=true;
yabahiro 31:45f97698857b 1541 break;
yabahiro 31:45f97698857b 1542 case 3:
yabahiro 31:45f97698857b 1543 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1544 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1545 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1546 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1547 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1548 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1549 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1550 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1551 mtc=true;
yabahiro 31:45f97698857b 1552 break;
yabahiro 31:45f97698857b 1553 case 2:
yabahiro 31:45f97698857b 1554 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1555 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1556 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1557 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1558 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1559 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1560 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1561 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1562 mtc=true;
yabahiro 31:45f97698857b 1563 break;
yabahiro 31:45f97698857b 1564 }
yabahiro 36:788274604650 1565 if(g[0]!=98&&mtc==true) {
yabahiro 36:788274604650 1566 switch(g[1]) {
yabahiro 31:45f97698857b 1567 case 0:
yabahiro 31:45f97698857b 1568 mtc=false;
yabahiro 31:45f97698857b 1569 break;
yabahiro 31:45f97698857b 1570 case 255:
kishibekairohan 32:90f018b10ad6 1571 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1572 mtc=false;
yabahiro 31:45f97698857b 1573 break;
yabahiro 31:45f97698857b 1574 case 253:
kishibekairohan 32:90f018b10ad6 1575 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1576 mtc=false;
yabahiro 31:45f97698857b 1577 break;
yabahiro 31:45f97698857b 1578 case 254:
kishibekairohan 32:90f018b10ad6 1579 motor[TIRE_FL].pwm += 20;
kishibekairohan 32:90f018b10ad6 1580 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1581 mtc=false;
yabahiro 31:45f97698857b 1582 break;
yabahiro 31:45f97698857b 1583 case 1:
kishibekairohan 32:90f018b10ad6 1584 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1585 mtc=false;
yabahiro 31:45f97698857b 1586 break;
yabahiro 31:45f97698857b 1587 case 3:
kishibekairohan 32:90f018b10ad6 1588 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1589 mtc=false;
yabahiro 31:45f97698857b 1590 break;
yabahiro 31:45f97698857b 1591 case 2:
kishibekairohan 32:90f018b10ad6 1592 motor[TIRE_FR].pwm += 20;
kishibekairohan 32:90f018b10ad6 1593 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1594 mtc=false;
yabahiro 31:45f97698857b 1595 break;
yabahiro 29:c78a46f51209 1596 }
yabahiro 31:45f97698857b 1597 }
yabahiro 31:45f97698857b 1598 } else if(mode==11) {
yabahiro 36:788274604650 1599 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 1600 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1601 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1602 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1603 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1604 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1605 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 1606 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1607 if(g[2]==0) {
yabahiro 31:45f97698857b 1608 count=0;
yabahiro 36:788274604650 1609 cross=0;
yabahiro 31:45f97698857b 1610 mode=20;
yabahiro 31:45f97698857b 1611 }
yabahiro 31:45f97698857b 1612 } else if(mode==20) {
yabahiro 36:788274604650 1613
yabahiro 38:391a0c8e4c16 1614 if(Twsh==2) {
yabahiro 38:391a0c8e4c16 1615 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 38:391a0c8e4c16 1616 }
yabahiro 38:391a0c8e4c16 1617
yabahiro 36:788274604650 1618 if(counts==false&&g[1]==0) {
yabahiro 36:788274604650 1619 cros++;
yabahiro 36:788274604650 1620 counts=true;
yabahiro 36:788274604650 1621 }
yabahiro 36:788274604650 1622 if(counts==true&&g[1]==99) {
yabahiro 36:788274604650 1623 counts=false;
yabahiro 36:788274604650 1624 }
yabahiro 36:788274604650 1625
yabahiro 36:788274604650 1626 if(cros==2) {
yabahiro 38:391a0c8e4c16 1627 //mode=21;
yabahiro 38:391a0c8e4c16 1628 //count=0;
yabahiro 38:391a0c8e4c16 1629 //cros=0;
yabahiro 36:788274604650 1630 }//ゆっくりモードに入れなかった時の保険
yabahiro 36:788274604650 1631
yabahiro 31:45f97698857b 1632 switch(g[2]) {
yabahiro 31:45f97698857b 1633 case 98:
kishibekairohan 33:656aa14904a5 1634 motor[TIRE_FR].pwm = normal;
kishibekairohan 22:c47f29caded6 1635 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1636 motor[TIRE_FL].pwm = normal;
yabahiro 23:694ecfa97839 1637 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1638 motor[TIRE_BR].pwm = normal;
yabahiro 23:694ecfa97839 1639 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1640 motor[TIRE_BL].pwm = normal;
yabahiro 29:c78a46f51209 1641 motor[TIRE_BL].dir = BACK;
yabahiro 36:788274604650 1642 if(count>80000) {
yabahiro 31:45f97698857b 1643 cross++;
yabahiro 31:45f97698857b 1644 count=0;
yabahiro 31:45f97698857b 1645 }
yabahiro 31:45f97698857b 1646 mtc2=true;
yabahiro 36:788274604650 1647 if(cross==2) {
yabahiro 31:45f97698857b 1648 mode=21;
yabahiro 31:45f97698857b 1649 count=0;
yabahiro 31:45f97698857b 1650 cross=0;
yabahiro 31:45f97698857b 1651 }
yabahiro 31:45f97698857b 1652 break;
yabahiro 31:45f97698857b 1653 case 0:
kishibekairohan 33:656aa14904a5 1654 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1655 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1656 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1657 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1658 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1659 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1660 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1661 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1662 mtc2=true;
yabahiro 31:45f97698857b 1663 break;
yabahiro 31:45f97698857b 1664 case 255:
yabahiro 36:788274604650 1665 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1666 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1667 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1668 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1669 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1670 motor[TIRE_BR].dir = BACK;
yabahiro 36:788274604650 1671 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1672 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1673 mtc2=true;
yabahiro 31:45f97698857b 1674 break;
yabahiro 31:45f97698857b 1675 case 253:
yabahiro 31:45f97698857b 1676 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1677 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1678 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1679 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1680 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1681 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 1682 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1683 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1684 mtc2=true;
yabahiro 31:45f97698857b 1685 break;
yabahiro 31:45f97698857b 1686 case 254:
yabahiro 31:45f97698857b 1687 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1688 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1689 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1690 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1691 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1692 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 1693 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1694 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1695 mtc2=true;
yabahiro 31:45f97698857b 1696 break;
yabahiro 31:45f97698857b 1697 case 1:
kishibekairohan 33:656aa14904a5 1698 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1699 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1700 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1701 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1702 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1703 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1704 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1705 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1706 mtc2=true;
yabahiro 31:45f97698857b 1707 break;
yabahiro 31:45f97698857b 1708 case 3:
kishibekairohan 33:656aa14904a5 1709 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1710 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1711 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1712 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1713 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1714 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1715 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1716 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1717 mtc2=true;
yabahiro 31:45f97698857b 1718 break;
yabahiro 31:45f97698857b 1719 case 2:
kishibekairohan 33:656aa14904a5 1720 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1721 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1722 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1723 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1724 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1725 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1726 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1727 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1728 mtc2=true;
yabahiro 31:45f97698857b 1729 break;
yabahiro 31:45f97698857b 1730 }
yabahiro 31:45f97698857b 1731 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 1732 switch(g[3]) {
yabahiro 23:694ecfa97839 1733 case 0:
yabahiro 31:45f97698857b 1734 mtc2=false;
yabahiro 23:694ecfa97839 1735 break;
yabahiro 23:694ecfa97839 1736 case 255:
yabahiro 31:45f97698857b 1737 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 1738 mtc2=false;
yabahiro 23:694ecfa97839 1739 break;
yabahiro 23:694ecfa97839 1740 case 253:
yabahiro 31:45f97698857b 1741 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 1742 mtc2=false;
yabahiro 23:694ecfa97839 1743 break;
yabahiro 23:694ecfa97839 1744 case 254:
yabahiro 31:45f97698857b 1745 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 1746 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1747 mtc2=false;
yabahiro 23:694ecfa97839 1748 break;
yabahiro 23:694ecfa97839 1749 case 1:
yabahiro 31:45f97698857b 1750 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1751 mtc2=false;
yabahiro 23:694ecfa97839 1752 break;
yabahiro 23:694ecfa97839 1753 case 3:
yabahiro 31:45f97698857b 1754 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1755 mtc2=false;
yabahiro 23:694ecfa97839 1756 break;
yabahiro 23:694ecfa97839 1757 case 2:
yabahiro 31:45f97698857b 1758 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 1759 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1760 mtc2=false;
yabahiro 29:c78a46f51209 1761 break;
yabahiro 29:c78a46f51209 1762 }
yabahiro 31:45f97698857b 1763 }
yabahiro 31:45f97698857b 1764 } else if(mode==21) {
yabahiro 36:788274604650 1765 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 1766 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1767 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1768 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1769 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1770 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1771 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 1772 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1773 if(g[1]==0) {
yabahiro 31:45f97698857b 1774 mode=30;
yabahiro 36:788274604650 1775 cros=0;
yabahiro 36:788274604650 1776 zyouge=true;
yabahiro 31:45f97698857b 1777 }
yabahiro 31:45f97698857b 1778 } else if(mode==30) {
yabahiro 38:391a0c8e4c16 1779 motor[TIRE_FR].pwm = 60;
yabahiro 38:391a0c8e4c16 1780 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1781 motor[TIRE_FL].pwm = 60;
yabahiro 38:391a0c8e4c16 1782 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1783 motor[TIRE_BR].pwm = 60;
yabahiro 38:391a0c8e4c16 1784 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1785 motor[TIRE_BL].pwm = 60;
yabahiro 38:391a0c8e4c16 1786 motor[TIRE_BL].dir = BACK;
yabahiro 38:391a0c8e4c16 1787 if(g[1]==98) {
yabahiro 38:391a0c8e4c16 1788 mode=31;
yabahiro 38:391a0c8e4c16 1789 count=0;
yabahiro 38:391a0c8e4c16 1790 }
yabahiro 36:788274604650 1791 if(counts==false&&g[3]==0) {
yabahiro 36:788274604650 1792 counts=false;
yabahiro 36:788274604650 1793 mode=31;
yabahiro 36:788274604650 1794 count=0;
yabahiro 36:788274604650 1795 cros=0;
yabahiro 36:788274604650 1796 }//ゆっくりモードに入れなかった時の保険
yabahiro 38:391a0c8e4c16 1797 /*
kishibekairohan 32:90f018b10ad6 1798 switch(g[1]) {
yabahiro 31:45f97698857b 1799 case 98:
yabahiro 36:788274604650 1800 motor[TIRE_FR].pwm = 30;
yabahiro 31:45f97698857b 1801 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1802 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1803 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1804 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1805 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1806 motor[TIRE_BL].pwm = 30;
yabahiro 31:45f97698857b 1807 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1808 if(count>100000) {
yabahiro 31:45f97698857b 1809 cross++;
yabahiro 31:45f97698857b 1810 count=0;
yabahiro 31:45f97698857b 1811 }
yabahiro 31:45f97698857b 1812 mtc=true;
yabahiro 31:45f97698857b 1813 if(cross==1) {
yabahiro 36:788274604650 1814 if(Twsh==2) {
yabahiro 36:788274604650 1815 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 36:788274604650 1816 }
yabahiro 31:45f97698857b 1817 mode=31;
yabahiro 31:45f97698857b 1818 count=0;
yabahiro 29:c78a46f51209 1819 }
yabahiro 31:45f97698857b 1820 break;
yabahiro 31:45f97698857b 1821 case 0:
yabahiro 36:788274604650 1822 motor[TIRE_FR].pwm = 60;
yabahiro 31:45f97698857b 1823 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1824 motor[TIRE_FL].pwm = 60;
yabahiro 31:45f97698857b 1825 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1826 motor[TIRE_BR].pwm = 60;
yabahiro 31:45f97698857b 1827 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1828 motor[TIRE_BL].pwm = 60;
yabahiro 31:45f97698857b 1829 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1830 mtc=true;
yabahiro 31:45f97698857b 1831 break;
yabahiro 31:45f97698857b 1832 case 255:
yabahiro 36:788274604650 1833 motor[TIRE_FR].pwm = 60;
yabahiro 31:45f97698857b 1834 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1835 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1836 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1837 motor[TIRE_BR].pwm = 20;
yabahiro 31:45f97698857b 1838 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1839 motor[TIRE_BL].pwm = 60;
yabahiro 31:45f97698857b 1840 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1841 mtc=true;
yabahiro 31:45f97698857b 1842 break;
yabahiro 31:45f97698857b 1843 case 253:
yabahiro 36:788274604650 1844 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1845 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1846 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1847 motor[TIRE_FL].dir = BACK;
yabahiro 31:45f97698857b 1848 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1849 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1850 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1851 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1852 mtc=true;
yabahiro 31:45f97698857b 1853 break;
yabahiro 31:45f97698857b 1854 case 254:
yabahiro 36:788274604650 1855 motor[TIRE_FR].pwm = 30;
yabahiro 31:45f97698857b 1856 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1857 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1858 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1859 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1860 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1861 motor[TIRE_BL].pwm = 30;
yabahiro 31:45f97698857b 1862 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1863 mtc=true;
yabahiro 31:45f97698857b 1864 break;
yabahiro 31:45f97698857b 1865 case 1:
yabahiro 36:788274604650 1866 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1867 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1868 motor[TIRE_FL].pwm = 60;
yabahiro 31:45f97698857b 1869 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1870 motor[TIRE_BR].pwm = 60;
yabahiro 31:45f97698857b 1871 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1872 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1873 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1874 mtc=true;
yabahiro 31:45f97698857b 1875 break;
yabahiro 31:45f97698857b 1876 case 3:
yabahiro 31:45f97698857b 1877 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1878 motor[TIRE_FR].dir = FREE;
yabahiro 36:788274604650 1879 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1880 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1881 motor[TIRE_BR].pwm = 20;
yabahiro 31:45f97698857b 1882 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1883 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1884 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1885 mtc=true;
yabahiro 31:45f97698857b 1886 break;
yabahiro 31:45f97698857b 1887 case 2:
yabahiro 31:45f97698857b 1888 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1889 motor[TIRE_FR].dir = FREE;
yabahiro 36:788274604650 1890 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1891 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1892 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1893 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1894 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1895 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1896 mtc=true;
yabahiro 31:45f97698857b 1897 break;
yabahiro 31:45f97698857b 1898 }
kishibekairohan 32:90f018b10ad6 1899 if(g[1]!=98&&mtc==true) {
kishibekairohan 32:90f018b10ad6 1900 switch(g[0]) {
yabahiro 29:c78a46f51209 1901 case 0:
yabahiro 31:45f97698857b 1902 mtc=false;
yabahiro 29:c78a46f51209 1903 break;
yabahiro 29:c78a46f51209 1904 case 255:
kishibekairohan 34:cef6c124a52d 1905 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1906 mtc=false;
yabahiro 29:c78a46f51209 1907 break;
yabahiro 29:c78a46f51209 1908 case 253:
kishibekairohan 34:cef6c124a52d 1909 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1910 mtc=false;
yabahiro 29:c78a46f51209 1911 break;
yabahiro 29:c78a46f51209 1912 case 254:
kishibekairohan 34:cef6c124a52d 1913 motor[TIRE_FL].pwm += 20;
kishibekairohan 34:cef6c124a52d 1914 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1915 mtc=false;
yabahiro 29:c78a46f51209 1916 break;
yabahiro 29:c78a46f51209 1917 case 1:
kishibekairohan 34:cef6c124a52d 1918 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1919 mtc=false;
yabahiro 29:c78a46f51209 1920 break;
yabahiro 29:c78a46f51209 1921 case 3:
kishibekairohan 34:cef6c124a52d 1922 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1923 mtc=false;
yabahiro 29:c78a46f51209 1924 break;
yabahiro 29:c78a46f51209 1925 case 2:
kishibekairohan 34:cef6c124a52d 1926 motor[TIRE_FR].pwm += 20;
kishibekairohan 34:cef6c124a52d 1927 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1928 mtc=false;
yabahiro 23:694ecfa97839 1929 break;
yabahiro 23:694ecfa97839 1930 }
yabahiro 31:45f97698857b 1931 }
yabahiro 38:391a0c8e4c16 1932 */
yabahiro 31:45f97698857b 1933 } else if(mode==31) {
yabahiro 36:788274604650 1934 motor[TIRE_FR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1935 motor[TIRE_FR].dir = FREE;
yabahiro 36:788274604650 1936 motor[TIRE_FL].pwm = 30;
kishibekairohan 32:90f018b10ad6 1937 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1938 motor[TIRE_BR].pwm = 30;
kishibekairohan 32:90f018b10ad6 1939 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1940 motor[TIRE_BL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1941 motor[TIRE_BL].dir = FREE;
yabahiro 35:94f026ab4d30 1942 if(g[3]==0) {
yabahiro 31:45f97698857b 1943 count=0;
yabahiro 31:45f97698857b 1944 mode=40;
yabahiro 31:45f97698857b 1945 }
yabahiro 31:45f97698857b 1946 } else if(mode==40) {
yabahiro 36:788274604650 1947
yabahiro 38:391a0c8e4c16 1948 if(LIB) {
yabahiro 38:391a0c8e4c16 1949 mode=42;
yabahiro 38:391a0c8e4c16 1950 }
yabahiro 38:391a0c8e4c16 1951
yabahiro 36:788274604650 1952 if(counts==false&&g[0]==0) {
yabahiro 36:788274604650 1953 cros++;
yabahiro 36:788274604650 1954 counts=true;
yabahiro 36:788274604650 1955 }
yabahiro 36:788274604650 1956 if(counts==true&&g[0]==99) {
yabahiro 36:788274604650 1957 counts=false;
yabahiro 36:788274604650 1958 }
yabahiro 36:788274604650 1959
yabahiro 36:788274604650 1960 if(cros==2) {
yabahiro 36:788274604650 1961 // mode=41;
yabahiro 36:788274604650 1962 //count=0;
yabahiro 36:788274604650 1963 //cros=0;
yabahiro 36:788274604650 1964 }//ゆっくりモードに入れなかった時の保険
yabahiro 36:788274604650 1965
yabahiro 31:45f97698857b 1966 switch(g[3]) {
yabahiro 31:45f97698857b 1967 case 98:
kishibekairohan 33:656aa14904a5 1968 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1969 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1970 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1971 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1972 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1973 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1974 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1975 motor[TIRE_BL].dir = FOR;
yabahiro 36:788274604650 1976 if(count>20000) {
yabahiro 31:45f97698857b 1977 cross++;
yabahiro 31:45f97698857b 1978 count=0;
yabahiro 31:45f97698857b 1979 }
yabahiro 31:45f97698857b 1980 mtc2=true;
yabahiro 36:788274604650 1981 if(cross==3) {
yabahiro 31:45f97698857b 1982 mode=41;
yabahiro 31:45f97698857b 1983 count=0;
yabahiro 31:45f97698857b 1984 cross=0;
yabahiro 24:41c23c9a5058 1985 }
yabahiro 31:45f97698857b 1986 break;
yabahiro 31:45f97698857b 1987 case 0:
kishibekairohan 33:656aa14904a5 1988 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1989 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1990 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1991 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1992 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1993 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1994 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1995 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1996 mtc2=true;
yabahiro 31:45f97698857b 1997 break;
yabahiro 31:45f97698857b 1998 case 255:
kishibekairohan 33:656aa14904a5 1999 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2000 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2001 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 2002 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2003 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 2004 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 2005 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2006 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2007 mtc2=true;
yabahiro 31:45f97698857b 2008 break;
yabahiro 31:45f97698857b 2009 case 253:
yabahiro 31:45f97698857b 2010 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2011 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2012 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2013 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2014 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2015 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 2016 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2017 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2018 mtc2=true;
yabahiro 31:45f97698857b 2019 break;
yabahiro 31:45f97698857b 2020 case 254:
yabahiro 31:45f97698857b 2021 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2022 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2023 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2024 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2025 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2026 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 2027 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2028 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2029 mtc2=true;
yabahiro 31:45f97698857b 2030 break;
yabahiro 31:45f97698857b 2031 case 1:
kishibekairohan 33:656aa14904a5 2032 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2033 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2034 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2035 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2036 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2037 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 2038 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2039 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2040 mtc2=true;
yabahiro 31:45f97698857b 2041 break;
yabahiro 31:45f97698857b 2042 case 3:
kishibekairohan 33:656aa14904a5 2043 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2044 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2045 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2046 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2047 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2048 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2049 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2050 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2051 mtc2=true;
yabahiro 31:45f97698857b 2052 break;
yabahiro 31:45f97698857b 2053 case 2:
kishibekairohan 33:656aa14904a5 2054 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2055 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2056 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2057 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2058 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2059 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2060 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2061 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2062 mtc2=true;
yabahiro 31:45f97698857b 2063 break;
yabahiro 31:45f97698857b 2064 }
yabahiro 31:45f97698857b 2065 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 2066 switch(g[2]) {
yabahiro 23:694ecfa97839 2067 case 0:
yabahiro 31:45f97698857b 2068 mtc2=false;
yabahiro 23:694ecfa97839 2069 break;
yabahiro 23:694ecfa97839 2070 case 255:
yabahiro 31:45f97698857b 2071 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 2072 mtc2=false;
yabahiro 23:694ecfa97839 2073 break;
yabahiro 23:694ecfa97839 2074 case 253:
yabahiro 31:45f97698857b 2075 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 2076 mtc2=false;
yabahiro 23:694ecfa97839 2077 break;
yabahiro 23:694ecfa97839 2078 case 254:
yabahiro 31:45f97698857b 2079 motor[TIRE_FL].pwm += 20;
yabahiro 29:c78a46f51209 2080 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2081 mtc2=false;
yabahiro 23:694ecfa97839 2082 break;
yabahiro 23:694ecfa97839 2083 case 1:
yabahiro 31:45f97698857b 2084 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 2085 mtc2=false;
yabahiro 23:694ecfa97839 2086 break;
yabahiro 23:694ecfa97839 2087 case 3:
yabahiro 31:45f97698857b 2088 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 2089 mtc2=false;
yabahiro 23:694ecfa97839 2090 break;
yabahiro 23:694ecfa97839 2091 case 2:
yabahiro 31:45f97698857b 2092 motor[TIRE_BL].pwm += 20;
yabahiro 27:545148741493 2093 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2094 mtc2=false;
yabahiro 29:c78a46f51209 2095 break;
yabahiro 29:c78a46f51209 2096 }
yabahiro 31:45f97698857b 2097 }
yabahiro 31:45f97698857b 2098 } else if(mode==41) {
yabahiro 36:788274604650 2099 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 2100 motor[TIRE_FR].dir = BACK;
yabahiro 36:788274604650 2101 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 2102 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 2103 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 2104 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 2105 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 2106 motor[TIRE_BL].dir = FOR;
yabahiro 36:788274604650 2107 if(g[1]==0) {
yabahiro 31:45f97698857b 2108 count=0;
yabahiro 31:45f97698857b 2109 mode=70;
yabahiro 31:45f97698857b 2110 }
yabahiro 36:788274604650 2111
yabahiro 36:788274604650 2112 } else if(mode==70)
kishibekairohan 32:90f018b10ad6 2113 switch(g[0]) {
yabahiro 31:45f97698857b 2114 case 99:
yabahiro 31:45f97698857b 2115 if(count>=100000) {
yabahiro 31:45f97698857b 2116 mode=100;
yabahiro 28:9cd36721cf83 2117 }
yabahiro 31:45f97698857b 2118 break;
yabahiro 31:45f97698857b 2119 case 98:
kishibekairohan 33:656aa14904a5 2120 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2121 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2122 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2123 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2124 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2125 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2126 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2127 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2128 break;
yabahiro 31:45f97698857b 2129 case 0:
kishibekairohan 33:656aa14904a5 2130 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2131 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2132 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 2133 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2134 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 2135 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2136 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2137 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2138 mtc=true;
yabahiro 31:45f97698857b 2139 break;
yabahiro 31:45f97698857b 2140 case 255:
kishibekairohan 33:656aa14904a5 2141 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2142 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2143 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2144 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2145 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2146 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2147 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2148 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2149 mtc=true;
yabahiro 31:45f97698857b 2150 break;
yabahiro 31:45f97698857b 2151 case 253:
kishibekairohan 33:656aa14904a5 2152 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2153 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2154 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2155 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2156 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2157 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2158 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2159 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2160 mtc=true;
yabahiro 31:45f97698857b 2161 break;
yabahiro 31:45f97698857b 2162 case 254:
kishibekairohan 33:656aa14904a5 2163 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2164 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2165 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2166 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2167 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2168 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2169 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2170 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2171 mtc=true;
yabahiro 31:45f97698857b 2172 break;
yabahiro 31:45f97698857b 2173 case 1:
kishibekairohan 33:656aa14904a5 2174 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2175 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2176 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 2177 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2178 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 2179 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2180 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2181 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2182 mtc=true;
yabahiro 31:45f97698857b 2183 break;
yabahiro 31:45f97698857b 2184 case 3:
yabahiro 31:45f97698857b 2185 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2186 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2187 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2188 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2189 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2190 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 2191 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2192 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2193 mtc=true;
yabahiro 31:45f97698857b 2194 break;
yabahiro 31:45f97698857b 2195 case 2:
yabahiro 31:45f97698857b 2196 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2197 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2198 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2199 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2200 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2201 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 2202 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2203 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2204 mtc=true;
yabahiro 31:45f97698857b 2205 break;
yabahiro 31:45f97698857b 2206 }
yabahiro 36:788274604650 2207 if(g[1]!=98&&mtc==true) {
yabahiro 36:788274604650 2208 switch(g[0]) {
yabahiro 36:788274604650 2209 case 0:
yabahiro 36:788274604650 2210 mtc=false;
yabahiro 36:788274604650 2211 break;
yabahiro 36:788274604650 2212 case 255:
yabahiro 36:788274604650 2213 motor[TIRE_BR].pwm += 5;
yabahiro 36:788274604650 2214 mtc=false;
yabahiro 36:788274604650 2215 break;
yabahiro 36:788274604650 2216 case 253:
yabahiro 36:788274604650 2217 motor[TIRE_BR].pwm += 10;
yabahiro 36:788274604650 2218 mtc=false;
yabahiro 36:788274604650 2219 break;
yabahiro 36:788274604650 2220 case 254:
yabahiro 36:788274604650 2221 motor[TIRE_BR].pwm += 20;
yabahiro 36:788274604650 2222 motor[TIRE_BL].pwm = 0;
yabahiro 36:788274604650 2223 mtc=false;
yabahiro 36:788274604650 2224 break;
yabahiro 36:788274604650 2225 case 1:
yabahiro 36:788274604650 2226 motor[TIRE_BL].pwm += 5;
yabahiro 36:788274604650 2227 mtc=false;
yabahiro 36:788274604650 2228 break;
yabahiro 36:788274604650 2229 case 3:
yabahiro 36:788274604650 2230 motor[TIRE_BL].pwm += 10;
yabahiro 36:788274604650 2231 mtc=false;
yabahiro 36:788274604650 2232 break;
yabahiro 36:788274604650 2233 case 2:
yabahiro 36:788274604650 2234 motor[TIRE_BL].pwm += 20;
yabahiro 36:788274604650 2235 motor[TIRE_BR].pwm = 0;
yabahiro 36:788274604650 2236 mtc=false;
yabahiro 36:788274604650 2237 break;
yabahiro 24:41c23c9a5058 2238 }
yabahiro 31:45f97698857b 2239 } else if(mode==100) {
yabahiro 36:788274604650 2240 motor[TIRE_FR].pwm = 20;
kishibekairohan 32:90f018b10ad6 2241 motor[TIRE_FR].dir = BACK;
yabahiro 36:788274604650 2242 motor[TIRE_FL].pwm = 20;
kishibekairohan 32:90f018b10ad6 2243 motor[TIRE_FL].dir = FOR;
yabahiro 31:45f97698857b 2244 motor[TIRE_BR].pwm = 23;
kishibekairohan 32:90f018b10ad6 2245 motor[TIRE_BR].dir = BACK;
yabahiro 36:788274604650 2246 motor[TIRE_BL].pwm = 20;
kishibekairohan 32:90f018b10ad6 2247 motor[TIRE_BL].dir = FOR;
yabahiro 36:788274604650 2248 }
yabahiro 36:788274604650 2249 /* else {
yabahiro 31:45f97698857b 2250 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2251 motor[TIRE_FR].dir = BRAKE;
yabahiro 31:45f97698857b 2252 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2253 motor[TIRE_FL].dir = BRAKE;
yabahiro 31:45f97698857b 2254 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2255 motor[TIRE_BR].dir = BRAKE;
yabahiro 31:45f97698857b 2256 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2257 motor[TIRE_BL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 2258 }
yabahiro 36:788274604650 2259 */
kishibekairohan 13:b6e02d6261d7 2260 }
kishibekairohan 13:b6e02d6261d7 2261 #endif
kishibekairohan 13:b6e02d6261d7 2262
kishibekairohan 13:b6e02d6261d7 2263 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 2264 static void Process5()
yabahiro 21:1f1e9c585da8 2265 {
yabahiro 21:1f1e9c585da8 2266
kishibekairohan 13:b6e02d6261d7 2267 }
kishibekairohan 13:b6e02d6261d7 2268 #endif
kishibekairohan 13:b6e02d6261d7 2269
kishibekairohan 13:b6e02d6261d7 2270 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 2271 static void Process6()
kishibekairohan 13:b6e02d6261d7 2272 {
yabahiro 21:1f1e9c585da8 2273
kishibekairohan 13:b6e02d6261d7 2274 }
kishibekairohan 13:b6e02d6261d7 2275 #endif
kishibekairohan 13:b6e02d6261d7 2276
kishibekairohan 13:b6e02d6261d7 2277 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 2278 static void Process7()
kishibekairohan 13:b6e02d6261d7 2279 {
yabahiro 21:1f1e9c585da8 2280
kishibekairohan 12:c09b3e08a316 2281 }
kishibekairohan 12:c09b3e08a316 2282 #endif
kishibekairohan 12:c09b3e08a316 2283
yabahiro 21:1f1e9c585da8 2284 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 2285 static void Process8()
kishibekairohan 12:c09b3e08a316 2286 {
yabahiro 21:1f1e9c585da8 2287
kishibekairohan 12:c09b3e08a316 2288 }
kishibekairohan 12:c09b3e08a316 2289 #endif
kishibekairohan 12:c09b3e08a316 2290
kishibekairohan 13:b6e02d6261d7 2291 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 2292 static void Process9()
kishibekairohan 12:c09b3e08a316 2293 {
yabahiro 21:1f1e9c585da8 2294
t_yamamoto 0:669ef71cba68 2295 }
t_yamamoto 0:669ef71cba68 2296 #endif
t_yamamoto 0:669ef71cba68 2297 #endif
t_yamamoto 0:669ef71cba68 2298 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 2299
t_yamamoto 0:669ef71cba68 2300 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 2301 {
t_yamamoto 0:669ef71cba68 2302
yabahiro 21:1f1e9c585da8 2303 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 2304 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 2305 #endif
t_yamamoto 0:669ef71cba68 2306
yabahiro 21:1f1e9c585da8 2307 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 2308 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 21:1f1e9c585da8 2309 motor[i].dir = FREE;
yabahiro 21:1f1e9c585da8 2310 motor[i].pwm = 0;
yabahiro 21:1f1e9c585da8 2311 }
yabahiro 21:1f1e9c585da8 2312 #endif
t_yamamoto 0:669ef71cba68 2313 }
t_yamamoto 0:669ef71cba68 2314
yabahiro 21:1f1e9c585da8 2315 /*
yabahiro 21:1f1e9c585da8 2316 void BuzzerTimer_func()
yabahiro 21:1f1e9c585da8 2317 {
kishibekairohan 16:3f2c2d89372b 2318 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 2319 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 2320 }
yabahiro 21:1f1e9c585da8 2321 */
yabahiro 21:1f1e9c585da8 2322 void TapeLedEms_func()
yabahiro 21:1f1e9c585da8 2323 {
kishibekairohan 16:3f2c2d89372b 2324 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 2325 }
kishibekairohan 16:3f2c2d89372b 2326
kishibekairohan 16:3f2c2d89372b 2327 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 2328
kishibekairohan 16:3f2c2d89372b 2329
t_yamamoto 0:669ef71cba68 2330 #pragma endregion