大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_1123

aa

Dependencies:   mbed

System/Process/Process.cpp@54:4167f10ecad0, 2019-11-17 (annotated)

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