大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_lhazime

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

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

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