大季 矢花 / Mbed 2 deprecated 2019_Master_9_2

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@36:788274604650, 2019-09-25 (annotated)

Committer:
yabahiro
Date:
Wed Sep 25 11:03:03 2019 +0000
Revision:
36:788274604650
Parent:
35:94f026ab4d30
Child:
37:7cfa39c41245
aa;

Who changed what in which revision?

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