大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_old

aa

Dependencies:   mbed

System/Process/Process.cpp@41:99a1158f3eca, 2019-11-08 (annotated)

Committer:
kishibekairohan
Date:
Fri Nov 08 13:57:55 2019 +0000
Revision:
41:99a1158f3eca
Parent:
40:ed2409e0ce6b
Child:
42:980af34f6fe3
uwa

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