大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@45:0cf48a0605cd, 2019-11-11 (annotated)

Committer:
kishibekairohan
Date:
Mon Nov 11 08:11:29 2019 +0000
Revision:
45:0cf48a0605cd
Parent:
44:2aff0da5ebdb
Child:
46:68296caae1af
akaka

Who changed what in which revision?

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