大季 矢花 / Mbed 2 deprecated 2019_Master_9_2

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@39:f89166d7411c, 2019-09-28 (annotated)

Committer:
yabahiro
Date:
Sat Sep 28 02:49:10 2019 +0000
Revision:
39:f89166d7411c
Parent:
38:391a0c8e4c16
Child:
40:7f93984e8ba1
aiueo;

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