大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@50:87f5fa1bc460, 2019-11-13 (annotated)

Committer:
yabahiro
Date:
Wed Nov 13 10:00:45 2019 +0000
Revision:
50:87f5fa1bc460
Parent:
49:697b4e9ce884
Child:
51:a0bdbddd08f7
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
Ryosei 27:dd9f27fce7d1 3 #include <stdlib.h>
7ka884 4:ba9df71868df 4 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 5 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 6 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
M_souta 30:8f092276b2ba 8 #include "../../Switch/Switch.h"
Ryosei 27:dd9f27fce7d1 9
t_yamamoto 0:669ef71cba68 10 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 12 #include "../Using.h"
t_yamamoto 0:669ef71cba68 13
t_yamamoto 0:669ef71cba68 14 using namespace SWITCH;
7ka884 4:ba9df71868df 15 using namespace PID_SPACE;
Ryosei 27:dd9f27fce7d1 16
M_souta 21:e3b58d675c1c 17 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 18
t_yamamoto 0:669ef71cba68 19 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 20 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 21 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static bool lock;
t_yamamoto 0:669ef71cba68 24 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 25 static int current;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 30 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 31 #endif
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 36
kishibekairohan 2:c015739085d3 37
M_souta 30:8f092276b2ba 38 //Serial pc(USBTX, USBRX);
kishibekairohan 16:3f2c2d89372b 39
Ryosei 27:dd9f27fce7d1 40 const int omni[15][15] = {
Ryosei 27:dd9f27fce7d1 41 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 42 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 43 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 44 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei 27:dd9f27fce7d1 45 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei 27:dd9f27fce7d1 46 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei 27:dd9f27fce7d1 47 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei 27:dd9f27fce7d1 48 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei 27:dd9f27fce7d1 49 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei 27:dd9f27fce7d1 50 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei 27:dd9f27fce7d1 51 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei 27:dd9f27fce7d1 52 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei 27:dd9f27fce7d1 53 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei 27:dd9f27fce7d1 54 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei 27:dd9f27fce7d1 55 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 56 };
M_souta 21:e3b58d675c1c 57
M_souta 21:e3b58d675c1c 58 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 59
kishibekairohan 42:980af34f6fe3 60 uint8_t SetStatus(int pwmVal);
kishibekairohan 42:980af34f6fe3 61 uint8_t SetPWM(int pwmVal);
yabahiro 33:dd598bb23ad1 62
kishibekairohan 35:33c6cdba5630 63 DigitalIn EMC(PC_13); //非常停止確認用
yabahiro 33:dd598bb23ad1 64
yabahiro 33:dd598bb23ad1 65 bool start_flag = true;
yabahiro 33:dd598bb23ad1 66 bool UP_flag = false;
yabahiro 33:dd598bb23ad1 67 bool SW_flag = false;
yabahiro 33:dd598bb23ad1 68 bool Air_flag = false;
yabahiro 33:dd598bb23ad1 69 bool zyouge;
yabahiro 33:dd598bb23ad1 70 bool zyougedo;
yabahiro 33:dd598bb23ad1 71 bool dz2=true;
yabahiro 33:dd598bb23ad1 72 bool dz2i=false;
yabahiro 33:dd598bb23ad1 73 bool dz3=true;
yabahiro 33:dd598bb23ad1 74 bool dz3i=false;
yabahiro 33:dd598bb23ad1 75 bool dz4=true;
yabahiro 33:dd598bb23ad1 76 bool dz4i=false;
yabahiro 33:dd598bb23ad1 77 bool zone;
yabahiro 33:dd598bb23ad1 78 bool through=false;
yabahiro 33:dd598bb23ad1 79 bool counts=false;
yabahiro 33:dd598bb23ad1 80 bool mtc=false;
yabahiro 33:dd598bb23ad1 81 bool mtc2 = false;
yabahiro 33:dd598bb23ad1 82 bool emcs=false;
yabahiro 33:dd598bb23ad1 83 bool kiri;
yabahiro 33:dd598bb23ad1 84
yabahiro 33:dd598bb23ad1 85 int g[8];
yabahiro 33:dd598bb23ad1 86 int Twsh;
yabahiro 38:40e7fdf3c694 87 int testmode =0;
yabahiro 33:dd598bb23ad1 88 int mode =0;
yabahiro 33:dd598bb23ad1 89 int cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 90 int cros=0;
yabahiro 33:dd598bb23ad1 91 int countss=100000;//wait代替え
yabahiro 33:dd598bb23ad1 92
yabahiro 33:dd598bb23ad1 93 ///*********PWM調整用*********///
kishibekairohan 41:99a1158f3eca 94
kishibekairohan 42:980af34f6fe3 95 int fast = 60;
yabahiro 33:dd598bb23ad1 96 int normal = 40;
yabahiro 46:68296caae1af 97 int slow = 25;
yabahiro 33:dd598bb23ad1 98
t_yamamoto 0:669ef71cba68 99 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 100
t_yamamoto 0:669ef71cba68 101 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 102 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 103 static void Process0(void);
t_yamamoto 0:669ef71cba68 104 #endif
t_yamamoto 0:669ef71cba68 105 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 106 static void Process1(void);
t_yamamoto 0:669ef71cba68 107 #endif
t_yamamoto 0:669ef71cba68 108 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 109 static void Process2(void);
t_yamamoto 0:669ef71cba68 110 #endif
t_yamamoto 0:669ef71cba68 111 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 112 static void Process3(void);
t_yamamoto 0:669ef71cba68 113 #endif
t_yamamoto 0:669ef71cba68 114 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 115 static void Process4(void);
t_yamamoto 0:669ef71cba68 116 #endif
t_yamamoto 0:669ef71cba68 117 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 118 static void Process5(void);
t_yamamoto 0:669ef71cba68 119 #endif
t_yamamoto 0:669ef71cba68 120 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 121 static void Process6(void);
t_yamamoto 0:669ef71cba68 122 #endif
t_yamamoto 0:669ef71cba68 123 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 124 static void Process7(void);
t_yamamoto 0:669ef71cba68 125 #endif
t_yamamoto 0:669ef71cba68 126 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 127 static void Process8(void);
t_yamamoto 0:669ef71cba68 128 #endif
t_yamamoto 0:669ef71cba68 129 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 130 static void Process9(void);
t_yamamoto 0:669ef71cba68 131 #endif
t_yamamoto 0:669ef71cba68 132 #endif
t_yamamoto 0:669ef71cba68 133
t_yamamoto 0:669ef71cba68 134 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 135 {
Ryosei 27:dd9f27fce7d1 136 #pragma region USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 137 /*Replace here with the initialization code of your variables.*/
Ryosei 27:dd9f27fce7d1 138
Ryosei 27:dd9f27fce7d1 139 #pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 140
Ryosei 27:dd9f27fce7d1 141 lock = true;
Ryosei 27:dd9f27fce7d1 142 processChangeComp = true;
Ryosei 27:dd9f27fce7d1 143 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 144
Ryosei 27:dd9f27fce7d1 145 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 146 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 147 Process[0] = Process0;
Ryosei 27:dd9f27fce7d1 148 #endif
Ryosei 27:dd9f27fce7d1 149 #if USE_PROCESS_NUM>1
Ryosei 27:dd9f27fce7d1 150 Process[1] = Process1;
Ryosei 27:dd9f27fce7d1 151 #endif
Ryosei 27:dd9f27fce7d1 152 #if USE_PROCESS_NUM>2
Ryosei 27:dd9f27fce7d1 153 Process[2] = Process2;
Ryosei 27:dd9f27fce7d1 154 #endif
Ryosei 27:dd9f27fce7d1 155 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 156 Process[3] = Process3;
Ryosei 27:dd9f27fce7d1 157 #endif
Ryosei 27:dd9f27fce7d1 158 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 159 Process[4] = Process4;
Ryosei 27:dd9f27fce7d1 160 #endif
Ryosei 27:dd9f27fce7d1 161 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 162 Process[5] = Process5;
Ryosei 27:dd9f27fce7d1 163 #endif
Ryosei 27:dd9f27fce7d1 164 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 165 Process[6] = Process6;
Ryosei 27:dd9f27fce7d1 166 #endif
Ryosei 27:dd9f27fce7d1 167 #if USE_PROCESS_NUM>7
Ryosei 27:dd9f27fce7d1 168 Process[7] = Process7;
Ryosei 27:dd9f27fce7d1 169 #endif
Ryosei 27:dd9f27fce7d1 170 #if USE_PROCESS_NUM>8
Ryosei 27:dd9f27fce7d1 171 Process[8] = Process8;
Ryosei 27:dd9f27fce7d1 172 #endif
Ryosei 27:dd9f27fce7d1 173 #if USE_PROCESS_NUM>9
Ryosei 27:dd9f27fce7d1 174 Process[9] = Process9;
Ryosei 27:dd9f27fce7d1 175 #endif
Ryosei 27:dd9f27fce7d1 176 #endif
t_yamamoto 0:669ef71cba68 177 }
t_yamamoto 0:669ef71cba68 178
t_yamamoto 0:669ef71cba68 179 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 180 {
Ryosei 27:dd9f27fce7d1 181 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 182 if(controller->Button.HOME) lock = false;
Ryosei 27:dd9f27fce7d1 183
Ryosei 27:dd9f27fce7d1 184 if(controller->Button.START && processChangeComp) {
Ryosei 27:dd9f27fce7d1 185 current++;
Ryosei 27:dd9f27fce7d1 186 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei 27:dd9f27fce7d1 187 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 188 } else if(controller->Button.SELECT && processChangeComp) {
Ryosei 27:dd9f27fce7d1 189 current--;
Ryosei 27:dd9f27fce7d1 190 if (current < 0) current = 0;
Ryosei 27:dd9f27fce7d1 191 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 192 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei 27:dd9f27fce7d1 193 #endif
Ryosei 27:dd9f27fce7d1 194
Ryosei 27:dd9f27fce7d1 195 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 196 ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei 27:dd9f27fce7d1 197 #endif
Ryosei 27:dd9f27fce7d1 198
Ryosei 27:dd9f27fce7d1 199 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 200 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei 27:dd9f27fce7d1 201 #endif
Ryosei 27:dd9f27fce7d1 202
Ryosei 27:dd9f27fce7d1 203 #ifdef USE_RS485
Ryosei 27:dd9f27fce7d1 204 ACTUATORHUB::ActuatorHub::Update();
Ryosei 27:dd9f27fce7d1 205 //LINEHUB::LineHub::Update();
Ryosei 27:dd9f27fce7d1 206 #endif
Ryosei 27:dd9f27fce7d1 207
Ryosei 27:dd9f27fce7d1 208 }
Ryosei 27:dd9f27fce7d1 209
yabahiro 39:784552c26163 210 int point1;
Ryosei 27:dd9f27fce7d1 211 void SystemProcess()
Ryosei 27:dd9f27fce7d1 212 {
Ryosei 27:dd9f27fce7d1 213 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 214
Ryosei 27:dd9f27fce7d1 215 while(1) {
yabahiro 39:784552c26163 216 //pc.printf("4:%d\n\r",g[4]);
yabahiro 46:68296caae1af 217
yabahiro 33:dd598bb23ad1 218 if(EMC==0) {
yabahiro 33:dd598bb23ad1 219 fast=40;
yabahiro 33:dd598bb23ad1 220 normal=30;
yabahiro 33:dd598bb23ad1 221 slow=20;
yabahiro 33:dd598bb23ad1 222 cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 223 cros=0;
yabahiro 33:dd598bb23ad1 224 countss=0;//wait代替え
yabahiro 33:dd598bb23ad1 225 UP_flag = false;
yabahiro 33:dd598bb23ad1 226 SW_flag = false;
yabahiro 33:dd598bb23ad1 227 Air_flag = false;
yabahiro 33:dd598bb23ad1 228 zyouge=false;
yabahiro 33:dd598bb23ad1 229 zyougedo=false;
yabahiro 33:dd598bb23ad1 230 dz2=true;
yabahiro 33:dd598bb23ad1 231 dz2i=false;
yabahiro 33:dd598bb23ad1 232 dz3=true;
yabahiro 33:dd598bb23ad1 233 dz3i=false;
yabahiro 33:dd598bb23ad1 234 dz4=true;
yabahiro 33:dd598bb23ad1 235 dz4i=false;
yabahiro 33:dd598bb23ad1 236 through=false;
yabahiro 33:dd598bb23ad1 237 counts=false;
yabahiro 33:dd598bb23ad1 238 mtc=false;
yabahiro 33:dd598bb23ad1 239 mtc2 = false;
yabahiro 33:dd598bb23ad1 240 current = 0;
yabahiro 33:dd598bb23ad1 241 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 242 if(emcs==false) {
yabahiro 33:dd598bb23ad1 243 AllActuatorReset();
yabahiro 33:dd598bb23ad1 244 emcs=true;
yabahiro 33:dd598bb23ad1 245 }
yabahiro 33:dd598bb23ad1 246 } else {
yabahiro 33:dd598bb23ad1 247 emcs=false;
yabahiro 33:dd598bb23ad1 248 }
Ryosei 27:dd9f27fce7d1 249
yabahiro 33:dd598bb23ad1 250 if(mode==0) {
yabahiro 33:dd598bb23ad1 251 if(ZoneSW) {
kishibekairohan 35:33c6cdba5630 252 LED::LED::TapeledMode(1);
yabahiro 33:dd598bb23ad1 253 } else {
kishibekairohan 35:33c6cdba5630 254 LED::LED::TapeledMode(2);
yabahiro 33:dd598bb23ad1 255 }
yabahiro 33:dd598bb23ad1 256 }
yabahiro 33:dd598bb23ad1 257 for(int i = 0; i < 8; i++) {
yabahiro 33:dd598bb23ad1 258 g[i] = LineHub::GetPara(i);
yabahiro 33:dd598bb23ad1 259 }
yabahiro 33:dd598bb23ad1 260 if(ThSW) {
yabahiro 33:dd598bb23ad1 261 Twsh=1;
yabahiro 33:dd598bb23ad1 262 } else {
yabahiro 33:dd598bb23ad1 263 Twsh=2;
yabahiro 33:dd598bb23ad1 264 }
yabahiro 33:dd598bb23ad1 265
yabahiro 33:dd598bb23ad1 266 if(StertSW && start_flag == true) {
yabahiro 33:dd598bb23ad1 267 start_flag = false;
yabahiro 33:dd598bb23ad1 268 lock = false;
yabahiro 33:dd598bb23ad1 269 if(ZoneSW) {
yabahiro 33:dd598bb23ad1 270 countss=0;
yabahiro 33:dd598bb23ad1 271 mode=1;
yabahiro 40:ed2409e0ce6b 272 current = 3;//10GATU30変更した
yabahiro 33:dd598bb23ad1 273
yabahiro 33:dd598bb23ad1 274 } else {
kishibekairohan 34:2fce61e3ebc4 275 countss = 0;
yabahiro 33:dd598bb23ad1 276 mode=1;
yabahiro 49:697b4e9ce884 277 current = 2;//11ガツ13ニチヘンコウ
yabahiro 33:dd598bb23ad1 278 }
yabahiro 33:dd598bb23ad1 279 }
yabahiro 33:dd598bb23ad1 280 if(DWSW||DOSW) {
yabahiro 33:dd598bb23ad1 281 if(DWSW) {
yabahiro 33:dd598bb23ad1 282 zyouge=true;
yabahiro 33:dd598bb23ad1 283 }
yabahiro 33:dd598bb23ad1 284 if(DOSW) {
yabahiro 33:dd598bb23ad1 285 if(DOLS) {
yabahiro 33:dd598bb23ad1 286 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 287 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 288 } else {
yabahiro 38:40e7fdf3c694 289 motor[MOTOR_0].pwm = 200;
yabahiro 33:dd598bb23ad1 290 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 291 }
yabahiro 33:dd598bb23ad1 292 }
yabahiro 33:dd598bb23ad1 293 } else if(zyouge==false) {
yabahiro 33:dd598bb23ad1 294 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 295 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 296 }
yabahiro 33:dd598bb23ad1 297 if(zyouge) {
yabahiro 33:dd598bb23ad1 298 if(Twsh==1) {
yabahiro 38:40e7fdf3c694 299 motor[MOTOR_0].pwm = 255;
yabahiro 33:dd598bb23ad1 300 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 301 if(UPLS) {
yabahiro 33:dd598bb23ad1 302 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 303 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 304 zyouge=false;
yabahiro 33:dd598bb23ad1 305 }
yabahiro 33:dd598bb23ad1 306 }
yabahiro 33:dd598bb23ad1 307 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 308 motor[MOTOR_0].pwm = 245;
yabahiro 33:dd598bb23ad1 309 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 310 if(UPLS2) {
yabahiro 33:dd598bb23ad1 311 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 312 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 313 zyouge=false;
yabahiro 33:dd598bb23ad1 314 }
yabahiro 33:dd598bb23ad1 315 }
yabahiro 33:dd598bb23ad1 316 }
yabahiro 33:dd598bb23ad1 317 if(AIRSW) {
yabahiro 33:dd598bb23ad1 318 if(SW_flag==false) {
yabahiro 33:dd598bb23ad1 319 if(Air_flag==false) {
yabahiro 33:dd598bb23ad1 320 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 321 Air_flag=true;
yabahiro 33:dd598bb23ad1 322 SW_flag=true;
yabahiro 33:dd598bb23ad1 323 } else {
yabahiro 33:dd598bb23ad1 324 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 325 Air_flag=false;
yabahiro 33:dd598bb23ad1 326 SW_flag=true;
yabahiro 33:dd598bb23ad1 327 }
yabahiro 33:dd598bb23ad1 328 }
yabahiro 33:dd598bb23ad1 329 } else {
yabahiro 33:dd598bb23ad1 330 if(SW_flag==true) {
yabahiro 33:dd598bb23ad1 331 countss++;
yabahiro 33:dd598bb23ad1 332 if(countss>=2000) {
yabahiro 33:dd598bb23ad1 333 SW_flag=false;
yabahiro 33:dd598bb23ad1 334 countss=0;
yabahiro 33:dd598bb23ad1 335 }
yabahiro 33:dd598bb23ad1 336 }
yabahiro 33:dd598bb23ad1 337 }
yabahiro 33:dd598bb23ad1 338
yabahiro 33:dd598bb23ad1 339
yabahiro 33:dd598bb23ad1 340 #ifdef USE_MU
Ryosei 27:dd9f27fce7d1 341 controller = CONTROLLER::Controller::GetData();
yabahiro 33:dd598bb23ad1 342 #endif
yabahiro 33:dd598bb23ad1 343
Ryosei 27:dd9f27fce7d1 344
yabahiro 33:dd598bb23ad1 345 #ifdef USE_ERRORCHECK
yabahiro 38:40e7fdf3c694 346 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
Ryosei 27:dd9f27fce7d1 347 CONTROLLER::Controller::DataReset();
Ryosei 27:dd9f27fce7d1 348 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 349 lock = true;
Ryosei 27:dd9f27fce7d1 350 } else
yabahiro 38:40e7fdf3c694 351
yabahiro 33:dd598bb23ad1 352 #endif
Ryosei 27:dd9f27fce7d1 353 {
Ryosei 27:dd9f27fce7d1 354
yabahiro 33:dd598bb23ad1 355 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 356 if(!lock) {
Ryosei 27:dd9f27fce7d1 357 Process[current]();
Ryosei 27:dd9f27fce7d1 358 } else
yabahiro 33:dd598bb23ad1 359 #endif
Ryosei 27:dd9f27fce7d1 360 {
Ryosei 27:dd9f27fce7d1 361 //ロック時の処理
Ryosei 27:dd9f27fce7d1 362 }
Ryosei 27:dd9f27fce7d1 363 }
yabahiro 33:dd598bb23ad1 364
Ryosei 27:dd9f27fce7d1 365 SystemProcessUpdate();
Ryosei 27:dd9f27fce7d1 366 }
t_yamamoto 0:669ef71cba68 367 }
t_yamamoto 0:669ef71cba68 368
kishibekairohan 2:c015739085d3 369
kishibekairohan 2:c015739085d3 370
kishibekairohan 2:c015739085d3 371
t_yamamoto 0:669ef71cba68 372 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 373 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 374 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 375 static void Process0()
Ryosei 27:dd9f27fce7d1 376 {
yabahiro 33:dd598bb23ad1 377
yabahiro 33:dd598bb23ad1 378 //AllActuatorReset();
t_yamamoto 0:669ef71cba68 379 }
t_yamamoto 0:669ef71cba68 380 #endif
t_yamamoto 0:669ef71cba68 381
t_yamamoto 0:669ef71cba68 382 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 383 static void Process1()
t_yamamoto 0:669ef71cba68 384 {
kishibekairohan 35:33c6cdba5630 385 LED::LED::TapeledMode(6);
yabahiro 33:dd598bb23ad1 386 start_flag = true;
yabahiro 33:dd598bb23ad1 387 mtc=false;
yabahiro 33:dd598bb23ad1 388 mtc2 = false;
yabahiro 33:dd598bb23ad1 389 mode=1;
yabahiro 33:dd598bb23ad1 390 cross=0;
yabahiro 33:dd598bb23ad1 391 Twsh=1;
yabahiro 33:dd598bb23ad1 392 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 393 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 394 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 395 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 396
kishibekairohan 42:980af34f6fe3 397 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 398 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 399 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 400 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 401
yabahiro 33:dd598bb23ad1 402 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 403
yabahiro 33:dd598bb23ad1 404 if(controller->Button.UP) {
yabahiro 33:dd598bb23ad1 405 motor[MOTOR_0].pwm =200;
yabahiro 33:dd598bb23ad1 406 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 407 if(UPLS) {
yabahiro 33:dd598bb23ad1 408 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 409 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 410 }
yabahiro 33:dd598bb23ad1 411 }
yabahiro 33:dd598bb23ad1 412 if(controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 413 motor[MOTOR_0].pwm = 50;
yabahiro 33:dd598bb23ad1 414 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 415 }
yabahiro 33:dd598bb23ad1 416 } else {
yabahiro 33:dd598bb23ad1 417 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 418 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 419 }
yabahiro 33:dd598bb23ad1 420
yabahiro 33:dd598bb23ad1 421 if(controller->Button.Y) {
yabahiro 33:dd598bb23ad1 422 if(dz2==true) {
yabahiro 33:dd598bb23ad1 423 if(dz2i==false) {
yabahiro 33:dd598bb23ad1 424 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 425 dz2i=true;
yabahiro 33:dd598bb23ad1 426 } else {
yabahiro 33:dd598bb23ad1 427 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 428 dz2i=false;
yabahiro 33:dd598bb23ad1 429 }
yabahiro 33:dd598bb23ad1 430 dz2=false;
yabahiro 33:dd598bb23ad1 431 }
yabahiro 33:dd598bb23ad1 432 } else {
yabahiro 33:dd598bb23ad1 433 dz2=true;
yabahiro 33:dd598bb23ad1 434 }
yabahiro 33:dd598bb23ad1 435
yabahiro 33:dd598bb23ad1 436 if(controller->Button.A) {
yabahiro 33:dd598bb23ad1 437 if(dz3==true) {
yabahiro 33:dd598bb23ad1 438 if(dz3i==false) {
kishibekairohan 37:e77816810627 439 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 440 dz3i=true;
yabahiro 33:dd598bb23ad1 441 } else {
yabahiro 33:dd598bb23ad1 442 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 443 dz3i=false;
yabahiro 33:dd598bb23ad1 444 }
yabahiro 33:dd598bb23ad1 445 dz3=false;
yabahiro 33:dd598bb23ad1 446 }
yabahiro 33:dd598bb23ad1 447 } else {
yabahiro 33:dd598bb23ad1 448 dz3=true;
yabahiro 33:dd598bb23ad1 449 }
yabahiro 33:dd598bb23ad1 450
yabahiro 33:dd598bb23ad1 451 if(controller->Button.B) {
yabahiro 33:dd598bb23ad1 452 if(dz4==true) {
yabahiro 33:dd598bb23ad1 453 if(dz4i==false) {
yabahiro 38:40e7fdf3c694 454 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 455 dz4i=true;
yabahiro 33:dd598bb23ad1 456 } else {
yabahiro 38:40e7fdf3c694 457 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 458 dz4i=false;
yabahiro 33:dd598bb23ad1 459 }
yabahiro 33:dd598bb23ad1 460 dz4=false;
yabahiro 33:dd598bb23ad1 461 }
yabahiro 33:dd598bb23ad1 462 } else {
yabahiro 33:dd598bb23ad1 463 dz4=true;
yabahiro 33:dd598bb23ad1 464 }
t_yamamoto 0:669ef71cba68 465 }
t_yamamoto 0:669ef71cba68 466 #endif
t_yamamoto 0:669ef71cba68 467
t_yamamoto 0:669ef71cba68 468 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 469 static void Process2()
Ryosei 27:dd9f27fce7d1 470 {
yabahiro 33:dd598bb23ad1 471 //Oh So 青ゾーン
yabahiro 39:784552c26163 472 LED::LED::TapeledMode(2);
yabahiro 33:dd598bb23ad1 473 countss++;
yabahiro 50:87f5fa1bc460 474 switch(mode) {
yabahiro 50:87f5fa1bc460 475 case 1:
yabahiro 50:87f5fa1bc460 476 break;
yabahiro 50:87f5fa1bc460 477 case 2:
yabahiro 50:87f5fa1bc460 478 break;
yabahiro 50:87f5fa1bc460 479 case 10:
yabahiro 50:87f5fa1bc460 480 break;
yabahiro 50:87f5fa1bc460 481 case 11:
yabahiro 50:87f5fa1bc460 482 break;
yabahiro 50:87f5fa1bc460 483 case 20:
yabahiro 50:87f5fa1bc460 484 break;
yabahiro 50:87f5fa1bc460 485 case 21:
yabahiro 50:87f5fa1bc460 486 break;
yabahiro 50:87f5fa1bc460 487 case 22:
yabahiro 50:87f5fa1bc460 488 break;
yabahiro 50:87f5fa1bc460 489 case 23:
yabahiro 50:87f5fa1bc460 490 break;
yabahiro 50:87f5fa1bc460 491 case 24:
yabahiro 50:87f5fa1bc460 492 break;
yabahiro 50:87f5fa1bc460 493 case 25:
yabahiro 50:87f5fa1bc460 494 break;
yabahiro 50:87f5fa1bc460 495 case 26:
yabahiro 50:87f5fa1bc460 496 break;
yabahiro 50:87f5fa1bc460 497 case 31:
yabahiro 50:87f5fa1bc460 498 break;
yabahiro 50:87f5fa1bc460 499 case 40:
yabahiro 50:87f5fa1bc460 500 break;
yabahiro 50:87f5fa1bc460 501 case 41:
yabahiro 50:87f5fa1bc460 502 break;
yabahiro 50:87f5fa1bc460 503 case 42:
yabahiro 50:87f5fa1bc460 504 break;
yabahiro 50:87f5fa1bc460 505 case 70:
yabahiro 50:87f5fa1bc460 506 break;
yabahiro 50:87f5fa1bc460 507 case 100:
yabahiro 50:87f5fa1bc460 508 break;
yabahiro 50:87f5fa1bc460 509 default:
yabahiro 50:87f5fa1bc460 510 　break;
yabahiro 50:87f5fa1bc460 511 }
yabahiro 33:dd598bb23ad1 512 if(mode==1) {//スタートゾーンから白線検知
yabahiro 33:dd598bb23ad1 513 fast=40;
yabahiro 33:dd598bb23ad1 514 normal=30;
yabahiro 33:dd598bb23ad1 515 slow=20;
kishibekairohan 45:0cf48a0605cd 516 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 517 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 518 motor[TIRE_FL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 519 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 520 motor[TIRE_BR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 521 motor[TIRE_BR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 522 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 523 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 524 if(g[2]==0&&countss>=100000) {
yabahiro 33:dd598bb23ad1 525 countss=100000;
yabahiro 33:dd598bb23ad1 526 cross=0;
yabahiro 33:dd598bb23ad1 527 mode=3;
yabahiro 33:dd598bb23ad1 528 }
yabahiro 46:68296caae1af 529 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 33:dd598bb23ad1 530 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 531 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 532 motor[TIRE_FL].pwm = 255;
kishibekairohan 45:0cf48a0605cd 533 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 45:0cf48a0605cd 534 motor[TIRE_BR].pwm = 255;
kishibekairohan 45:0cf48a0605cd 535 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 536 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 537 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 538 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 539 mode=10;
yabahiro 33:dd598bb23ad1 540 countss=100000;
yabahiro 33:dd598bb23ad1 541 }
yabahiro 46:68296caae1af 542 } else if(mode==10) { //縦ライントレース
yabahiro 33:dd598bb23ad1 543 switch(g[1]) {
yabahiro 33:dd598bb23ad1 544 case 98:
yabahiro 49:697b4e9ce884 545 motor[TIRE_FR].pwm = normal+55;
yabahiro 33:dd598bb23ad1 546 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 547 motor[TIRE_FL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 548 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 549 motor[TIRE_BR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 550 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 551 motor[TIRE_BL].pwm = normal+45;
yabahiro 33:dd598bb23ad1 552 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 553 mtc=true;
yabahiro 39:784552c26163 554 if(countss>80000) {
yabahiro 33:dd598bb23ad1 555 cross++;
yabahiro 33:dd598bb23ad1 556 countss=0;
yabahiro 33:dd598bb23ad1 557 }
yabahiro 46:68296caae1af 558
yabahiro 46:68296caae1af 559 if(cross==2) {
yabahiro 46:68296caae1af 560 fast = 10;
yabahiro 46:68296caae1af 561 normal = 10;
kishibekairohan 45:0cf48a0605cd 562 slow = 5;
kishibekairohan 45:0cf48a0605cd 563 }
yabahiro 46:68296caae1af 564
yabahiro 39:784552c26163 565 if(cross==3) {
kishibekairohan 45:0cf48a0605cd 566 fast=40;
kishibekairohan 45:0cf48a0605cd 567 normal=30;
kishibekairohan 45:0cf48a0605cd 568 slow=20;
yabahiro 33:dd598bb23ad1 569 mode=11;
yabahiro 33:dd598bb23ad1 570 countss=0;
yabahiro 33:dd598bb23ad1 571 cross=0;
yabahiro 33:dd598bb23ad1 572 }
yabahiro 33:dd598bb23ad1 573 break;
yabahiro 33:dd598bb23ad1 574 case 0:
yabahiro 49:697b4e9ce884 575 motor[TIRE_FR].pwm = fast+55;
yabahiro 33:dd598bb23ad1 576 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 577 motor[TIRE_FL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 578 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 579 motor[TIRE_BR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 580 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 581 motor[TIRE_BL].pwm = fast+45;
yabahiro 33:dd598bb23ad1 582 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 583 mtc=true;
yabahiro 33:dd598bb23ad1 584 break;
yabahiro 33:dd598bb23ad1 585 case 255:
yabahiro 49:697b4e9ce884 586 motor[TIRE_FR].pwm = fast+55;
yabahiro 33:dd598bb23ad1 587 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 588 motor[TIRE_FL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 589 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 590 motor[TIRE_BR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 591 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 592 motor[TIRE_BL].pwm = fast+45;
yabahiro 33:dd598bb23ad1 593 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 594 mtc=true;
yabahiro 33:dd598bb23ad1 595 break;
yabahiro 33:dd598bb23ad1 596 case 253:
yabahiro 49:697b4e9ce884 597 motor[TIRE_FR].pwm = slow+55;
yabahiro 33:dd598bb23ad1 598 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 599 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 600 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 601 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 602 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 603 motor[TIRE_BL].pwm = slow+45;
yabahiro 33:dd598bb23ad1 604 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 605 mtc=true;
yabahiro 33:dd598bb23ad1 606 break;
yabahiro 33:dd598bb23ad1 607 case 254:
yabahiro 49:697b4e9ce884 608 motor[TIRE_FR].pwm = normal+55;
yabahiro 33:dd598bb23ad1 609 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 610 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 611 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 612 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 613 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 614 motor[TIRE_BL].pwm = normal+45;
yabahiro 33:dd598bb23ad1 615 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 616 mtc=true;
yabahiro 33:dd598bb23ad1 617 break;
yabahiro 33:dd598bb23ad1 618 case 1:
yabahiro 49:697b4e9ce884 619 motor[TIRE_FR].pwm = slow+55;
yabahiro 33:dd598bb23ad1 620 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 621 motor[TIRE_FL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 622 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 623 motor[TIRE_BR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 624 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 625 motor[TIRE_BL].pwm = slow+45;
yabahiro 33:dd598bb23ad1 626 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 627 mtc=true;
yabahiro 33:dd598bb23ad1 628 break;
yabahiro 33:dd598bb23ad1 629 case 3:
yabahiro 33:dd598bb23ad1 630 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 631 motor[TIRE_FR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 632 motor[TIRE_FL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 633 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 634 motor[TIRE_BR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 635 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 636 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 637 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 638 mtc=true;
yabahiro 33:dd598bb23ad1 639 break;
yabahiro 33:dd598bb23ad1 640 case 2:
yabahiro 33:dd598bb23ad1 641 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 642 motor[TIRE_FR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 643 motor[TIRE_FL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 644 motor[TIRE_FL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 645 motor[TIRE_BR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 646 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 647 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 648 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 649 mtc=true;
yabahiro 33:dd598bb23ad1 650 break;
yabahiro 33:dd598bb23ad1 651 }
kishibekairohan 45:0cf48a0605cd 652 if(g[1]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 653 switch(g[0]) {
yabahiro 33:dd598bb23ad1 654 case 0:
yabahiro 33:dd598bb23ad1 655 mtc=false;
yabahiro 33:dd598bb23ad1 656 break;
yabahiro 33:dd598bb23ad1 657 case 255:
kishibekairohan 45:0cf48a0605cd 658 motor[TIRE_FL].pwm += 3;
yabahiro 33:dd598bb23ad1 659 mtc=false;
yabahiro 33:dd598bb23ad1 660 break;
yabahiro 33:dd598bb23ad1 661 case 253:
kishibekairohan 45:0cf48a0605cd 662 motor[TIRE_FL].pwm += 6;
yabahiro 33:dd598bb23ad1 663 mtc=false;
yabahiro 33:dd598bb23ad1 664 break;
yabahiro 33:dd598bb23ad1 665 case 254:
kishibekairohan 45:0cf48a0605cd 666 motor[TIRE_FL].pwm += 9;
yabahiro 33:dd598bb23ad1 667 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 668 mtc=false;
yabahiro 33:dd598bb23ad1 669 break;
yabahiro 33:dd598bb23ad1 670 case 1:
kishibekairohan 45:0cf48a0605cd 671 motor[TIRE_FR].pwm += 3;
yabahiro 33:dd598bb23ad1 672 mtc=false;
yabahiro 33:dd598bb23ad1 673 break;
yabahiro 33:dd598bb23ad1 674 case 3:
kishibekairohan 45:0cf48a0605cd 675 motor[TIRE_FR].pwm += 6;
yabahiro 33:dd598bb23ad1 676 mtc=false;
yabahiro 33:dd598bb23ad1 677 break;
yabahiro 33:dd598bb23ad1 678 case 2:
kishibekairohan 45:0cf48a0605cd 679 motor[TIRE_FR].pwm += 9;
yabahiro 33:dd598bb23ad1 680 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 681 mtc=false;
yabahiro 33:dd598bb23ad1 682 break;
yabahiro 33:dd598bb23ad1 683 }
yabahiro 33:dd598bb23ad1 684 }
yabahiro 33:dd598bb23ad1 685 } else if(mode==11) {
yabahiro 33:dd598bb23ad1 686 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 687 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 688 motor[TIRE_FL].pwm = 50;
yabahiro 48:5562a99479e5 689 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 690 motor[TIRE_BR].pwm = 50;
yabahiro 48:5562a99479e5 691 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 692 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 693 motor[TIRE_BL].dir = BACK;
kishibekairohan 45:0cf48a0605cd 694 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 33:dd598bb23ad1 695 countss=0;
yabahiro 33:dd598bb23ad1 696 cross=0;
yabahiro 33:dd598bb23ad1 697 mode=20;
yabahiro 33:dd598bb23ad1 698 }
yabahiro 46:68296caae1af 699 } else if(mode==20) {
yabahiro 48:5562a99479e5 700 solenoid.solenoid2= SOLENOID_ON;
yabahiro 33:dd598bb23ad1 701 switch(g[2]) {
yabahiro 33:dd598bb23ad1 702 case 98:
yabahiro 49:697b4e9ce884 703 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 704 motor[TIRE_FR].dir = FOR;
yabahiro 49:697b4e9ce884 705 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 706 motor[TIRE_FL].dir = FOR;
yabahiro 49:697b4e9ce884 707 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 708 motor[TIRE_BR].dir = BACK;
yabahiro 49:697b4e9ce884 709 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 710 motor[TIRE_BL].dir = BACK;
yabahiro 49:697b4e9ce884 711 if(countss>20000) {
yabahiro 33:dd598bb23ad1 712 cross++;
yabahiro 33:dd598bb23ad1 713 countss=0;
yabahiro 33:dd598bb23ad1 714 }
yabahiro 33:dd598bb23ad1 715 mtc2=true;
yabahiro 46:68296caae1af 716
kishibekairohan 45:0cf48a0605cd 717 if(YELLOW) {
kishibekairohan 45:0cf48a0605cd 718 if(cross==1) {
kishibekairohan 47:a9cad8502999 719 fast=20;
kishibekairohan 47:a9cad8502999 720 normal=15;
kishibekairohan 47:a9cad8502999 721 slow=15;
yabahiro 49:697b4e9ce884 722 }
yabahiro 49:697b4e9ce884 723 if(cross==2) {
yabahiro 39:784552c26163 724 mode=21;
yabahiro 39:784552c26163 725 countss=0;
yabahiro 39:784552c26163 726 cross=0;
yabahiro 39:784552c26163 727 }
yabahiro 39:784552c26163 728 } else {
kishibekairohan 45:0cf48a0605cd 729 if(cross==3) {
yabahiro 39:784552c26163 730 mode=21;
yabahiro 39:784552c26163 731 countss=0;
yabahiro 39:784552c26163 732 cross=0;
yabahiro 39:784552c26163 733 }
yabahiro 39:784552c26163 734 }
yabahiro 33:dd598bb23ad1 735 break;
yabahiro 33:dd598bb23ad1 736 case 0:
kishibekairohan 45:0cf48a0605cd 737 motor[TIRE_FR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 738 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 739 motor[TIRE_FL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 740 motor[TIRE_FL].dir = FOR;
kishibekairohan 45:0cf48a0605cd 741 motor[TIRE_BR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 742 motor[TIRE_BR].dir = BACK;
kishibekairohan 45:0cf48a0605cd 743 motor[TIRE_BL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 744 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 745 mtc2=true;
yabahiro 33:dd598bb23ad1 746 break;
yabahiro 33:dd598bb23ad1 747 case 255:
kishibekairohan 45:0cf48a0605cd 748 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 749 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 750 motor[TIRE_FL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 751 motor[TIRE_FL].dir = FOR;
kishibekairohan 45:0cf48a0605cd 752 motor[TIRE_BR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 753 motor[TIRE_BR].dir = BACK;
kishibekairohan 45:0cf48a0605cd 754 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 755 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 756 mtc2=true;
yabahiro 33:dd598bb23ad1 757 break;
yabahiro 33:dd598bb23ad1 758 case 253:
yabahiro 33:dd598bb23ad1 759 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 760 motor[TIRE_FR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 761 motor[TIRE_FL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 762 motor[TIRE_FL].dir = FOR;
kishibekairohan 45:0cf48a0605cd 763 motor[TIRE_BR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 764 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 765 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 766 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 767 mtc2=true;
yabahiro 33:dd598bb23ad1 768 break;
yabahiro 33:dd598bb23ad1 769 case 254:
yabahiro 33:dd598bb23ad1 770 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 771 motor[TIRE_FR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 772 motor[TIRE_FL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 773 motor[TIRE_FL].dir = FOR;
kishibekairohan 45:0cf48a0605cd 774 motor[TIRE_BR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 775 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 776 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 777 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 778 mtc2=true;
yabahiro 33:dd598bb23ad1 779 break;
yabahiro 33:dd598bb23ad1 780 case 1:
kishibekairohan 45:0cf48a0605cd 781 motor[TIRE_FR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 782 motor[TIRE_FR].dir = FOR;
kishibekairohan 45:0cf48a0605cd 783 motor[TIRE_FL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 784 motor[TIRE_FL].dir = FOR;
kishibekairohan 45:0cf48a0605cd 785 motor[TIRE_BR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 786 motor[TIRE_BR].dir = BACK;
kishibekairohan 45:0cf48a0605cd 787 motor[TIRE_BL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 788 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 789 mtc2=true;
yabahiro 33:dd598bb23ad1 790 break;
yabahiro 33:dd598bb23ad1 791 case 3:
kishibekairohan 45:0cf48a0605cd 792 motor[TIRE_FR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 793 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 794 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 795 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 796 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 797 motor[TIRE_BR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 798 motor[TIRE_BL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 799 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 800 mtc2=true;
yabahiro 33:dd598bb23ad1 801 break;
yabahiro 33:dd598bb23ad1 802 case 2:
kishibekairohan 45:0cf48a0605cd 803 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 804 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 805 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 806 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 807 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 808 motor[TIRE_BR].dir = FREE;
kishibekairohan 45:0cf48a0605cd 809 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 810 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 811 mtc2=true;
yabahiro 33:dd598bb23ad1 812 break;
yabahiro 33:dd598bb23ad1 813 }
yabahiro 33:dd598bb23ad1 814 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 815 switch(g[3]) {
yabahiro 33:dd598bb23ad1 816 case 0:
yabahiro 33:dd598bb23ad1 817 mtc2=false;
yabahiro 33:dd598bb23ad1 818 break;
yabahiro 33:dd598bb23ad1 819 case 255:
yabahiro 33:dd598bb23ad1 820 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 821 mtc2=false;
yabahiro 33:dd598bb23ad1 822 break;
yabahiro 33:dd598bb23ad1 823 case 253:
yabahiro 33:dd598bb23ad1 824 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 825 mtc2=false;
yabahiro 33:dd598bb23ad1 826 break;
yabahiro 33:dd598bb23ad1 827 case 254:
kishibekairohan 45:0cf48a0605cd 828 motor[TIRE_BL].pwm += 15;
yabahiro 33:dd598bb23ad1 829 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 830 mtc2=false;
yabahiro 33:dd598bb23ad1 831 break;
yabahiro 33:dd598bb23ad1 832 case 1:
yabahiro 33:dd598bb23ad1 833 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 834 mtc2=false;
yabahiro 33:dd598bb23ad1 835 break;
yabahiro 33:dd598bb23ad1 836 case 3:
yabahiro 33:dd598bb23ad1 837 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 838 mtc2=false;
yabahiro 33:dd598bb23ad1 839 break;
yabahiro 33:dd598bb23ad1 840 case 2:
kishibekairohan 45:0cf48a0605cd 841 motor[TIRE_FL].pwm += 15;
yabahiro 33:dd598bb23ad1 842 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 843 mtc2=false;
yabahiro 33:dd598bb23ad1 844 break;
yabahiro 33:dd598bb23ad1 845 }
yabahiro 33:dd598bb23ad1 846 }
yabahiro 50:87f5fa1bc460 847 } else if(mode==21) {
yabahiro 50:87f5fa1bc460 848 if(YELLOW) {
yabahiro 33:dd598bb23ad1 849 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 850 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 851 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 852 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 853 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 854 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 855 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 856 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 857 if(g[3]==98&&countss>=10000) {
yabahiro 39:784552c26163 858 mode=22;
yabahiro 33:dd598bb23ad1 859 countss=0;
yabahiro 39:784552c26163 860 fast=60;
yabahiro 39:784552c26163 861 normal=40;
yabahiro 39:784552c26163 862 slow=20;
yabahiro 39:784552c26163 863 }
yabahiro 50:87f5fa1bc460 864 } else {
yabahiro 50:87f5fa1bc460 865 motor[TIRE_FR].pwm = 255;
yabahiro 50:87f5fa1bc460 866 motor[TIRE_FR].dir = BRAKE;
yabahiro 50:87f5fa1bc460 867 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 868 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 869 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 870 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 871 motor[TIRE_BL].pwm = 255;
yabahiro 50:87f5fa1bc460 872 motor[TIRE_BL].dir = BRAKE;
yabahiro 50:87f5fa1bc460 873 if(g[0]==0) {
yabahiro 50:87f5fa1bc460 874 mode=22;
yabahiro 50:87f5fa1bc460 875 fast=60;
yabahiro 50:87f5fa1bc460 876 normal=40;
yabahiro 50:87f5fa1bc460 877 slow=25;
yabahiro 50:87f5fa1bc460 878 }
yabahiro 50:87f5fa1bc460 879 }
yabahiro 50:87f5fa1bc460 880 } else if(mode==22) {
yabahiro 50:87f5fa1bc460 881 if(YELLOW) {
yabahiro 39:784552c26163 882 if(g[4]==3||g[4]==2) {
yabahiro 39:784552c26163 883 motor[TIRE_FR].pwm = 30;
yabahiro 39:784552c26163 884 motor[TIRE_FR].dir = FOR;
yabahiro 39:784552c26163 885 motor[TIRE_FL].pwm = 30;
yabahiro 39:784552c26163 886 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 887 motor[TIRE_BR].pwm = 30;
yabahiro 39:784552c26163 888 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 889 motor[TIRE_BL].pwm = 30;
yabahiro 39:784552c26163 890 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 891 }
yabahiro 39:784552c26163 892 if(g[4]==253||g[4]==254) {
yabahiro 39:784552c26163 893 motor[TIRE_FR].pwm = 30;
yabahiro 39:784552c26163 894 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 895 motor[TIRE_FL].pwm = 30;
yabahiro 39:784552c26163 896 motor[TIRE_FL].dir = BACK;
yabahiro 39:784552c26163 897 motor[TIRE_BR].pwm = 30;
yabahiro 39:784552c26163 898 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 899 motor[TIRE_BL].pwm = 30;
yabahiro 39:784552c26163 900 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 901 }
yabahiro 39:784552c26163 902 switch(g[2]) {
yabahiro 39:784552c26163 903 case 0:
yabahiro 39:784552c26163 904 motor[TIRE_FR].pwm = 20;
yabahiro 39:784552c26163 905 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 906 motor[TIRE_BR].pwm = 20;
yabahiro 39:784552c26163 907 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 908 break;
yabahiro 39:784552c26163 909 case 255:
yabahiro 39:784552c26163 910 motor[TIRE_FR].pwm = 30;
yabahiro 39:784552c26163 911 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 912 motor[TIRE_BR].pwm = 20;
yabahiro 39:784552c26163 913 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 914 break;
yabahiro 39:784552c26163 915 case 253:
yabahiro 39:784552c26163 916 motor[TIRE_FR].pwm = 40;
yabahiro 39:784552c26163 917 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 918 motor[TIRE_BR].pwm = 20;
yabahiro 39:784552c26163 919 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 920 break;
yabahiro 39:784552c26163 921 case 254:
yabahiro 39:784552c26163 922 motor[TIRE_FR].pwm = 30;
yabahiro 39:784552c26163 923 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 924 motor[TIRE_BR].pwm = 30;
yabahiro 39:784552c26163 925 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 926 break;
yabahiro 39:784552c26163 927 case 1:
yabahiro 39:784552c26163 928 motor[TIRE_FR].pwm = 20;
yabahiro 39:784552c26163 929 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 930 motor[TIRE_BR].pwm = 30;
yabahiro 39:784552c26163 931 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 932 break;
yabahiro 39:784552c26163 933 case 3:
yabahiro 39:784552c26163 934 motor[TIRE_FR].pwm = 20;
yabahiro 39:784552c26163 935 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 936 motor[TIRE_BR].pwm = 40;
yabahiro 39:784552c26163 937 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 938 break;
yabahiro 39:784552c26163 939 case 2:
yabahiro 39:784552c26163 940 motor[TIRE_FR].pwm = 30;
yabahiro 39:784552c26163 941 motor[TIRE_FR].dir = FOR;
yabahiro 39:784552c26163 942 motor[TIRE_BR].pwm = 30;
yabahiro 39:784552c26163 943 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 944 break;
yabahiro 39:784552c26163 945 }
yabahiro 39:784552c26163 946 switch(g[3]) {
yabahiro 39:784552c26163 947 case 0:
yabahiro 39:784552c26163 948 motor[TIRE_FL].pwm = 20;
yabahiro 39:784552c26163 949 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 950 motor[TIRE_BL].pwm = 20;
yabahiro 39:784552c26163 951 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 952 break;
yabahiro 39:784552c26163 953 case 255:
yabahiro 39:784552c26163 954 motor[TIRE_FL].pwm = 20;
yabahiro 39:784552c26163 955 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 956 motor[TIRE_BL].pwm = 30;
yabahiro 39:784552c26163 957 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 958 case 253:
yabahiro 39:784552c26163 959 motor[TIRE_FL].pwm = 20;
yabahiro 39:784552c26163 960 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 961 motor[TIRE_BL].pwm = 40;
yabahiro 39:784552c26163 962 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 963 break;
yabahiro 39:784552c26163 964 case 254:
yabahiro 39:784552c26163 965 motor[TIRE_FL].pwm = 30;
yabahiro 39:784552c26163 966 motor[TIRE_FL].dir = BACK;
yabahiro 39:784552c26163 967 motor[TIRE_BL].pwm = 30;
yabahiro 39:784552c26163 968 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 969 break;
yabahiro 39:784552c26163 970 case 1:
yabahiro 39:784552c26163 971 motor[TIRE_FL].pwm = 30;
yabahiro 39:784552c26163 972 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 973 motor[TIRE_BL].pwm = 20;
yabahiro 39:784552c26163 974 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 975 break;
yabahiro 39:784552c26163 976 case 3:
yabahiro 39:784552c26163 977 motor[TIRE_FL].pwm = 40;
yabahiro 39:784552c26163 978 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 979 motor[TIRE_BL].pwm = 20;
yabahiro 39:784552c26163 980 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 981 break;
yabahiro 39:784552c26163 982 case 2:
yabahiro 39:784552c26163 983 motor[TIRE_FL].pwm = 30;
yabahiro 39:784552c26163 984 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 985 motor[TIRE_BL].pwm = 30;
yabahiro 39:784552c26163 986 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 987 break;
yabahiro 39:784552c26163 988 }
yabahiro 39:784552c26163 989 if(g[3]==0&&g[2]==0) {
yabahiro 39:784552c26163 990 mode=23;
yabahiro 33:dd598bb23ad1 991 countss=0;
yabahiro 33:dd598bb23ad1 992 }
yabahiro 50:87f5fa1bc460 993 } else {
yabahiro 48:5562a99479e5 994 if(g[0]==98) {
kishibekairohan 47:a9cad8502999 995 slow=25;
kishibekairohan 47:a9cad8502999 996 normal=30;
kishibekairohan 47:a9cad8502999 997 fast=40;
kishibekairohan 47:a9cad8502999 998 }
yabahiro 39:784552c26163 999 if(g[3]==0) {
kishibekairohan 47:a9cad8502999 1000 slow=25;
kishibekairohan 47:a9cad8502999 1001 normal=40;
kishibekairohan 47:a9cad8502999 1002 fast=40;
yabahiro 39:784552c26163 1003 mode=23;
yabahiro 39:784552c26163 1004 }
yabahiro 39:784552c26163 1005 switch(g[0]) {
yabahiro 39:784552c26163 1006 case 98:
yabahiro 48:5562a99479e5 1007 solenoid.solenoid2 = SOLENOID_OFF;
kishibekairohan 47:a9cad8502999 1008 fast=40;
kishibekairohan 47:a9cad8502999 1009 normal=30;
kishibekairohan 47:a9cad8502999 1010 slow=25;
yabahiro 39:784552c26163 1011 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1012 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1013 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1014 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1015 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1016 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1017 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1018 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1019 break;
yabahiro 39:784552c26163 1020 case 0:
yabahiro 39:784552c26163 1021 motor[TIRE_FR].pwm = fast;
yabahiro 39:784552c26163 1022 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1023 motor[TIRE_FL].pwm = fast;
yabahiro 39:784552c26163 1024 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1025 motor[TIRE_BR].pwm = fast;
yabahiro 39:784552c26163 1026 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1027 motor[TIRE_BL].pwm = fast;
yabahiro 39:784552c26163 1028 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1029 mtc=true;
yabahiro 39:784552c26163 1030 break;
yabahiro 39:784552c26163 1031 case 255:
yabahiro 39:784552c26163 1032 motor[TIRE_FR].pwm = fast;
yabahiro 39:784552c26163 1033 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1034 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1035 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1036 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1037 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1038 motor[TIRE_BL].pwm = fast;
yabahiro 39:784552c26163 1039 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1040 mtc=true;
yabahiro 39:784552c26163 1041 break;
yabahiro 39:784552c26163 1042 case 253:
yabahiro 39:784552c26163 1043 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1044 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1045 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1046 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1047 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1048 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1049 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1050 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1051 mtc=true;
yabahiro 39:784552c26163 1052 break;
yabahiro 39:784552c26163 1053 case 254:
yabahiro 39:784552c26163 1054 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1055 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1056 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1057 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1058 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1059 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1060 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1061 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1062 mtc=true;
yabahiro 39:784552c26163 1063 break;
yabahiro 39:784552c26163 1064 case 1:
yabahiro 39:784552c26163 1065 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1066 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1067 motor[TIRE_FL].pwm = fast;
yabahiro 39:784552c26163 1068 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1069 motor[TIRE_BR].pwm = fast;
yabahiro 39:784552c26163 1070 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1071 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1072 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1073 mtc=true;
yabahiro 39:784552c26163 1074 break;
yabahiro 39:784552c26163 1075 case 3:
yabahiro 39:784552c26163 1076 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1077 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1078 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1079 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1080 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1081 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1082 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1083 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1084 mtc=true;
yabahiro 39:784552c26163 1085 break;
yabahiro 39:784552c26163 1086 case 2:
yabahiro 39:784552c26163 1087 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1088 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1089 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1090 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1091 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1092 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1093 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1094 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1095 mtc=true;
yabahiro 39:784552c26163 1096 break;
yabahiro 39:784552c26163 1097 }
yabahiro 39:784552c26163 1098 if(g[0]!=98&&mtc==true) {
yabahiro 39:784552c26163 1099 switch(g[0]) {
yabahiro 39:784552c26163 1100 case 0:
yabahiro 39:784552c26163 1101 mtc=false;
yabahiro 39:784552c26163 1102 break;
yabahiro 39:784552c26163 1103 case 255:
yabahiro 39:784552c26163 1104 motor[TIRE_BR].pwm += 5;
yabahiro 39:784552c26163 1105 mtc=false;
yabahiro 39:784552c26163 1106 break;
yabahiro 39:784552c26163 1107 case 253:
yabahiro 39:784552c26163 1108 motor[TIRE_BR].pwm += 10;
yabahiro 39:784552c26163 1109 mtc=false;
yabahiro 39:784552c26163 1110 break;
yabahiro 39:784552c26163 1111 case 254:
yabahiro 39:784552c26163 1112 motor[TIRE_BR].pwm += 20;
yabahiro 39:784552c26163 1113 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1114 mtc=false;
yabahiro 39:784552c26163 1115 break;
yabahiro 39:784552c26163 1116 case 1:
yabahiro 39:784552c26163 1117 motor[TIRE_BL].pwm += 5;
yabahiro 39:784552c26163 1118 mtc=false;
yabahiro 39:784552c26163 1119 break;
yabahiro 39:784552c26163 1120 case 3:
yabahiro 39:784552c26163 1121 motor[TIRE_BL].pwm += 10;
yabahiro 39:784552c26163 1122 mtc=false;
yabahiro 39:784552c26163 1123 break;
yabahiro 39:784552c26163 1124 case 2:
yabahiro 39:784552c26163 1125 motor[TIRE_BL].pwm += 20;
yabahiro 39:784552c26163 1126 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1127 mtc=false;
yabahiro 39:784552c26163 1128 break;
yabahiro 39:784552c26163 1129 }
yabahiro 39:784552c26163 1130 }
yabahiro 50:87f5fa1bc460 1131 }
yabahiro 50:87f5fa1bc460 1132 } else if(mode==23) {
yabahiro 50:87f5fa1bc460 1133 if(YELLOW) {
yabahiro 50:87f5fa1bc460 1134 switch(g[4]) {
yabahiro 50:87f5fa1bc460 1135 case 98:
yabahiro 50:87f5fa1bc460 1136 motor[TIRE_FR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1137 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1138 motor[TIRE_FL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1139 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1140 motor[TIRE_BR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1141 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1142 motor[TIRE_BL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1143 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1144 break;
yabahiro 50:87f5fa1bc460 1145 case 99:
yabahiro 50:87f5fa1bc460 1146 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1147 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1148 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1149 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1150 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1151 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1152 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1153 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1154 break;
yabahiro 50:87f5fa1bc460 1155 case 0:
yabahiro 50:87f5fa1bc460 1156 motor[TIRE_FR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1157 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1158 motor[TIRE_FL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1159 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1160 motor[TIRE_BR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1161 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1162 motor[TIRE_BL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1163 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1164 break;
yabahiro 50:87f5fa1bc460 1165 case 255:
yabahiro 50:87f5fa1bc460 1166 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1167 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1168 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1169 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1170 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1171 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1172 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1173 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1174 break;
yabahiro 50:87f5fa1bc460 1175 case 253:
yabahiro 50:87f5fa1bc460 1176 motor[TIRE_FR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1177 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1178 motor[TIRE_FL].pwm = slow-5;
yabahiro 50:87f5fa1bc460 1179 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1180 motor[TIRE_BR].pwm = slow-5;
yabahiro 50:87f5fa1bc460 1181 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1182 motor[TIRE_BL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1183 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1184 break;
yabahiro 50:87f5fa1bc460 1185 case 254:
yabahiro 50:87f5fa1bc460 1186 motor[TIRE_FR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1187 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1188 motor[TIRE_FL].pwm = normal-5;
yabahiro 50:87f5fa1bc460 1189 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1190 motor[TIRE_BR].pwm = normal-5;
yabahiro 50:87f5fa1bc460 1191 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1192 motor[TIRE_BL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1193 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1194 break;
yabahiro 50:87f5fa1bc460 1195 case 1:
yabahiro 50:87f5fa1bc460 1196 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1197 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1198 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1199 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1200 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1201 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1202 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1203 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1204 break;
yabahiro 50:87f5fa1bc460 1205 case 3:
yabahiro 50:87f5fa1bc460 1206 motor[TIRE_FR].pwm = slow-5;
yabahiro 50:87f5fa1bc460 1207 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1208 motor[TIRE_FL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1209 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1210 motor[TIRE_BR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1211 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1212 motor[TIRE_BL].pwm = slow-5;
yabahiro 50:87f5fa1bc460 1213 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1214 break;
yabahiro 50:87f5fa1bc460 1215 case 2:
yabahiro 50:87f5fa1bc460 1216 motor[TIRE_FR].pwm = normal-5;
yabahiro 50:87f5fa1bc460 1217 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1218 motor[TIRE_FL].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1219 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1220 motor[TIRE_BR].pwm = fast-5;
yabahiro 50:87f5fa1bc460 1221 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1222 motor[TIRE_BL].pwm = normal-5;
yabahiro 50:87f5fa1bc460 1223 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1224 break;
yabahiro 50:87f5fa1bc460 1225 }
yabahiro 50:87f5fa1bc460 1226 if(g[0]==98) {
yabahiro 50:87f5fa1bc460 1227 fast=30;
yabahiro 50:87f5fa1bc460 1228 normal=25;
yabahiro 50:87f5fa1bc460 1229 slow=20;
yabahiro 50:87f5fa1bc460 1230 }
yabahiro 50:87f5fa1bc460 1231 if(g[2]==254&&countss>=100000) {
yabahiro 50:87f5fa1bc460 1232 mode=24;
yabahiro 50:87f5fa1bc460 1233 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 50:87f5fa1bc460 1234 countss=0;
yabahiro 50:87f5fa1bc460 1235 fast=60;
yabahiro 50:87f5fa1bc460 1236 normal=40;
yabahiro 50:87f5fa1bc460 1237 slow=20;
yabahiro 50:87f5fa1bc460 1238 }
yabahiro 50:87f5fa1bc460 1239 } else {
yabahiro 39:784552c26163 1240 if(UPLS==true||DOLS==true) {
yabahiro 48:5562a99479e5 1241 if(UPLS==true||DOLS==true) {
yabahiro 48:5562a99479e5 1242 motor[MOTOR_0].pwm = 200;
yabahiro 48:5562a99479e5 1243 motor[MOTOR_0].dir = BRAKE;
yabahiro 48:5562a99479e5 1244 fast=60;
yabahiro 48:5562a99479e5 1245 normal=40;
yabahiro 48:5562a99479e5 1246 slow=20;
yabahiro 48:5562a99479e5 1247 countss=0;
yabahiro 48:5562a99479e5 1248 mode=24;
yabahiro 48:5562a99479e5 1249 }
yabahiro 39:784552c26163 1250 } else {
yabahiro 39:784552c26163 1251 motor[MOTOR_0].pwm = 100;
yabahiro 39:784552c26163 1252 motor[MOTOR_0].dir = BACK;
yabahiro 39:784552c26163 1253 }
kishibekairohan 47:a9cad8502999 1254 motor[TIRE_FR].pwm = 100;
kishibekairohan 47:a9cad8502999 1255 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 1256 motor[TIRE_FL].pwm = 100;
kishibekairohan 47:a9cad8502999 1257 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 47:a9cad8502999 1258 motor[TIRE_BR].pwm = 100;
kishibekairohan 47:a9cad8502999 1259 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 1260 motor[TIRE_BL].pwm = 100;
kishibekairohan 47:a9cad8502999 1261 motor[TIRE_BL].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1262 }
yabahiro 50:87f5fa1bc460 1263 } else if(mode==24) {
yabahiro 50:87f5fa1bc460 1264 if(YELLOW) {
yabahiro 50:87f5fa1bc460 1265 if(g[4]==3||g[4]==2) {
yabahiro 50:87f5fa1bc460 1266 motor[TIRE_FR].pwm = 30;
yabahiro 50:87f5fa1bc460 1267 motor[TIRE_FR].dir = FOR;
yabahiro 50:87f5fa1bc460 1268 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1269 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1270 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 1271 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1272 motor[TIRE_BL].pwm = 30;
yabahiro 50:87f5fa1bc460 1273 motor[TIRE_BL].dir = BACK;
yabahiro 50:87f5fa1bc460 1274 }
yabahiro 50:87f5fa1bc460 1275 if(g[4]==253||g[4]==254) {
yabahiro 50:87f5fa1bc460 1276 motor[TIRE_FR].pwm = 30;
yabahiro 50:87f5fa1bc460 1277 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1278 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1279 motor[TIRE_FL].dir = BACK;
yabahiro 50:87f5fa1bc460 1280 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 1281 motor[TIRE_BR].dir = FOR;
yabahiro 50:87f5fa1bc460 1282 motor[TIRE_BL].pwm = 30;
yabahiro 50:87f5fa1bc460 1283 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1284 }
yabahiro 50:87f5fa1bc460 1285 switch(g[2]) {
yabahiro 50:87f5fa1bc460 1286 case 0:
yabahiro 50:87f5fa1bc460 1287 motor[TIRE_FR].pwm = 20;
yabahiro 50:87f5fa1bc460 1288 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1289 motor[TIRE_BR].pwm = 20;
yabahiro 50:87f5fa1bc460 1290 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1291 break;
yabahiro 50:87f5fa1bc460 1292 case 255:
yabahiro 50:87f5fa1bc460 1293 motor[TIRE_FR].pwm = 30;
yabahiro 50:87f5fa1bc460 1294 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1295 motor[TIRE_BR].pwm = 20;
yabahiro 50:87f5fa1bc460 1296 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1297 break;
yabahiro 50:87f5fa1bc460 1298 case 253:
yabahiro 50:87f5fa1bc460 1299 motor[TIRE_FR].pwm = 40;
yabahiro 50:87f5fa1bc460 1300 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1301 motor[TIRE_BR].pwm = 20;
yabahiro 50:87f5fa1bc460 1302 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1303 break;
yabahiro 50:87f5fa1bc460 1304 case 254:
yabahiro 50:87f5fa1bc460 1305 motor[TIRE_FR].pwm = 30;
yabahiro 50:87f5fa1bc460 1306 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1307 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 1308 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1309 break;
yabahiro 50:87f5fa1bc460 1310 case 1:
yabahiro 50:87f5fa1bc460 1311 motor[TIRE_FR].pwm = 20;
yabahiro 50:87f5fa1bc460 1312 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1313 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 1314 motor[TIRE_BR].dir = FOR;
yabahiro 50:87f5fa1bc460 1315 break;
yabahiro 50:87f5fa1bc460 1316 case 3:
yabahiro 50:87f5fa1bc460 1317 motor[TIRE_FR].pwm = 20;
yabahiro 50:87f5fa1bc460 1318 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1319 motor[TIRE_BR].pwm = 40;
yabahiro 50:87f5fa1bc460 1320 motor[TIRE_BR].dir = FOR;
yabahiro 50:87f5fa1bc460 1321 break;
yabahiro 50:87f5fa1bc460 1322 case 2:
yabahiro 50:87f5fa1bc460 1323 motor[TIRE_FR].pwm = 30;
yabahiro 50:87f5fa1bc460 1324 motor[TIRE_FR].dir = FOR;
yabahiro 50:87f5fa1bc460 1325 motor[TIRE_BR].pwm = 30;
yabahiro 50:87f5fa1bc460 1326 motor[TIRE_BR].dir = FOR;
yabahiro 50:87f5fa1bc460 1327 break;
yabahiro 50:87f5fa1bc460 1328 }
yabahiro 50:87f5fa1bc460 1329 switch(g[3]) {
yabahiro 50:87f5fa1bc460 1330 case 0:
yabahiro 50:87f5fa1bc460 1331 motor[TIRE_FL].pwm = 20;
yabahiro 50:87f5fa1bc460 1332 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1333 motor[TIRE_BL].pwm = 20;
yabahiro 50:87f5fa1bc460 1334 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1335 break;
yabahiro 50:87f5fa1bc460 1336 case 255:
yabahiro 50:87f5fa1bc460 1337 motor[TIRE_FL].pwm = 20;
yabahiro 50:87f5fa1bc460 1338 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1339 motor[TIRE_BL].pwm = 30;
yabahiro 50:87f5fa1bc460 1340 motor[TIRE_BL].dir = BACK;
yabahiro 50:87f5fa1bc460 1341 case 253:
yabahiro 50:87f5fa1bc460 1342 motor[TIRE_FL].pwm = 20;
yabahiro 50:87f5fa1bc460 1343 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1344 motor[TIRE_BL].pwm = 40;
yabahiro 50:87f5fa1bc460 1345 motor[TIRE_BL].dir = BACK;
yabahiro 50:87f5fa1bc460 1346 break;
yabahiro 50:87f5fa1bc460 1347 case 254:
yabahiro 50:87f5fa1bc460 1348 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1349 motor[TIRE_FL].dir = BACK;
yabahiro 50:87f5fa1bc460 1350 motor[TIRE_BL].pwm = 30;
yabahiro 50:87f5fa1bc460 1351 motor[TIRE_BL].dir = BACK;
yabahiro 50:87f5fa1bc460 1352 break;
yabahiro 50:87f5fa1bc460 1353 case 1:
yabahiro 50:87f5fa1bc460 1354 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1355 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1356 motor[TIRE_BL].pwm = 20;
yabahiro 50:87f5fa1bc460 1357 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1358 break;
yabahiro 50:87f5fa1bc460 1359 case 3:
yabahiro 50:87f5fa1bc460 1360 motor[TIRE_FL].pwm = 40;
yabahiro 50:87f5fa1bc460 1361 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1362 motor[TIRE_BL].pwm = 20;
yabahiro 50:87f5fa1bc460 1363 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1364 break;
yabahiro 50:87f5fa1bc460 1365 case 2:
yabahiro 50:87f5fa1bc460 1366 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1367 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1368 motor[TIRE_BL].pwm = 30;
yabahiro 50:87f5fa1bc460 1369 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1370 break;
yabahiro 50:87f5fa1bc460 1371 }
yabahiro 50:87f5fa1bc460 1372 if(g[3]==0&&g[2]==0) {
yabahiro 50:87f5fa1bc460 1373 mode=25;
yabahiro 50:87f5fa1bc460 1374 }
yabahiro 50:87f5fa1bc460 1375 } else {
yabahiro 39:784552c26163 1376 if(g[0]==98&&countss>=40000) {
yabahiro 39:784552c26163 1377 mode=31;
yabahiro 39:784552c26163 1378 countss=0;
yabahiro 39:784552c26163 1379 }
yabahiro 39:784552c26163 1380 switch(g[0]) {
yabahiro 39:784552c26163 1381 case 98:
kishibekairohan 47:a9cad8502999 1382 fast=40;
kishibekairohan 47:a9cad8502999 1383 normal=30;
kishibekairohan 47:a9cad8502999 1384 slow=25;
yabahiro 39:784552c26163 1385 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1386 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1387 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1388 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1389 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1390 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1391 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1392 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1393 break;
yabahiro 39:784552c26163 1394 case 0:
yabahiro 39:784552c26163 1395 motor[TIRE_FR].pwm = fast;
yabahiro 39:784552c26163 1396 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1397 motor[TIRE_FL].pwm = fast;
yabahiro 39:784552c26163 1398 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1399 motor[TIRE_BR].pwm = fast;
yabahiro 39:784552c26163 1400 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1401 motor[TIRE_BL].pwm = fast;
yabahiro 39:784552c26163 1402 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1403 mtc=true;
yabahiro 39:784552c26163 1404 break;
yabahiro 39:784552c26163 1405 case 255:
yabahiro 39:784552c26163 1406 motor[TIRE_FR].pwm = fast;
yabahiro 39:784552c26163 1407 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1408 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1409 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1410 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1411 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1412 motor[TIRE_BL].pwm = fast;
yabahiro 39:784552c26163 1413 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1414 mtc=true;
yabahiro 39:784552c26163 1415 break;
yabahiro 39:784552c26163 1416 case 253:
yabahiro 39:784552c26163 1417 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1418 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1419 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1420 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1421 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1422 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1423 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1424 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1425 mtc=true;
yabahiro 39:784552c26163 1426 break;
yabahiro 39:784552c26163 1427 case 254:
yabahiro 39:784552c26163 1428 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1429 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1430 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1431 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1432 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1433 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1434 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1435 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1436 mtc=true;
yabahiro 39:784552c26163 1437 break;
yabahiro 39:784552c26163 1438 case 1:
yabahiro 39:784552c26163 1439 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1440 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1441 motor[TIRE_FL].pwm = fast;
yabahiro 39:784552c26163 1442 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1443 motor[TIRE_BR].pwm = fast;
yabahiro 39:784552c26163 1444 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1445 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1446 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1447 mtc=true;
yabahiro 39:784552c26163 1448 break;
yabahiro 39:784552c26163 1449 case 3:
yabahiro 39:784552c26163 1450 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1451 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1452 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1453 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1454 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1455 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1456 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1457 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1458 mtc=true;
yabahiro 39:784552c26163 1459 break;
yabahiro 39:784552c26163 1460 case 2:
yabahiro 39:784552c26163 1461 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1462 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1463 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1464 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1465 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1466 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1467 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1468 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1469 mtc=true;
yabahiro 39:784552c26163 1470 break;
yabahiro 39:784552c26163 1471 }
yabahiro 39:784552c26163 1472 if(g[1]!=98&&mtc==true) {
yabahiro 39:784552c26163 1473 switch(g[1]) {
yabahiro 39:784552c26163 1474 case 0:
yabahiro 39:784552c26163 1475 mtc=false;
yabahiro 39:784552c26163 1476 break;
yabahiro 39:784552c26163 1477 case 255:
yabahiro 39:784552c26163 1478 motor[TIRE_BR].pwm += 5;
yabahiro 39:784552c26163 1479 mtc=false;
yabahiro 39:784552c26163 1480 break;
yabahiro 39:784552c26163 1481 case 253:
yabahiro 39:784552c26163 1482 motor[TIRE_BR].pwm += 10;
yabahiro 39:784552c26163 1483 mtc=false;
yabahiro 39:784552c26163 1484 break;
yabahiro 39:784552c26163 1485 case 254:
yabahiro 39:784552c26163 1486 motor[TIRE_BR].pwm += 20;
yabahiro 39:784552c26163 1487 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1488 mtc=false;
yabahiro 39:784552c26163 1489 break;
yabahiro 39:784552c26163 1490 case 1:
yabahiro 39:784552c26163 1491 motor[TIRE_BL].pwm += 5;
yabahiro 39:784552c26163 1492 mtc=false;
yabahiro 39:784552c26163 1493 break;
yabahiro 39:784552c26163 1494 case 3:
yabahiro 39:784552c26163 1495 motor[TIRE_BL].pwm += 10;
yabahiro 39:784552c26163 1496 mtc=false;
yabahiro 39:784552c26163 1497 break;
yabahiro 39:784552c26163 1498 case 2:
yabahiro 39:784552c26163 1499 motor[TIRE_BL].pwm += 20;
yabahiro 39:784552c26163 1500 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1501 mtc=false;
yabahiro 39:784552c26163 1502 break;
yabahiro 39:784552c26163 1503 }
yabahiro 33:dd598bb23ad1 1504 }
yabahiro 33:dd598bb23ad1 1505 }
yabahiro 50:87f5fa1bc460 1506 } else if(mode==25) {
yabahiro 50:87f5fa1bc460 1507 motor[TIRE_FR].pwm = 100;
yabahiro 50:87f5fa1bc460 1508 motor[TIRE_FR].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1509 motor[TIRE_FL].pwm = 100;
yabahiro 50:87f5fa1bc460 1510 motor[TIRE_FL].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1511 motor[TIRE_BR].pwm = 100;
yabahiro 50:87f5fa1bc460 1512 motor[TIRE_BR].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1513 motor[TIRE_BL].pwm = 100;
yabahiro 50:87f5fa1bc460 1514 motor[TIRE_BL].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1515 if(DOLS==true||UPLS==true) {
yabahiro 50:87f5fa1bc460 1516 if(DOLS==true||UPLS==true) {
yabahiro 50:87f5fa1bc460 1517 motor[MOTOR_0].pwm = 200;
yabahiro 50:87f5fa1bc460 1518 motor[MOTOR_0].dir = BRAKE;
yabahiro 50:87f5fa1bc460 1519 countss=0;
yabahiro 50:87f5fa1bc460 1520 mode=26;
yabahiro 50:87f5fa1bc460 1521 }
yabahiro 50:87f5fa1bc460 1522 } else {
yabahiro 50:87f5fa1bc460 1523 motor[MOTOR_0].pwm = 100;
yabahiro 50:87f5fa1bc460 1524 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 1525 }
yabahiro 50:87f5fa1bc460 1526 } else if(mode==26) {
yabahiro 50:87f5fa1bc460 1527 switch(g[4]) {
yabahiro 50:87f5fa1bc460 1528 case 98:
yabahiro 50:87f5fa1bc460 1529 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1530 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1531 motor[TIRE_FL].pwm = 60;
yabahiro 50:87f5fa1bc460 1532 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1533 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1534 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1535 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1536 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1537 break;
yabahiro 50:87f5fa1bc460 1538 case 99:
yabahiro 50:87f5fa1bc460 1539 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1540 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1541 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1542 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1543 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1544 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1545 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1546 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1547 break;
yabahiro 50:87f5fa1bc460 1548 case 0:
yabahiro 50:87f5fa1bc460 1549 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1550 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1551 motor[TIRE_FL].pwm = 60;
yabahiro 50:87f5fa1bc460 1552 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1553 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1554 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1555 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1556 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1557 break;
yabahiro 50:87f5fa1bc460 1558 case 255:
yabahiro 50:87f5fa1bc460 1559 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1560 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1561 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1562 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1563 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1564 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1565 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1566 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1567 break;
yabahiro 50:87f5fa1bc460 1568 case 253:
yabahiro 50:87f5fa1bc460 1569 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1570 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1571 motor[TIRE_FL].pwm = 20;
yabahiro 50:87f5fa1bc460 1572 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1573 motor[TIRE_BR].pwm = 20;
yabahiro 50:87f5fa1bc460 1574 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1575 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1576 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1577 break;
yabahiro 50:87f5fa1bc460 1578 case 254:
yabahiro 50:87f5fa1bc460 1579 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1580 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1581 motor[TIRE_FL].pwm = 40;
yabahiro 50:87f5fa1bc460 1582 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1583 motor[TIRE_BR].pwm = 40;
yabahiro 50:87f5fa1bc460 1584 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1585 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1586 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1587 break;
yabahiro 50:87f5fa1bc460 1588 case 1:
yabahiro 50:87f5fa1bc460 1589 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1590 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1591 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1592 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1593 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1594 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1595 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1596 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1597 break;
yabahiro 50:87f5fa1bc460 1598 case 3:
yabahiro 50:87f5fa1bc460 1599 motor[TIRE_FR].pwm = 40;
yabahiro 50:87f5fa1bc460 1600 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1601 motor[TIRE_FL].pwm = 60;
yabahiro 50:87f5fa1bc460 1602 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1603 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1604 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1605 motor[TIRE_BL].pwm = 40;
yabahiro 50:87f5fa1bc460 1606 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1607 break;
yabahiro 50:87f5fa1bc460 1608 case 2:
yabahiro 50:87f5fa1bc460 1609 motor[TIRE_FR].pwm = 40;
yabahiro 50:87f5fa1bc460 1610 motor[TIRE_FR].dir = BACK;
yabahiro 50:87f5fa1bc460 1611 motor[TIRE_FL].pwm = 60;
yabahiro 50:87f5fa1bc460 1612 motor[TIRE_FL].dir = FOR;
yabahiro 50:87f5fa1bc460 1613 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1614 motor[TIRE_BR].dir = BACK;
yabahiro 50:87f5fa1bc460 1615 motor[TIRE_BL].pwm = 40;
yabahiro 50:87f5fa1bc460 1616 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1617 break;
yabahiro 33:dd598bb23ad1 1618 }
yabahiro 50:87f5fa1bc460 1619 if(g[0]==98) {
yabahiro 50:87f5fa1bc460 1620 mode=31;
yabahiro 50:87f5fa1bc460 1621 }
yabahiro 50:87f5fa1bc460 1622 } else if(mode==31) {
kishibekairohan 47:a9cad8502999 1623 motor[TIRE_FR].pwm = 45;
kishibekairohan 47:a9cad8502999 1624 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1625 motor[TIRE_FL].pwm = 255;
kishibekairohan 47:a9cad8502999 1626 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 47:a9cad8502999 1627 motor[TIRE_BR].pwm = 255;
kishibekairohan 47:a9cad8502999 1628 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 1629 motor[TIRE_BL].pwm = 45;
kishibekairohan 47:a9cad8502999 1630 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 1631 if(g[2]==0) {
kishibekairohan 47:a9cad8502999 1632 cross=0;
kishibekairohan 47:a9cad8502999 1633 countss=0;
kishibekairohan 47:a9cad8502999 1634 mode=40;
kishibekairohan 47:a9cad8502999 1635 }
yabahiro 33:dd598bb23ad1 1636 } else if(mode==40) {
yabahiro 49:697b4e9ce884 1637 if(LIF||LIB) {
yabahiro 49:697b4e9ce884 1638 mode=42;
yabahiro 49:697b4e9ce884 1639 }
yabahiro 33:dd598bb23ad1 1640 switch(g[3]) {
yabahiro 33:dd598bb23ad1 1641 case 98:
kishibekairohan 47:a9cad8502999 1642 motor[TIRE_FR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1643 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1644 motor[TIRE_FL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1645 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1646 motor[TIRE_BR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1647 motor[TIRE_BR].dir = FOR;
kishibekairohan 47:a9cad8502999 1648 motor[TIRE_BL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1649 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1650 if(countss>10000) {
yabahiro 33:dd598bb23ad1 1651 cross++;
yabahiro 33:dd598bb23ad1 1652 countss=0;
yabahiro 33:dd598bb23ad1 1653 }
yabahiro 33:dd598bb23ad1 1654 mtc2=true;
yabahiro 49:697b4e9ce884 1655 if(YELLOW) {
yabahiro 49:697b4e9ce884 1656 if(cross==1) {
yabahiro 49:697b4e9ce884 1657 mode=41;
yabahiro 49:697b4e9ce884 1658 countss=0;
yabahiro 49:697b4e9ce884 1659 cross=0;
yabahiro 49:697b4e9ce884 1660 }
yabahiro 49:697b4e9ce884 1661 } else {
yabahiro 49:697b4e9ce884 1662 if(cross==3) {
yabahiro 49:697b4e9ce884 1663 mode=41;
yabahiro 49:697b4e9ce884 1664 countss=0;
yabahiro 49:697b4e9ce884 1665 cross=0;
yabahiro 49:697b4e9ce884 1666 }
yabahiro 33:dd598bb23ad1 1667 }
yabahiro 33:dd598bb23ad1 1668 break;
yabahiro 33:dd598bb23ad1 1669 case 0:
kishibekairohan 47:a9cad8502999 1670 motor[TIRE_FR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1671 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1672 motor[TIRE_FL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1673 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1674 motor[TIRE_BR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1675 motor[TIRE_BR].dir = FOR;
kishibekairohan 47:a9cad8502999 1676 motor[TIRE_BL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1677 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1678 mtc2=true;
yabahiro 33:dd598bb23ad1 1679 break;
yabahiro 33:dd598bb23ad1 1680 case 255:
kishibekairohan 47:a9cad8502999 1681 motor[TIRE_FR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1682 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1683 motor[TIRE_FL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1684 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1685 motor[TIRE_BR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1686 motor[TIRE_BR].dir = FOR;
kishibekairohan 47:a9cad8502999 1687 motor[TIRE_BL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1688 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1689 mtc2=true;
yabahiro 33:dd598bb23ad1 1690 break;
yabahiro 33:dd598bb23ad1 1691 case 253:
yabahiro 33:dd598bb23ad1 1692 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1693 motor[TIRE_FR].dir = FREE;
kishibekairohan 47:a9cad8502999 1694 motor[TIRE_FL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1695 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1696 motor[TIRE_BR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1697 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1698 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1699 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1700 mtc2=true;
yabahiro 33:dd598bb23ad1 1701 break;
yabahiro 33:dd598bb23ad1 1702 case 254:
yabahiro 33:dd598bb23ad1 1703 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1704 motor[TIRE_FR].dir = FREE;
kishibekairohan 47:a9cad8502999 1705 motor[TIRE_FL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1706 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1707 motor[TIRE_BR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1708 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1709 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1710 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1711 mtc2=true;
yabahiro 33:dd598bb23ad1 1712 break;
yabahiro 33:dd598bb23ad1 1713 case 1:
kishibekairohan 47:a9cad8502999 1714 motor[TIRE_FR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1715 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1716 motor[TIRE_FL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1717 motor[TIRE_FL].dir = BACK;
kishibekairohan 47:a9cad8502999 1718 motor[TIRE_BR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1719 motor[TIRE_BR].dir = FOR;
kishibekairohan 47:a9cad8502999 1720 motor[TIRE_BL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 1721 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1722 mtc2=true;
yabahiro 33:dd598bb23ad1 1723 break;
yabahiro 33:dd598bb23ad1 1724 case 3:
kishibekairohan 47:a9cad8502999 1725 motor[TIRE_FR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1726 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1727 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1728 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1729 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1730 motor[TIRE_BR].dir = FREE;
kishibekairohan 47:a9cad8502999 1731 motor[TIRE_BL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 1732 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1733 mtc2=true;
yabahiro 33:dd598bb23ad1 1734 break;
yabahiro 33:dd598bb23ad1 1735 case 2:
kishibekairohan 47:a9cad8502999 1736 motor[TIRE_FR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1737 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1738 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1739 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1740 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1741 motor[TIRE_BR].dir = FREE;
kishibekairohan 47:a9cad8502999 1742 motor[TIRE_BL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 1743 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1744 mtc2=true;
yabahiro 33:dd598bb23ad1 1745 break;
yabahiro 33:dd598bb23ad1 1746 }
yabahiro 33:dd598bb23ad1 1747 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 1748 switch(g[2]) {
yabahiro 33:dd598bb23ad1 1749 case 0:
yabahiro 33:dd598bb23ad1 1750 mtc2=false;
yabahiro 33:dd598bb23ad1 1751 break;
yabahiro 33:dd598bb23ad1 1752 case 255:
yabahiro 39:784552c26163 1753 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 1754 mtc2=false;
yabahiro 33:dd598bb23ad1 1755 break;
yabahiro 33:dd598bb23ad1 1756 case 253:
yabahiro 39:784552c26163 1757 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 1758 mtc2=false;
yabahiro 33:dd598bb23ad1 1759 break;
yabahiro 33:dd598bb23ad1 1760 case 254:
yabahiro 39:784552c26163 1761 motor[TIRE_FL].pwm += 20;
yabahiro 39:784552c26163 1762 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1763 mtc2=false;
yabahiro 33:dd598bb23ad1 1764 break;
yabahiro 33:dd598bb23ad1 1765 case 1:
yabahiro 39:784552c26163 1766 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 1767 mtc2=false;
yabahiro 33:dd598bb23ad1 1768 break;
yabahiro 33:dd598bb23ad1 1769 case 3:
yabahiro 39:784552c26163 1770 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 1771 mtc2=false;
yabahiro 33:dd598bb23ad1 1772 break;
yabahiro 33:dd598bb23ad1 1773 case 2:
yabahiro 39:784552c26163 1774 motor[TIRE_BL].pwm += 20;
yabahiro 39:784552c26163 1775 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1776 mtc2=false;
yabahiro 33:dd598bb23ad1 1777 break;
yabahiro 33:dd598bb23ad1 1778 }
yabahiro 33:dd598bb23ad1 1779 }
yabahiro 33:dd598bb23ad1 1780 } else if(mode==41) {
yabahiro 33:dd598bb23ad1 1781 if(LIF||LIB) {
yabahiro 33:dd598bb23ad1 1782 mode=42;
yabahiro 33:dd598bb23ad1 1783 }
yabahiro 33:dd598bb23ad1 1784 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 1785 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1786 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1787 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1788 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1789 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1790 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 1791 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1792 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 1793 countss=0;
yabahiro 33:dd598bb23ad1 1794 mode=70;
yabahiro 33:dd598bb23ad1 1795 normal=60;
yabahiro 33:dd598bb23ad1 1796 fast=70;
yabahiro 33:dd598bb23ad1 1797 slow=40;
yabahiro 33:dd598bb23ad1 1798 }
yabahiro 33:dd598bb23ad1 1799 } else if(mode==42) {
kishibekairohan 47:a9cad8502999 1800 motor[TIRE_FR].pwm = 40;
yabahiro 39:784552c26163 1801 motor[TIRE_FR].dir = FOR;
yabahiro 39:784552c26163 1802 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1803 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1804 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1805 motor[TIRE_BR].dir = FREE;
kishibekairohan 47:a9cad8502999 1806 motor[TIRE_BL].pwm = 40;
yabahiro 39:784552c26163 1807 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 1808 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 1809 countss=0;
yabahiro 39:784552c26163 1810 mode=70;
yabahiro 33:dd598bb23ad1 1811 }
yabahiro 39:784552c26163 1812 } else if(mode==70) {
yabahiro 33:dd598bb23ad1 1813 switch(g[0]) {
yabahiro 33:dd598bb23ad1 1814 case 99:
yabahiro 33:dd598bb23ad1 1815 if(countss>=100000) {
yabahiro 33:dd598bb23ad1 1816 mode=100;
yabahiro 33:dd598bb23ad1 1817 }
yabahiro 33:dd598bb23ad1 1818 break;
yabahiro 33:dd598bb23ad1 1819 case 98:
yabahiro 33:dd598bb23ad1 1820 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1821 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1822 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1823 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1824 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1825 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1826 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1827 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1828 break;
yabahiro 33:dd598bb23ad1 1829 case 0:
yabahiro 39:784552c26163 1830 motor[TIRE_FR].pwm = 80;
yabahiro 33:dd598bb23ad1 1831 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1832 motor[TIRE_FL].pwm = 80;
yabahiro 33:dd598bb23ad1 1833 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1834 motor[TIRE_BR].pwm = 80;
yabahiro 33:dd598bb23ad1 1835 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1836 motor[TIRE_BL].pwm = 80;
yabahiro 33:dd598bb23ad1 1837 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1838 mtc=true;
yabahiro 33:dd598bb23ad1 1839 break;
yabahiro 33:dd598bb23ad1 1840 case 255:
yabahiro 33:dd598bb23ad1 1841 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1842 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1843 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1844 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1845 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1846 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1847 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1848 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1849 mtc=true;
yabahiro 33:dd598bb23ad1 1850 break;
yabahiro 33:dd598bb23ad1 1851 case 253:
yabahiro 33:dd598bb23ad1 1852 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1853 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1854 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1855 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1856 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1857 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1858 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1859 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1860 mtc=true;
yabahiro 33:dd598bb23ad1 1861 break;
yabahiro 33:dd598bb23ad1 1862 case 254:
yabahiro 33:dd598bb23ad1 1863 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1864 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1865 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1866 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1867 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1868 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1869 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1870 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1871 mtc=true;
yabahiro 33:dd598bb23ad1 1872 break;
yabahiro 33:dd598bb23ad1 1873 case 1:
yabahiro 33:dd598bb23ad1 1874 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1875 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1876 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1877 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1878 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1879 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1880 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1881 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1882 mtc=true;
yabahiro 33:dd598bb23ad1 1883 break;
yabahiro 33:dd598bb23ad1 1884 case 3:
yabahiro 33:dd598bb23ad1 1885 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1886 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1887 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1888 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1889 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1890 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1891 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1892 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1893 mtc=true;
yabahiro 33:dd598bb23ad1 1894 break;
yabahiro 33:dd598bb23ad1 1895 case 2:
yabahiro 33:dd598bb23ad1 1896 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1897 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1898 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1899 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1900 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1901 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1902 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1903 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1904 mtc=true;
yabahiro 33:dd598bb23ad1 1905 break;
yabahiro 33:dd598bb23ad1 1906 }
yabahiro 39:784552c26163 1907 if(g[1]!=98&&mtc==true) {
yabahiro 39:784552c26163 1908 switch(g[1]) {
yabahiro 39:784552c26163 1909 case 0:
yabahiro 39:784552c26163 1910 mtc=false;
yabahiro 39:784552c26163 1911 break;
yabahiro 39:784552c26163 1912 case 255:
yabahiro 39:784552c26163 1913 motor[TIRE_BR].pwm += 5;
yabahiro 39:784552c26163 1914 mtc=false;
yabahiro 39:784552c26163 1915 break;
yabahiro 39:784552c26163 1916 case 253:
yabahiro 39:784552c26163 1917 motor[TIRE_BR].pwm += 10;
yabahiro 39:784552c26163 1918 mtc=false;
yabahiro 39:784552c26163 1919 break;
yabahiro 39:784552c26163 1920 case 254:
yabahiro 39:784552c26163 1921 motor[TIRE_BR].pwm += 20;
yabahiro 39:784552c26163 1922 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1923 mtc=false;
yabahiro 39:784552c26163 1924 break;
yabahiro 39:784552c26163 1925 case 1:
yabahiro 39:784552c26163 1926 motor[TIRE_BL].pwm += 5;
yabahiro 39:784552c26163 1927 mtc=false;
yabahiro 39:784552c26163 1928 break;
yabahiro 39:784552c26163 1929 case 3:
yabahiro 39:784552c26163 1930 motor[TIRE_BL].pwm += 10;
yabahiro 39:784552c26163 1931 mtc=false;
yabahiro 39:784552c26163 1932 break;
yabahiro 39:784552c26163 1933 case 2:
yabahiro 39:784552c26163 1934 motor[TIRE_BL].pwm += 20;
yabahiro 39:784552c26163 1935 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1936 mtc=false;
yabahiro 39:784552c26163 1937 break;
yabahiro 39:784552c26163 1938 }
yabahiro 33:dd598bb23ad1 1939 }
yabahiro 33:dd598bb23ad1 1940 } else if(mode==100) {
kishibekairohan 47:a9cad8502999 1941 motor[TIRE_FR].pwm = 50;
yabahiro 33:dd598bb23ad1 1942 motor[TIRE_FR].dir = BACK;
kishibekairohan 47:a9cad8502999 1943 motor[TIRE_FL].pwm = 50;
yabahiro 33:dd598bb23ad1 1944 motor[TIRE_FL].dir = FOR;
kishibekairohan 47:a9cad8502999 1945 motor[TIRE_BR].pwm = 50;
yabahiro 33:dd598bb23ad1 1946 motor[TIRE_BR].dir = BACK;
kishibekairohan 47:a9cad8502999 1947 motor[TIRE_BL].pwm = 50;
yabahiro 33:dd598bb23ad1 1948 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1949 } else {
yabahiro 33:dd598bb23ad1 1950 }
yabahiro 39:784552c26163 1951
kishibekairohan 13:b6e02d6261d7 1952 }
kishibekairohan 13:b6e02d6261d7 1953 #endif
kishibekairohan 13:b6e02d6261d7 1954
kishibekairohan 13:b6e02d6261d7 1955 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 1956 static void Process3()
kishibekairohan 13:b6e02d6261d7 1957 {
yabahiro 33:dd598bb23ad1 1958 //Clearly 赤ゾーン
kishibekairohan 35:33c6cdba5630 1959 LED::LED::TapeledMode(3);
yabahiro 33:dd598bb23ad1 1960 countss++;
yabahiro 33:dd598bb23ad1 1961 if(mode==1) {//スタートゾーンから白線検知
yabahiro 33:dd598bb23ad1 1962 fast=40;
yabahiro 33:dd598bb23ad1 1963 normal=30;
yabahiro 33:dd598bb23ad1 1964 slow=20;
kishibekairohan 42:980af34f6fe3 1965 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 1966 motor[TIRE_FR].dir = BACK;
kishibekairohan 42:980af34f6fe3 1967 motor[TIRE_FL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 1968 motor[TIRE_FL].dir = FOR;
kishibekairohan 42:980af34f6fe3 1969 motor[TIRE_BR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 1970 motor[TIRE_BR].dir = BACK;
kishibekairohan 42:980af34f6fe3 1971 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 1972 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1973 if(g[2]==0&&countss>=100000) {
yabahiro 33:dd598bb23ad1 1974 countss=100000;
yabahiro 33:dd598bb23ad1 1975 cross=0;
yabahiro 33:dd598bb23ad1 1976 mode=3;
yabahiro 33:dd598bb23ad1 1977 }
yabahiro 46:68296caae1af 1978 } else if(mode==3) { //横ライントレースから縦ライントレースへ
kishibekairohan 44:2aff0da5ebdb 1979 motor[TIRE_FR].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 1980 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 42:980af34f6fe3 1981 motor[TIRE_FL].pwm = 30;
yabahiro 33:dd598bb23ad1 1982 motor[TIRE_FL].dir = FOR;
kishibekairohan 42:980af34f6fe3 1983 motor[TIRE_BR].pwm = 30;
yabahiro 33:dd598bb23ad1 1984 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 1985 motor[TIRE_BL].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 1986 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1987 if(g[0]==0) {
yabahiro 33:dd598bb23ad1 1988 mode=10;
yabahiro 33:dd598bb23ad1 1989 countss=100000;
yabahiro 33:dd598bb23ad1 1990 }
yabahiro 46:68296caae1af 1991 } else if(mode==10) { //縦ライントレース
yabahiro 33:dd598bb23ad1 1992 switch(g[0]) {
yabahiro 33:dd598bb23ad1 1993 case 98:
kishibekairohan 44:2aff0da5ebdb 1994 motor[TIRE_FR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 1995 motor[TIRE_FR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 1996 motor[TIRE_FL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 1997 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 1998 motor[TIRE_BR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 1999 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2000 motor[TIRE_BL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 2001 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2002 mtc=true;
yabahiro 33:dd598bb23ad1 2003 if(countss>80000) {
yabahiro 33:dd598bb23ad1 2004 cross++;
yabahiro 33:dd598bb23ad1 2005 countss=0;
yabahiro 33:dd598bb23ad1 2006 }
yabahiro 46:68296caae1af 2007 if(cross==2) {
yabahiro 46:68296caae1af 2008 fast = 10;
yabahiro 46:68296caae1af 2009 normal = 10;
kishibekairohan 44:2aff0da5ebdb 2010 slow = 5;
yabahiro 46:68296caae1af 2011 }
yabahiro 46:68296caae1af 2012
yabahiro 38:40e7fdf3c694 2013 if(cross==3) {
kishibekairohan 44:2aff0da5ebdb 2014 fast=40;
kishibekairohan 44:2aff0da5ebdb 2015 normal=30;
kishibekairohan 44:2aff0da5ebdb 2016 slow=20;
yabahiro 33:dd598bb23ad1 2017 mode=11;
yabahiro 33:dd598bb23ad1 2018 countss=0;
yabahiro 33:dd598bb23ad1 2019 cross=0;
yabahiro 46:68296caae1af 2020
yabahiro 33:dd598bb23ad1 2021 }
yabahiro 33:dd598bb23ad1 2022 break;
yabahiro 33:dd598bb23ad1 2023 case 0:
kishibekairohan 44:2aff0da5ebdb 2024 motor[TIRE_FR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2025 motor[TIRE_FR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2026 motor[TIRE_FL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2027 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2028 motor[TIRE_BR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2029 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2030 motor[TIRE_BL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2031 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2032 mtc=true;
yabahiro 33:dd598bb23ad1 2033 break;
yabahiro 33:dd598bb23ad1 2034 case 255:
kishibekairohan 44:2aff0da5ebdb 2035 motor[TIRE_FR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2036 motor[TIRE_FR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2037 motor[TIRE_FL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2038 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2039 motor[TIRE_BR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2040 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2041 motor[TIRE_BL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2042 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2043 mtc=true;
yabahiro 33:dd598bb23ad1 2044 break;
yabahiro 33:dd598bb23ad1 2045 case 253:
kishibekairohan 44:2aff0da5ebdb 2046 motor[TIRE_FR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2047 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 2048 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2049 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2050 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2051 motor[TIRE_BR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2052 motor[TIRE_BL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2053 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2054 mtc=true;
yabahiro 33:dd598bb23ad1 2055 break;
yabahiro 33:dd598bb23ad1 2056 case 254:
kishibekairohan 44:2aff0da5ebdb 2057 motor[TIRE_FR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 2058 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 2059 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2060 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2061 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2062 motor[TIRE_BR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2063 motor[TIRE_BL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 2064 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2065 mtc=true;
yabahiro 33:dd598bb23ad1 2066 break;
yabahiro 33:dd598bb23ad1 2067 case 1:
kishibekairohan 44:2aff0da5ebdb 2068 motor[TIRE_FR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2069 motor[TIRE_FR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2070 motor[TIRE_FL].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2071 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2072 motor[TIRE_BR].pwm = fast+50;
yabahiro 33:dd598bb23ad1 2073 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2074 motor[TIRE_BL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2075 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2076 mtc=true;
yabahiro 33:dd598bb23ad1 2077 break;
yabahiro 33:dd598bb23ad1 2078 case 3:
yabahiro 33:dd598bb23ad1 2079 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2080 motor[TIRE_FR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2081 motor[TIRE_FL].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2082 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2083 motor[TIRE_BR].pwm = slow+50;
yabahiro 33:dd598bb23ad1 2084 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 2085 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2086 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2087 mtc=true;
yabahiro 33:dd598bb23ad1 2088 break;
yabahiro 33:dd598bb23ad1 2089 case 2:
yabahiro 33:dd598bb23ad1 2090 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2091 motor[TIRE_FR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2092 motor[TIRE_FL].pwm = normal+50;
yabahiro 33:dd598bb23ad1 2093 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2094 motor[TIRE_BR].pwm = normal+50;
yabahiro 33:dd598bb23ad1 2095 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 2096 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2097 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2098 mtc=true;
yabahiro 33:dd598bb23ad1 2099 break;
yabahiro 33:dd598bb23ad1 2100 }
yabahiro 33:dd598bb23ad1 2101 if(g[0]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 2102 switch(g[1]) {
yabahiro 33:dd598bb23ad1 2103 case 0:
yabahiro 33:dd598bb23ad1 2104 mtc=false;
yabahiro 33:dd598bb23ad1 2105 break;
yabahiro 33:dd598bb23ad1 2106 case 255:
kishibekairohan 44:2aff0da5ebdb 2107 motor[TIRE_BR].pwm += 3;
yabahiro 33:dd598bb23ad1 2108 mtc=false;
yabahiro 33:dd598bb23ad1 2109 break;
yabahiro 33:dd598bb23ad1 2110 case 253:
kishibekairohan 44:2aff0da5ebdb 2111 motor[TIRE_BR].pwm += 6;
yabahiro 33:dd598bb23ad1 2112 mtc=false;
yabahiro 33:dd598bb23ad1 2113 break;
yabahiro 33:dd598bb23ad1 2114 case 254:
kishibekairohan 44:2aff0da5ebdb 2115 motor[TIRE_BR].pwm += 9;
yabahiro 33:dd598bb23ad1 2116 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2117 mtc=false;
yabahiro 33:dd598bb23ad1 2118 break;
yabahiro 33:dd598bb23ad1 2119 case 1:
kishibekairohan 44:2aff0da5ebdb 2120 motor[TIRE_BL].pwm += 3;
yabahiro 33:dd598bb23ad1 2121 mtc=false;
yabahiro 33:dd598bb23ad1 2122 break;
yabahiro 33:dd598bb23ad1 2123 case 3:
kishibekairohan 44:2aff0da5ebdb 2124 motor[TIRE_BL].pwm += 6;
yabahiro 33:dd598bb23ad1 2125 mtc=false;
yabahiro 33:dd598bb23ad1 2126 break;
yabahiro 33:dd598bb23ad1 2127 case 2:
kishibekairohan 44:2aff0da5ebdb 2128 motor[TIRE_BL].pwm += 9;
yabahiro 33:dd598bb23ad1 2129 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2130 mtc=false;
yabahiro 33:dd598bb23ad1 2131 break;
yabahiro 33:dd598bb23ad1 2132 }
yabahiro 33:dd598bb23ad1 2133 }
yabahiro 46:68296caae1af 2134 } else if(mode==11) {
kishibekairohan 44:2aff0da5ebdb 2135 motor[TIRE_FR].pwm = 25;
kishibekairohan 44:2aff0da5ebdb 2136 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2137 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 2138 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 2139 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 2140 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2141 motor[TIRE_BL].pwm = 25;
kishibekairohan 44:2aff0da5ebdb 2142 motor[TIRE_BL].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2143 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 33:dd598bb23ad1 2144 countss=0;
yabahiro 33:dd598bb23ad1 2145 cross=0;
yabahiro 33:dd598bb23ad1 2146 mode=20;
yabahiro 33:dd598bb23ad1 2147 }
yabahiro 46:68296caae1af 2148 } else if(mode==20) {
yabahiro 38:40e7fdf3c694 2149 solenoid.solenoid4= SOLENOID_ON;
yabahiro 33:dd598bb23ad1 2150 switch(g[2]) {
yabahiro 33:dd598bb23ad1 2151 case 98:
kishibekairohan 44:2aff0da5ebdb 2152 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2153 motor[TIRE_FR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2154 motor[TIRE_FL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2155 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2156 motor[TIRE_BR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2157 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2158 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2159 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2160 if(countss>30000) {
yabahiro 33:dd598bb23ad1 2161 cross++;
yabahiro 33:dd598bb23ad1 2162 countss=0;
yabahiro 33:dd598bb23ad1 2163 }
yabahiro 33:dd598bb23ad1 2164 mtc2=true;
yabahiro 46:68296caae1af 2165
kishibekairohan 44:2aff0da5ebdb 2166 if(YELLOW) {
kishibekairohan 44:2aff0da5ebdb 2167 if(cross==1) {
yabahiro 46:68296caae1af 2168 fast=20;
yabahiro 46:68296caae1af 2169 normal=15;
yabahiro 46:68296caae1af 2170 slow=15;
yabahiro 46:68296caae1af 2171 } else if(cross==2) {
yabahiro 38:40e7fdf3c694 2172 mode=21;
yabahiro 38:40e7fdf3c694 2173 countss=0;
yabahiro 38:40e7fdf3c694 2174 cross=0;
yabahiro 38:40e7fdf3c694 2175 }
yabahiro 38:40e7fdf3c694 2176 } else {
yabahiro 40:ed2409e0ce6b 2177 if(cross==3) {
yabahiro 38:40e7fdf3c694 2178 mode=21;
yabahiro 38:40e7fdf3c694 2179 countss=0;
yabahiro 38:40e7fdf3c694 2180 cross=0;
yabahiro 38:40e7fdf3c694 2181 }
yabahiro 38:40e7fdf3c694 2182 }
yabahiro 33:dd598bb23ad1 2183 break;
yabahiro 33:dd598bb23ad1 2184 case 0:
kishibekairohan 44:2aff0da5ebdb 2185 motor[TIRE_FR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2186 motor[TIRE_FR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2187 motor[TIRE_FL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2188 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2189 motor[TIRE_BR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2190 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2191 motor[TIRE_BL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2192 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2193 mtc2=true;
yabahiro 33:dd598bb23ad1 2194 break;
yabahiro 33:dd598bb23ad1 2195 case 255:
kishibekairohan 44:2aff0da5ebdb 2196 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2197 motor[TIRE_FR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2198 motor[TIRE_FL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2199 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2200 motor[TIRE_BR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2201 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2202 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2203 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2204 mtc2=true;
yabahiro 33:dd598bb23ad1 2205 break;
yabahiro 33:dd598bb23ad1 2206 case 253:
yabahiro 33:dd598bb23ad1 2207 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2208 motor[TIRE_FR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2209 motor[TIRE_FL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2210 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2211 motor[TIRE_BR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2212 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 2213 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2214 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2215 mtc2=true;
yabahiro 33:dd598bb23ad1 2216 break;
yabahiro 33:dd598bb23ad1 2217 case 254:
yabahiro 33:dd598bb23ad1 2218 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2219 motor[TIRE_FR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2220 motor[TIRE_FL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2221 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2222 motor[TIRE_BR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2223 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 2224 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2225 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2226 mtc2=true;
yabahiro 33:dd598bb23ad1 2227 break;
yabahiro 33:dd598bb23ad1 2228 case 1:
kishibekairohan 44:2aff0da5ebdb 2229 motor[TIRE_FR].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2230 motor[TIRE_FR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2231 motor[TIRE_FL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2232 motor[TIRE_FL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2233 motor[TIRE_BR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2234 motor[TIRE_BR].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 2235 motor[TIRE_BL].pwm = fast+20;
yabahiro 33:dd598bb23ad1 2236 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2237 mtc2=true;
yabahiro 33:dd598bb23ad1 2238 break;
yabahiro 33:dd598bb23ad1 2239 case 3:
kishibekairohan 44:2aff0da5ebdb 2240 motor[TIRE_FR].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2241 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2242 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2243 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2244 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2245 motor[TIRE_BR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2246 motor[TIRE_BL].pwm = slow+20;
yabahiro 33:dd598bb23ad1 2247 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2248 mtc2=true;
yabahiro 33:dd598bb23ad1 2249 break;
yabahiro 33:dd598bb23ad1 2250 case 2:
kishibekairohan 44:2aff0da5ebdb 2251 motor[TIRE_FR].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2252 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2253 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2254 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2255 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2256 motor[TIRE_BR].dir = FREE;
kishibekairohan 44:2aff0da5ebdb 2257 motor[TIRE_BL].pwm = normal+20;
yabahiro 33:dd598bb23ad1 2258 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2259 mtc2=true;
yabahiro 33:dd598bb23ad1 2260 break;
yabahiro 33:dd598bb23ad1 2261 }
yabahiro 33:dd598bb23ad1 2262 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 2263 switch(g[3]) {
yabahiro 33:dd598bb23ad1 2264 case 0:
yabahiro 33:dd598bb23ad1 2265 mtc2=false;
yabahiro 33:dd598bb23ad1 2266 break;
yabahiro 33:dd598bb23ad1 2267 case 255:
yabahiro 33:dd598bb23ad1 2268 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 2269 mtc2=false;
yabahiro 33:dd598bb23ad1 2270 break;
yabahiro 33:dd598bb23ad1 2271 case 253:
yabahiro 33:dd598bb23ad1 2272 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 2273 mtc2=false;
yabahiro 33:dd598bb23ad1 2274 break;
yabahiro 33:dd598bb23ad1 2275 case 254:
kishibekairohan 44:2aff0da5ebdb 2276 motor[TIRE_BL].pwm += 15;
yabahiro 33:dd598bb23ad1 2277 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2278 mtc2=false;
yabahiro 33:dd598bb23ad1 2279 break;
yabahiro 33:dd598bb23ad1 2280 case 1:
yabahiro 33:dd598bb23ad1 2281 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 2282 mtc2=false;
yabahiro 33:dd598bb23ad1 2283 break;
yabahiro 33:dd598bb23ad1 2284 case 3:
yabahiro 33:dd598bb23ad1 2285 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 2286 mtc2=false;
yabahiro 33:dd598bb23ad1 2287 break;
yabahiro 33:dd598bb23ad1 2288 case 2:
kishibekairohan 44:2aff0da5ebdb 2289 motor[TIRE_FL].pwm += 15;
yabahiro 33:dd598bb23ad1 2290 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2291 mtc2=false;
yabahiro 33:dd598bb23ad1 2292 break;
yabahiro 33:dd598bb23ad1 2293 }
yabahiro 33:dd598bb23ad1 2294 }
yabahiro 33:dd598bb23ad1 2295 }
yabahiro 38:40e7fdf3c694 2296 if(YELLOW) {
yabahiro 38:40e7fdf3c694 2297 if(mode==21) {
yabahiro 33:dd598bb23ad1 2298 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 2299 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 2300 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 2301 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2302 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 2303 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2304 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 2305 motor[TIRE_BL].dir = FOR;
yabahiro 38:40e7fdf3c694 2306 if(g[3]==98&&countss>=10000) {
yabahiro 38:40e7fdf3c694 2307 mode=22;
yabahiro 33:dd598bb23ad1 2308 countss=0;
yabahiro 38:40e7fdf3c694 2309 fast=60;
yabahiro 38:40e7fdf3c694 2310 normal=40;
yabahiro 38:40e7fdf3c694 2311 slow=20;
yabahiro 38:40e7fdf3c694 2312 }
yabahiro 46:68296caae1af 2313 } else if(mode==22) {
yabahiro 38:40e7fdf3c694 2314 if(g[4]==3||g[4]==2) {
yabahiro 38:40e7fdf3c694 2315 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2316 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2317 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2318 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2319 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2320 motor[TIRE_BR].dir = BACK;
yabahiro 38:40e7fdf3c694 2321 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2322 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2323 }
yabahiro 38:40e7fdf3c694 2324 if(g[4]==253||g[4]==254) {
yabahiro 38:40e7fdf3c694 2325 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2326 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2327 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2328 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2329 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2330 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2331 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2332 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 2333 }
yabahiro 38:40e7fdf3c694 2334 switch(g[2]) {
yabahiro 38:40e7fdf3c694 2335 case 0:
yabahiro 38:40e7fdf3c694 2336 motor[TIRE_FR].pwm = 20;
yabahiro 38:40e7fdf3c694 2337 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2338 motor[TIRE_BR].pwm = 20;
yabahiro 38:40e7fdf3c694 2339 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2340 break;
yabahiro 38:40e7fdf3c694 2341 case 255:
yabahiro 38:40e7fdf3c694 2342 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2343 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2344 motor[TIRE_BR].pwm = 20;
yabahiro 38:40e7fdf3c694 2345 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2346 break;
yabahiro 38:40e7fdf3c694 2347 case 253:
yabahiro 38:40e7fdf3c694 2348 motor[TIRE_FR].pwm = 40;
yabahiro 38:40e7fdf3c694 2349 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2350 motor[TIRE_BR].pwm = 20;
yabahiro 38:40e7fdf3c694 2351 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2352 break;
yabahiro 38:40e7fdf3c694 2353 case 254:
yabahiro 38:40e7fdf3c694 2354 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2355 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2356 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2357 motor[TIRE_BR].dir = BACK;
yabahiro 38:40e7fdf3c694 2358 break;
yabahiro 38:40e7fdf3c694 2359 case 1:
yabahiro 38:40e7fdf3c694 2360 motor[TIRE_FR].pwm = 20;
yabahiro 38:40e7fdf3c694 2361 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2362 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2363 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2364 break;
yabahiro 38:40e7fdf3c694 2365 case 3:
yabahiro 38:40e7fdf3c694 2366 motor[TIRE_FR].pwm = 20;
yabahiro 38:40e7fdf3c694 2367 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2368 motor[TIRE_BR].pwm = 40;
yabahiro 38:40e7fdf3c694 2369 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2370 break;
yabahiro 38:40e7fdf3c694 2371 case 2:
yabahiro 38:40e7fdf3c694 2372 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2373 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2374 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2375 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2376 break;
yabahiro 38:40e7fdf3c694 2377 }
yabahiro 38:40e7fdf3c694 2378 switch(g[3]) {
yabahiro 38:40e7fdf3c694 2379 case 0:
yabahiro 38:40e7fdf3c694 2380 motor[TIRE_FL].pwm = 20;
yabahiro 38:40e7fdf3c694 2381 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2382 motor[TIRE_BL].pwm = 20;
yabahiro 38:40e7fdf3c694 2383 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2384 break;
yabahiro 38:40e7fdf3c694 2385 case 255:
yabahiro 38:40e7fdf3c694 2386 motor[TIRE_FL].pwm = 20;
yabahiro 38:40e7fdf3c694 2387 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2388 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2389 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2390 case 253:
yabahiro 38:40e7fdf3c694 2391 motor[TIRE_FL].pwm = 20;
yabahiro 38:40e7fdf3c694 2392 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2393 motor[TIRE_BL].pwm = 40;
yabahiro 38:40e7fdf3c694 2394 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2395 break;
yabahiro 38:40e7fdf3c694 2396 case 254:
yabahiro 38:40e7fdf3c694 2397 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2398 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2399 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2400 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2401 break;
yabahiro 38:40e7fdf3c694 2402 case 1:
yabahiro 38:40e7fdf3c694 2403 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2404 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2405 motor[TIRE_BL].pwm = 20;
yabahiro 38:40e7fdf3c694 2406 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2407 break;
yabahiro 38:40e7fdf3c694 2408 case 3:
yabahiro 38:40e7fdf3c694 2409 motor[TIRE_FL].pwm = 40;
yabahiro 38:40e7fdf3c694 2410 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2411 motor[TIRE_BL].pwm = 20;
yabahiro 38:40e7fdf3c694 2412 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2413 break;
yabahiro 38:40e7fdf3c694 2414 case 2:
yabahiro 38:40e7fdf3c694 2415 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2416 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2417 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2418 motor[TIRE_BL].dir = FOR;
yabahiro 38:40e7fdf3c694 2419 break;
yabahiro 38:40e7fdf3c694 2420 }
yabahiro 38:40e7fdf3c694 2421 if(g[3]==0&&g[2]==0) {
yabahiro 38:40e7fdf3c694 2422 mode=23;
yabahiro 33:dd598bb23ad1 2423 countss=0;
yabahiro 33:dd598bb23ad1 2424 }
yabahiro 46:68296caae1af 2425 } else if(mode==23) {
yabahiro 38:40e7fdf3c694 2426 switch(g[4]) {
yabahiro 38:40e7fdf3c694 2427 case 98:
kishibekairohan 41:99a1158f3eca 2428 motor[TIRE_FR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2429 motor[TIRE_FR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2430 motor[TIRE_FL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2431 motor[TIRE_FL].dir = BACK;
kishibekairohan 41:99a1158f3eca 2432 motor[TIRE_BR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2433 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2434 motor[TIRE_BL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2435 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2436 break;
yabahiro 38:40e7fdf3c694 2437 case 99:
yabahiro 38:40e7fdf3c694 2438 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2439 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2440 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2441 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2442 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2443 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2444 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2445 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2446 break;
yabahiro 38:40e7fdf3c694 2447 case 0:
kishibekairohan 41:99a1158f3eca 2448 motor[TIRE_FR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2449 motor[TIRE_FR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2450 motor[TIRE_FL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2451 motor[TIRE_FL].dir = BACK;
kishibekairohan 41:99a1158f3eca 2452 motor[TIRE_BR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2453 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2454 motor[TIRE_BL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2455 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2456 break;
yabahiro 38:40e7fdf3c694 2457 case 255:
yabahiro 38:40e7fdf3c694 2458 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2459 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2460 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2461 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2462 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2463 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2464 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2465 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2466 break;
yabahiro 38:40e7fdf3c694 2467 case 253:
yabahiro 46:68296caae1af 2468 motor[TIRE_FR].pwm = slow+5;
yabahiro 38:40e7fdf3c694 2469 motor[TIRE_FR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2470 motor[TIRE_FL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2471 motor[TIRE_FL].dir = BACK;
kishibekairohan 41:99a1158f3eca 2472 motor[TIRE_BR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2473 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 2474 motor[TIRE_BL].pwm = slow+5;
yabahiro 38:40e7fdf3c694 2475 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2476 break;
yabahiro 38:40e7fdf3c694 2477 case 254:
yabahiro 46:68296caae1af 2478 motor[TIRE_FR].pwm = normal+5;
yabahiro 38:40e7fdf3c694 2479 motor[TIRE_FR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2480 motor[TIRE_FL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2481 motor[TIRE_FL].dir = BACK;
kishibekairohan 41:99a1158f3eca 2482 motor[TIRE_BR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2483 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 2484 motor[TIRE_BL].pwm = normal+5;
yabahiro 38:40e7fdf3c694 2485 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2486 break;
yabahiro 38:40e7fdf3c694 2487 case 1:
yabahiro 38:40e7fdf3c694 2488 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2489 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2490 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2491 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2492 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2493 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2494 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2495 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2496 break;
yabahiro 38:40e7fdf3c694 2497 case 3:
kishibekairohan 41:99a1158f3eca 2498 motor[TIRE_FR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2499 motor[TIRE_FR].dir = FOR;
yabahiro 46:68296caae1af 2500 motor[TIRE_FL].pwm = slow+5;
yabahiro 38:40e7fdf3c694 2501 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 2502 motor[TIRE_BR].pwm = slow+5;
yabahiro 38:40e7fdf3c694 2503 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2504 motor[TIRE_BL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2505 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2506 break;
yabahiro 38:40e7fdf3c694 2507 case 2:
kishibekairohan 41:99a1158f3eca 2508 motor[TIRE_FR].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2509 motor[TIRE_FR].dir = FOR;
yabahiro 46:68296caae1af 2510 motor[TIRE_FL].pwm = normal+5;
yabahiro 38:40e7fdf3c694 2511 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 2512 motor[TIRE_BR].pwm = normal+5;
yabahiro 38:40e7fdf3c694 2513 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2514 motor[TIRE_BL].pwm = fast-5;
yabahiro 38:40e7fdf3c694 2515 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2516 break;
yabahiro 33:dd598bb23ad1 2517 }
yabahiro 38:40e7fdf3c694 2518 if(g[1]==98) {
kishibekairohan 42:980af34f6fe3 2519 fast=20;
kishibekairohan 42:980af34f6fe3 2520 normal=15;
kishibekairohan 42:980af34f6fe3 2521 slow=10;
yabahiro 38:40e7fdf3c694 2522 }
yabahiro 38:40e7fdf3c694 2523 if(g[3]==254&&countss>=100000) {
yabahiro 38:40e7fdf3c694 2524 mode=24;
yabahiro 38:40e7fdf3c694 2525 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 2526 countss=0;
yabahiro 38:40e7fdf3c694 2527 fast=60;
yabahiro 38:40e7fdf3c694 2528 normal=40;
kishibekairohan 42:980af34f6fe3 2529 slow=20;
yabahiro 33:dd598bb23ad1 2530 }
yabahiro 46:68296caae1af 2531 } else if(mode==24) {
yabahiro 38:40e7fdf3c694 2532 if(g[4]==3||g[4]==2) {
yabahiro 38:40e7fdf3c694 2533 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2534 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2535 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2536 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2537 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2538 motor[TIRE_BR].dir = BACK;
yabahiro 38:40e7fdf3c694 2539 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2540 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2541 }
yabahiro 38:40e7fdf3c694 2542 if(g[4]==253||g[4]==254) {
yabahiro 38:40e7fdf3c694 2543 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2544 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2545 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2546 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2547 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2548 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2549 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2550 motor[TIRE_BL].dir = FOR;
yabahiro 38:40e7fdf3c694 2551 }
yabahiro 38:40e7fdf3c694 2552 switch(g[2]) {
yabahiro 38:40e7fdf3c694 2553 case 0:
yabahiro 46:68296caae1af 2554 motor[TIRE_FR].pwm = 25;
yabahiro 38:40e7fdf3c694 2555 motor[TIRE_FR].dir = FREE;
yabahiro 46:68296caae1af 2556 motor[TIRE_BR].pwm = 25;
yabahiro 38:40e7fdf3c694 2557 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2558 break;
yabahiro 38:40e7fdf3c694 2559 case 255:
yabahiro 38:40e7fdf3c694 2560 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2561 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 2562 motor[TIRE_BR].pwm = 25;
yabahiro 38:40e7fdf3c694 2563 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2564 break;
yabahiro 38:40e7fdf3c694 2565 case 253:
yabahiro 38:40e7fdf3c694 2566 motor[TIRE_FR].pwm = 40;
yabahiro 38:40e7fdf3c694 2567 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 2568 motor[TIRE_BR].pwm = 25;
yabahiro 38:40e7fdf3c694 2569 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2570 break;
yabahiro 38:40e7fdf3c694 2571 case 254:
yabahiro 38:40e7fdf3c694 2572 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2573 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2574 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2575 motor[TIRE_BR].dir = BACK;
yabahiro 38:40e7fdf3c694 2576 break;
yabahiro 38:40e7fdf3c694 2577 case 1:
yabahiro 46:68296caae1af 2578 motor[TIRE_FR].pwm = 25;
yabahiro 38:40e7fdf3c694 2579 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2580 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2581 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2582 break;
yabahiro 38:40e7fdf3c694 2583 case 3:
yabahiro 46:68296caae1af 2584 motor[TIRE_FR].pwm = 25;
yabahiro 38:40e7fdf3c694 2585 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2586 motor[TIRE_BR].pwm = 40;
yabahiro 38:40e7fdf3c694 2587 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2588 break;
yabahiro 38:40e7fdf3c694 2589 case 2:
yabahiro 38:40e7fdf3c694 2590 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2591 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2592 motor[TIRE_BR].pwm = 30;
yabahiro 38:40e7fdf3c694 2593 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2594 break;
yabahiro 38:40e7fdf3c694 2595 }
yabahiro 38:40e7fdf3c694 2596 switch(g[3]) {
yabahiro 38:40e7fdf3c694 2597 case 0:
yabahiro 46:68296caae1af 2598 motor[TIRE_FL].pwm = 25;
yabahiro 38:40e7fdf3c694 2599 motor[TIRE_FL].dir = FREE;
yabahiro 46:68296caae1af 2600 motor[TIRE_BL].pwm = 25;
yabahiro 38:40e7fdf3c694 2601 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2602 break;
yabahiro 38:40e7fdf3c694 2603 case 255:
yabahiro 46:68296caae1af 2604 motor[TIRE_FL].pwm = 25;
yabahiro 38:40e7fdf3c694 2605 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2606 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2607 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2608 case 253:
yabahiro 46:68296caae1af 2609 motor[TIRE_FL].pwm = 25;
yabahiro 38:40e7fdf3c694 2610 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2611 motor[TIRE_BL].pwm = 40;
yabahiro 38:40e7fdf3c694 2612 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2613 break;
yabahiro 38:40e7fdf3c694 2614 case 254:
yabahiro 38:40e7fdf3c694 2615 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2616 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2617 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2618 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2619 break;
yabahiro 38:40e7fdf3c694 2620 case 1:
yabahiro 38:40e7fdf3c694 2621 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2622 motor[TIRE_FL].dir = FOR;
yabahiro 46:68296caae1af 2623 motor[TIRE_BL].pwm = 25;
yabahiro 38:40e7fdf3c694 2624 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2625 break;
yabahiro 38:40e7fdf3c694 2626 case 3:
yabahiro 38:40e7fdf3c694 2627 motor[TIRE_FL].pwm = 40;
yabahiro 38:40e7fdf3c694 2628 motor[TIRE_FL].dir = FOR;
yabahiro 46:68296caae1af 2629 motor[TIRE_BL].pwm = 25;
yabahiro 38:40e7fdf3c694 2630 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2631 break;
yabahiro 38:40e7fdf3c694 2632 case 2:
yabahiro 38:40e7fdf3c694 2633 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2634 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2635 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2636 motor[TIRE_BL].dir = FOR;
yabahiro 38:40e7fdf3c694 2637 break;
yabahiro 38:40e7fdf3c694 2638 }
yabahiro 38:40e7fdf3c694 2639 if(g[3]==0&&g[2]==0) {
yabahiro 46:68296caae1af 2640 mode=25;
yabahiro 38:40e7fdf3c694 2641 }
yabahiro 46:68296caae1af 2642 } else if(mode==25) {
kishibekairohan 47:a9cad8502999 2643 motor[TIRE_FR].pwm = 100;
kishibekairohan 47:a9cad8502999 2644 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 2645 motor[TIRE_FL].pwm = 100;
kishibekairohan 47:a9cad8502999 2646 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 47:a9cad8502999 2647 motor[TIRE_BR].pwm = 100;
kishibekairohan 47:a9cad8502999 2648 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 2649 motor[TIRE_BL].pwm = 100;
kishibekairohan 47:a9cad8502999 2650 motor[TIRE_BL].dir = BRAKE;
yabahiro 46:68296caae1af 2651 if(UPLS==true||DOLS==true) {
yabahiro 46:68296caae1af 2652 motor[MOTOR_0].pwm = 200;
yabahiro 46:68296caae1af 2653 motor[MOTOR_0].dir = BRAKE;
yabahiro 46:68296caae1af 2654 mode=26;
yabahiro 46:68296caae1af 2655 } else {
yabahiro 46:68296caae1af 2656 motor[MOTOR_0].pwm = 100;
yabahiro 46:68296caae1af 2657 motor[MOTOR_0].dir = BACK;
yabahiro 46:68296caae1af 2658 }
yabahiro 46:68296caae1af 2659 } else if(mode==26) {
yabahiro 38:40e7fdf3c694 2660 switch(g[4]) {
yabahiro 38:40e7fdf3c694 2661 case 98:
yabahiro 38:40e7fdf3c694 2662 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 2663 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2664 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 2665 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2666 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 2667 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2668 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 2669 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2670 break;
yabahiro 38:40e7fdf3c694 2671 case 99:
yabahiro 38:40e7fdf3c694 2672 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2673 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2674 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2675 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2676 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2677 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2678 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2679 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2680 break;
yabahiro 38:40e7fdf3c694 2681 case 0:
yabahiro 38:40e7fdf3c694 2682 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 2683 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2684 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 2685 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2686 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 2687 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2688 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 2689 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2690 break;
yabahiro 38:40e7fdf3c694 2691 case 255:
yabahiro 38:40e7fdf3c694 2692 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2693 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2694 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2695 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2696 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2697 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2698 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2699 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2700 break;
yabahiro 38:40e7fdf3c694 2701 case 253:
yabahiro 46:68296caae1af 2702 motor[TIRE_FR].pwm = 25;
yabahiro 38:40e7fdf3c694 2703 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2704 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 2705 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2706 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 2707 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 2708 motor[TIRE_BL].pwm = 25;
yabahiro 38:40e7fdf3c694 2709 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2710 break;
yabahiro 38:40e7fdf3c694 2711 case 254:
yabahiro 38:40e7fdf3c694 2712 motor[TIRE_FR].pwm = 40;
yabahiro 38:40e7fdf3c694 2713 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2714 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 2715 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2716 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 2717 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2718 motor[TIRE_BL].pwm = 40;
yabahiro 38:40e7fdf3c694 2719 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2720 break;
yabahiro 38:40e7fdf3c694 2721 case 1:
yabahiro 38:40e7fdf3c694 2722 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2723 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2724 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2725 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2726 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2727 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2728 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2729 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2730 break;
yabahiro 38:40e7fdf3c694 2731 case 3:
yabahiro 38:40e7fdf3c694 2732 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 2733 motor[TIRE_FR].dir = FOR;
yabahiro 46:68296caae1af 2734 motor[TIRE_FL].pwm = 25;
yabahiro 38:40e7fdf3c694 2735 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 2736 motor[TIRE_BR].pwm = 25;
yabahiro 38:40e7fdf3c694 2737 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2738 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 2739 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2740 break;
yabahiro 38:40e7fdf3c694 2741 case 2:
yabahiro 38:40e7fdf3c694 2742 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 2743 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2744 motor[TIRE_FL].pwm = 40;
yabahiro 38:40e7fdf3c694 2745 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2746 motor[TIRE_BR].pwm = 40;
yabahiro 38:40e7fdf3c694 2747 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 2748 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 2749 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2750 break;
yabahiro 38:40e7fdf3c694 2751 }
yabahiro 38:40e7fdf3c694 2752 if(g[1]==98) {
kishibekairohan 45:0cf48a0605cd 2753 mode=31;
yabahiro 33:dd598bb23ad1 2754 }
yabahiro 33:dd598bb23ad1 2755 }
yabahiro 46:68296caae1af 2756 } else {
yabahiro 38:40e7fdf3c694 2757 if(mode==21) {
yabahiro 38:40e7fdf3c694 2758 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2759 motor[TIRE_FR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 2760 motor[TIRE_FL].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 2761 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 44:2aff0da5ebdb 2762 motor[TIRE_BR].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 2763 motor[TIRE_BR].dir = BRAKE;
yabahiro 38:40e7fdf3c694 2764 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2765 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2766 if(g[1]==0) {
yabahiro 38:40e7fdf3c694 2767 mode=22;
yabahiro 38:40e7fdf3c694 2768 fast=60;
yabahiro 38:40e7fdf3c694 2769 normal=40;
yabahiro 46:68296caae1af 2770 slow=25;
yabahiro 38:40e7fdf3c694 2771 }
yabahiro 46:68296caae1af 2772 } else if(mode==22) {
yabahiro 40:ed2409e0ce6b 2773 if(g[1]==98) {
yabahiro 46:68296caae1af 2774 slow=25;
kishibekairohan 44:2aff0da5ebdb 2775 normal=30;
kishibekairohan 44:2aff0da5ebdb 2776 fast=40;
yabahiro 40:ed2409e0ce6b 2777 }
yabahiro 46:68296caae1af 2778 if(g[3]==0) {
yabahiro 46:68296caae1af 2779 slow=25;
yabahiro 40:ed2409e0ce6b 2780 normal=40;
yabahiro 40:ed2409e0ce6b 2781 fast=60;
yabahiro 38:40e7fdf3c694 2782 mode=23;
yabahiro 46:68296caae1af 2783 }
yabahiro 38:40e7fdf3c694 2784 switch(g[1]) {
yabahiro 38:40e7fdf3c694 2785 case 98:
yabahiro 38:40e7fdf3c694 2786 solenoid.solenoid4 = SOLENOID_OFF;
kishibekairohan 44:2aff0da5ebdb 2787 fast=40;
kishibekairohan 44:2aff0da5ebdb 2788 normal=30;
yabahiro 46:68296caae1af 2789 slow=25;
yabahiro 38:40e7fdf3c694 2790 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 2791 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2792 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 2793 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2794 motor[TIRE_BR].pwm = normal;
yabahiro 38:40e7fdf3c694 2795 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2796 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 2797 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2798 break;
yabahiro 38:40e7fdf3c694 2799 case 0:
yabahiro 38:40e7fdf3c694 2800 motor[TIRE_FR].pwm = fast;
yabahiro 38:40e7fdf3c694 2801 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2802 motor[TIRE_FL].pwm = fast;
yabahiro 38:40e7fdf3c694 2803 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2804 motor[TIRE_BR].pwm = fast;
yabahiro 38:40e7fdf3c694 2805 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2806 motor[TIRE_BL].pwm = fast;
yabahiro 38:40e7fdf3c694 2807 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2808 mtc=true;
yabahiro 38:40e7fdf3c694 2809 break;
yabahiro 38:40e7fdf3c694 2810 case 255:
yabahiro 38:40e7fdf3c694 2811 motor[TIRE_FR].pwm = fast;
yabahiro 38:40e7fdf3c694 2812 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2813 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 2814 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2815 motor[TIRE_BR].pwm = slow;
yabahiro 38:40e7fdf3c694 2816 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2817 motor[TIRE_BL].pwm = fast;
yabahiro 38:40e7fdf3c694 2818 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2819 mtc=true;
yabahiro 38:40e7fdf3c694 2820 break;
yabahiro 38:40e7fdf3c694 2821 case 253:
yabahiro 38:40e7fdf3c694 2822 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 2823 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2824 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2825 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2826 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2827 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2828 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 2829 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2830 mtc=true;
yabahiro 38:40e7fdf3c694 2831 break;
yabahiro 38:40e7fdf3c694 2832 case 254:
yabahiro 38:40e7fdf3c694 2833 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 2834 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2835 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2836 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2837 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2838 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2839 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 2840 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2841 mtc=true;
yabahiro 38:40e7fdf3c694 2842 break;
yabahiro 38:40e7fdf3c694 2843 case 1:
yabahiro 38:40e7fdf3c694 2844 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 2845 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2846 motor[TIRE_FL].pwm = fast;
yabahiro 38:40e7fdf3c694 2847 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2848 motor[TIRE_BR].pwm = fast;
yabahiro 38:40e7fdf3c694 2849 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2850 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 2851 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2852 mtc=true;
yabahiro 38:40e7fdf3c694 2853 break;
yabahiro 38:40e7fdf3c694 2854 case 3:
yabahiro 38:40e7fdf3c694 2855 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2856 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2857 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 2858 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2859 motor[TIRE_BR].pwm = slow;
yabahiro 38:40e7fdf3c694 2860 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2861 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2862 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2863 mtc=true;
yabahiro 38:40e7fdf3c694 2864 break;
yabahiro 38:40e7fdf3c694 2865 case 2:
yabahiro 38:40e7fdf3c694 2866 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2867 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 2868 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 2869 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2870 motor[TIRE_BR].pwm = normal;
yabahiro 38:40e7fdf3c694 2871 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2872 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 2873 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 2874 mtc=true;
yabahiro 38:40e7fdf3c694 2875 break;
yabahiro 38:40e7fdf3c694 2876 }
yabahiro 38:40e7fdf3c694 2877 if(g[1]!=98&&mtc==true) {
yabahiro 38:40e7fdf3c694 2878 switch(g[0]) {
yabahiro 38:40e7fdf3c694 2879 case 0:
yabahiro 38:40e7fdf3c694 2880 mtc=false;
yabahiro 38:40e7fdf3c694 2881 break;
yabahiro 38:40e7fdf3c694 2882 case 255:
kishibekairohan 44:2aff0da5ebdb 2883 motor[TIRE_FL].pwm += 3;
yabahiro 38:40e7fdf3c694 2884 mtc=false;
yabahiro 38:40e7fdf3c694 2885 break;
yabahiro 38:40e7fdf3c694 2886 case 253:
kishibekairohan 44:2aff0da5ebdb 2887 motor[TIRE_FL].pwm += 6;
yabahiro 38:40e7fdf3c694 2888 mtc=false;
yabahiro 38:40e7fdf3c694 2889 break;
yabahiro 38:40e7fdf3c694 2890 case 254:
kishibekairohan 44:2aff0da5ebdb 2891 motor[TIRE_FL].pwm += 9;
yabahiro 38:40e7fdf3c694 2892 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 2893 mtc=false;
yabahiro 38:40e7fdf3c694 2894 break;
yabahiro 38:40e7fdf3c694 2895 case 1:
kishibekairohan 44:2aff0da5ebdb 2896 motor[TIRE_FR].pwm += 3;
yabahiro 38:40e7fdf3c694 2897 mtc=false;
yabahiro 38:40e7fdf3c694 2898 break;
yabahiro 38:40e7fdf3c694 2899 case 3:
kishibekairohan 44:2aff0da5ebdb 2900 motor[TIRE_FR].pwm += 6;
yabahiro 38:40e7fdf3c694 2901 mtc=false;
yabahiro 38:40e7fdf3c694 2902 break;
yabahiro 38:40e7fdf3c694 2903 case 2:
kishibekairohan 44:2aff0da5ebdb 2904 motor[TIRE_FR].pwm += 9;
yabahiro 38:40e7fdf3c694 2905 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2906 mtc=false;
yabahiro 38:40e7fdf3c694 2907 break;
yabahiro 38:40e7fdf3c694 2908 }
yabahiro 38:40e7fdf3c694 2909 }
yabahiro 46:68296caae1af 2910 } else if(mode==23) {
yabahiro 46:68296caae1af 2911 if(UPLS||DOLS) {
yabahiro 46:68296caae1af 2912 if(UPLS||DOLS) {
yabahiro 46:68296caae1af 2913 motor[MOTOR_0].pwm = 200;
yabahiro 46:68296caae1af 2914 motor[MOTOR_0].dir = BRAKE;
yabahiro 46:68296caae1af 2915 fast=60;
yabahiro 46:68296caae1af 2916 normal=40;
yabahiro 46:68296caae1af 2917 slow=25;
yabahiro 46:68296caae1af 2918 countss=0;
yabahiro 46:68296caae1af 2919 mode=24;
yabahiro 46:68296caae1af 2920 }
yabahiro 46:68296caae1af 2921 } else {
yabahiro 38:40e7fdf3c694 2922 motor[MOTOR_0].pwm = 100;
yabahiro 38:40e7fdf3c694 2923 motor[MOTOR_0].dir = BACK;
yabahiro 38:40e7fdf3c694 2924 }
yabahiro 40:ed2409e0ce6b 2925 motor[TIRE_FR].pwm = 100;
yabahiro 40:ed2409e0ce6b 2926 motor[TIRE_FR].dir = BRAKE;
yabahiro 40:ed2409e0ce6b 2927 motor[TIRE_FL].pwm = 100;
yabahiro 40:ed2409e0ce6b 2928 motor[TIRE_FL].dir = BRAKE;
yabahiro 40:ed2409e0ce6b 2929 motor[TIRE_BR].pwm = 100;
yabahiro 40:ed2409e0ce6b 2930 motor[TIRE_BR].dir = BRAKE;
yabahiro 40:ed2409e0ce6b 2931 motor[TIRE_BL].pwm = 100;
yabahiro 40:ed2409e0ce6b 2932 motor[TIRE_BL].dir = BRAKE;
yabahiro 46:68296caae1af 2933 } else if(mode==24) {
yabahiro 38:40e7fdf3c694 2934 if(g[1]==98&&countss>=40000) {
kishibekairohan 45:0cf48a0605cd 2935 mode=31;
yabahiro 38:40e7fdf3c694 2936 countss=0;
yabahiro 38:40e7fdf3c694 2937 }
yabahiro 38:40e7fdf3c694 2938 switch(g[1]) {
yabahiro 38:40e7fdf3c694 2939 case 98:
kishibekairohan 44:2aff0da5ebdb 2940 fast=40;
kishibekairohan 44:2aff0da5ebdb 2941 normal=30;
yabahiro 46:68296caae1af 2942 slow=25;
yabahiro 38:40e7fdf3c694 2943 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 2944 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2945 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 2946 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2947 motor[TIRE_BR].pwm = normal;
yabahiro 38:40e7fdf3c694 2948 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2949 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 2950 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2951 break;
yabahiro 38:40e7fdf3c694 2952 case 0:
yabahiro 38:40e7fdf3c694 2953 motor[TIRE_FR].pwm = fast;
yabahiro 38:40e7fdf3c694 2954 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2955 motor[TIRE_FL].pwm = fast;
yabahiro 38:40e7fdf3c694 2956 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2957 motor[TIRE_BR].pwm = fast;
yabahiro 38:40e7fdf3c694 2958 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2959 motor[TIRE_BL].pwm = fast;
yabahiro 38:40e7fdf3c694 2960 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2961 mtc=true;
yabahiro 38:40e7fdf3c694 2962 break;
yabahiro 38:40e7fdf3c694 2963 case 255:
yabahiro 38:40e7fdf3c694 2964 motor[TIRE_FR].pwm = fast;
yabahiro 38:40e7fdf3c694 2965 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2966 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 2967 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 2968 motor[TIRE_BR].pwm = slow;
yabahiro 38:40e7fdf3c694 2969 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2970 motor[TIRE_BL].pwm = fast;
yabahiro 38:40e7fdf3c694 2971 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2972 mtc=true;
yabahiro 38:40e7fdf3c694 2973 break;
yabahiro 38:40e7fdf3c694 2974 case 253:
yabahiro 38:40e7fdf3c694 2975 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 2976 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2977 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2978 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2979 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2980 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2981 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 2982 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2983 mtc=true;
yabahiro 38:40e7fdf3c694 2984 break;
yabahiro 38:40e7fdf3c694 2985 case 254:
yabahiro 38:40e7fdf3c694 2986 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 2987 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2988 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2989 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2990 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2991 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 2992 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 2993 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 2994 mtc=true;
yabahiro 38:40e7fdf3c694 2995 break;
yabahiro 38:40e7fdf3c694 2996 case 1:
yabahiro 38:40e7fdf3c694 2997 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 2998 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2999 motor[TIRE_FL].pwm = fast;
yabahiro 38:40e7fdf3c694 3000 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3001 motor[TIRE_BR].pwm = fast;
yabahiro 38:40e7fdf3c694 3002 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3003 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 3004 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3005 mtc=true;
yabahiro 38:40e7fdf3c694 3006 break;
yabahiro 38:40e7fdf3c694 3007 case 3:
yabahiro 38:40e7fdf3c694 3008 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3009 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3010 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 3011 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3012 motor[TIRE_BR].pwm = slow;
yabahiro 38:40e7fdf3c694 3013 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3014 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3015 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3016 mtc=true;
yabahiro 38:40e7fdf3c694 3017 break;
yabahiro 38:40e7fdf3c694 3018 case 2:
yabahiro 38:40e7fdf3c694 3019 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3020 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3021 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 3022 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3023 motor[TIRE_BR].pwm = normal;
yabahiro 38:40e7fdf3c694 3024 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3025 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3026 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3027 mtc=true;
yabahiro 38:40e7fdf3c694 3028 break;
yabahiro 38:40e7fdf3c694 3029 }
yabahiro 38:40e7fdf3c694 3030 if(g[1]!=98&&mtc==true) {
yabahiro 38:40e7fdf3c694 3031 switch(g[0]) {
yabahiro 38:40e7fdf3c694 3032 case 0:
yabahiro 38:40e7fdf3c694 3033 mtc=false;
yabahiro 38:40e7fdf3c694 3034 break;
yabahiro 38:40e7fdf3c694 3035 case 255:
kishibekairohan 44:2aff0da5ebdb 3036 motor[TIRE_FL].pwm += 3;
yabahiro 38:40e7fdf3c694 3037 mtc=false;
yabahiro 38:40e7fdf3c694 3038 break;
yabahiro 38:40e7fdf3c694 3039 case 253:
kishibekairohan 44:2aff0da5ebdb 3040 motor[TIRE_FL].pwm += 6;
yabahiro 38:40e7fdf3c694 3041 mtc=false;
yabahiro 38:40e7fdf3c694 3042 break;
yabahiro 38:40e7fdf3c694 3043 case 254:
kishibekairohan 44:2aff0da5ebdb 3044 motor[TIRE_FL].pwm += 9;
yabahiro 38:40e7fdf3c694 3045 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3046 mtc=false;
yabahiro 38:40e7fdf3c694 3047 break;
yabahiro 38:40e7fdf3c694 3048 case 1:
kishibekairohan 44:2aff0da5ebdb 3049 motor[TIRE_FR].pwm += 3;
yabahiro 38:40e7fdf3c694 3050 mtc=false;
yabahiro 38:40e7fdf3c694 3051 break;
yabahiro 38:40e7fdf3c694 3052 case 3:
kishibekairohan 44:2aff0da5ebdb 3053 motor[TIRE_FR].pwm += 6;
yabahiro 38:40e7fdf3c694 3054 mtc=false;
yabahiro 38:40e7fdf3c694 3055 break;
yabahiro 38:40e7fdf3c694 3056 case 2:
kishibekairohan 44:2aff0da5ebdb 3057 motor[TIRE_FR].pwm += 9;
yabahiro 38:40e7fdf3c694 3058 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3059 mtc=false;
yabahiro 38:40e7fdf3c694 3060 break;
yabahiro 38:40e7fdf3c694 3061 }
yabahiro 33:dd598bb23ad1 3062 }
yabahiro 33:dd598bb23ad1 3063 }
yabahiro 33:dd598bb23ad1 3064 }
yabahiro 46:68296caae1af 3065
kishibekairohan 44:2aff0da5ebdb 3066 /*
yabahiro 38:40e7fdf3c694 3067 if(mode==31) {
yabahiro 33:dd598bb23ad1 3068 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3069 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 3070 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3071 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3072 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3073 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 3074 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 3075 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 3076 if(countss>=5000) {
yabahiro 33:dd598bb23ad1 3077 countss=0;
yabahiro 33:dd598bb23ad1 3078 mode=32;
yabahiro 33:dd598bb23ad1 3079 }
yabahiro 33:dd598bb23ad1 3080 } else if(mode==32) {
yabahiro 33:dd598bb23ad1 3081 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 3082 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 3083 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 3084 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 3085 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 3086 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 3087 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 3088 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 3089 if(countss>=40000) {
yabahiro 33:dd598bb23ad1 3090 mode=33;
yabahiro 33:dd598bb23ad1 3091 countss=0;
yabahiro 33:dd598bb23ad1 3092 }
yabahiro 46:68296caae1af 3093 }
kishibekairohan 44:2aff0da5ebdb 3094 */
kishibekairohan 45:0cf48a0605cd 3095 if(mode==31) {
kishibekairohan 44:2aff0da5ebdb 3096 motor[TIRE_FR].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 3097 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 44:2aff0da5ebdb 3098 motor[TIRE_FL].pwm = 45;
yabahiro 38:40e7fdf3c694 3099 motor[TIRE_FL].dir = BACK;
kishibekairohan 44:2aff0da5ebdb 3100 motor[TIRE_BR].pwm = 45;
yabahiro 38:40e7fdf3c694 3101 motor[TIRE_BR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 3102 motor[TIRE_BL].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 3103 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 44:2aff0da5ebdb 3104 if(g[2]==0) {
yabahiro 38:40e7fdf3c694 3105 cross=0;
yabahiro 33:dd598bb23ad1 3106 countss=0;
yabahiro 33:dd598bb23ad1 3107 mode=40;
yabahiro 33:dd598bb23ad1 3108 }
yabahiro 46:68296caae1af 3109 } else if(mode==40) {
yabahiro 33:dd598bb23ad1 3110 switch(g[3]) {
yabahiro 33:dd598bb23ad1 3111 case 98:
yabahiro 46:68296caae1af 3112 motor[TIRE_FR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3113 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 3114 motor[TIRE_FL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3115 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3116 motor[TIRE_BR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3117 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 3118 motor[TIRE_BL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3119 motor[TIRE_BL].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 3120 if(countss>30000) {
yabahiro 33:dd598bb23ad1 3121 cross++;
yabahiro 33:dd598bb23ad1 3122 countss=0;
yabahiro 33:dd598bb23ad1 3123 }
yabahiro 33:dd598bb23ad1 3124 mtc2=true;
yabahiro 33:dd598bb23ad1 3125 break;
yabahiro 33:dd598bb23ad1 3126 case 0:
yabahiro 46:68296caae1af 3127 motor[TIRE_FR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3128 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 3129 motor[TIRE_FL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3130 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3131 motor[TIRE_BR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3132 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 3133 motor[TIRE_BL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3134 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 3135 mtc2=true;
yabahiro 33:dd598bb23ad1 3136 break;
yabahiro 33:dd598bb23ad1 3137 case 255:
yabahiro 46:68296caae1af 3138 motor[TIRE_FR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3139 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 3140 motor[TIRE_FL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3141 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3142 motor[TIRE_BR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3143 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 3144 motor[TIRE_BL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3145 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 3146 mtc2=true;
yabahiro 33:dd598bb23ad1 3147 break;
yabahiro 33:dd598bb23ad1 3148 case 253:
yabahiro 33:dd598bb23ad1 3149 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3150 motor[TIRE_FR].dir = FREE;
yabahiro 46:68296caae1af 3151 motor[TIRE_FL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3152 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3153 motor[TIRE_BR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3154 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3155 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 3156 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 3157 mtc2=true;
yabahiro 33:dd598bb23ad1 3158 break;
yabahiro 33:dd598bb23ad1 3159 case 254:
yabahiro 33:dd598bb23ad1 3160 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3161 motor[TIRE_FR].dir = FREE;
yabahiro 46:68296caae1af 3162 motor[TIRE_FL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3163 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3164 motor[TIRE_BR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3165 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3166 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 3167 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 3168 mtc2=true;
yabahiro 33:dd598bb23ad1 3169 break;
yabahiro 33:dd598bb23ad1 3170 case 1:
yabahiro 46:68296caae1af 3171 motor[TIRE_FR].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3172 motor[TIRE_FR].dir = BACK;
yabahiro 46:68296caae1af 3173 motor[TIRE_FL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3174 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3175 motor[TIRE_BR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3176 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 3177 motor[TIRE_BL].pwm = fast+15;
yabahiro 33:dd598bb23ad1 3178 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 3179 mtc2=true;
yabahiro 33:dd598bb23ad1 3180 break;
yabahiro 33:dd598bb23ad1 3181 case 3:
yabahiro 46:68296caae1af 3182 motor[TIRE_FR].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3183 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 3184 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3185 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3186 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3187 motor[TIRE_BR].dir = FREE;
yabahiro 46:68296caae1af 3188 motor[TIRE_BL].pwm = slow+15;
yabahiro 33:dd598bb23ad1 3189 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 3190 mtc2=true;
yabahiro 33:dd598bb23ad1 3191 break;
yabahiro 33:dd598bb23ad1 3192 case 2:
yabahiro 46:68296caae1af 3193 motor[TIRE_FR].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3194 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 3195 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3196 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3197 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3198 motor[TIRE_BR].dir = FREE;
yabahiro 46:68296caae1af 3199 motor[TIRE_BL].pwm = normal+15;
yabahiro 33:dd598bb23ad1 3200 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 3201 mtc2=true;
yabahiro 33:dd598bb23ad1 3202 break;
yabahiro 33:dd598bb23ad1 3203 }
yabahiro 33:dd598bb23ad1 3204 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 3205 switch(g[2]) {
yabahiro 33:dd598bb23ad1 3206 case 0:
yabahiro 33:dd598bb23ad1 3207 mtc2=false;
yabahiro 33:dd598bb23ad1 3208 break;
yabahiro 33:dd598bb23ad1 3209 case 255:
kishibekairohan 44:2aff0da5ebdb 3210 motor[TIRE_FR].pwm += 3;
yabahiro 33:dd598bb23ad1 3211 mtc2=false;
yabahiro 33:dd598bb23ad1 3212 break;
yabahiro 33:dd598bb23ad1 3213 case 253:
kishibekairohan 44:2aff0da5ebdb 3214 motor[TIRE_FR].pwm += 6;
yabahiro 33:dd598bb23ad1 3215 mtc2=false;
yabahiro 33:dd598bb23ad1 3216 break;
yabahiro 33:dd598bb23ad1 3217 case 254:
kishibekairohan 44:2aff0da5ebdb 3218 motor[TIRE_FR].pwm += 9;
kishibekairohan 44:2aff0da5ebdb 3219 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3220 mtc2=false;
yabahiro 33:dd598bb23ad1 3221 break;
yabahiro 33:dd598bb23ad1 3222 case 1:
kishibekairohan 44:2aff0da5ebdb 3223 motor[TIRE_BR].pwm += 3;
yabahiro 33:dd598bb23ad1 3224 mtc2=false;
yabahiro 33:dd598bb23ad1 3225 break;
yabahiro 33:dd598bb23ad1 3226 case 3:
kishibekairohan 44:2aff0da5ebdb 3227 motor[TIRE_BR].pwm += 6;
yabahiro 33:dd598bb23ad1 3228 mtc2=false;
yabahiro 33:dd598bb23ad1 3229 break;
yabahiro 33:dd598bb23ad1 3230 case 2:
kishibekairohan 44:2aff0da5ebdb 3231 motor[TIRE_BR].pwm += 9;
kishibekairohan 44:2aff0da5ebdb 3232 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3233 mtc2=false;
yabahiro 33:dd598bb23ad1 3234 break;
yabahiro 33:dd598bb23ad1 3235 }
yabahiro 33:dd598bb23ad1 3236 }
yabahiro 46:68296caae1af 3237
kishibekairohan 42:980af34f6fe3 3238 if(YELLOW) {
kishibekairohan 42:980af34f6fe3 3239 if(cross==1) {
kishibekairohan 42:980af34f6fe3 3240 mode=41;
kishibekairohan 42:980af34f6fe3 3241 countss=0;
kishibekairohan 42:980af34f6fe3 3242 cross=0;
kishibekairohan 42:980af34f6fe3 3243 }
yabahiro 46:68296caae1af 3244 } else {
kishibekairohan 42:980af34f6fe3 3245 if(cross==3) {
kishibekairohan 42:980af34f6fe3 3246 mode=41;
kishibekairohan 42:980af34f6fe3 3247 countss=0;
kishibekairohan 42:980af34f6fe3 3248 cross=0;
kishibekairohan 42:980af34f6fe3 3249 }
kishibekairohan 42:980af34f6fe3 3250 }
yabahiro 46:68296caae1af 3251 if(LIF||LIB) {
yabahiro 46:68296caae1af 3252 mode=42;
yabahiro 46:68296caae1af 3253 }
yabahiro 46:68296caae1af 3254
yabahiro 46:68296caae1af 3255 } else if(mode==41) {
yabahiro 33:dd598bb23ad1 3256 if(LIF||LIB) {
yabahiro 33:dd598bb23ad1 3257 mode=42;
yabahiro 33:dd598bb23ad1 3258 }
kishibekairohan 44:2aff0da5ebdb 3259 motor[TIRE_FR].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 3260 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 42:980af34f6fe3 3261 motor[TIRE_FL].pwm = 40;
yabahiro 33:dd598bb23ad1 3262 motor[TIRE_FL].dir = BACK;
kishibekairohan 42:980af34f6fe3 3263 motor[TIRE_BR].pwm = 40;
yabahiro 33:dd598bb23ad1 3264 motor[TIRE_BR].dir = FOR;
kishibekairohan 44:2aff0da5ebdb 3265 motor[TIRE_BL].pwm = 255;
kishibekairohan 44:2aff0da5ebdb 3266 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 3267 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 3268 countss=0;
yabahiro 33:dd598bb23ad1 3269 mode=70;
kishibekairohan 42:980af34f6fe3 3270 normal=80;
kishibekairohan 42:980af34f6fe3 3271 fast=100;
kishibekairohan 42:980af34f6fe3 3272 slow=60;
yabahiro 33:dd598bb23ad1 3273 }
yabahiro 46:68296caae1af 3274 } else if(mode==42) {
kishibekairohan 42:980af34f6fe3 3275 motor[TIRE_FR].pwm = 40;
yabahiro 33:dd598bb23ad1 3276 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3277 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3278 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3279 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3280 motor[TIRE_BR].dir = FREE;
kishibekairohan 42:980af34f6fe3 3281 motor[TIRE_BL].pwm = 40;
yabahiro 33:dd598bb23ad1 3282 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3283 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 3284 countss=0;
yabahiro 33:dd598bb23ad1 3285 mode=70;
yabahiro 33:dd598bb23ad1 3286 }
yabahiro 46:68296caae1af 3287 } else if(mode==70) {
yabahiro 33:dd598bb23ad1 3288 switch(g[1]) {
yabahiro 33:dd598bb23ad1 3289 case 99:
yabahiro 33:dd598bb23ad1 3290 if(countss>=100000) {
yabahiro 33:dd598bb23ad1 3291 mode=100;
yabahiro 33:dd598bb23ad1 3292 }
yabahiro 33:dd598bb23ad1 3293 break;
yabahiro 33:dd598bb23ad1 3294 case 98:
yabahiro 33:dd598bb23ad1 3295 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 3296 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3297 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 3298 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3299 motor[TIRE_BR].pwm = normal+5;
yabahiro 33:dd598bb23ad1 3300 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3301 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 3302 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3303 break;
yabahiro 33:dd598bb23ad1 3304 case 0:
kishibekairohan 42:980af34f6fe3 3305 motor[TIRE_FR].pwm = 100;
yabahiro 33:dd598bb23ad1 3306 motor[TIRE_FR].dir = FOR;
kishibekairohan 42:980af34f6fe3 3307 motor[TIRE_FL].pwm = 100;
yabahiro 33:dd598bb23ad1 3308 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3309 motor[TIRE_BR].pwm = 105;
yabahiro 33:dd598bb23ad1 3310 motor[TIRE_BR].dir = FOR;
kishibekairohan 42:980af34f6fe3 3311 motor[TIRE_BL].pwm = 100;
yabahiro 33:dd598bb23ad1 3312 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3313 mtc=true;
yabahiro 33:dd598bb23ad1 3314 break;
yabahiro 33:dd598bb23ad1 3315 case 255:
yabahiro 33:dd598bb23ad1 3316 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 3317 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3318 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 3319 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3320 motor[TIRE_BR].pwm = slow+5;
yabahiro 33:dd598bb23ad1 3321 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3322 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 3323 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3324 mtc=true;
yabahiro 33:dd598bb23ad1 3325 break;
yabahiro 33:dd598bb23ad1 3326 case 253:
yabahiro 33:dd598bb23ad1 3327 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 3328 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3329 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3330 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3331 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3332 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 3333 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 3334 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3335 mtc=true;
yabahiro 33:dd598bb23ad1 3336 break;
yabahiro 33:dd598bb23ad1 3337 case 254:
yabahiro 33:dd598bb23ad1 3338 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 3339 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3340 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3341 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 3342 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 3343 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 3344 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 3345 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3346 mtc=true;
yabahiro 33:dd598bb23ad1 3347 break;
yabahiro 33:dd598bb23ad1 3348 case 1:
yabahiro 33:dd598bb23ad1 3349 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 3350 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 3351 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 3352 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3353 motor[TIRE_BR].pwm = fast+5;
yabahiro 33:dd598bb23ad1 3354 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3355 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 3356 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 3357 mtc=true;
yabahiro 33:dd598bb23ad1 3358 break;
yabahiro 33:dd598bb23ad1 3359 case 3:
yabahiro 33:dd598bb23ad1 3360 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3361 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 3362 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 3363 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3364 motor[TIRE_BR].pwm = slow+5;
yabahiro 33:dd598bb23ad1 3365 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3366 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 3367 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 3368 mtc=true;
yabahiro 33:dd598bb23ad1 3369 break;
yabahiro 33:dd598bb23ad1 3370 case 2:
yabahiro 33:dd598bb23ad1 3371 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3372 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 3373 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 3374 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3375 motor[TIRE_BR].pwm = normal+5;
yabahiro 33:dd598bb23ad1 3376 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 3377 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 3378 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 3379 mtc=true;
yabahiro 33:dd598bb23ad1 3380 break;
yabahiro 33:dd598bb23ad1 3381 }
yabahiro 33:dd598bb23ad1 3382 if(g[1]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 3383 switch(g[0]) {
yabahiro 33:dd598bb23ad1 3384 case 0:
yabahiro 33:dd598bb23ad1 3385 mtc=false;
yabahiro 33:dd598bb23ad1 3386 break;
yabahiro 33:dd598bb23ad1 3387 case 255:
kishibekairohan 44:2aff0da5ebdb 3388 motor[TIRE_FL].pwm += 3;
yabahiro 33:dd598bb23ad1 3389 mtc=false;
yabahiro 33:dd598bb23ad1 3390 break;
yabahiro 33:dd598bb23ad1 3391 case 253:
kishibekairohan 44:2aff0da5ebdb 3392 motor[TIRE_FL].pwm += 6;
yabahiro 33:dd598bb23ad1 3393 mtc=false;
yabahiro 33:dd598bb23ad1 3394 break;
yabahiro 33:dd598bb23ad1 3395 case 254:
kishibekairohan 44:2aff0da5ebdb 3396 motor[TIRE_FL].pwm += 9;
yabahiro 33:dd598bb23ad1 3397 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 3398 mtc=false;
yabahiro 33:dd598bb23ad1 3399 break;
yabahiro 33:dd598bb23ad1 3400 case 1:
kishibekairohan 44:2aff0da5ebdb 3401 motor[TIRE_FR].pwm += 3;
yabahiro 33:dd598bb23ad1 3402 mtc=false;
yabahiro 33:dd598bb23ad1 3403 break;
yabahiro 33:dd598bb23ad1 3404 case 3:
kishibekairohan 44:2aff0da5ebdb 3405 motor[TIRE_FR].pwm += 6;
yabahiro 33:dd598bb23ad1 3406 mtc=false;
yabahiro 33:dd598bb23ad1 3407 break;
yabahiro 33:dd598bb23ad1 3408 case 2:
kishibekairohan 44:2aff0da5ebdb 3409 motor[TIRE_FR].pwm += 9;
yabahiro 33:dd598bb23ad1 3410 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 3411 mtc=false;
yabahiro 33:dd598bb23ad1 3412 break;
yabahiro 33:dd598bb23ad1 3413 }
yabahiro 33:dd598bb23ad1 3414 }
yabahiro 46:68296caae1af 3415 } else if(mode==100) {
kishibekairohan 42:980af34f6fe3 3416 motor[TIRE_FR].pwm = 50;
yabahiro 33:dd598bb23ad1 3417 motor[TIRE_FR].dir = FOR;
yabahiro 46:68296caae1af 3418 motor[TIRE_FL].pwm = 55;
yabahiro 33:dd598bb23ad1 3419 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3420 motor[TIRE_BR].pwm = 60;
yabahiro 33:dd598bb23ad1 3421 motor[TIRE_BR].dir = FOR;
kishibekairohan 42:980af34f6fe3 3422 motor[TIRE_BL].pwm = 50;
yabahiro 33:dd598bb23ad1 3423 motor[TIRE_BL].dir = BACK;
yabahiro 46:68296caae1af 3424 }
yabahiro 46:68296caae1af 3425
kishibekairohan 13:b6e02d6261d7 3426 }
kishibekairohan 13:b6e02d6261d7 3427 #endif
kishibekairohan 13:b6e02d6261d7 3428
kishibekairohan 13:b6e02d6261d7 3429 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 3430 static void Process4()
M_souta 22:7d93f79a3686 3431 {
yabahiro 48:5562a99479e5 3432 //Oh So 決勝青ゾーン
yabahiro 48:5562a99479e5 3433 LED::LED::TapeledMode(2);
yabahiro 48:5562a99479e5 3434 countss++;
yabahiro 48:5562a99479e5 3435 if(mode==1) {//スタートゾーンから白線検知
yabahiro 48:5562a99479e5 3436 fast=40;
yabahiro 48:5562a99479e5 3437 normal=30;
yabahiro 48:5562a99479e5 3438 slow=20;
yabahiro 48:5562a99479e5 3439 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3440 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3441 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3442 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3443 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3444 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3445 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3446 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3447 if(g[2]==0&&countss>=100000) {
yabahiro 48:5562a99479e5 3448 countss=100000;
yabahiro 48:5562a99479e5 3449 cross=0;
yabahiro 48:5562a99479e5 3450 mode=3;
yabahiro 48:5562a99479e5 3451 }
yabahiro 48:5562a99479e5 3452 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 48:5562a99479e5 3453 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3454 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3455 motor[TIRE_FL].pwm = 255;
yabahiro 48:5562a99479e5 3456 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 3457 motor[TIRE_BR].pwm = 255;
yabahiro 48:5562a99479e5 3458 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 3459 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3460 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3461 if(g[1]==0) {
yabahiro 48:5562a99479e5 3462 mode=10;
yabahiro 48:5562a99479e5 3463 countss=100000;
yabahiro 48:5562a99479e5 3464 }
yabahiro 48:5562a99479e5 3465 } else if(mode==10) { //縦ライントレース
yabahiro 48:5562a99479e5 3466 switch(g[1]) {
yabahiro 48:5562a99479e5 3467 case 98:
yabahiro 48:5562a99479e5 3468 motor[TIRE_FR].pwm = normal+50;
yabahiro 48:5562a99479e5 3469 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3470 motor[TIRE_FL].pwm = normal+50;
yabahiro 48:5562a99479e5 3471 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3472 motor[TIRE_BR].pwm = normal+50;
yabahiro 48:5562a99479e5 3473 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3474 motor[TIRE_BL].pwm = normal+50;
yabahiro 48:5562a99479e5 3475 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3476 mtc=true;
yabahiro 49:697b4e9ce884 3477 if(YELLOW) {
yabahiro 49:697b4e9ce884 3478 if(countss>80000) {
yabahiro 49:697b4e9ce884 3479 cross++;
yabahiro 49:697b4e9ce884 3480 countss=0;
yabahiro 49:697b4e9ce884 3481 }
yabahiro 48:5562a99479e5 3482
yabahiro 49:697b4e9ce884 3483 if(cross==2) {
yabahiro 49:697b4e9ce884 3484 fast = 10;
yabahiro 49:697b4e9ce884 3485 normal = 10;
yabahiro 49:697b4e9ce884 3486 slow = 5;
yabahiro 49:697b4e9ce884 3487 }
yabahiro 48:5562a99479e5 3488
yabahiro 49:697b4e9ce884 3489 if(cross==3) {
yabahiro 49:697b4e9ce884 3490 fast=40;
yabahiro 49:697b4e9ce884 3491 normal=30;
yabahiro 49:697b4e9ce884 3492 slow=20;
yabahiro 49:697b4e9ce884 3493 mode=11;
yabahiro 49:697b4e9ce884 3494 countss=0;
yabahiro 49:697b4e9ce884 3495 cross=0;
yabahiro 49:697b4e9ce884 3496 }
yabahiro 49:697b4e9ce884 3497 } else {
yabahiro 48:5562a99479e5 3498 mode=11;
yabahiro 48:5562a99479e5 3499 }
yabahiro 48:5562a99479e5 3500 break;
yabahiro 48:5562a99479e5 3501 case 0:
yabahiro 48:5562a99479e5 3502 motor[TIRE_FR].pwm = fast+50;
yabahiro 48:5562a99479e5 3503 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3504 motor[TIRE_FL].pwm = fast+50;
yabahiro 48:5562a99479e5 3505 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3506 motor[TIRE_BR].pwm = fast+50;
yabahiro 48:5562a99479e5 3507 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3508 motor[TIRE_BL].pwm = fast+50;
yabahiro 48:5562a99479e5 3509 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3510 mtc=true;
yabahiro 48:5562a99479e5 3511 break;
yabahiro 48:5562a99479e5 3512 case 255:
yabahiro 48:5562a99479e5 3513 motor[TIRE_FR].pwm = fast+50;
yabahiro 48:5562a99479e5 3514 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3515 motor[TIRE_FL].pwm = slow+50;
yabahiro 48:5562a99479e5 3516 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3517 motor[TIRE_BR].pwm = slow+50;
yabahiro 48:5562a99479e5 3518 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3519 motor[TIRE_BL].pwm = fast+50;
yabahiro 48:5562a99479e5 3520 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3521 mtc=true;
yabahiro 48:5562a99479e5 3522 break;
yabahiro 48:5562a99479e5 3523 case 253:
yabahiro 48:5562a99479e5 3524 motor[TIRE_FR].pwm = slow+50;
yabahiro 48:5562a99479e5 3525 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3526 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3527 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3528 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3529 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3530 motor[TIRE_BL].pwm = slow+50;
yabahiro 48:5562a99479e5 3531 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3532 mtc=true;
yabahiro 48:5562a99479e5 3533 break;
yabahiro 48:5562a99479e5 3534 case 254:
yabahiro 48:5562a99479e5 3535 motor[TIRE_FR].pwm = normal+50;
yabahiro 48:5562a99479e5 3536 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3537 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3538 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3539 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3540 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3541 motor[TIRE_BL].pwm = normal+50;
yabahiro 48:5562a99479e5 3542 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3543 mtc=true;
yabahiro 48:5562a99479e5 3544 break;
yabahiro 48:5562a99479e5 3545 case 1:
yabahiro 48:5562a99479e5 3546 motor[TIRE_FR].pwm = slow+50;
yabahiro 48:5562a99479e5 3547 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3548 motor[TIRE_FL].pwm = fast+50;
yabahiro 48:5562a99479e5 3549 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3550 motor[TIRE_BR].pwm = fast+50;
yabahiro 48:5562a99479e5 3551 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3552 motor[TIRE_BL].pwm = slow+50;
yabahiro 48:5562a99479e5 3553 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3554 mtc=true;
yabahiro 48:5562a99479e5 3555 break;
yabahiro 48:5562a99479e5 3556 case 3:
yabahiro 48:5562a99479e5 3557 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3558 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3559 motor[TIRE_FL].pwm = slow+50;
yabahiro 48:5562a99479e5 3560 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3561 motor[TIRE_BR].pwm = slow+50;
yabahiro 48:5562a99479e5 3562 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3563 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3564 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3565 mtc=true;
yabahiro 48:5562a99479e5 3566 break;
yabahiro 48:5562a99479e5 3567 case 2:
yabahiro 48:5562a99479e5 3568 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3569 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3570 motor[TIRE_FL].pwm = normal+50;
yabahiro 48:5562a99479e5 3571 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3572 motor[TIRE_BR].pwm = normal+50;
yabahiro 48:5562a99479e5 3573 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3574 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3575 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3576 mtc=true;
yabahiro 48:5562a99479e5 3577 break;
yabahiro 48:5562a99479e5 3578 }
yabahiro 48:5562a99479e5 3579 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 3580 switch(g[0]) {
yabahiro 48:5562a99479e5 3581 case 0:
yabahiro 48:5562a99479e5 3582 mtc=false;
yabahiro 48:5562a99479e5 3583 break;
yabahiro 48:5562a99479e5 3584 case 255:
yabahiro 48:5562a99479e5 3585 motor[TIRE_FL].pwm += 3;
yabahiro 48:5562a99479e5 3586 mtc=false;
yabahiro 48:5562a99479e5 3587 break;
yabahiro 48:5562a99479e5 3588 case 253:
yabahiro 48:5562a99479e5 3589 motor[TIRE_FL].pwm += 6;
yabahiro 48:5562a99479e5 3590 mtc=false;
yabahiro 48:5562a99479e5 3591 break;
yabahiro 48:5562a99479e5 3592 case 254:
yabahiro 48:5562a99479e5 3593 motor[TIRE_FL].pwm += 9;
yabahiro 48:5562a99479e5 3594 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3595 mtc=false;
yabahiro 48:5562a99479e5 3596 break;
yabahiro 48:5562a99479e5 3597 case 1:
yabahiro 48:5562a99479e5 3598 motor[TIRE_FR].pwm += 3;
yabahiro 48:5562a99479e5 3599 mtc=false;
yabahiro 48:5562a99479e5 3600 break;
yabahiro 48:5562a99479e5 3601 case 3:
yabahiro 48:5562a99479e5 3602 motor[TIRE_FR].pwm += 6;
yabahiro 48:5562a99479e5 3603 mtc=false;
yabahiro 48:5562a99479e5 3604 break;
yabahiro 48:5562a99479e5 3605 case 2:
yabahiro 48:5562a99479e5 3606 motor[TIRE_FR].pwm += 9;
yabahiro 48:5562a99479e5 3607 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3608 mtc=false;
yabahiro 48:5562a99479e5 3609 break;
yabahiro 48:5562a99479e5 3610 }
yabahiro 48:5562a99479e5 3611 }
yabahiro 48:5562a99479e5 3612 } else if(mode==11) {
yabahiro 48:5562a99479e5 3613 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 3614 motor[TIRE_FR].dir = FOR;
yabahiro 49:697b4e9ce884 3615 motor[TIRE_FL].pwm = 50;
yabahiro 49:697b4e9ce884 3616 motor[TIRE_FL].dir = BRAKE;
yabahiro 49:697b4e9ce884 3617 motor[TIRE_BR].pwm = 50;
yabahiro 49:697b4e9ce884 3618 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 3619 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 3620 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3621 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 48:5562a99479e5 3622 countss=0;
yabahiro 48:5562a99479e5 3623 cross=0;
yabahiro 48:5562a99479e5 3624 mode=20;
yabahiro 48:5562a99479e5 3625 }
yabahiro 48:5562a99479e5 3626 } else if(mode==20) {
yabahiro 48:5562a99479e5 3627 switch(g[2]) {
yabahiro 48:5562a99479e5 3628 case 98:
yabahiro 48:5562a99479e5 3629 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3630 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3631 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3632 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3633 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3634 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3635 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3636 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3637 if(countss>30000) {
yabahiro 48:5562a99479e5 3638 cross++;
yabahiro 48:5562a99479e5 3639 countss=0;
yabahiro 48:5562a99479e5 3640 }
yabahiro 48:5562a99479e5 3641 mtc2=true;
yabahiro 48:5562a99479e5 3642
yabahiro 48:5562a99479e5 3643 if(YELLOW) {
yabahiro 49:697b4e9ce884 3644 solenoid.solenoid4= SOLENOID_ON;
yabahiro 48:5562a99479e5 3645 if(cross==1) {
yabahiro 48:5562a99479e5 3646 fast=20;
yabahiro 48:5562a99479e5 3647 normal=15;
yabahiro 48:5562a99479e5 3648 slow=15;
yabahiro 48:5562a99479e5 3649 } else if(cross==2) {
yabahiro 48:5562a99479e5 3650 mode=21;
yabahiro 48:5562a99479e5 3651 countss=0;
yabahiro 48:5562a99479e5 3652 cross=0;
yabahiro 48:5562a99479e5 3653 }
yabahiro 48:5562a99479e5 3654 } else {
yabahiro 49:697b4e9ce884 3655 if(cross==2) {
yabahiro 49:697b4e9ce884 3656 fast=20;
yabahiro 49:697b4e9ce884 3657 normal=15;
yabahiro 49:697b4e9ce884 3658 slow=15;
yabahiro 49:697b4e9ce884 3659 }
yabahiro 48:5562a99479e5 3660 if(cross==3) {
yabahiro 48:5562a99479e5 3661 mode=21;
yabahiro 48:5562a99479e5 3662 countss=0;
yabahiro 48:5562a99479e5 3663 cross=0;
yabahiro 48:5562a99479e5 3664 }
yabahiro 48:5562a99479e5 3665 }
yabahiro 48:5562a99479e5 3666 break;
yabahiro 48:5562a99479e5 3667 case 0:
yabahiro 48:5562a99479e5 3668 motor[TIRE_FR].pwm = fast+20;
yabahiro 48:5562a99479e5 3669 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3670 motor[TIRE_FL].pwm = fast+20;
yabahiro 48:5562a99479e5 3671 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3672 motor[TIRE_BR].pwm = fast+20;
yabahiro 48:5562a99479e5 3673 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3674 motor[TIRE_BL].pwm = fast+20;
yabahiro 48:5562a99479e5 3675 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3676 mtc2=true;
yabahiro 48:5562a99479e5 3677 break;
yabahiro 48:5562a99479e5 3678 case 255:
yabahiro 48:5562a99479e5 3679 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3680 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3681 motor[TIRE_FL].pwm = fast+20;
yabahiro 48:5562a99479e5 3682 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3683 motor[TIRE_BR].pwm = fast+20;
yabahiro 48:5562a99479e5 3684 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3685 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3686 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3687 mtc2=true;
yabahiro 48:5562a99479e5 3688 break;
yabahiro 48:5562a99479e5 3689 case 253:
yabahiro 48:5562a99479e5 3690 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3691 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3692 motor[TIRE_FL].pwm = slow+20;
yabahiro 48:5562a99479e5 3693 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3694 motor[TIRE_BR].pwm = slow+20;
yabahiro 48:5562a99479e5 3695 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3696 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3697 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3698 mtc2=true;
yabahiro 48:5562a99479e5 3699 break;
yabahiro 48:5562a99479e5 3700 case 254:
yabahiro 48:5562a99479e5 3701 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3702 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3703 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3704 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3705 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3706 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3707 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3708 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3709 mtc2=true;
yabahiro 48:5562a99479e5 3710 break;
yabahiro 48:5562a99479e5 3711 case 1:
yabahiro 48:5562a99479e5 3712 motor[TIRE_FR].pwm = fast+20;
yabahiro 48:5562a99479e5 3713 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3714 motor[TIRE_FL].pwm = slow+20;
yabahiro 48:5562a99479e5 3715 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3716 motor[TIRE_BR].pwm = slow+20;
yabahiro 48:5562a99479e5 3717 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3718 motor[TIRE_BL].pwm = fast+20;
yabahiro 48:5562a99479e5 3719 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3720 mtc2=true;
yabahiro 48:5562a99479e5 3721 break;
yabahiro 48:5562a99479e5 3722 case 3:
yabahiro 48:5562a99479e5 3723 motor[TIRE_FR].pwm = slow+20;
yabahiro 48:5562a99479e5 3724 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3725 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3726 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3727 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3728 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3729 motor[TIRE_BL].pwm = slow+20;
yabahiro 48:5562a99479e5 3730 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3731 mtc2=true;
yabahiro 48:5562a99479e5 3732 break;
yabahiro 48:5562a99479e5 3733 case 2:
yabahiro 48:5562a99479e5 3734 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3735 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3736 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3737 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3738 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3739 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3740 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3741 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3742 mtc2=true;
yabahiro 48:5562a99479e5 3743 break;
yabahiro 48:5562a99479e5 3744 }
yabahiro 48:5562a99479e5 3745 if(g[2]!=98&&mtc2==true) {
yabahiro 48:5562a99479e5 3746 switch(g[3]) {
yabahiro 48:5562a99479e5 3747 case 0:
yabahiro 48:5562a99479e5 3748 mtc2=false;
yabahiro 48:5562a99479e5 3749 break;
yabahiro 48:5562a99479e5 3750 case 255:
yabahiro 48:5562a99479e5 3751 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 3752 mtc2=false;
yabahiro 48:5562a99479e5 3753 break;
yabahiro 48:5562a99479e5 3754 case 253:
yabahiro 48:5562a99479e5 3755 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 3756 mtc2=false;
yabahiro 48:5562a99479e5 3757 break;
yabahiro 48:5562a99479e5 3758 case 254:
yabahiro 48:5562a99479e5 3759 motor[TIRE_BL].pwm += 15;
yabahiro 48:5562a99479e5 3760 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3761 mtc2=false;
yabahiro 48:5562a99479e5 3762 break;
yabahiro 48:5562a99479e5 3763 case 1:
yabahiro 48:5562a99479e5 3764 motor[TIRE_FL].pwm += 5;
yabahiro 48:5562a99479e5 3765 mtc2=false;
yabahiro 48:5562a99479e5 3766 break;
yabahiro 48:5562a99479e5 3767 case 3:
yabahiro 48:5562a99479e5 3768 motor[TIRE_FL].pwm += 10;
yabahiro 48:5562a99479e5 3769 mtc2=false;
yabahiro 48:5562a99479e5 3770 break;
yabahiro 48:5562a99479e5 3771 case 2:
yabahiro 48:5562a99479e5 3772 motor[TIRE_FL].pwm += 15;
yabahiro 48:5562a99479e5 3773 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3774 mtc2=false;
yabahiro 48:5562a99479e5 3775 break;
yabahiro 48:5562a99479e5 3776 }
yabahiro 48:5562a99479e5 3777 }
yabahiro 48:5562a99479e5 3778 }
yabahiro 48:5562a99479e5 3779 if(YELLOW) {
yabahiro 48:5562a99479e5 3780 if(mode==21) {
yabahiro 48:5562a99479e5 3781 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3782 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3783 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3784 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3785 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3786 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3787 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3788 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3789 if(g[3]==98&&countss>=10000) {
yabahiro 48:5562a99479e5 3790 mode=22;
yabahiro 48:5562a99479e5 3791 countss=0;
yabahiro 48:5562a99479e5 3792 fast=60;
yabahiro 48:5562a99479e5 3793 normal=40;
yabahiro 48:5562a99479e5 3794 slow=20;
yabahiro 48:5562a99479e5 3795 }
yabahiro 48:5562a99479e5 3796 } else if(mode==22) {
yabahiro 48:5562a99479e5 3797 if(g[4]==3||g[4]==2) {
yabahiro 48:5562a99479e5 3798 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3799 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3800 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3801 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3802 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3803 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3804 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3805 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3806 }
yabahiro 48:5562a99479e5 3807 if(g[4]==253||g[4]==254) {
yabahiro 48:5562a99479e5 3808 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3809 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3810 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3811 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3812 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3813 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3814 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3815 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3816 }
yabahiro 48:5562a99479e5 3817 switch(g[2]) {
yabahiro 48:5562a99479e5 3818 case 0:
yabahiro 48:5562a99479e5 3819 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3820 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3821 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3822 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3823 break;
yabahiro 48:5562a99479e5 3824 case 255:
yabahiro 48:5562a99479e5 3825 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3826 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3827 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3828 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3829 break;
yabahiro 48:5562a99479e5 3830 case 253:
yabahiro 48:5562a99479e5 3831 motor[TIRE_FR].pwm = 40;
yabahiro 48:5562a99479e5 3832 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3833 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3834 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3835 break;
yabahiro 48:5562a99479e5 3836 case 254:
yabahiro 48:5562a99479e5 3837 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3838 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3839 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3840 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3841 break;
yabahiro 48:5562a99479e5 3842 case 1:
yabahiro 48:5562a99479e5 3843 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3844 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3845 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3846 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3847 break;
yabahiro 48:5562a99479e5 3848 case 3:
yabahiro 48:5562a99479e5 3849 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3850 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3851 motor[TIRE_BR].pwm = 40;
yabahiro 48:5562a99479e5 3852 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3853 break;
yabahiro 48:5562a99479e5 3854 case 2:
yabahiro 48:5562a99479e5 3855 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3856 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3857 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3858 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3859 break;
yabahiro 48:5562a99479e5 3860 }
yabahiro 48:5562a99479e5 3861 switch(g[3]) {
yabahiro 48:5562a99479e5 3862 case 0:
yabahiro 48:5562a99479e5 3863 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3864 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3865 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3866 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3867 break;
yabahiro 48:5562a99479e5 3868 case 255:
yabahiro 48:5562a99479e5 3869 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3870 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3871 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3872 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3873 case 253:
yabahiro 48:5562a99479e5 3874 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3875 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3876 motor[TIRE_BL].pwm = 40;
yabahiro 48:5562a99479e5 3877 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3878 break;
yabahiro 48:5562a99479e5 3879 case 254:
yabahiro 48:5562a99479e5 3880 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3881 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3882 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3883 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3884 break;
yabahiro 48:5562a99479e5 3885 case 1:
yabahiro 48:5562a99479e5 3886 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3887 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3888 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3889 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3890 break;
yabahiro 48:5562a99479e5 3891 case 3:
yabahiro 48:5562a99479e5 3892 motor[TIRE_FL].pwm = 40;
yabahiro 48:5562a99479e5 3893 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3894 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3895 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3896 break;
yabahiro 48:5562a99479e5 3897 case 2:
yabahiro 48:5562a99479e5 3898 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3899 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3900 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3901 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3902 break;
yabahiro 48:5562a99479e5 3903 }
yabahiro 48:5562a99479e5 3904 if(g[3]==0&&g[2]==0) {
yabahiro 48:5562a99479e5 3905 mode=23;
yabahiro 48:5562a99479e5 3906 countss=0;
yabahiro 48:5562a99479e5 3907 }
yabahiro 48:5562a99479e5 3908 } else if(mode==23) {
yabahiro 48:5562a99479e5 3909 switch(g[4]) {
yabahiro 48:5562a99479e5 3910 case 98:
yabahiro 48:5562a99479e5 3911 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 3912 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3913 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 3914 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3915 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 3916 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3917 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 3918 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3919 break;
yabahiro 48:5562a99479e5 3920 case 99:
yabahiro 48:5562a99479e5 3921 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3922 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3923 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3924 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3925 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3926 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3927 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3928 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3929 break;
yabahiro 48:5562a99479e5 3930 case 0:
yabahiro 48:5562a99479e5 3931 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 3932 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3933 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 3934 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3935 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 3936 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3937 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 3938 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3939 break;
yabahiro 48:5562a99479e5 3940 case 255:
yabahiro 48:5562a99479e5 3941 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3942 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3943 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3944 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3945 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3946 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3947 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3948 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3949 break;
yabahiro 48:5562a99479e5 3950 case 253:
yabahiro 48:5562a99479e5 3951 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 3952 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3953 motor[TIRE_FL].pwm = slow-5;
yabahiro 48:5562a99479e5 3954 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3955 motor[TIRE_BR].pwm = slow-5;
yabahiro 48:5562a99479e5 3956 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3957 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 3958 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3959 break;
yabahiro 48:5562a99479e5 3960 case 254:
yabahiro 48:5562a99479e5 3961 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 3962 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3963 motor[TIRE_FL].pwm = normal-5;
yabahiro 48:5562a99479e5 3964 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3965 motor[TIRE_BR].pwm = normal-5;
yabahiro 48:5562a99479e5 3966 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3967 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 3968 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3969 break;
yabahiro 48:5562a99479e5 3970 case 1:
yabahiro 48:5562a99479e5 3971 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3972 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3973 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3974 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3975 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3976 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3977 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3978 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3979 break;
yabahiro 48:5562a99479e5 3980 case 3:
yabahiro 48:5562a99479e5 3981 motor[TIRE_FR].pwm = slow-5;
yabahiro 48:5562a99479e5 3982 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3983 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 3984 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3985 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 3986 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3987 motor[TIRE_BL].pwm = slow-5;
yabahiro 48:5562a99479e5 3988 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3989 break;
yabahiro 48:5562a99479e5 3990 case 2:
yabahiro 48:5562a99479e5 3991 motor[TIRE_FR].pwm = normal-5;
yabahiro 48:5562a99479e5 3992 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3993 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 3994 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3995 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 3996 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3997 motor[TIRE_BL].pwm = normal-5;
yabahiro 48:5562a99479e5 3998 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3999 break;
yabahiro 48:5562a99479e5 4000 }
yabahiro 48:5562a99479e5 4001 if(g[2]==254&&countss>=100000) {
yabahiro 48:5562a99479e5 4002 mode=24;
yabahiro 48:5562a99479e5 4003 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 48:5562a99479e5 4004 countss=0;
yabahiro 48:5562a99479e5 4005 fast=60;
yabahiro 48:5562a99479e5 4006 normal=40;
yabahiro 48:5562a99479e5 4007 slow=20;
yabahiro 48:5562a99479e5 4008 }
yabahiro 48:5562a99479e5 4009 } else if(mode==24) {
yabahiro 48:5562a99479e5 4010 motor[TIRE_FR].pwm = 60;
yabahiro 48:5562a99479e5 4011 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4012 motor[TIRE_FL].pwm = 60;
yabahiro 48:5562a99479e5 4013 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4014 motor[TIRE_BR].pwm = 60;
yabahiro 48:5562a99479e5 4015 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4016 motor[TIRE_BL].pwm = 60;
yabahiro 48:5562a99479e5 4017 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4018 if(g[0]==0) {
yabahiro 48:5562a99479e5 4019 mode=25;
yabahiro 48:5562a99479e5 4020 }
yabahiro 48:5562a99479e5 4021 } else if(mode==25) {
yabahiro 48:5562a99479e5 4022 motor[TIRE_FR].pwm = 100;
yabahiro 48:5562a99479e5 4023 motor[TIRE_FR].dir = BRAKE;
yabahiro 48:5562a99479e5 4024 motor[TIRE_FL].pwm = 100;
yabahiro 48:5562a99479e5 4025 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4026 motor[TIRE_BR].pwm = 100;
yabahiro 48:5562a99479e5 4027 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4028 motor[TIRE_BL].pwm = 100;
yabahiro 48:5562a99479e5 4029 motor[TIRE_BL].dir = BRAKE;
yabahiro 48:5562a99479e5 4030 if(DOLS==true||UPLS==true) {
yabahiro 48:5562a99479e5 4031 motor[MOTOR_0].pwm = 200;
yabahiro 48:5562a99479e5 4032 motor[MOTOR_0].dir = BRAKE;
yabahiro 48:5562a99479e5 4033 mode=26;
yabahiro 48:5562a99479e5 4034 countss=0;
yabahiro 48:5562a99479e5 4035 } else {
yabahiro 48:5562a99479e5 4036 motor[MOTOR_0].pwm = 100;
yabahiro 48:5562a99479e5 4037 motor[MOTOR_0].dir = BACK;
yabahiro 48:5562a99479e5 4038 }
yabahiro 48:5562a99479e5 4039 } else if(mode==26) {
yabahiro 48:5562a99479e5 4040 if(g[0]==98&&countss>=40000) {
yabahiro 48:5562a99479e5 4041 mode=31;
yabahiro 48:5562a99479e5 4042 countss=0;
yabahiro 48:5562a99479e5 4043 }
yabahiro 48:5562a99479e5 4044 switch(g[0]) {
yabahiro 48:5562a99479e5 4045 case 98:
yabahiro 48:5562a99479e5 4046 fast=40;
yabahiro 48:5562a99479e5 4047 normal=30;
yabahiro 48:5562a99479e5 4048 slow=25;
yabahiro 48:5562a99479e5 4049 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4050 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4051 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4052 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4053 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4054 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4055 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4056 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4057 break;
yabahiro 48:5562a99479e5 4058 case 0:
yabahiro 48:5562a99479e5 4059 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4060 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4061 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4062 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4063 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4064 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4065 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4066 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4067 mtc=true;
yabahiro 48:5562a99479e5 4068 break;
yabahiro 48:5562a99479e5 4069 case 255:
yabahiro 48:5562a99479e5 4070 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4071 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4072 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4073 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4074 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4075 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4076 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4077 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4078 mtc=true;
yabahiro 48:5562a99479e5 4079 break;
yabahiro 48:5562a99479e5 4080 case 253:
yabahiro 48:5562a99479e5 4081 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4082 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4083 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4084 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4085 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4086 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4087 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4088 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4089 mtc=true;
yabahiro 48:5562a99479e5 4090 break;
yabahiro 48:5562a99479e5 4091 case 254:
yabahiro 48:5562a99479e5 4092 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4093 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4094 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4095 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4096 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4097 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4098 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4099 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4100 mtc=true;
yabahiro 48:5562a99479e5 4101 break;
yabahiro 48:5562a99479e5 4102 case 1:
yabahiro 48:5562a99479e5 4103 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4104 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4105 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4106 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4107 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4108 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4109 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4110 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4111 mtc=true;
yabahiro 48:5562a99479e5 4112 break;
yabahiro 48:5562a99479e5 4113 case 3:
yabahiro 48:5562a99479e5 4114 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4115 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4116 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4117 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4118 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4119 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4120 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4121 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4122 mtc=true;
yabahiro 48:5562a99479e5 4123 break;
yabahiro 48:5562a99479e5 4124 case 2:
yabahiro 48:5562a99479e5 4125 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4126 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4127 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4128 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4129 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4130 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4131 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4132 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4133 mtc=true;
yabahiro 48:5562a99479e5 4134 break;
yabahiro 48:5562a99479e5 4135 }
yabahiro 48:5562a99479e5 4136 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4137 switch(g[1]) {
yabahiro 48:5562a99479e5 4138 case 0:
yabahiro 48:5562a99479e5 4139 mtc=false;
yabahiro 48:5562a99479e5 4140 break;
yabahiro 48:5562a99479e5 4141 case 255:
yabahiro 48:5562a99479e5 4142 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4143 mtc=false;
yabahiro 48:5562a99479e5 4144 break;
yabahiro 48:5562a99479e5 4145 case 253:
yabahiro 48:5562a99479e5 4146 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4147 mtc=false;
yabahiro 48:5562a99479e5 4148 break;
yabahiro 48:5562a99479e5 4149 case 254:
yabahiro 48:5562a99479e5 4150 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4151 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4152 mtc=false;
yabahiro 48:5562a99479e5 4153 break;
yabahiro 48:5562a99479e5 4154 case 1:
yabahiro 48:5562a99479e5 4155 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4156 mtc=false;
yabahiro 48:5562a99479e5 4157 break;
yabahiro 48:5562a99479e5 4158 case 3:
yabahiro 48:5562a99479e5 4159 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4160 mtc=false;
yabahiro 48:5562a99479e5 4161 break;
yabahiro 48:5562a99479e5 4162 case 2:
yabahiro 48:5562a99479e5 4163 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4164 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4165 mtc=false;
yabahiro 48:5562a99479e5 4166 break;
yabahiro 48:5562a99479e5 4167 }
yabahiro 48:5562a99479e5 4168 }
yabahiro 48:5562a99479e5 4169 }
yabahiro 48:5562a99479e5 4170 } else {
yabahiro 48:5562a99479e5 4171 if(mode==21) {
yabahiro 49:697b4e9ce884 4172 if(g[0]==98&&countss>=40000) {
yabahiro 48:5562a99479e5 4173 fast=60;
yabahiro 48:5562a99479e5 4174 normal=30;
yabahiro 48:5562a99479e5 4175 slow=25;
yabahiro 48:5562a99479e5 4176 mode=31;
yabahiro 48:5562a99479e5 4177 countss=0;
yabahiro 48:5562a99479e5 4178 }
yabahiro 48:5562a99479e5 4179 switch(g[0]) {
yabahiro 48:5562a99479e5 4180 case 98:
yabahiro 48:5562a99479e5 4181 fast=40;
yabahiro 48:5562a99479e5 4182 normal=30;
yabahiro 48:5562a99479e5 4183 slow=25;
yabahiro 48:5562a99479e5 4184 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4185 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4186 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4187 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4188 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4189 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4190 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4191 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4192 break;
yabahiro 48:5562a99479e5 4193 case 0:
yabahiro 48:5562a99479e5 4194 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4195 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4196 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4197 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4198 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4199 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4200 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4201 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4202 mtc=true;
yabahiro 48:5562a99479e5 4203 break;
yabahiro 48:5562a99479e5 4204 case 255:
yabahiro 48:5562a99479e5 4205 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4206 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4207 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4208 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4209 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4210 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4211 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4212 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4213 mtc=true;
yabahiro 48:5562a99479e5 4214 break;
yabahiro 48:5562a99479e5 4215 case 253:
yabahiro 48:5562a99479e5 4216 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4217 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4218 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4219 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4220 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4221 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4222 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4223 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4224 mtc=true;
yabahiro 48:5562a99479e5 4225 break;
yabahiro 48:5562a99479e5 4226 case 254:
yabahiro 48:5562a99479e5 4227 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4228 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4229 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4230 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4231 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4232 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4233 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4234 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4235 mtc=true;
yabahiro 48:5562a99479e5 4236 break;
yabahiro 48:5562a99479e5 4237 case 1:
yabahiro 48:5562a99479e5 4238 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4239 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4240 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4241 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4242 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4243 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4244 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4245 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4246 mtc=true;
yabahiro 48:5562a99479e5 4247 break;
yabahiro 48:5562a99479e5 4248 case 3:
yabahiro 48:5562a99479e5 4249 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4250 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4251 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4252 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4253 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4254 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4255 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4256 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4257 mtc=true;
yabahiro 48:5562a99479e5 4258 break;
yabahiro 48:5562a99479e5 4259 case 2:
yabahiro 48:5562a99479e5 4260 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4261 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4262 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4263 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4264 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4265 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4266 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4267 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4268 mtc=true;
yabahiro 48:5562a99479e5 4269 break;
yabahiro 48:5562a99479e5 4270 }
yabahiro 48:5562a99479e5 4271 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4272 switch(g[1]) {
yabahiro 48:5562a99479e5 4273 case 0:
yabahiro 48:5562a99479e5 4274 mtc=false;
yabahiro 48:5562a99479e5 4275 break;
yabahiro 48:5562a99479e5 4276 case 255:
yabahiro 48:5562a99479e5 4277 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4278 mtc=false;
yabahiro 48:5562a99479e5 4279 break;
yabahiro 48:5562a99479e5 4280 case 253:
yabahiro 48:5562a99479e5 4281 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4282 mtc=false;
yabahiro 48:5562a99479e5 4283 break;
yabahiro 48:5562a99479e5 4284 case 254:
yabahiro 48:5562a99479e5 4285 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4286 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4287 mtc=false;
yabahiro 48:5562a99479e5 4288 break;
yabahiro 48:5562a99479e5 4289 case 1:
yabahiro 48:5562a99479e5 4290 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4291 mtc=false;
yabahiro 48:5562a99479e5 4292 break;
yabahiro 48:5562a99479e5 4293 case 3:
yabahiro 48:5562a99479e5 4294 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4295 mtc=false;
yabahiro 48:5562a99479e5 4296 break;
yabahiro 48:5562a99479e5 4297 case 2:
yabahiro 48:5562a99479e5 4298 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4299 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4300 mtc=false;
yabahiro 48:5562a99479e5 4301 break;
yabahiro 48:5562a99479e5 4302 }
yabahiro 48:5562a99479e5 4303 }
yabahiro 48:5562a99479e5 4304 }
yabahiro 48:5562a99479e5 4305 }
yabahiro 48:5562a99479e5 4306
yabahiro 48:5562a99479e5 4307 /*
yabahiro 48:5562a99479e5 4308 if(mode==31) {
yabahiro 48:5562a99479e5 4309 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4310 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4311 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4312 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4313 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4314 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4315 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4316 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4317 if(countss>=5000) {
yabahiro 48:5562a99479e5 4318 countss=0;
yabahiro 48:5562a99479e5 4319 mode=33;
yabahiro 48:5562a99479e5 4320 }
yabahiro 48:5562a99479e5 4321 } else if(mode==33) {
yabahiro 48:5562a99479e5 4322 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 4323 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4324 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4325 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4326 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4327 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4328 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4329 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4330 if(g[3]==0) {
yabahiro 48:5562a99479e5 4331 cross=0;
yabahiro 48:5562a99479e5 4332 countss=0;
yabahiro 48:5562a99479e5 4333 mode=40;
yabahiro 48:5562a99479e5 4334 }
yabahiro 48:5562a99479e5 4335 } else
yabahiro 48:5562a99479e5 4336 */
yabahiro 48:5562a99479e5 4337 if(mode==31) {
yabahiro 48:5562a99479e5 4338 motor[TIRE_FR].pwm = 45;
yabahiro 48:5562a99479e5 4339 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4340 motor[TIRE_FL].pwm = 255;
yabahiro 48:5562a99479e5 4341 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4342 motor[TIRE_BR].pwm = 255;
yabahiro 48:5562a99479e5 4343 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4344 motor[TIRE_BL].pwm = 45;
yabahiro 48:5562a99479e5 4345 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4346 if(g[3]==0) {
yabahiro 48:5562a99479e5 4347 cross=0;
yabahiro 48:5562a99479e5 4348 countss=0;
yabahiro 48:5562a99479e5 4349 mode=40;
yabahiro 48:5562a99479e5 4350 }
yabahiro 48:5562a99479e5 4351 } else if(mode==40) {
yabahiro 48:5562a99479e5 4352 switch(g[3]) {
yabahiro 48:5562a99479e5 4353 case 98:
yabahiro 48:5562a99479e5 4354 motor[TIRE_FR].pwm = normal+15;
yabahiro 48:5562a99479e5 4355 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4356 motor[TIRE_FL].pwm = normal+15;
yabahiro 48:5562a99479e5 4357 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4358 motor[TIRE_BR].pwm = normal+15;
yabahiro 48:5562a99479e5 4359 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4360 motor[TIRE_BL].pwm = normal+15;
yabahiro 48:5562a99479e5 4361 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4362 if(countss>10000) {
yabahiro 48:5562a99479e5 4363 cross++;
yabahiro 48:5562a99479e5 4364 countss=0;
yabahiro 48:5562a99479e5 4365 }
yabahiro 48:5562a99479e5 4366 mtc2=true;
yabahiro 48:5562a99479e5 4367 if(cross==1) {
yabahiro 48:5562a99479e5 4368 mode=41;
yabahiro 48:5562a99479e5 4369 countss=0;
yabahiro 48:5562a99479e5 4370 cross=0;
yabahiro 48:5562a99479e5 4371 }
yabahiro 48:5562a99479e5 4372 break;
yabahiro 48:5562a99479e5 4373 case 0:
yabahiro 48:5562a99479e5 4374 motor[TIRE_FR].pwm = fast+15;
yabahiro 48:5562a99479e5 4375 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4376 motor[TIRE_FL].pwm = fast+15;
yabahiro 48:5562a99479e5 4377 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4378 motor[TIRE_BR].pwm = fast+15;
yabahiro 48:5562a99479e5 4379 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4380 motor[TIRE_BL].pwm = fast+15;
yabahiro 48:5562a99479e5 4381 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4382 mtc2=true;
yabahiro 48:5562a99479e5 4383 break;
yabahiro 48:5562a99479e5 4384 case 255:
yabahiro 48:5562a99479e5 4385 motor[TIRE_FR].pwm = slow+15;
yabahiro 48:5562a99479e5 4386 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4387 motor[TIRE_FL].pwm = fast+15;
yabahiro 48:5562a99479e5 4388 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4389 motor[TIRE_BR].pwm = fast+15;
yabahiro 48:5562a99479e5 4390 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4391 motor[TIRE_BL].pwm = slow+15;
yabahiro 48:5562a99479e5 4392 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4393 mtc2=true;
yabahiro 48:5562a99479e5 4394 break;
yabahiro 48:5562a99479e5 4395 case 253:
yabahiro 48:5562a99479e5 4396 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4397 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4398 motor[TIRE_FL].pwm = slow+15;
yabahiro 48:5562a99479e5 4399 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4400 motor[TIRE_BR].pwm = slow+15;
yabahiro 48:5562a99479e5 4401 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4402 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4403 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4404 mtc2=true;
yabahiro 48:5562a99479e5 4405 break;
yabahiro 48:5562a99479e5 4406 case 254:
yabahiro 48:5562a99479e5 4407 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4408 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4409 motor[TIRE_FL].pwm = normal+15;
yabahiro 48:5562a99479e5 4410 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4411 motor[TIRE_BR].pwm = normal+15;
yabahiro 48:5562a99479e5 4412 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4413 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4414 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4415 mtc2=true;
yabahiro 48:5562a99479e5 4416 break;
yabahiro 48:5562a99479e5 4417 case 1:
yabahiro 48:5562a99479e5 4418 motor[TIRE_FR].pwm = fast+15;
yabahiro 48:5562a99479e5 4419 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4420 motor[TIRE_FL].pwm = slow+15;
yabahiro 48:5562a99479e5 4421 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4422 motor[TIRE_BR].pwm = slow+15;
yabahiro 48:5562a99479e5 4423 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4424 motor[TIRE_BL].pwm = fast+15;
yabahiro 48:5562a99479e5 4425 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4426 mtc2=true;
yabahiro 48:5562a99479e5 4427 break;
yabahiro 48:5562a99479e5 4428 case 3:
yabahiro 48:5562a99479e5 4429 motor[TIRE_FR].pwm = slow+15;
yabahiro 48:5562a99479e5 4430 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4431 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4432 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4433 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4434 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4435 motor[TIRE_BL].pwm = slow+15;
yabahiro 48:5562a99479e5 4436 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4437 mtc2=true;
yabahiro 48:5562a99479e5 4438 break;
yabahiro 48:5562a99479e5 4439 case 2:
yabahiro 48:5562a99479e5 4440 motor[TIRE_FR].pwm = normal+15;
yabahiro 48:5562a99479e5 4441 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4442 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4443 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4444 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4445 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4446 motor[TIRE_BL].pwm = normal+15;
yabahiro 48:5562a99479e5 4447 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4448 mtc2=true;
yabahiro 48:5562a99479e5 4449 break;
yabahiro 48:5562a99479e5 4450 }
yabahiro 48:5562a99479e5 4451 if(g[2]!=98&&mtc2==true) {
yabahiro 48:5562a99479e5 4452 switch(g[2]) {
yabahiro 48:5562a99479e5 4453 case 0:
yabahiro 48:5562a99479e5 4454 mtc2=false;
yabahiro 48:5562a99479e5 4455 break;
yabahiro 48:5562a99479e5 4456 case 255:
yabahiro 48:5562a99479e5 4457 motor[TIRE_FL].pwm += 5;
yabahiro 48:5562a99479e5 4458 mtc2=false;
yabahiro 48:5562a99479e5 4459 break;
yabahiro 48:5562a99479e5 4460 case 253:
yabahiro 48:5562a99479e5 4461 motor[TIRE_FL].pwm += 10;
yabahiro 48:5562a99479e5 4462 mtc2=false;
yabahiro 48:5562a99479e5 4463 break;
yabahiro 48:5562a99479e5 4464 case 254:
yabahiro 48:5562a99479e5 4465 motor[TIRE_FL].pwm += 20;
yabahiro 48:5562a99479e5 4466 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4467 mtc2=false;
yabahiro 48:5562a99479e5 4468 break;
yabahiro 48:5562a99479e5 4469 case 1:
yabahiro 48:5562a99479e5 4470 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4471 mtc2=false;
yabahiro 48:5562a99479e5 4472 break;
yabahiro 48:5562a99479e5 4473 case 3:
yabahiro 48:5562a99479e5 4474 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4475 mtc2=false;
yabahiro 48:5562a99479e5 4476 break;
yabahiro 48:5562a99479e5 4477 case 2:
yabahiro 48:5562a99479e5 4478 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4479 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4480 mtc2=false;
yabahiro 48:5562a99479e5 4481 break;
yabahiro 48:5562a99479e5 4482 }
yabahiro 48:5562a99479e5 4483 }
yabahiro 48:5562a99479e5 4484 } else if(mode==41) {
yabahiro 48:5562a99479e5 4485 if(LIF||LIB) {
yabahiro 48:5562a99479e5 4486 mode=42;
yabahiro 48:5562a99479e5 4487 }
yabahiro 48:5562a99479e5 4488 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 4489 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4490 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4491 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4492 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4493 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4494 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4495 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4496 if(g[1]==0) {
yabahiro 48:5562a99479e5 4497 countss=0;
yabahiro 48:5562a99479e5 4498 mode=70;
yabahiro 48:5562a99479e5 4499 normal=60;
yabahiro 48:5562a99479e5 4500 fast=70;
yabahiro 48:5562a99479e5 4501 slow=40;
yabahiro 48:5562a99479e5 4502 }
yabahiro 48:5562a99479e5 4503 } else if(mode==42) {
yabahiro 48:5562a99479e5 4504 motor[TIRE_FR].pwm = 40;
yabahiro 48:5562a99479e5 4505 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 4506 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4507 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4508 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4509 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4510 motor[TIRE_BL].pwm = 40;
yabahiro 48:5562a99479e5 4511 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 4512 if(g[1]==0) {
yabahiro 48:5562a99479e5 4513 countss=0;
yabahiro 48:5562a99479e5 4514 mode=70;
yabahiro 48:5562a99479e5 4515 }
yabahiro 48:5562a99479e5 4516 } else if(mode==70) {
yabahiro 48:5562a99479e5 4517 switch(g[0]) {
yabahiro 48:5562a99479e5 4518 case 99:
yabahiro 48:5562a99479e5 4519 if(countss>=100000) {
yabahiro 48:5562a99479e5 4520 mode=100;
yabahiro 48:5562a99479e5 4521 }
yabahiro 48:5562a99479e5 4522 break;
yabahiro 48:5562a99479e5 4523 case 98:
yabahiro 48:5562a99479e5 4524 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4525 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4526 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4527 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4528 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4529 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4530 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4531 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4532 break;
yabahiro 48:5562a99479e5 4533 case 0:
yabahiro 48:5562a99479e5 4534 motor[TIRE_FR].pwm = 80;
yabahiro 48:5562a99479e5 4535 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4536 motor[TIRE_FL].pwm = 80;
yabahiro 48:5562a99479e5 4537 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4538 motor[TIRE_BR].pwm = 80;
yabahiro 48:5562a99479e5 4539 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4540 motor[TIRE_BL].pwm = 80;
yabahiro 48:5562a99479e5 4541 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4542 mtc=true;
yabahiro 48:5562a99479e5 4543 break;
yabahiro 48:5562a99479e5 4544 case 255:
yabahiro 48:5562a99479e5 4545 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4546 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4547 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4548 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4549 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4550 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4551 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4552 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4553 mtc=true;
yabahiro 48:5562a99479e5 4554 break;
yabahiro 48:5562a99479e5 4555 case 253:
yabahiro 48:5562a99479e5 4556 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4557 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4558 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4559 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4560 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4561 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4562 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4563 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4564 mtc=true;
yabahiro 48:5562a99479e5 4565 break;
yabahiro 48:5562a99479e5 4566 case 254:
yabahiro 48:5562a99479e5 4567 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4568 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4569 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4570 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4571 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4572 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4573 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4574 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4575 mtc=true;
yabahiro 48:5562a99479e5 4576 break;
yabahiro 48:5562a99479e5 4577 case 1:
yabahiro 48:5562a99479e5 4578 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4579 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4580 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4581 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4582 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4583 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4584 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4585 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4586 mtc=true;
yabahiro 48:5562a99479e5 4587 break;
yabahiro 48:5562a99479e5 4588 case 3:
yabahiro 48:5562a99479e5 4589 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4590 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4591 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4592 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4593 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4594 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4595 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4596 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4597 mtc=true;
yabahiro 48:5562a99479e5 4598 break;
yabahiro 48:5562a99479e5 4599 case 2:
yabahiro 48:5562a99479e5 4600 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4601 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4602 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4603 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4604 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4605 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4606 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4607 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4608 mtc=true;
yabahiro 48:5562a99479e5 4609 break;
yabahiro 48:5562a99479e5 4610 }
yabahiro 48:5562a99479e5 4611 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4612 switch(g[1]) {
yabahiro 48:5562a99479e5 4613 case 0:
yabahiro 48:5562a99479e5 4614 mtc=false;
yabahiro 48:5562a99479e5 4615 break;
yabahiro 48:5562a99479e5 4616 case 255:
yabahiro 48:5562a99479e5 4617 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4618 mtc=false;
yabahiro 48:5562a99479e5 4619 break;
yabahiro 48:5562a99479e5 4620 case 253:
yabahiro 48:5562a99479e5 4621 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4622 mtc=false;
yabahiro 48:5562a99479e5 4623 break;
yabahiro 48:5562a99479e5 4624 case 254:
yabahiro 48:5562a99479e5 4625 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4626 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4627 mtc=false;
yabahiro 48:5562a99479e5 4628 break;
yabahiro 48:5562a99479e5 4629 case 1:
yabahiro 48:5562a99479e5 4630 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4631 mtc=false;
yabahiro 48:5562a99479e5 4632 break;
yabahiro 48:5562a99479e5 4633 case 3:
yabahiro 48:5562a99479e5 4634 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4635 mtc=false;
yabahiro 48:5562a99479e5 4636 break;
yabahiro 48:5562a99479e5 4637 case 2:
yabahiro 48:5562a99479e5 4638 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4639 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4640 mtc=false;
yabahiro 48:5562a99479e5 4641 break;
yabahiro 48:5562a99479e5 4642 }
yabahiro 48:5562a99479e5 4643 }
yabahiro 48:5562a99479e5 4644 } else if(mode==100) {
yabahiro 48:5562a99479e5 4645 motor[TIRE_FR].pwm = 50;
yabahiro 48:5562a99479e5 4646 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4647 motor[TIRE_FL].pwm = 50;
yabahiro 48:5562a99479e5 4648 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4649 motor[TIRE_BR].pwm = 50;
yabahiro 48:5562a99479e5 4650 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4651 motor[TIRE_BL].pwm = 50;
yabahiro 48:5562a99479e5 4652 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4653 } else {
yabahiro 48:5562a99479e5 4654 }
yabahiro 33:dd598bb23ad1 4655
kishibekairohan 13:b6e02d6261d7 4656 }
kishibekairohan 13:b6e02d6261d7 4657 #endif
kishibekairohan 13:b6e02d6261d7 4658
kishibekairohan 13:b6e02d6261d7 4659 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 4660 static void Process5()
Ryosei 27:dd9f27fce7d1 4661 {
yabahiro 46:68296caae1af 4662
kishibekairohan 13:b6e02d6261d7 4663 }
kishibekairohan 13:b6e02d6261d7 4664 #endif
kishibekairohan 13:b6e02d6261d7 4665
kishibekairohan 13:b6e02d6261d7 4666 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 4667 static void Process6()
kishibekairohan 13:b6e02d6261d7 4668 {
Ryosei 27:dd9f27fce7d1 4669
kishibekairohan 13:b6e02d6261d7 4670 }
kishibekairohan 13:b6e02d6261d7 4671 #endif
kishibekairohan 13:b6e02d6261d7 4672
kishibekairohan 13:b6e02d6261d7 4673 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 4674 static void Process7()
kishibekairohan 13:b6e02d6261d7 4675 {
yabahiro 33:dd598bb23ad1 4676
kishibekairohan 12:c09b3e08a316 4677 }
kishibekairohan 12:c09b3e08a316 4678 #endif
kishibekairohan 12:c09b3e08a316 4679
Ryosei 27:dd9f27fce7d1 4680 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 4681 static void Process8()
kishibekairohan 12:c09b3e08a316 4682 {
yabahiro 33:dd598bb23ad1 4683
kishibekairohan 12:c09b3e08a316 4684 }
kishibekairohan 12:c09b3e08a316 4685 #endif
kishibekairohan 12:c09b3e08a316 4686
kishibekairohan 13:b6e02d6261d7 4687 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 4688 static void Process9()
kishibekairohan 12:c09b3e08a316 4689 {
Ryosei 27:dd9f27fce7d1 4690
t_yamamoto 0:669ef71cba68 4691 }
t_yamamoto 0:669ef71cba68 4692 #endif
t_yamamoto 0:669ef71cba68 4693 #endif
t_yamamoto 0:669ef71cba68 4694 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 4695
t_yamamoto 0:669ef71cba68 4696 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 4697 {
yabahiro 33:dd598bb23ad1 4698 #ifdef USE_SOLENOID
yabahiro 33:dd598bb23ad1 4699 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 4700 #endif
yabahiro 33:dd598bb23ad1 4701
yabahiro 33:dd598bb23ad1 4702 #ifdef USE_MOTOR
yabahiro 33:dd598bb23ad1 4703 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 33:dd598bb23ad1 4704 motor[i].dir = FREE;
yabahiro 33:dd598bb23ad1 4705 motor[i].pwm = 0;
yabahiro 33:dd598bb23ad1 4706 }
yabahiro 33:dd598bb23ad1 4707 #endif
t_yamamoto 0:669ef71cba68 4708 }
t_yamamoto 0:669ef71cba68 4709
kishibekairohan 16:3f2c2d89372b 4710 #pragma region USER-DEFINED-FUNCTIONS
M_souta 29:44d5454ce8fa 4711
M_souta 29:44d5454ce8fa 4712 uint8_t SetStatus(int pwmVal)
Ryosei 27:dd9f27fce7d1 4713 {
M_souta 29:44d5454ce8fa 4714 if (pwmVal < 0) return BACK;
M_souta 29:44d5454ce8fa 4715 else if (pwmVal > 0) return FOR;
M_souta 29:44d5454ce8fa 4716 else if (pwmVal == 0) return BRAKE;
M_souta 29:44d5454ce8fa 4717 else return BRAKE;
M_souta 22:7d93f79a3686 4718 }
kishibekairohan 16:3f2c2d89372b 4719
M_souta 29:44d5454ce8fa 4720 uint8_t SetPWM(int pwmVal)
Ryosei 27:dd9f27fce7d1 4721 {
M_souta 29:44d5454ce8fa 4722 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 29:44d5454ce8fa 4723 else return abs(pwmVal);
M_souta 22:7d93f79a3686 4724 }
kishibekairohan 16:3f2c2d89372b 4725
t_yamamoto 0:669ef71cba68 4726 #pragma endregion