大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@38:40e7fdf3c694, 2019-11-03 (annotated)

Committer:
yabahiro
Date:
Sun Nov 03 05:56:53 2019 +0000
Revision:
38:40e7fdf3c694
Parent:
37:e77816810627
Child:
39:784552c26163
aa;

Who changed what in which revision?

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