大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@35:94f026ab4d30, 2019-09-23 (annotated)

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