大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes

aa

Dependencies:   mbed

System/Process/Process.cpp@60:24ebb611f26e, 2019-12-12 (annotated)

Committer:
yabahiro
Date:
Thu Dec 12 07:16:20 2019 +0000
Revision:
60:24ebb611f26e
Parent:
59:5cd3875fa96a 
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
Ryosei 27:dd9f27fce7d1 3 #include <stdlib.h>
7ka884 4:ba9df71868df 4 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 5 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 6 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
M_souta 30:8f092276b2ba 8 #include "../../Switch/Switch.h"
Ryosei 27:dd9f27fce7d1 9
t_yamamoto 0:669ef71cba68 10 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 12 #include "../Using.h"
t_yamamoto 0:669ef71cba68 13
t_yamamoto 0:669ef71cba68 14 using namespace SWITCH;
7ka884 4:ba9df71868df 15 using namespace PID_SPACE;
Ryosei 27:dd9f27fce7d1 16
M_souta 21:e3b58d675c1c 17 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 18
t_yamamoto 0:669ef71cba68 19 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 20 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 21 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static bool lock;
t_yamamoto 0:669ef71cba68 24 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 25 static int current;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 30 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 31 #endif
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 36
kishibekairohan 2:c015739085d3 37
M_souta 30:8f092276b2ba 38 //Serial pc(USBTX, USBRX);
kishibekairohan 16:3f2c2d89372b 39
Ryosei 27:dd9f27fce7d1 40 const int omni[15][15] = {
Ryosei 27:dd9f27fce7d1 41 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 42 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 43 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 44 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei 27:dd9f27fce7d1 45 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei 27:dd9f27fce7d1 46 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei 27:dd9f27fce7d1 47 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei 27:dd9f27fce7d1 48 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei 27:dd9f27fce7d1 49 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei 27:dd9f27fce7d1 50 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei 27:dd9f27fce7d1 51 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei 27:dd9f27fce7d1 52 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei 27:dd9f27fce7d1 53 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei 27:dd9f27fce7d1 54 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei 27:dd9f27fce7d1 55 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 56 };
M_souta 21:e3b58d675c1c 57
M_souta 21:e3b58d675c1c 58 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 59
kishibekairohan 42:980af34f6fe3 60 uint8_t SetStatus(int pwmVal);
kishibekairohan 42:980af34f6fe3 61 uint8_t SetPWM(int pwmVal);
yabahiro 33:dd598bb23ad1 62
kishibekairohan 35:33c6cdba5630 63 DigitalIn EMC(PC_13); //非常停止確認用
yabahiro 33:dd598bb23ad1 64
yabahiro 33:dd598bb23ad1 65 bool start_flag = true;
yabahiro 33:dd598bb23ad1 66 bool UP_flag = false;
yabahiro 33:dd598bb23ad1 67 bool SW_flag = false;
yabahiro 33:dd598bb23ad1 68 bool Air_flag = false;
yabahiro 33:dd598bb23ad1 69 bool zyouge;
yabahiro 33:dd598bb23ad1 70 bool zyougedo;
yabahiro 33:dd598bb23ad1 71 bool dz2=true;
yabahiro 33:dd598bb23ad1 72 bool dz2i=false;
yabahiro 33:dd598bb23ad1 73 bool dz3=true;
yabahiro 33:dd598bb23ad1 74 bool dz3i=false;
yabahiro 33:dd598bb23ad1 75 bool dz4=true;
yabahiro 33:dd598bb23ad1 76 bool dz4i=false;
yabahiro 33:dd598bb23ad1 77 bool zone;
yabahiro 33:dd598bb23ad1 78 bool through=false;
yabahiro 33:dd598bb23ad1 79 bool counts=false;
yabahiro 33:dd598bb23ad1 80 bool mtc=false;
yabahiro 33:dd598bb23ad1 81 bool mtc2 = false;
yabahiro 33:dd598bb23ad1 82 bool emcs=false;
yabahiro 33:dd598bb23ad1 83 bool kiri;
yabahiro 33:dd598bb23ad1 84
yabahiro 33:dd598bb23ad1 85 int g[8];
yabahiro 33:dd598bb23ad1 86 int Twsh;
yabahiro 38:40e7fdf3c694 87 int testmode =0;
yabahiro 33:dd598bb23ad1 88 int mode =0;
yabahiro 33:dd598bb23ad1 89 int cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 90 int cros=0;
yabahiro 33:dd598bb23ad1 91 int countss=100000;//wait代替え
yabahiro 55:844ac0974740 92 int num=0;
yabahiro 33:dd598bb23ad1 93
yabahiro 33:dd598bb23ad1 94 ///*********PWM調整用*********///
kishibekairohan 41:99a1158f3eca 95
kishibekairohan 42:980af34f6fe3 96 int fast = 60;
yabahiro 33:dd598bb23ad1 97 int normal = 40;
yabahiro 46:68296caae1af 98 int slow = 25;
yabahiro 33:dd598bb23ad1 99
yabahiro 55:844ac0974740 100 /*
yabahiro 55:844ac0974740 101 void Count();
yabahiro 55:844ac0974740 102 Ticker bunzin;
yabahiro 55:844ac0974740 103 */
yabahiro 55:844ac0974740 104
yabahiro 56:61ebd4b759b4 105 Timer aime;
yabahiro 56:61ebd4b759b4 106 int value;
yabahiro 55:844ac0974740 107
t_yamamoto 0:669ef71cba68 108 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 109
t_yamamoto 0:669ef71cba68 110 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 111 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 112 static void Process0(void);
t_yamamoto 0:669ef71cba68 113 #endif
t_yamamoto 0:669ef71cba68 114 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 115 static void Process1(void);
t_yamamoto 0:669ef71cba68 116 #endif
t_yamamoto 0:669ef71cba68 117 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 118 static void Process2(void);
t_yamamoto 0:669ef71cba68 119 #endif
t_yamamoto 0:669ef71cba68 120 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 121 static void Process3(void);
t_yamamoto 0:669ef71cba68 122 #endif
t_yamamoto 0:669ef71cba68 123 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 124 static void Process4(void);
t_yamamoto 0:669ef71cba68 125 #endif
t_yamamoto 0:669ef71cba68 126 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 127 static void Process5(void);
t_yamamoto 0:669ef71cba68 128 #endif
t_yamamoto 0:669ef71cba68 129 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 130 static void Process6(void);
t_yamamoto 0:669ef71cba68 131 #endif
t_yamamoto 0:669ef71cba68 132 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 133 static void Process7(void);
t_yamamoto 0:669ef71cba68 134 #endif
t_yamamoto 0:669ef71cba68 135 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 136 static void Process8(void);
t_yamamoto 0:669ef71cba68 137 #endif
t_yamamoto 0:669ef71cba68 138 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 139 static void Process9(void);
t_yamamoto 0:669ef71cba68 140 #endif
t_yamamoto 0:669ef71cba68 141 #endif
t_yamamoto 0:669ef71cba68 142
t_yamamoto 0:669ef71cba68 143 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 144 {
Ryosei 27:dd9f27fce7d1 145 #pragma region USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 146 /*Replace here with the initialization code of your variables.*/
yabahiro 55:844ac0974740 147 //bunzin.attach(Count,0.001);
Ryosei 27:dd9f27fce7d1 148
Ryosei 27:dd9f27fce7d1 149 #pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 150
Ryosei 27:dd9f27fce7d1 151 lock = true;
Ryosei 27:dd9f27fce7d1 152 processChangeComp = true;
Ryosei 27:dd9f27fce7d1 153 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 154
Ryosei 27:dd9f27fce7d1 155 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 156 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 157 Process[0] = Process0;
Ryosei 27:dd9f27fce7d1 158 #endif
Ryosei 27:dd9f27fce7d1 159 #if USE_PROCESS_NUM>1
Ryosei 27:dd9f27fce7d1 160 Process[1] = Process1;
Ryosei 27:dd9f27fce7d1 161 #endif
Ryosei 27:dd9f27fce7d1 162 #if USE_PROCESS_NUM>2
Ryosei 27:dd9f27fce7d1 163 Process[2] = Process2;
Ryosei 27:dd9f27fce7d1 164 #endif
Ryosei 27:dd9f27fce7d1 165 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 166 Process[3] = Process3;
Ryosei 27:dd9f27fce7d1 167 #endif
Ryosei 27:dd9f27fce7d1 168 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 169 Process[4] = Process4;
Ryosei 27:dd9f27fce7d1 170 #endif
Ryosei 27:dd9f27fce7d1 171 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 172 Process[5] = Process5;
Ryosei 27:dd9f27fce7d1 173 #endif
Ryosei 27:dd9f27fce7d1 174 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 175 Process[6] = Process6;
Ryosei 27:dd9f27fce7d1 176 #endif
Ryosei 27:dd9f27fce7d1 177 #if USE_PROCESS_NUM>7
Ryosei 27:dd9f27fce7d1 178 Process[7] = Process7;
Ryosei 27:dd9f27fce7d1 179 #endif
Ryosei 27:dd9f27fce7d1 180 #if USE_PROCESS_NUM>8
Ryosei 27:dd9f27fce7d1 181 Process[8] = Process8;
Ryosei 27:dd9f27fce7d1 182 #endif
Ryosei 27:dd9f27fce7d1 183 #if USE_PROCESS_NUM>9
Ryosei 27:dd9f27fce7d1 184 Process[9] = Process9;
Ryosei 27:dd9f27fce7d1 185 #endif
Ryosei 27:dd9f27fce7d1 186 #endif
t_yamamoto 0:669ef71cba68 187 }
t_yamamoto 0:669ef71cba68 188
t_yamamoto 0:669ef71cba68 189 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 190 {
Ryosei 27:dd9f27fce7d1 191 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 192 if(controller->Button.HOME) lock = false;
Ryosei 27:dd9f27fce7d1 193
Ryosei 27:dd9f27fce7d1 194 if(controller->Button.START && processChangeComp) {
Ryosei 27:dd9f27fce7d1 195 current++;
Ryosei 27:dd9f27fce7d1 196 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei 27:dd9f27fce7d1 197 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 198 } else if(controller->Button.SELECT && processChangeComp) {
Ryosei 27:dd9f27fce7d1 199 current--;
Ryosei 27:dd9f27fce7d1 200 if (current < 0) current = 0;
Ryosei 27:dd9f27fce7d1 201 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 202 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei 27:dd9f27fce7d1 203 #endif
Ryosei 27:dd9f27fce7d1 204
Ryosei 27:dd9f27fce7d1 205 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 206 ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei 27:dd9f27fce7d1 207 #endif
Ryosei 27:dd9f27fce7d1 208
Ryosei 27:dd9f27fce7d1 209 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 210 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei 27:dd9f27fce7d1 211 #endif
Ryosei 27:dd9f27fce7d1 212
Ryosei 27:dd9f27fce7d1 213 #ifdef USE_RS485
Ryosei 27:dd9f27fce7d1 214 ACTUATORHUB::ActuatorHub::Update();
Ryosei 27:dd9f27fce7d1 215 //LINEHUB::LineHub::Update();
Ryosei 27:dd9f27fce7d1 216 #endif
Ryosei 27:dd9f27fce7d1 217
Ryosei 27:dd9f27fce7d1 218 }
Ryosei 27:dd9f27fce7d1 219
yabahiro 39:784552c26163 220 int point1;
Ryosei 27:dd9f27fce7d1 221 void SystemProcess()
Ryosei 27:dd9f27fce7d1 222 {
yabahiro 56:61ebd4b759b4 223
Ryosei 27:dd9f27fce7d1 224 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 225
Ryosei 27:dd9f27fce7d1 226 while(1) {
yabahiro 53:694dbd7dfa0e 227 //pc.printf("3:%d\n\r",g[3]);
yabahiro 46:68296caae1af 228
yabahiro 33:dd598bb23ad1 229 if(EMC==0) {
yabahiro 33:dd598bb23ad1 230 fast=40;
yabahiro 33:dd598bb23ad1 231 normal=30;
yabahiro 33:dd598bb23ad1 232 slow=20;
yabahiro 33:dd598bb23ad1 233 cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 234 cros=0;
yabahiro 33:dd598bb23ad1 235 countss=0;//wait代替え
yabahiro 33:dd598bb23ad1 236 UP_flag = false;
yabahiro 33:dd598bb23ad1 237 SW_flag = false;
yabahiro 33:dd598bb23ad1 238 Air_flag = false;
yabahiro 33:dd598bb23ad1 239 zyouge=false;
yabahiro 33:dd598bb23ad1 240 zyougedo=false;
yabahiro 33:dd598bb23ad1 241 dz2=true;
yabahiro 33:dd598bb23ad1 242 dz2i=false;
yabahiro 33:dd598bb23ad1 243 dz3=true;
yabahiro 33:dd598bb23ad1 244 dz3i=false;
yabahiro 33:dd598bb23ad1 245 dz4=true;
yabahiro 33:dd598bb23ad1 246 dz4i=false;
yabahiro 33:dd598bb23ad1 247 through=false;
yabahiro 33:dd598bb23ad1 248 counts=false;
yabahiro 33:dd598bb23ad1 249 mtc=false;
yabahiro 33:dd598bb23ad1 250 mtc2 = false;
yabahiro 33:dd598bb23ad1 251 current = 0;
yabahiro 33:dd598bb23ad1 252 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 253 if(emcs==false) {
yabahiro 33:dd598bb23ad1 254 AllActuatorReset();
yabahiro 33:dd598bb23ad1 255 emcs=true;
yabahiro 33:dd598bb23ad1 256 }
yabahiro 33:dd598bb23ad1 257 } else {
yabahiro 33:dd598bb23ad1 258 emcs=false;
yabahiro 33:dd598bb23ad1 259 }
Ryosei 27:dd9f27fce7d1 260
yabahiro 33:dd598bb23ad1 261 if(mode==0) {
yabahiro 33:dd598bb23ad1 262 if(ZoneSW) {
kishibekairohan 35:33c6cdba5630 263 LED::LED::TapeledMode(1);
yabahiro 33:dd598bb23ad1 264 } else {
kishibekairohan 35:33c6cdba5630 265 LED::LED::TapeledMode(2);
yabahiro 33:dd598bb23ad1 266 }
yabahiro 33:dd598bb23ad1 267 }
yabahiro 33:dd598bb23ad1 268 for(int i = 0; i < 8; i++) {
yabahiro 33:dd598bb23ad1 269 g[i] = LineHub::GetPara(i);
yabahiro 33:dd598bb23ad1 270 }
yabahiro 33:dd598bb23ad1 271 if(ThSW) {
yabahiro 33:dd598bb23ad1 272 Twsh=1;
yabahiro 33:dd598bb23ad1 273 } else {
yabahiro 33:dd598bb23ad1 274 Twsh=2;
yabahiro 33:dd598bb23ad1 275 }
yabahiro 33:dd598bb23ad1 276
yabahiro 33:dd598bb23ad1 277 if(StertSW && start_flag == true) {
yabahiro 33:dd598bb23ad1 278 start_flag = false;
yabahiro 33:dd598bb23ad1 279 lock = false;
yabahiro 33:dd598bb23ad1 280 if(ZoneSW) {
yabahiro 33:dd598bb23ad1 281 countss=0;
yabahiro 33:dd598bb23ad1 282 mode=1;
yabahiro 58:bfc38c43dadb 283 aime.start();
yabahiro 60:24ebb611f26e 284 current = 6;//1124変更した
yabahiro 33:dd598bb23ad1 285
yabahiro 33:dd598bb23ad1 286 } else {
kishibekairohan 34:2fce61e3ebc4 287 countss = 0;
yabahiro 33:dd598bb23ad1 288 mode=1;
yabahiro 58:bfc38c43dadb 289 aime.start();
yabahiro 59:5cd3875fa96a 290 current = 2;//2から6
yabahiro 33:dd598bb23ad1 291 }
yabahiro 33:dd598bb23ad1 292 }
yabahiro 33:dd598bb23ad1 293 if(DWSW||DOSW) {
yabahiro 33:dd598bb23ad1 294 if(DWSW) {
yabahiro 33:dd598bb23ad1 295 zyouge=true;
yabahiro 33:dd598bb23ad1 296 }
yabahiro 33:dd598bb23ad1 297 if(DOSW) {
yabahiro 33:dd598bb23ad1 298 if(DOLS) {
yabahiro 33:dd598bb23ad1 299 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 300 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 301 } else {
yabahiro 38:40e7fdf3c694 302 motor[MOTOR_0].pwm = 200;
yabahiro 33:dd598bb23ad1 303 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 304 }
yabahiro 33:dd598bb23ad1 305 }
yabahiro 33:dd598bb23ad1 306 } else if(zyouge==false) {
yabahiro 33:dd598bb23ad1 307 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 308 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 309 }
yabahiro 33:dd598bb23ad1 310 if(zyouge) {
yabahiro 33:dd598bb23ad1 311 if(Twsh==1) {
yabahiro 38:40e7fdf3c694 312 motor[MOTOR_0].pwm = 255;
yabahiro 33:dd598bb23ad1 313 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 314 if(UPLS) {
yabahiro 33:dd598bb23ad1 315 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 316 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 317 zyouge=false;
yabahiro 33:dd598bb23ad1 318 }
yabahiro 33:dd598bb23ad1 319 }
yabahiro 33:dd598bb23ad1 320 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 321 motor[MOTOR_0].pwm = 245;
yabahiro 33:dd598bb23ad1 322 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 323 if(UPLS2) {
yabahiro 33:dd598bb23ad1 324 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 325 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 326 zyouge=false;
yabahiro 33:dd598bb23ad1 327 }
yabahiro 33:dd598bb23ad1 328 }
yabahiro 33:dd598bb23ad1 329 }
yabahiro 33:dd598bb23ad1 330 if(AIRSW) {
yabahiro 33:dd598bb23ad1 331 if(SW_flag==false) {
yabahiro 33:dd598bb23ad1 332 if(Air_flag==false) {
yabahiro 33:dd598bb23ad1 333 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 334 Air_flag=true;
yabahiro 33:dd598bb23ad1 335 SW_flag=true;
yabahiro 33:dd598bb23ad1 336 } else {
yabahiro 33:dd598bb23ad1 337 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 338 Air_flag=false;
yabahiro 33:dd598bb23ad1 339 SW_flag=true;
yabahiro 33:dd598bb23ad1 340 }
yabahiro 33:dd598bb23ad1 341 }
yabahiro 33:dd598bb23ad1 342 } else {
yabahiro 33:dd598bb23ad1 343 if(SW_flag==true) {
yabahiro 33:dd598bb23ad1 344 countss++;
yabahiro 33:dd598bb23ad1 345 if(countss>=2000) {
yabahiro 33:dd598bb23ad1 346 SW_flag=false;
yabahiro 33:dd598bb23ad1 347 countss=0;
yabahiro 33:dd598bb23ad1 348 }
yabahiro 33:dd598bb23ad1 349 }
yabahiro 33:dd598bb23ad1 350 }
yabahiro 33:dd598bb23ad1 351
yabahiro 33:dd598bb23ad1 352
yabahiro 33:dd598bb23ad1 353 #ifdef USE_MU
Ryosei 27:dd9f27fce7d1 354 controller = CONTROLLER::Controller::GetData();
yabahiro 33:dd598bb23ad1 355 #endif
yabahiro 33:dd598bb23ad1 356
Ryosei 27:dd9f27fce7d1 357
yabahiro 33:dd598bb23ad1 358 #ifdef USE_ERRORCHECK
yabahiro 38:40e7fdf3c694 359 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
Ryosei 27:dd9f27fce7d1 360 CONTROLLER::Controller::DataReset();
Ryosei 27:dd9f27fce7d1 361 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 362 lock = true;
Ryosei 27:dd9f27fce7d1 363 } else
yabahiro 38:40e7fdf3c694 364
yabahiro 33:dd598bb23ad1 365 #endif
Ryosei 27:dd9f27fce7d1 366 {
Ryosei 27:dd9f27fce7d1 367
yabahiro 33:dd598bb23ad1 368 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 369 if(!lock) {
Ryosei 27:dd9f27fce7d1 370 Process[current]();
Ryosei 27:dd9f27fce7d1 371 } else
yabahiro 33:dd598bb23ad1 372 #endif
Ryosei 27:dd9f27fce7d1 373 {
Ryosei 27:dd9f27fce7d1 374 //ロック時の処理
Ryosei 27:dd9f27fce7d1 375 }
Ryosei 27:dd9f27fce7d1 376 }
yabahiro 33:dd598bb23ad1 377
Ryosei 27:dd9f27fce7d1 378 SystemProcessUpdate();
Ryosei 27:dd9f27fce7d1 379 }
t_yamamoto 0:669ef71cba68 380 }
t_yamamoto 0:669ef71cba68 381
kishibekairohan 2:c015739085d3 382
kishibekairohan 2:c015739085d3 383
kishibekairohan 2:c015739085d3 384
t_yamamoto 0:669ef71cba68 385 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 386 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 387 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 388 static void Process0()
Ryosei 27:dd9f27fce7d1 389 {
yabahiro 33:dd598bb23ad1 390
yabahiro 33:dd598bb23ad1 391 //AllActuatorReset();
t_yamamoto 0:669ef71cba68 392 }
t_yamamoto 0:669ef71cba68 393 #endif
t_yamamoto 0:669ef71cba68 394
t_yamamoto 0:669ef71cba68 395 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 396 static void Process1()
t_yamamoto 0:669ef71cba68 397 {
kishibekairohan 35:33c6cdba5630 398 LED::LED::TapeledMode(6);
yabahiro 33:dd598bb23ad1 399 start_flag = true;
yabahiro 33:dd598bb23ad1 400 mtc=false;
yabahiro 33:dd598bb23ad1 401 mtc2 = false;
yabahiro 33:dd598bb23ad1 402 mode=1;
yabahiro 33:dd598bb23ad1 403 cross=0;
yabahiro 33:dd598bb23ad1 404 Twsh=1;
yabahiro 33:dd598bb23ad1 405 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 406 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 407 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 408 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 409
kishibekairohan 42:980af34f6fe3 410 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 411 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 412 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
kishibekairohan 42:980af34f6fe3 413 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 414
yabahiro 33:dd598bb23ad1 415 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 416
yabahiro 33:dd598bb23ad1 417 if(controller->Button.UP) {
yabahiro 33:dd598bb23ad1 418 motor[MOTOR_0].pwm =200;
yabahiro 33:dd598bb23ad1 419 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 420 if(UPLS) {
yabahiro 33:dd598bb23ad1 421 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 422 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 423 }
yabahiro 33:dd598bb23ad1 424 }
yabahiro 33:dd598bb23ad1 425 if(controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 426 motor[MOTOR_0].pwm = 50;
yabahiro 33:dd598bb23ad1 427 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 428 }
yabahiro 33:dd598bb23ad1 429 } else {
yabahiro 33:dd598bb23ad1 430 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 431 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 432 }
yabahiro 33:dd598bb23ad1 433
yabahiro 33:dd598bb23ad1 434 if(controller->Button.Y) {
yabahiro 33:dd598bb23ad1 435 if(dz2==true) {
yabahiro 33:dd598bb23ad1 436 if(dz2i==false) {
yabahiro 33:dd598bb23ad1 437 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 438 dz2i=true;
yabahiro 33:dd598bb23ad1 439 } else {
yabahiro 33:dd598bb23ad1 440 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 441 dz2i=false;
yabahiro 33:dd598bb23ad1 442 }
yabahiro 33:dd598bb23ad1 443 dz2=false;
yabahiro 33:dd598bb23ad1 444 }
yabahiro 33:dd598bb23ad1 445 } else {
yabahiro 33:dd598bb23ad1 446 dz2=true;
yabahiro 33:dd598bb23ad1 447 }
yabahiro 33:dd598bb23ad1 448
yabahiro 33:dd598bb23ad1 449 if(controller->Button.A) {
yabahiro 33:dd598bb23ad1 450 if(dz3==true) {
yabahiro 33:dd598bb23ad1 451 if(dz3i==false) {
kishibekairohan 37:e77816810627 452 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 453 dz3i=true;
yabahiro 33:dd598bb23ad1 454 } else {
yabahiro 33:dd598bb23ad1 455 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 456 dz3i=false;
yabahiro 33:dd598bb23ad1 457 }
yabahiro 33:dd598bb23ad1 458 dz3=false;
yabahiro 33:dd598bb23ad1 459 }
yabahiro 33:dd598bb23ad1 460 } else {
yabahiro 33:dd598bb23ad1 461 dz3=true;
yabahiro 33:dd598bb23ad1 462 }
yabahiro 33:dd598bb23ad1 463
yabahiro 33:dd598bb23ad1 464 if(controller->Button.B) {
yabahiro 33:dd598bb23ad1 465 if(dz4==true) {
yabahiro 33:dd598bb23ad1 466 if(dz4i==false) {
yabahiro 38:40e7fdf3c694 467 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 468 dz4i=true;
yabahiro 33:dd598bb23ad1 469 } else {
yabahiro 38:40e7fdf3c694 470 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 471 dz4i=false;
yabahiro 33:dd598bb23ad1 472 }
yabahiro 33:dd598bb23ad1 473 dz4=false;
yabahiro 33:dd598bb23ad1 474 }
yabahiro 33:dd598bb23ad1 475 } else {
yabahiro 33:dd598bb23ad1 476 dz4=true;
yabahiro 33:dd598bb23ad1 477 }
t_yamamoto 0:669ef71cba68 478 }
t_yamamoto 0:669ef71cba68 479 #endif
t_yamamoto 0:669ef71cba68 480
t_yamamoto 0:669ef71cba68 481 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 482 static void Process2()
Ryosei 27:dd9f27fce7d1 483 {
yabahiro 33:dd598bb23ad1 484 //Oh So 青ゾーン
yabahiro 58:bfc38c43dadb 485 value=aime.read_ms();
yabahiro 53:694dbd7dfa0e 486 LED::LED::TapeledMode(4);
yabahiro 33:dd598bb23ad1 487 countss++;
yabahiro 50:87f5fa1bc460 488 switch(mode) {
yabahiro 50:87f5fa1bc460 489 case 1:
yabahiro 51:a0bdbddd08f7 490 fast=40;
yabahiro 51:a0bdbddd08f7 491 normal=30;
yabahiro 51:a0bdbddd08f7 492 slow=20;
yabahiro 51:a0bdbddd08f7 493 motor[TIRE_FR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 494 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 495 motor[TIRE_FL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 496 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 497 motor[TIRE_BR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 498 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 499 motor[TIRE_BL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 500 motor[TIRE_BL].dir = BACK;
yabahiro 58:bfc38c43dadb 501 if(g[2]==0&&value>=1500) {
yabahiro 51:a0bdbddd08f7 502 countss=100000;
yabahiro 58:bfc38c43dadb 503 aime.reset();
yabahiro 51:a0bdbddd08f7 504 cross=0;
yabahiro 51:a0bdbddd08f7 505 mode=3;
yabahiro 51:a0bdbddd08f7 506 }
yabahiro 50:87f5fa1bc460 507 break;
yabahiro 51:a0bdbddd08f7 508 case 3:
yabahiro 51:a0bdbddd08f7 509 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 510 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 511 motor[TIRE_FL].pwm = 255;
yabahiro 51:a0bdbddd08f7 512 motor[TIRE_FL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 513 motor[TIRE_BR].pwm = 255;
yabahiro 51:a0bdbddd08f7 514 motor[TIRE_BR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 515 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 516 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 517 if(g[1]==0) {
yabahiro 51:a0bdbddd08f7 518 mode=10;
yabahiro 58:bfc38c43dadb 519 aime.reset();
yabahiro 51:a0bdbddd08f7 520 countss=100000;
yabahiro 55:844ac0974740 521 fast=80;
yabahiro 55:844ac0974740 522 normal=60;
yabahiro 55:844ac0974740 523 slow=40;
yabahiro 51:a0bdbddd08f7 524 }
yabahiro 50:87f5fa1bc460 525 break;
yabahiro 50:87f5fa1bc460 526 case 10:
yabahiro 51:a0bdbddd08f7 527 switch(g[1]) {
yabahiro 51:a0bdbddd08f7 528 case 98:
yabahiro 51:a0bdbddd08f7 529 motor[TIRE_FR].pwm = normal+55;
yabahiro 51:a0bdbddd08f7 530 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 531 motor[TIRE_FL].pwm = normal+50;
yabahiro 51:a0bdbddd08f7 532 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 533 motor[TIRE_BR].pwm = normal+50;
yabahiro 51:a0bdbddd08f7 534 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 535 motor[TIRE_BL].pwm = normal+45;
yabahiro 51:a0bdbddd08f7 536 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 537 mtc=true;
yabahiro 58:bfc38c43dadb 538 if(value>=1200) {
yabahiro 51:a0bdbddd08f7 539 cross++;
yabahiro 51:a0bdbddd08f7 540 countss=0;
yabahiro 58:bfc38c43dadb 541 value=0;
yabahiro 58:bfc38c43dadb 542 aime.reset();
yabahiro 51:a0bdbddd08f7 543 }
yabahiro 46:68296caae1af 544
yabahiro 51:a0bdbddd08f7 545 if(cross==2) {
yabahiro 51:a0bdbddd08f7 546 fast = 10;
yabahiro 51:a0bdbddd08f7 547 normal = 10;
yabahiro 51:a0bdbddd08f7 548 slow = 5;
yabahiro 51:a0bdbddd08f7 549 }
yabahiro 46:68296caae1af 550
kishibekairohan 45:0cf48a0605cd 551 if(cross==3) {
yabahiro 51:a0bdbddd08f7 552 fast=40;
yabahiro 51:a0bdbddd08f7 553 normal=30;
yabahiro 51:a0bdbddd08f7 554 slow=20;
yabahiro 51:a0bdbddd08f7 555 mode=11;
yabahiro 58:bfc38c43dadb 556 aime.reset();
yabahiro 39:784552c26163 557 countss=0;
yabahiro 39:784552c26163 558 cross=0;
yabahiro 39:784552c26163 559 }
yabahiro 39:784552c26163 560 break;
yabahiro 39:784552c26163 561 case 0:
yabahiro 51:a0bdbddd08f7 562 motor[TIRE_FR].pwm = fast+55;
yabahiro 51:a0bdbddd08f7 563 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 564 motor[TIRE_FL].pwm = fast+50;
yabahiro 51:a0bdbddd08f7 565 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 566 motor[TIRE_BR].pwm = fast+50;
yabahiro 51:a0bdbddd08f7 567 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 568 motor[TIRE_BL].pwm = fast+45;
yabahiro 51:a0bdbddd08f7 569 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 570 mtc=true;
yabahiro 39:784552c26163 571 break;
yabahiro 39:784552c26163 572 case 255:
yabahiro 51:a0bdbddd08f7 573 motor[TIRE_FR].pwm = fast+55;
yabahiro 51:a0bdbddd08f7 574 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 575 motor[TIRE_FL].pwm = slow+50;
yabahiro 51:a0bdbddd08f7 576 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 577 motor[TIRE_BR].pwm = slow+50;
yabahiro 51:a0bdbddd08f7 578 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 579 motor[TIRE_BL].pwm = fast+45;
yabahiro 51:a0bdbddd08f7 580 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 581 mtc=true;
yabahiro 39:784552c26163 582 break;
yabahiro 39:784552c26163 583 case 253:
yabahiro 51:a0bdbddd08f7 584 motor[TIRE_FR].pwm = slow+55;
yabahiro 51:a0bdbddd08f7 585 motor[TIRE_FR].dir = FOR;
yabahiro 39:784552c26163 586 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 587 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 588 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 589 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 590 motor[TIRE_BL].pwm = slow+45;
yabahiro 51:a0bdbddd08f7 591 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 592 mtc=true;
yabahiro 39:784552c26163 593 break;
yabahiro 39:784552c26163 594 case 254:
yabahiro 51:a0bdbddd08f7 595 motor[TIRE_FR].pwm = normal+55;
yabahiro 51:a0bdbddd08f7 596 motor[TIRE_FR].dir = FOR;
yabahiro 39:784552c26163 597 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 598 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 599 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 600 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 601 motor[TIRE_BL].pwm = normal+45;
yabahiro 51:a0bdbddd08f7 602 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 603 mtc=true;
yabahiro 39:784552c26163 604 break;
yabahiro 39:784552c26163 605 case 1:
yabahiro 51:a0bdbddd08f7 606 motor[TIRE_FR].pwm = slow+55;
yabahiro 51:a0bdbddd08f7 607 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 608 motor[TIRE_FL].pwm = fast+50;
yabahiro 51:a0bdbddd08f7 609 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 610 motor[TIRE_BR].pwm = fast+50;
yabahiro 51:a0bdbddd08f7 611 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 612 motor[TIRE_BL].pwm = slow+45;
yabahiro 51:a0bdbddd08f7 613 motor[TIRE_BL].dir = BACK;
yabahiro 39:784552c26163 614 mtc=true;
yabahiro 39:784552c26163 615 break;
yabahiro 39:784552c26163 616 case 3:
yabahiro 39:784552c26163 617 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 618 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 619 motor[TIRE_FL].pwm = slow+50;
yabahiro 51:a0bdbddd08f7 620 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 621 motor[TIRE_BR].pwm = slow+50;
yabahiro 51:a0bdbddd08f7 622 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 623 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 624 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 625 mtc=true;
yabahiro 39:784552c26163 626 break;
yabahiro 39:784552c26163 627 case 2:
yabahiro 39:784552c26163 628 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 629 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 630 motor[TIRE_FL].pwm = normal+50;
yabahiro 51:a0bdbddd08f7 631 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 632 motor[TIRE_BR].pwm = normal+50;
yabahiro 51:a0bdbddd08f7 633 motor[TIRE_BR].dir = FOR;
yabahiro 39:784552c26163 634 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 635 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 636 mtc=true;
yabahiro 39:784552c26163 637 break;
yabahiro 39:784552c26163 638 }
yabahiro 51:a0bdbddd08f7 639 if(g[1]!=98&&mtc==true) {
yabahiro 39:784552c26163 640 switch(g[0]) {
yabahiro 39:784552c26163 641 case 0:
yabahiro 39:784552c26163 642 mtc=false;
yabahiro 39:784552c26163 643 break;
yabahiro 39:784552c26163 644 case 255:
yabahiro 51:a0bdbddd08f7 645 motor[TIRE_FL].pwm += 3;
yabahiro 39:784552c26163 646 mtc=false;
yabahiro 39:784552c26163 647 break;
yabahiro 39:784552c26163 648 case 253:
yabahiro 51:a0bdbddd08f7 649 motor[TIRE_FL].pwm += 6;
yabahiro 39:784552c26163 650 mtc=false;
yabahiro 39:784552c26163 651 break;
yabahiro 39:784552c26163 652 case 254:
yabahiro 51:a0bdbddd08f7 653 motor[TIRE_FL].pwm += 9;
yabahiro 51:a0bdbddd08f7 654 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 655 mtc=false;
yabahiro 39:784552c26163 656 break;
yabahiro 39:784552c26163 657 case 1:
yabahiro 51:a0bdbddd08f7 658 motor[TIRE_FR].pwm += 3;
yabahiro 39:784552c26163 659 mtc=false;
yabahiro 39:784552c26163 660 break;
yabahiro 39:784552c26163 661 case 3:
yabahiro 51:a0bdbddd08f7 662 motor[TIRE_FR].pwm += 6;
yabahiro 39:784552c26163 663 mtc=false;
yabahiro 39:784552c26163 664 break;
yabahiro 39:784552c26163 665 case 2:
yabahiro 51:a0bdbddd08f7 666 motor[TIRE_FR].pwm += 9;
yabahiro 51:a0bdbddd08f7 667 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 668 mtc=false;
yabahiro 39:784552c26163 669 break;
yabahiro 39:784552c26163 670 }
yabahiro 39:784552c26163 671 }
yabahiro 51:a0bdbddd08f7 672 break;
yabahiro 51:a0bdbddd08f7 673 case 11:
yabahiro 53:694dbd7dfa0e 674 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 675 motor[TIRE_FR].dir = FOR;
yabahiro 53:694dbd7dfa0e 676 motor[TIRE_FL].pwm = 15;
yabahiro 53:694dbd7dfa0e 677 motor[TIRE_FL].dir = FOR;
yabahiro 53:694dbd7dfa0e 678 motor[TIRE_BR].pwm = 15;
yabahiro 53:694dbd7dfa0e 679 motor[TIRE_BR].dir = BACK;
yabahiro 53:694dbd7dfa0e 680 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 681 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 682 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 51:a0bdbddd08f7 683 countss=0;
yabahiro 51:a0bdbddd08f7 684 cross=0;
yabahiro 51:a0bdbddd08f7 685 mode=20;
yabahiro 51:a0bdbddd08f7 686 }
yabahiro 51:a0bdbddd08f7 687 break;
yabahiro 51:a0bdbddd08f7 688 case 20:
yabahiro 51:a0bdbddd08f7 689 solenoid.solenoid2= SOLENOID_ON;
yabahiro 51:a0bdbddd08f7 690 switch(g[2]) {
yabahiro 51:a0bdbddd08f7 691 case 98:
yabahiro 51:a0bdbddd08f7 692 motor[TIRE_FR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 693 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 694 motor[TIRE_FL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 695 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 696 motor[TIRE_BR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 697 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 698 motor[TIRE_BL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 699 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 700 if(countss>30000) {
yabahiro 51:a0bdbddd08f7 701 cross++;
yabahiro 51:a0bdbddd08f7 702 countss=0;
yabahiro 51:a0bdbddd08f7 703 }
yabahiro 51:a0bdbddd08f7 704 mtc2=true;
yabahiro 51:a0bdbddd08f7 705 if(YELLOW) {
yabahiro 51:a0bdbddd08f7 706 if(cross==1) {
yabahiro 51:a0bdbddd08f7 707 fast=20;
yabahiro 51:a0bdbddd08f7 708 normal=15;
yabahiro 51:a0bdbddd08f7 709 slow=15;
yabahiro 51:a0bdbddd08f7 710 }
yabahiro 51:a0bdbddd08f7 711 if(cross==2) {
yabahiro 51:a0bdbddd08f7 712 mode=21;
yabahiro 51:a0bdbddd08f7 713 countss=0;
yabahiro 51:a0bdbddd08f7 714 cross=0;
yabahiro 51:a0bdbddd08f7 715 }
yabahiro 51:a0bdbddd08f7 716 } else {
yabahiro 51:a0bdbddd08f7 717 if(cross==3) {
yabahiro 51:a0bdbddd08f7 718 mode=21;
yabahiro 51:a0bdbddd08f7 719 countss=0;
yabahiro 51:a0bdbddd08f7 720 cross=0;
yabahiro 51:a0bdbddd08f7 721 }
yabahiro 51:a0bdbddd08f7 722 }
yabahiro 51:a0bdbddd08f7 723 break;
yabahiro 51:a0bdbddd08f7 724 case 0:
yabahiro 51:a0bdbddd08f7 725 motor[TIRE_FR].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 726 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 727 motor[TIRE_FL].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 728 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 729 motor[TIRE_BR].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 730 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 731 motor[TIRE_BL].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 732 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 733 mtc2=true;
yabahiro 51:a0bdbddd08f7 734 break;
yabahiro 51:a0bdbddd08f7 735 case 255:
yabahiro 51:a0bdbddd08f7 736 motor[TIRE_FR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 737 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 738 motor[TIRE_FL].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 739 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 740 motor[TIRE_BR].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 741 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 742 motor[TIRE_BL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 743 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 744 mtc2=true;
yabahiro 51:a0bdbddd08f7 745 break;
yabahiro 51:a0bdbddd08f7 746 case 253:
yabahiro 51:a0bdbddd08f7 747 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 748 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 749 motor[TIRE_FL].pwm = slow+20;
yabahiro 51:a0bdbddd08f7 750 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 751 motor[TIRE_BR].pwm = slow+20;
yabahiro 51:a0bdbddd08f7 752 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 753 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 754 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 755 mtc2=true;
yabahiro 51:a0bdbddd08f7 756 break;
yabahiro 51:a0bdbddd08f7 757 case 254:
yabahiro 51:a0bdbddd08f7 758 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 759 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 760 motor[TIRE_FL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 761 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 762 motor[TIRE_BR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 763 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 764 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 765 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 766 mtc2=true;
yabahiro 51:a0bdbddd08f7 767 break;
yabahiro 51:a0bdbddd08f7 768 case 1:
yabahiro 51:a0bdbddd08f7 769 motor[TIRE_FR].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 770 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 771 motor[TIRE_FL].pwm = slow+20;
yabahiro 51:a0bdbddd08f7 772 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 773 motor[TIRE_BR].pwm = slow+20;
yabahiro 51:a0bdbddd08f7 774 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 775 motor[TIRE_BL].pwm = fast+20;
yabahiro 51:a0bdbddd08f7 776 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 777 mtc2=true;
yabahiro 60:24ebb611f26e 778 break;
yabahiro 60:24ebb611f26e 779 case 3:
yabahiro 60:24ebb611f26e 780 motor[TIRE_FR].pwm = slow+20;
yabahiro 60:24ebb611f26e 781 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 782 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 783 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 784 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 785 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 786 motor[TIRE_BL].pwm = slow+20;
yabahiro 51:a0bdbddd08f7 787 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 788 mtc2=true;
yabahiro 51:a0bdbddd08f7 789 break;
yabahiro 51:a0bdbddd08f7 790 case 2:
yabahiro 51:a0bdbddd08f7 791 motor[TIRE_FR].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 792 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 793 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 794 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 795 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 796 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 797 motor[TIRE_BL].pwm = normal+20;
yabahiro 51:a0bdbddd08f7 798 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 799 mtc2=true;
yabahiro 51:a0bdbddd08f7 800 break;
yabahiro 51:a0bdbddd08f7 801 }
yabahiro 51:a0bdbddd08f7 802 if(g[2]!=98&&mtc2==true) {
yabahiro 51:a0bdbddd08f7 803 switch(g[3]) {
yabahiro 51:a0bdbddd08f7 804 case 0:
yabahiro 51:a0bdbddd08f7 805 mtc2=false;
yabahiro 51:a0bdbddd08f7 806 break;
yabahiro 51:a0bdbddd08f7 807 case 255:
yabahiro 51:a0bdbddd08f7 808 motor[TIRE_BL].pwm += 5;
yabahiro 51:a0bdbddd08f7 809 mtc2=false;
yabahiro 51:a0bdbddd08f7 810 break;
yabahiro 51:a0bdbddd08f7 811 case 253:
yabahiro 51:a0bdbddd08f7 812 motor[TIRE_BL].pwm += 10;
yabahiro 51:a0bdbddd08f7 813 mtc2=false;
yabahiro 51:a0bdbddd08f7 814 break;
yabahiro 51:a0bdbddd08f7 815 case 254:
yabahiro 51:a0bdbddd08f7 816 motor[TIRE_BL].pwm += 15;
yabahiro 51:a0bdbddd08f7 817 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 818 mtc2=false;
yabahiro 51:a0bdbddd08f7 819 break;
yabahiro 51:a0bdbddd08f7 820 case 1:
yabahiro 51:a0bdbddd08f7 821 motor[TIRE_FL].pwm += 5;
yabahiro 51:a0bdbddd08f7 822 mtc2=false;
yabahiro 51:a0bdbddd08f7 823 break;
yabahiro 51:a0bdbddd08f7 824 case 3:
yabahiro 51:a0bdbddd08f7 825 motor[TIRE_FL].pwm += 10;
yabahiro 51:a0bdbddd08f7 826 mtc2=false;
yabahiro 51:a0bdbddd08f7 827 break;
yabahiro 51:a0bdbddd08f7 828 case 2:
yabahiro 51:a0bdbddd08f7 829 motor[TIRE_FL].pwm += 15;
yabahiro 51:a0bdbddd08f7 830 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 831 mtc2=false;
yabahiro 51:a0bdbddd08f7 832 break;
yabahiro 51:a0bdbddd08f7 833 }
yabahiro 51:a0bdbddd08f7 834 }
yabahiro 51:a0bdbddd08f7 835 break;
yabahiro 51:a0bdbddd08f7 836 case 21:
yabahiro 51:a0bdbddd08f7 837 if(YELLOW) {
yabahiro 51:a0bdbddd08f7 838 motor[TIRE_FR].pwm = 20;
yabahiro 51:a0bdbddd08f7 839 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 840 motor[TIRE_FL].pwm = 20;
yabahiro 51:a0bdbddd08f7 841 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 842 motor[TIRE_BR].pwm = 20;
yabahiro 51:a0bdbddd08f7 843 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 844 motor[TIRE_BL].pwm = 20;
yabahiro 51:a0bdbddd08f7 845 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 846 if(g[3]==98&&countss>=10000) {
yabahiro 51:a0bdbddd08f7 847 mode=22;
yabahiro 51:a0bdbddd08f7 848 countss=0;
yabahiro 51:a0bdbddd08f7 849 fast=60;
yabahiro 51:a0bdbddd08f7 850 normal=40;
yabahiro 51:a0bdbddd08f7 851 slow=20;
yabahiro 51:a0bdbddd08f7 852 }
yabahiro 51:a0bdbddd08f7 853 } else {
yabahiro 51:a0bdbddd08f7 854 motor[TIRE_FR].pwm = 255;
yabahiro 51:a0bdbddd08f7 855 motor[TIRE_FR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 856 motor[TIRE_FL].pwm = 30;
yabahiro 51:a0bdbddd08f7 857 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 858 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 859 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 860 motor[TIRE_BL].pwm = 255;
yabahiro 51:a0bdbddd08f7 861 motor[TIRE_BL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 862 if(g[0]==0) {
yabahiro 51:a0bdbddd08f7 863 mode=22;
yabahiro 51:a0bdbddd08f7 864 fast=60;
yabahiro 51:a0bdbddd08f7 865 normal=40;
yabahiro 51:a0bdbddd08f7 866 slow=25;
yabahiro 58:bfc38c43dadb 867 value=0;
yabahiro 58:bfc38c43dadb 868 aime.reset();
yabahiro 51:a0bdbddd08f7 869 }
yabahiro 51:a0bdbddd08f7 870 }
yabahiro 51:a0bdbddd08f7 871 break;
yabahiro 51:a0bdbddd08f7 872 case 22:
yabahiro 51:a0bdbddd08f7 873 if(YELLOW) {
yabahiro 51:a0bdbddd08f7 874 if(g[4]==3||g[4]==2) {
yabahiro 51:a0bdbddd08f7 875 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 876 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 877 motor[TIRE_FL].pwm = 30;
yabahiro 51:a0bdbddd08f7 878 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 879 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 880 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 881 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 882 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 883 }
yabahiro 51:a0bdbddd08f7 884 if(g[4]==253||g[4]==254) {
yabahiro 51:a0bdbddd08f7 885 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 886 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 887 motor[TIRE_FL].pwm = 30;
yabahiro 51:a0bdbddd08f7 888 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 889 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 890 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 891 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 892 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 893 }
yabahiro 51:a0bdbddd08f7 894 switch(g[2]) {
yabahiro 51:a0bdbddd08f7 895 case 0:
yabahiro 51:a0bdbddd08f7 896 motor[TIRE_FR].pwm = 20;
yabahiro 51:a0bdbddd08f7 897 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 898 motor[TIRE_BR].pwm = 20;
yabahiro 51:a0bdbddd08f7 899 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 900 break;
yabahiro 51:a0bdbddd08f7 901 case 255:
yabahiro 51:a0bdbddd08f7 902 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 903 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 904 motor[TIRE_BR].pwm = 20;
yabahiro 51:a0bdbddd08f7 905 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 906 break;
yabahiro 51:a0bdbddd08f7 907 case 253:
yabahiro 51:a0bdbddd08f7 908 motor[TIRE_FR].pwm = 40;
yabahiro 51:a0bdbddd08f7 909 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 910 motor[TIRE_BR].pwm = 20;
yabahiro 51:a0bdbddd08f7 911 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 912 break;
yabahiro 51:a0bdbddd08f7 913 case 254:
yabahiro 51:a0bdbddd08f7 914 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 915 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 916 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 917 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 918 break;
yabahiro 51:a0bdbddd08f7 919 case 1:
yabahiro 51:a0bdbddd08f7 920 motor[TIRE_FR].pwm = 20;
yabahiro 51:a0bdbddd08f7 921 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 922 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 923 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 924 break;
yabahiro 51:a0bdbddd08f7 925 case 3:
yabahiro 51:a0bdbddd08f7 926 motor[TIRE_FR].pwm = 20;
yabahiro 51:a0bdbddd08f7 927 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 928 motor[TIRE_BR].pwm = 40;
yabahiro 51:a0bdbddd08f7 929 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 930 break;
yabahiro 51:a0bdbddd08f7 931 case 2:
yabahiro 51:a0bdbddd08f7 932 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 933 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 934 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 935 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 936 break;
yabahiro 51:a0bdbddd08f7 937 }
yabahiro 51:a0bdbddd08f7 938 switch(g[3]) {
yabahiro 51:a0bdbddd08f7 939 case 0:
yabahiro 53:694dbd7dfa0e 940 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 941 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 942 motor[TIRE_BL].pwm = 20;
yabahiro 51:a0bdbddd08f7 943 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 944 break;
yabahiro 51:a0bdbddd08f7 945 case 255:
yabahiro 53:694dbd7dfa0e 946 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 947 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 948 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 949 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 950 case 253:
yabahiro 53:694dbd7dfa0e 951 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 952 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 953 motor[TIRE_BL].pwm = 40;
yabahiro 51:a0bdbddd08f7 954 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 955 break;
yabahiro 51:a0bdbddd08f7 956 case 254:
yabahiro 53:694dbd7dfa0e 957 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 958 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 959 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 960 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 961 break;
yabahiro 51:a0bdbddd08f7 962 case 1:
yabahiro 53:694dbd7dfa0e 963 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 964 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 965 motor[TIRE_BL].pwm = 20;
yabahiro 51:a0bdbddd08f7 966 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 967 break;
yabahiro 51:a0bdbddd08f7 968 case 3:
yabahiro 53:694dbd7dfa0e 969 motor[TIRE_FL].pwm = 45;
yabahiro 51:a0bdbddd08f7 970 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 971 motor[TIRE_BL].pwm = 20;
yabahiro 51:a0bdbddd08f7 972 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 973 break;
yabahiro 51:a0bdbddd08f7 974 case 2:
yabahiro 53:694dbd7dfa0e 975 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 976 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 977 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 978 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 979 break;
yabahiro 51:a0bdbddd08f7 980 }
yabahiro 51:a0bdbddd08f7 981 if(g[3]==0&&g[2]==0) {
yabahiro 58:bfc38c43dadb 982 aime.reset();
yabahiro 51:a0bdbddd08f7 983 mode=23;
yabahiro 51:a0bdbddd08f7 984 countss=0;
yabahiro 54:4167f10ecad0 985 fast=80;
yabahiro 54:4167f10ecad0 986 normal=60;
yabahiro 54:4167f10ecad0 987 slow=30;
yabahiro 51:a0bdbddd08f7 988 }
yabahiro 51:a0bdbddd08f7 989 } else {
yabahiro 59:5cd3875fa96a 990 if(value>=2400) {
yabahiro 59:5cd3875fa96a 991 slow=25;
yabahiro 59:5cd3875fa96a 992 normal=30;
yabahiro 59:5cd3875fa96a 993 fast=30;
yabahiro 59:5cd3875fa96a 994 }
yabahiro 59:5cd3875fa96a 995 if(value>=3000) {
yabahiro 58:bfc38c43dadb 996 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 51:a0bdbddd08f7 997 slow=25;
yabahiro 51:a0bdbddd08f7 998 normal=40;
yabahiro 51:a0bdbddd08f7 999 fast=60;
yabahiro 58:bfc38c43dadb 1000 aime.reset();
yabahiro 51:a0bdbddd08f7 1001 mode=23;
yabahiro 51:a0bdbddd08f7 1002 }
yabahiro 51:a0bdbddd08f7 1003 switch(g[0]) {
yabahiro 51:a0bdbddd08f7 1004 case 98:
yabahiro 51:a0bdbddd08f7 1005 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1006 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1007 motor[TIRE_FL].pwm = normal+5;
yabahiro 51:a0bdbddd08f7 1008 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1009 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1010 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1011 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1012 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1013 break;
yabahiro 51:a0bdbddd08f7 1014 case 0:
yabahiro 51:a0bdbddd08f7 1015 motor[TIRE_FR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1016 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1017 motor[TIRE_FL].pwm = fast+5;
yabahiro 51:a0bdbddd08f7 1018 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1019 motor[TIRE_BR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1020 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1021 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1022 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1023 mtc=true;
yabahiro 51:a0bdbddd08f7 1024 break;
yabahiro 51:a0bdbddd08f7 1025 case 255:
yabahiro 51:a0bdbddd08f7 1026 motor[TIRE_FR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1027 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1028 motor[TIRE_FL].pwm = slow+5;
yabahiro 51:a0bdbddd08f7 1029 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1030 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1031 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1032 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1033 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1034 mtc=true;
yabahiro 51:a0bdbddd08f7 1035 break;
yabahiro 51:a0bdbddd08f7 1036 case 253:
yabahiro 51:a0bdbddd08f7 1037 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1038 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1039 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1040 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1041 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1042 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1043 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1044 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1045 mtc=true;
yabahiro 51:a0bdbddd08f7 1046 break;
yabahiro 51:a0bdbddd08f7 1047 case 254:
yabahiro 51:a0bdbddd08f7 1048 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1049 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1050 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1051 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1052 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1053 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1054 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1055 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1056 mtc=true;
yabahiro 51:a0bdbddd08f7 1057 break;
yabahiro 51:a0bdbddd08f7 1058 case 1:
yabahiro 51:a0bdbddd08f7 1059 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1060 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1061 motor[TIRE_FL].pwm = fast+5;
yabahiro 51:a0bdbddd08f7 1062 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1063 motor[TIRE_BR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1064 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1065 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1066 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1067 mtc=true;
yabahiro 51:a0bdbddd08f7 1068 break;
yabahiro 51:a0bdbddd08f7 1069 case 3:
yabahiro 51:a0bdbddd08f7 1070 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1071 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1072 motor[TIRE_FL].pwm = slow+5;
yabahiro 51:a0bdbddd08f7 1073 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1074 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1075 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1076 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1077 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1078 mtc=true;
yabahiro 51:a0bdbddd08f7 1079 break;
yabahiro 51:a0bdbddd08f7 1080 case 2:
yabahiro 51:a0bdbddd08f7 1081 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1082 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1083 motor[TIRE_FL].pwm = normal+5;
yabahiro 51:a0bdbddd08f7 1084 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1085 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1086 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1087 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1088 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1089 mtc=true;
yabahiro 51:a0bdbddd08f7 1090 break;
yabahiro 51:a0bdbddd08f7 1091 }
yabahiro 51:a0bdbddd08f7 1092 if(g[0]!=98&&mtc==true) {
yabahiro 51:a0bdbddd08f7 1093 switch(g[0]) {
yabahiro 51:a0bdbddd08f7 1094 case 0:
yabahiro 51:a0bdbddd08f7 1095 mtc=false;
yabahiro 51:a0bdbddd08f7 1096 break;
yabahiro 51:a0bdbddd08f7 1097 case 255:
yabahiro 51:a0bdbddd08f7 1098 motor[TIRE_BR].pwm += 5;
yabahiro 51:a0bdbddd08f7 1099 mtc=false;
yabahiro 51:a0bdbddd08f7 1100 break;
yabahiro 51:a0bdbddd08f7 1101 case 253:
yabahiro 51:a0bdbddd08f7 1102 motor[TIRE_BR].pwm += 10;
yabahiro 51:a0bdbddd08f7 1103 mtc=false;
yabahiro 51:a0bdbddd08f7 1104 break;
yabahiro 51:a0bdbddd08f7 1105 case 254:
yabahiro 51:a0bdbddd08f7 1106 motor[TIRE_BR].pwm += 20;
yabahiro 51:a0bdbddd08f7 1107 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1108 mtc=false;
yabahiro 51:a0bdbddd08f7 1109 break;
yabahiro 51:a0bdbddd08f7 1110 case 1:
yabahiro 51:a0bdbddd08f7 1111 motor[TIRE_BL].pwm += 5;
yabahiro 51:a0bdbddd08f7 1112 mtc=false;
yabahiro 51:a0bdbddd08f7 1113 break;
yabahiro 51:a0bdbddd08f7 1114 case 3:
yabahiro 51:a0bdbddd08f7 1115 motor[TIRE_BL].pwm += 10;
yabahiro 51:a0bdbddd08f7 1116 mtc=false;
yabahiro 51:a0bdbddd08f7 1117 break;
yabahiro 51:a0bdbddd08f7 1118 case 2:
yabahiro 51:a0bdbddd08f7 1119 motor[TIRE_BL].pwm += 20;
yabahiro 51:a0bdbddd08f7 1120 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1121 mtc=false;
yabahiro 51:a0bdbddd08f7 1122 break;
yabahiro 51:a0bdbddd08f7 1123 }
yabahiro 51:a0bdbddd08f7 1124 }
yabahiro 51:a0bdbddd08f7 1125 }
yabahiro 51:a0bdbddd08f7 1126 break;
yabahiro 51:a0bdbddd08f7 1127 case 23:
yabahiro 51:a0bdbddd08f7 1128 if(YELLOW) {
yabahiro 51:a0bdbddd08f7 1129 switch(g[4]) {
yabahiro 51:a0bdbddd08f7 1130 case 98:
yabahiro 51:a0bdbddd08f7 1131 motor[TIRE_FR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1132 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1133 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1134 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1135 motor[TIRE_BR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1136 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1137 motor[TIRE_BL].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1138 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1139 break;
yabahiro 51:a0bdbddd08f7 1140 case 99:
yabahiro 51:a0bdbddd08f7 1141 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1142 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1143 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1144 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1145 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1146 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1147 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1148 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1149 break;
yabahiro 51:a0bdbddd08f7 1150 case 0:
yabahiro 51:a0bdbddd08f7 1151 motor[TIRE_FR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1152 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1153 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1154 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1155 motor[TIRE_BR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1156 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1157 motor[TIRE_BL].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1158 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1159 break;
yabahiro 51:a0bdbddd08f7 1160 case 255:
yabahiro 51:a0bdbddd08f7 1161 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1162 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1163 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1164 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1165 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1166 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1167 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1168 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1169 break;
yabahiro 51:a0bdbddd08f7 1170 case 253:
yabahiro 51:a0bdbddd08f7 1171 motor[TIRE_FR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1172 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1173 motor[TIRE_FL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1174 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1175 motor[TIRE_BR].pwm = slow-5;
yabahiro 51:a0bdbddd08f7 1176 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1177 motor[TIRE_BL].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1178 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1179 break;
yabahiro 51:a0bdbddd08f7 1180 case 254:
yabahiro 51:a0bdbddd08f7 1181 motor[TIRE_FR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1182 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1183 motor[TIRE_FL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1184 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1185 motor[TIRE_BR].pwm = normal-5;
yabahiro 51:a0bdbddd08f7 1186 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1187 motor[TIRE_BL].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1188 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1189 break;
yabahiro 51:a0bdbddd08f7 1190 case 1:
yabahiro 51:a0bdbddd08f7 1191 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1192 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1193 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1194 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1195 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1196 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1197 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1198 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1199 break;
yabahiro 51:a0bdbddd08f7 1200 case 3:
yabahiro 51:a0bdbddd08f7 1201 motor[TIRE_FR].pwm = slow-5;
yabahiro 51:a0bdbddd08f7 1202 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1203 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1204 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1205 motor[TIRE_BR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1206 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1207 motor[TIRE_BL].pwm = slow-5;
yabahiro 51:a0bdbddd08f7 1208 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1209 break;
yabahiro 51:a0bdbddd08f7 1210 case 2:
yabahiro 51:a0bdbddd08f7 1211 motor[TIRE_FR].pwm = normal-5;
yabahiro 51:a0bdbddd08f7 1212 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1213 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1214 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1215 motor[TIRE_BR].pwm = fast-5;
yabahiro 51:a0bdbddd08f7 1216 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1217 motor[TIRE_BL].pwm = normal-5;
yabahiro 51:a0bdbddd08f7 1218 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1219 break;
yabahiro 51:a0bdbddd08f7 1220 }
yabahiro 58:bfc38c43dadb 1221
yabahiro 58:bfc38c43dadb 1222
yabahiro 58:bfc38c43dadb 1223 /*
yabahiro 58:bfc38c43dadb 1224 if(value>=3000) {
yabahiro 58:bfc38c43dadb 1225 mode=24;
yabahiro 58:bfc38c43dadb 1226 aime.reset();
yabahiro 58:bfc38c43dadb 1227 value=0;
yabahiro 58:bfc38c43dadb 1228 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 58:bfc38c43dadb 1229 countss=0;
yabahiro 58:bfc38c43dadb 1230 fast=80;
yabahiro 58:bfc38c43dadb 1231 normal=60;
yabahiro 58:bfc38c43dadb 1232 slow=30;
yabahiro 58:bfc38c43dadb 1233 }
yabahiro 51:a0bdbddd08f7 1234 if(g[0]==98) {
yabahiro 51:a0bdbddd08f7 1235 fast=20;
yabahiro 51:a0bdbddd08f7 1236 normal=15;
yabahiro 51:a0bdbddd08f7 1237 slow=10;
yabahiro 51:a0bdbddd08f7 1238 }
yabahiro 58:bfc38c43dadb 1239 */
yabahiro 59:5cd3875fa96a 1240 if(value>=1800) {
yabahiro 58:bfc38c43dadb 1241 fast=20;
yabahiro 58:bfc38c43dadb 1242 normal=15;
yabahiro 58:bfc38c43dadb 1243 slow=10;
yabahiro 58:bfc38c43dadb 1244 }
yabahiro 59:5cd3875fa96a 1245 if(value>=2600) {
yabahiro 51:a0bdbddd08f7 1246 mode=24;
yabahiro 51:a0bdbddd08f7 1247 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 51:a0bdbddd08f7 1248 countss=0;
yabahiro 58:bfc38c43dadb 1249 value=0;
yabahiro 58:bfc38c43dadb 1250 aime.reset();
yabahiro 54:4167f10ecad0 1251 fast=80;
yabahiro 54:4167f10ecad0 1252 normal=60;
yabahiro 54:4167f10ecad0 1253 slow=30;
yabahiro 51:a0bdbddd08f7 1254 }
yabahiro 51:a0bdbddd08f7 1255 } else {
yabahiro 51:a0bdbddd08f7 1256 if(UPLS==true||DOLS==true) {
yabahiro 51:a0bdbddd08f7 1257 if(UPLS==true||DOLS==true) {
yabahiro 51:a0bdbddd08f7 1258 motor[MOTOR_0].pwm = 200;
yabahiro 51:a0bdbddd08f7 1259 motor[MOTOR_0].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1260 fast=60;
yabahiro 51:a0bdbddd08f7 1261 normal=40;
yabahiro 53:694dbd7dfa0e 1262 slow=30;
yabahiro 51:a0bdbddd08f7 1263 countss=0;
yabahiro 58:bfc38c43dadb 1264 value=0;
yabahiro 58:bfc38c43dadb 1265 aime.reset();
yabahiro 51:a0bdbddd08f7 1266 mode=24;
yabahiro 51:a0bdbddd08f7 1267 }
yabahiro 51:a0bdbddd08f7 1268 } else {
yabahiro 51:a0bdbddd08f7 1269 motor[MOTOR_0].pwm = 100;
yabahiro 51:a0bdbddd08f7 1270 motor[MOTOR_0].dir = BACK;
yabahiro 51:a0bdbddd08f7 1271 }
yabahiro 51:a0bdbddd08f7 1272 motor[TIRE_FR].pwm = 100;
yabahiro 51:a0bdbddd08f7 1273 motor[TIRE_FR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1274 motor[TIRE_FL].pwm = 100;
yabahiro 51:a0bdbddd08f7 1275 motor[TIRE_FL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1276 motor[TIRE_BR].pwm = 100;
yabahiro 51:a0bdbddd08f7 1277 motor[TIRE_BR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1278 motor[TIRE_BL].pwm = 100;
yabahiro 51:a0bdbddd08f7 1279 motor[TIRE_BL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1280 }
yabahiro 51:a0bdbddd08f7 1281 break;
yabahiro 51:a0bdbddd08f7 1282 case 24:
yabahiro 51:a0bdbddd08f7 1283 if(YELLOW) {
yabahiro 58:bfc38c43dadb 1284 /*
yabahiro 51:a0bdbddd08f7 1285 if(g[4]==3||g[4]==2) {
yabahiro 51:a0bdbddd08f7 1286 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1287 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 1288 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1289 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 1290 motor[TIRE_BR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1291 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1292 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1293 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1294 }
yabahiro 51:a0bdbddd08f7 1295 if(g[4]==253||g[4]==254) {
yabahiro 54:4167f10ecad0 1296 motor[TIRE_FR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1297 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1298 motor[TIRE_FL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1299 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1300 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1301 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 1302 motor[TIRE_BL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1303 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1304 }
yabahiro 51:a0bdbddd08f7 1305 switch(g[2]) {
yabahiro 51:a0bdbddd08f7 1306 case 0:
yabahiro 51:a0bdbddd08f7 1307 motor[TIRE_FR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1308 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1309 motor[TIRE_BR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1310 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1311 break;
yabahiro 51:a0bdbddd08f7 1312 case 255:
yabahiro 51:a0bdbddd08f7 1313 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1314 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1315 motor[TIRE_BR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1316 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1317 break;
yabahiro 51:a0bdbddd08f7 1318 case 253:
yabahiro 51:a0bdbddd08f7 1319 motor[TIRE_FR].pwm = 40;
yabahiro 51:a0bdbddd08f7 1320 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1321 motor[TIRE_BR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1322 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1323 break;
yabahiro 51:a0bdbddd08f7 1324 case 254:
yabahiro 51:a0bdbddd08f7 1325 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1326 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1327 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1328 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1329 break;
yabahiro 51:a0bdbddd08f7 1330 case 1:
yabahiro 51:a0bdbddd08f7 1331 motor[TIRE_FR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1332 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1333 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1334 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1335 break;
yabahiro 51:a0bdbddd08f7 1336 case 3:
yabahiro 51:a0bdbddd08f7 1337 motor[TIRE_FR].pwm = 25;
yabahiro 51:a0bdbddd08f7 1338 motor[TIRE_FR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1339 motor[TIRE_BR].pwm = 40;
yabahiro 51:a0bdbddd08f7 1340 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1341 break;
yabahiro 51:a0bdbddd08f7 1342 case 2:
yabahiro 51:a0bdbddd08f7 1343 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1344 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1345 motor[TIRE_BR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1346 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1347 break;
yabahiro 51:a0bdbddd08f7 1348 }
yabahiro 51:a0bdbddd08f7 1349 switch(g[3]) {
yabahiro 51:a0bdbddd08f7 1350 case 0:
yabahiro 51:a0bdbddd08f7 1351 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1352 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1353 motor[TIRE_BL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1354 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1355 break;
yabahiro 51:a0bdbddd08f7 1356 case 255:
yabahiro 51:a0bdbddd08f7 1357 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1358 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1359 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1360 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1361 case 253:
yabahiro 51:a0bdbddd08f7 1362 motor[TIRE_FL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1363 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1364 motor[TIRE_BL].pwm = 40;
yabahiro 51:a0bdbddd08f7 1365 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1366 break;
yabahiro 51:a0bdbddd08f7 1367 case 254:
yabahiro 53:694dbd7dfa0e 1368 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 1369 motor[TIRE_FL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1370 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1371 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1372 break;
yabahiro 51:a0bdbddd08f7 1373 case 1:
yabahiro 53:694dbd7dfa0e 1374 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 1375 motor[TIRE_FL].dir = FOR;
yabahiro 53:694dbd7dfa0e 1376 motor[TIRE_BL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1377 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1378 break;
yabahiro 51:a0bdbddd08f7 1379 case 3:
yabahiro 53:694dbd7dfa0e 1380 motor[TIRE_FL].pwm = 45;
yabahiro 51:a0bdbddd08f7 1381 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1382 motor[TIRE_BL].pwm = 25;
yabahiro 51:a0bdbddd08f7 1383 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1384 break;
yabahiro 51:a0bdbddd08f7 1385 case 2:
yabahiro 53:694dbd7dfa0e 1386 motor[TIRE_FL].pwm = 35;
yabahiro 51:a0bdbddd08f7 1387 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1388 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1389 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1390 break;
yabahiro 51:a0bdbddd08f7 1391 }
yabahiro 58:bfc38c43dadb 1392
yabahiro 51:a0bdbddd08f7 1393 if(g[3]==0&&g[2]==0) {
yabahiro 51:a0bdbddd08f7 1394 mode=25;
yabahiro 51:a0bdbddd08f7 1395 }
yabahiro 58:bfc38c43dadb 1396 */
yabahiro 58:bfc38c43dadb 1397 mode=25;
yabahiro 51:a0bdbddd08f7 1398 } else {
yabahiro 59:5cd3875fa96a 1399 /*
yabahiro 58:bfc38c43dadb 1400 if(g[0]==98&&value>=2000) {
yabahiro 51:a0bdbddd08f7 1401 mode=31;
yabahiro 53:694dbd7dfa0e 1402 fast=60;
yabahiro 53:694dbd7dfa0e 1403 normal=40;
yabahiro 53:694dbd7dfa0e 1404 slow=30;
yabahiro 51:a0bdbddd08f7 1405 countss=0;
yabahiro 51:a0bdbddd08f7 1406 }
yabahiro 59:5cd3875fa96a 1407 */
yabahiro 51:a0bdbddd08f7 1408 switch(g[0]) {
yabahiro 51:a0bdbddd08f7 1409 case 98:
yabahiro 51:a0bdbddd08f7 1410 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1411 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1412 motor[TIRE_FL].pwm = normal+5;
yabahiro 51:a0bdbddd08f7 1413 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1414 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1415 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1416 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1417 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1418 break;
yabahiro 51:a0bdbddd08f7 1419 case 0:
yabahiro 51:a0bdbddd08f7 1420 motor[TIRE_FR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1421 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1422 motor[TIRE_FL].pwm = fast+5;
yabahiro 51:a0bdbddd08f7 1423 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1424 motor[TIRE_BR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1425 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1426 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1427 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1428 mtc=true;
yabahiro 51:a0bdbddd08f7 1429 break;
yabahiro 51:a0bdbddd08f7 1430 case 255:
yabahiro 51:a0bdbddd08f7 1431 motor[TIRE_FR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1432 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1433 motor[TIRE_FL].pwm = slow+5;
yabahiro 51:a0bdbddd08f7 1434 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1435 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1436 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1437 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1438 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1439 mtc=true;
yabahiro 51:a0bdbddd08f7 1440 break;
yabahiro 51:a0bdbddd08f7 1441 case 253:
yabahiro 51:a0bdbddd08f7 1442 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1443 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1444 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1445 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1446 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1447 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1448 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1449 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1450 mtc=true;
yabahiro 51:a0bdbddd08f7 1451 break;
yabahiro 51:a0bdbddd08f7 1452 case 254:
yabahiro 51:a0bdbddd08f7 1453 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1454 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1455 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1456 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1457 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1458 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1459 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1460 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1461 mtc=true;
yabahiro 51:a0bdbddd08f7 1462 break;
yabahiro 51:a0bdbddd08f7 1463 case 1:
yabahiro 51:a0bdbddd08f7 1464 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1465 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1466 motor[TIRE_FL].pwm = fast+5;
yabahiro 51:a0bdbddd08f7 1467 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1468 motor[TIRE_BR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1469 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1470 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1471 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1472 mtc=true;
yabahiro 51:a0bdbddd08f7 1473 break;
yabahiro 51:a0bdbddd08f7 1474 case 3:
yabahiro 51:a0bdbddd08f7 1475 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1476 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1477 motor[TIRE_FL].pwm = slow+5;
yabahiro 51:a0bdbddd08f7 1478 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1479 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1480 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1481 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1482 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1483 mtc=true;
yabahiro 51:a0bdbddd08f7 1484 break;
yabahiro 51:a0bdbddd08f7 1485 case 2:
yabahiro 51:a0bdbddd08f7 1486 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1487 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1488 motor[TIRE_FL].pwm = normal+5;
yabahiro 51:a0bdbddd08f7 1489 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1490 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1491 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1492 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1493 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1494 mtc=true;
yabahiro 51:a0bdbddd08f7 1495 break;
yabahiro 51:a0bdbddd08f7 1496 }
yabahiro 51:a0bdbddd08f7 1497 if(g[1]!=98&&mtc==true) {
yabahiro 51:a0bdbddd08f7 1498 switch(g[1]) {
yabahiro 51:a0bdbddd08f7 1499 case 0:
yabahiro 51:a0bdbddd08f7 1500 mtc=false;
yabahiro 51:a0bdbddd08f7 1501 break;
yabahiro 51:a0bdbddd08f7 1502 case 255:
yabahiro 51:a0bdbddd08f7 1503 motor[TIRE_BR].pwm += 5;
yabahiro 51:a0bdbddd08f7 1504 mtc=false;
yabahiro 51:a0bdbddd08f7 1505 break;
yabahiro 51:a0bdbddd08f7 1506 case 253:
yabahiro 51:a0bdbddd08f7 1507 motor[TIRE_BR].pwm += 10;
yabahiro 51:a0bdbddd08f7 1508 mtc=false;
yabahiro 51:a0bdbddd08f7 1509 break;
yabahiro 51:a0bdbddd08f7 1510 case 254:
yabahiro 51:a0bdbddd08f7 1511 motor[TIRE_BR].pwm += 20;
yabahiro 51:a0bdbddd08f7 1512 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1513 mtc=false;
yabahiro 51:a0bdbddd08f7 1514 break;
yabahiro 51:a0bdbddd08f7 1515 case 1:
yabahiro 51:a0bdbddd08f7 1516 motor[TIRE_BL].pwm += 5;
yabahiro 51:a0bdbddd08f7 1517 mtc=false;
yabahiro 51:a0bdbddd08f7 1518 break;
yabahiro 51:a0bdbddd08f7 1519 case 3:
yabahiro 51:a0bdbddd08f7 1520 motor[TIRE_BL].pwm += 10;
yabahiro 51:a0bdbddd08f7 1521 mtc=false;
yabahiro 51:a0bdbddd08f7 1522 break;
yabahiro 51:a0bdbddd08f7 1523 case 2:
yabahiro 51:a0bdbddd08f7 1524 motor[TIRE_BL].pwm += 20;
yabahiro 51:a0bdbddd08f7 1525 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1526 mtc=false;
yabahiro 51:a0bdbddd08f7 1527 break;
yabahiro 51:a0bdbddd08f7 1528 }
yabahiro 51:a0bdbddd08f7 1529 }
yabahiro 51:a0bdbddd08f7 1530 }
yabahiro 51:a0bdbddd08f7 1531 break;
yabahiro 51:a0bdbddd08f7 1532 case 25:
yabahiro 51:a0bdbddd08f7 1533 motor[TIRE_FR].pwm = 100;
yabahiro 51:a0bdbddd08f7 1534 motor[TIRE_FR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1535 motor[TIRE_FL].pwm = 100;
yabahiro 51:a0bdbddd08f7 1536 motor[TIRE_FL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1537 motor[TIRE_BR].pwm = 100;
yabahiro 51:a0bdbddd08f7 1538 motor[TIRE_BR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1539 motor[TIRE_BL].pwm = 100;
yabahiro 51:a0bdbddd08f7 1540 motor[TIRE_BL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1541 if(DOLS==true||UPLS==true) {
yabahiro 51:a0bdbddd08f7 1542 if(DOLS==true||UPLS==true) {
yabahiro 51:a0bdbddd08f7 1543 motor[MOTOR_0].pwm = 200;
yabahiro 51:a0bdbddd08f7 1544 motor[MOTOR_0].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1545 countss=0;
yabahiro 51:a0bdbddd08f7 1546 mode=26;
yabahiro 58:bfc38c43dadb 1547 value=0;
yabahiro 58:bfc38c43dadb 1548 aime.reset();
yabahiro 51:a0bdbddd08f7 1549 }
yabahiro 51:a0bdbddd08f7 1550 } else {
yabahiro 51:a0bdbddd08f7 1551 motor[MOTOR_0].pwm = 100;
yabahiro 51:a0bdbddd08f7 1552 motor[MOTOR_0].dir = BACK;
yabahiro 51:a0bdbddd08f7 1553 }
yabahiro 51:a0bdbddd08f7 1554 break;
yabahiro 51:a0bdbddd08f7 1555 case 26:
yabahiro 59:5cd3875fa96a 1556 /*
yabahiro 58:bfc38c43dadb 1557 if(g[0]==98&&value>=2500) {
yabahiro 53:694dbd7dfa0e 1558 mode=31;
yabahiro 53:694dbd7dfa0e 1559 }
yabahiro 59:5cd3875fa96a 1560 */
yabahiro 50:87f5fa1bc460 1561 switch(g[4]) {
yabahiro 50:87f5fa1bc460 1562 case 98:
yabahiro 51:a0bdbddd08f7 1563 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1564 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1565 motor[TIRE_FL].pwm = 65;
yabahiro 50:87f5fa1bc460 1566 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1567 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1568 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1569 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1570 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1571 break;
yabahiro 50:87f5fa1bc460 1572 case 99:
yabahiro 50:87f5fa1bc460 1573 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1574 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1575 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1576 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1577 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1578 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1579 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1580 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1581 break;
yabahiro 50:87f5fa1bc460 1582 case 0:
yabahiro 51:a0bdbddd08f7 1583 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1584 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1585 motor[TIRE_FL].pwm = 65;
yabahiro 50:87f5fa1bc460 1586 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1587 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1588 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1589 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1590 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1591 break;
yabahiro 50:87f5fa1bc460 1592 case 255:
yabahiro 50:87f5fa1bc460 1593 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1594 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1595 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1596 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1597 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1598 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1599 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1600 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1601 break;
yabahiro 50:87f5fa1bc460 1602 case 253:
yabahiro 51:a0bdbddd08f7 1603 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1604 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1605 motor[TIRE_FL].pwm = 30;
yabahiro 50:87f5fa1bc460 1606 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1607 motor[TIRE_BR].pwm = 25;
yabahiro 50:87f5fa1bc460 1608 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1609 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1610 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1611 break;
yabahiro 50:87f5fa1bc460 1612 case 254:
yabahiro 51:a0bdbddd08f7 1613 motor[TIRE_FR].pwm = 60;
yabahiro 50:87f5fa1bc460 1614 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1615 motor[TIRE_FL].pwm = 45;
yabahiro 50:87f5fa1bc460 1616 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1617 motor[TIRE_BR].pwm = 40;
yabahiro 50:87f5fa1bc460 1618 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1619 motor[TIRE_BL].pwm = 60;
yabahiro 50:87f5fa1bc460 1620 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1621 break;
yabahiro 50:87f5fa1bc460 1622 case 1:
yabahiro 50:87f5fa1bc460 1623 motor[TIRE_FR].pwm = 0;
yabahiro 50:87f5fa1bc460 1624 motor[TIRE_FR].dir = FREE;
yabahiro 50:87f5fa1bc460 1625 motor[TIRE_FL].pwm = 0;
yabahiro 50:87f5fa1bc460 1626 motor[TIRE_FL].dir = FREE;
yabahiro 50:87f5fa1bc460 1627 motor[TIRE_BR].pwm = 0;
yabahiro 50:87f5fa1bc460 1628 motor[TIRE_BR].dir = FREE;
yabahiro 50:87f5fa1bc460 1629 motor[TIRE_BL].pwm = 0;
yabahiro 50:87f5fa1bc460 1630 motor[TIRE_BL].dir = FREE;
yabahiro 50:87f5fa1bc460 1631 break;
yabahiro 50:87f5fa1bc460 1632 case 3:
yabahiro 51:a0bdbddd08f7 1633 motor[TIRE_FR].pwm = 25;
yabahiro 50:87f5fa1bc460 1634 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1635 motor[TIRE_FL].pwm = 65;
yabahiro 50:87f5fa1bc460 1636 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1637 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1638 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1639 motor[TIRE_BL].pwm = 25;
yabahiro 50:87f5fa1bc460 1640 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1641 break;
yabahiro 50:87f5fa1bc460 1642 case 2:
yabahiro 51:a0bdbddd08f7 1643 motor[TIRE_FR].pwm = 40;
yabahiro 50:87f5fa1bc460 1644 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1645 motor[TIRE_FL].pwm = 65;
yabahiro 50:87f5fa1bc460 1646 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1647 motor[TIRE_BR].pwm = 60;
yabahiro 50:87f5fa1bc460 1648 motor[TIRE_BR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1649 motor[TIRE_BL].pwm = 40;
yabahiro 50:87f5fa1bc460 1650 motor[TIRE_BL].dir = FOR;
yabahiro 50:87f5fa1bc460 1651 break;
yabahiro 50:87f5fa1bc460 1652 }
yabahiro 51:a0bdbddd08f7 1653 break;
yabahiro 51:a0bdbddd08f7 1654 case 31:
yabahiro 53:694dbd7dfa0e 1655 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1656 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1657 motor[TIRE_FL].pwm = 5;
yabahiro 53:694dbd7dfa0e 1658 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1659 motor[TIRE_BR].pwm = 5;
yabahiro 53:694dbd7dfa0e 1660 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 1661 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1662 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1663 if(g[2]==0) {
yabahiro 51:a0bdbddd08f7 1664 cross=0;
yabahiro 51:a0bdbddd08f7 1665 countss=0;
yabahiro 51:a0bdbddd08f7 1666 mode=40;
yabahiro 55:844ac0974740 1667 fast=70;
yabahiro 55:844ac0974740 1668 normal=50;
yabahiro 55:844ac0974740 1669 slow=40;
yabahiro 51:a0bdbddd08f7 1670 }
yabahiro 51:a0bdbddd08f7 1671 break;
yabahiro 51:a0bdbddd08f7 1672 case 40:
yabahiro 51:a0bdbddd08f7 1673 switch(g[3]) {
yabahiro 51:a0bdbddd08f7 1674 case 98:
yabahiro 53:694dbd7dfa0e 1675 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1676 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1677 motor[TIRE_FL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1678 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1679 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1680 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 1681 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1682 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1683 if(countss>30000) {
yabahiro 51:a0bdbddd08f7 1684 cross++;
yabahiro 51:a0bdbddd08f7 1685 countss=0;
yabahiro 51:a0bdbddd08f7 1686 }
yabahiro 51:a0bdbddd08f7 1687 mtc2=true;
yabahiro 51:a0bdbddd08f7 1688 if(YELLOW) {
yabahiro 51:a0bdbddd08f7 1689 if(cross==1) {
yabahiro 51:a0bdbddd08f7 1690 mode=41;
yabahiro 51:a0bdbddd08f7 1691 countss=0;
yabahiro 51:a0bdbddd08f7 1692 cross=0;
yabahiro 51:a0bdbddd08f7 1693 }
yabahiro 51:a0bdbddd08f7 1694 } else {
yabahiro 51:a0bdbddd08f7 1695 if(cross==3) {
yabahiro 53:694dbd7dfa0e 1696 fast=60;
yabahiro 53:694dbd7dfa0e 1697 normal=40;
yabahiro 53:694dbd7dfa0e 1698 slow=30;
yabahiro 51:a0bdbddd08f7 1699 mode=41;
yabahiro 51:a0bdbddd08f7 1700 countss=0;
yabahiro 51:a0bdbddd08f7 1701 cross=0;
yabahiro 51:a0bdbddd08f7 1702 }
yabahiro 51:a0bdbddd08f7 1703 }
yabahiro 51:a0bdbddd08f7 1704 break;
yabahiro 50:87f5fa1bc460 1705 case 0:
yabahiro 53:694dbd7dfa0e 1706 motor[TIRE_FR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1707 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1708 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1709 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1710 motor[TIRE_BR].pwm = fast;
yabahiro 51:a0bdbddd08f7 1711 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 1712 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1713 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1714 mtc2=true;
yabahiro 50:87f5fa1bc460 1715 break;
yabahiro 50:87f5fa1bc460 1716 case 255:
yabahiro 53:694dbd7dfa0e 1717 motor[TIRE_FR].pwm = slow;
yabahiro 50:87f5fa1bc460 1718 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1719 motor[TIRE_FL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1720 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1721 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1722 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 1723 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1724 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1725 mtc2=true;
yabahiro 50:87f5fa1bc460 1726 break;
yabahiro 50:87f5fa1bc460 1727 case 253:
yabahiro 51:a0bdbddd08f7 1728 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1729 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1730 motor[TIRE_FL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1731 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1732 motor[TIRE_BR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1733 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1734 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1735 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1736 mtc2=true;
yabahiro 50:87f5fa1bc460 1737 break;
yabahiro 50:87f5fa1bc460 1738 case 254:
yabahiro 51:a0bdbddd08f7 1739 motor[TIRE_FR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1740 motor[TIRE_FR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1741 motor[TIRE_FL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1742 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1743 motor[TIRE_BR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1744 motor[TIRE_BR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1745 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1746 motor[TIRE_BL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1747 mtc2=true;
yabahiro 50:87f5fa1bc460 1748 break;
yabahiro 50:87f5fa1bc460 1749 case 1:
yabahiro 53:694dbd7dfa0e 1750 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1751 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1752 motor[TIRE_FL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1753 motor[TIRE_FL].dir = BACK;
yabahiro 53:694dbd7dfa0e 1754 motor[TIRE_BR].pwm = slow;
yabahiro 50:87f5fa1bc460 1755 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 1756 motor[TIRE_BL].pwm = fast;
yabahiro 51:a0bdbddd08f7 1757 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1758 mtc2=true;
yabahiro 50:87f5fa1bc460 1759 break;
yabahiro 50:87f5fa1bc460 1760 case 3:
yabahiro 53:694dbd7dfa0e 1761 motor[TIRE_FR].pwm = slow;
yabahiro 51:a0bdbddd08f7 1762 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1763 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1764 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1765 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1766 motor[TIRE_BR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1767 motor[TIRE_BL].pwm = slow;
yabahiro 51:a0bdbddd08f7 1768 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1769 mtc2=true;
yabahiro 50:87f5fa1bc460 1770 break;
yabahiro 50:87f5fa1bc460 1771 case 2:
yabahiro 53:694dbd7dfa0e 1772 motor[TIRE_FR].pwm = normal;
yabahiro 51:a0bdbddd08f7 1773 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1774 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1775 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1776 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1777 motor[TIRE_BR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1778 motor[TIRE_BL].pwm = normal;
yabahiro 51:a0bdbddd08f7 1779 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1780 mtc2=true;
yabahiro 50:87f5fa1bc460 1781 break;
yabahiro 50:87f5fa1bc460 1782 }
yabahiro 51:a0bdbddd08f7 1783 if(g[2]!=98&&mtc2==true) {
yabahiro 51:a0bdbddd08f7 1784 switch(g[2]) {
yabahiro 51:a0bdbddd08f7 1785 case 0:
yabahiro 51:a0bdbddd08f7 1786 mtc2=false;
yabahiro 51:a0bdbddd08f7 1787 break;
yabahiro 51:a0bdbddd08f7 1788 case 255:
yabahiro 51:a0bdbddd08f7 1789 motor[TIRE_FL].pwm += 5;
yabahiro 51:a0bdbddd08f7 1790 mtc2=false;
yabahiro 51:a0bdbddd08f7 1791 break;
yabahiro 51:a0bdbddd08f7 1792 case 253:
yabahiro 51:a0bdbddd08f7 1793 motor[TIRE_FL].pwm += 10;
yabahiro 51:a0bdbddd08f7 1794 mtc2=false;
yabahiro 51:a0bdbddd08f7 1795 break;
yabahiro 51:a0bdbddd08f7 1796 case 254:
yabahiro 51:a0bdbddd08f7 1797 motor[TIRE_FL].pwm += 20;
yabahiro 51:a0bdbddd08f7 1798 motor[TIRE_BL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1799 mtc2=false;
yabahiro 51:a0bdbddd08f7 1800 break;
yabahiro 51:a0bdbddd08f7 1801 case 1:
yabahiro 51:a0bdbddd08f7 1802 motor[TIRE_BL].pwm += 5;
yabahiro 51:a0bdbddd08f7 1803 mtc2=false;
yabahiro 51:a0bdbddd08f7 1804 break;
yabahiro 51:a0bdbddd08f7 1805 case 3:
yabahiro 51:a0bdbddd08f7 1806 motor[TIRE_BL].pwm += 10;
yabahiro 51:a0bdbddd08f7 1807 mtc2=false;
yabahiro 51:a0bdbddd08f7 1808 break;
yabahiro 51:a0bdbddd08f7 1809 case 2:
yabahiro 51:a0bdbddd08f7 1810 motor[TIRE_BL].pwm += 20;
yabahiro 51:a0bdbddd08f7 1811 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1812 mtc2=false;
yabahiro 51:a0bdbddd08f7 1813 break;
yabahiro 51:a0bdbddd08f7 1814 }
yabahiro 51:a0bdbddd08f7 1815 }
yabahiro 51:a0bdbddd08f7 1816 break;
yabahiro 51:a0bdbddd08f7 1817 case 41:
yabahiro 53:694dbd7dfa0e 1818 /*
yabahiro 53:694dbd7dfa0e 1819 if(LIF||LIB) {
yabahiro 53:694dbd7dfa0e 1820 mode=42;
yabahiro 53:694dbd7dfa0e 1821 }
yabahiro 53:694dbd7dfa0e 1822 */
yabahiro 53:694dbd7dfa0e 1823 motor[TIRE_FR].pwm = 30;
yabahiro 51:a0bdbddd08f7 1824 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1825 motor[TIRE_FL].pwm = 0;
yabahiro 53:694dbd7dfa0e 1826 motor[TIRE_FL].dir = FREE;
yabahiro 53:694dbd7dfa0e 1827 motor[TIRE_BR].pwm = 0;
yabahiro 53:694dbd7dfa0e 1828 motor[TIRE_BR].dir = FREE;
yabahiro 53:694dbd7dfa0e 1829 motor[TIRE_BL].pwm = 30;
yabahiro 51:a0bdbddd08f7 1830 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1831 if(g[1]==0) {
yabahiro 51:a0bdbddd08f7 1832 countss=0;
yabahiro 51:a0bdbddd08f7 1833 mode=70;
yabahiro 55:844ac0974740 1834 normal=120;
yabahiro 55:844ac0974740 1835 fast=140;
yabahiro 55:844ac0974740 1836 slow=100;
yabahiro 51:a0bdbddd08f7 1837 }
yabahiro 51:a0bdbddd08f7 1838 break;
yabahiro 51:a0bdbddd08f7 1839 case 42:
yabahiro 51:a0bdbddd08f7 1840 motor[TIRE_FR].pwm = 40;
yabahiro 51:a0bdbddd08f7 1841 motor[TIRE_FR].dir = FOR;
yabahiro 51:a0bdbddd08f7 1842 motor[TIRE_FL].pwm = 0;
yabahiro 51:a0bdbddd08f7 1843 motor[TIRE_FL].dir = FREE;
yabahiro 51:a0bdbddd08f7 1844 motor[TIRE_BR].pwm = 0;
yabahiro 51:a0bdbddd08f7 1845 motor[TIRE_BR].dir = FREE;
yabahiro 51:a0bdbddd08f7 1846 motor[TIRE_BL].pwm = 40;
yabahiro 51:a0bdbddd08f7 1847 motor[TIRE_BL].dir = BACK;
yabahiro 51:a0bdbddd08f7 1848 if(g[1]==0) {
yabahiro 51:a0bdbddd08f7 1849 countss=0;
yabahiro 51:a0bdbddd08f7 1850 mode=70;
yabahiro 51:a0bdbddd08f7 1851 }
yabahiro 51:a0bdbddd08f7 1852 break;
yabahiro 51:a0bdbddd08f7 1853 case 70:
yabahiro 51:a0bdbddd08f7 1854 switch(g[0]) {
yabahiro 51:a0bdbddd08f7 1855 case 99:
yabahiro 51:a0bdbddd08f7 1856 if(countss>=100000) {
yabahiro 51:a0bdbddd08f7 1857 mode=100;
yabahiro 51:a0bdbddd08f7 1858 }
yabahiro 50:87f5fa1bc460 1859 break;
yabahiro 39:784552c26163 1860 case 98:
yabahiro 39:784552c26163 1861 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1862 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1863 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1864 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1865 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1866 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1867 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1868 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1869 break;
yabahiro 39:784552c26163 1870 case 0:
yabahiro 53:694dbd7dfa0e 1871 motor[TIRE_FR].pwm = 80;
yabahiro 39:784552c26163 1872 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1873 motor[TIRE_FL].pwm = 80;
yabahiro 39:784552c26163 1874 motor[TIRE_FL].dir = FOR;
yabahiro 53:694dbd7dfa0e 1875 motor[TIRE_BR].pwm = 80;
yabahiro 39:784552c26163 1876 motor[TIRE_BR].dir = BACK;
yabahiro 53:694dbd7dfa0e 1877 motor[TIRE_BL].pwm = 80;
yabahiro 39:784552c26163 1878 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1879 mtc=true;
yabahiro 39:784552c26163 1880 break;
yabahiro 39:784552c26163 1881 case 255:
yabahiro 39:784552c26163 1882 motor[TIRE_FR].pwm = fast;
yabahiro 39:784552c26163 1883 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1884 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1885 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1886 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1887 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1888 motor[TIRE_BL].pwm = fast;
yabahiro 39:784552c26163 1889 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1890 mtc=true;
yabahiro 39:784552c26163 1891 break;
yabahiro 39:784552c26163 1892 case 253:
yabahiro 39:784552c26163 1893 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1894 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1895 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1896 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1897 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1898 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1899 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1900 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1901 mtc=true;
yabahiro 39:784552c26163 1902 break;
yabahiro 39:784552c26163 1903 case 254:
yabahiro 39:784552c26163 1904 motor[TIRE_FR].pwm = normal;
yabahiro 39:784552c26163 1905 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1906 motor[TIRE_FL].pwm = 0;
yabahiro 39:784552c26163 1907 motor[TIRE_FL].dir = FREE;
yabahiro 39:784552c26163 1908 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1909 motor[TIRE_BR].dir = FREE;
yabahiro 39:784552c26163 1910 motor[TIRE_BL].pwm = normal;
yabahiro 39:784552c26163 1911 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1912 mtc=true;
yabahiro 39:784552c26163 1913 break;
yabahiro 39:784552c26163 1914 case 1:
yabahiro 39:784552c26163 1915 motor[TIRE_FR].pwm = slow;
yabahiro 39:784552c26163 1916 motor[TIRE_FR].dir = BACK;
yabahiro 39:784552c26163 1917 motor[TIRE_FL].pwm = fast;
yabahiro 39:784552c26163 1918 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1919 motor[TIRE_BR].pwm = fast;
yabahiro 39:784552c26163 1920 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1921 motor[TIRE_BL].pwm = slow;
yabahiro 39:784552c26163 1922 motor[TIRE_BL].dir = FOR;
yabahiro 39:784552c26163 1923 mtc=true;
yabahiro 39:784552c26163 1924 break;
yabahiro 39:784552c26163 1925 case 3:
yabahiro 39:784552c26163 1926 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1927 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1928 motor[TIRE_FL].pwm = slow;
yabahiro 39:784552c26163 1929 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1930 motor[TIRE_BR].pwm = slow;
yabahiro 39:784552c26163 1931 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1932 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1933 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1934 mtc=true;
yabahiro 39:784552c26163 1935 break;
yabahiro 39:784552c26163 1936 case 2:
yabahiro 39:784552c26163 1937 motor[TIRE_FR].pwm = 0;
yabahiro 39:784552c26163 1938 motor[TIRE_FR].dir = FREE;
yabahiro 39:784552c26163 1939 motor[TIRE_FL].pwm = normal;
yabahiro 39:784552c26163 1940 motor[TIRE_FL].dir = FOR;
yabahiro 39:784552c26163 1941 motor[TIRE_BR].pwm = normal;
yabahiro 39:784552c26163 1942 motor[TIRE_BR].dir = BACK;
yabahiro 39:784552c26163 1943 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1944 motor[TIRE_BL].dir = FREE;
yabahiro 39:784552c26163 1945 mtc=true;
yabahiro 39:784552c26163 1946 break;
yabahiro 39:784552c26163 1947 }
yabahiro 39:784552c26163 1948 if(g[1]!=98&&mtc==true) {
yabahiro 39:784552c26163 1949 switch(g[1]) {
yabahiro 39:784552c26163 1950 case 0:
yabahiro 39:784552c26163 1951 mtc=false;
yabahiro 39:784552c26163 1952 break;
yabahiro 39:784552c26163 1953 case 255:
yabahiro 39:784552c26163 1954 motor[TIRE_BR].pwm += 5;
yabahiro 39:784552c26163 1955 mtc=false;
yabahiro 39:784552c26163 1956 break;
yabahiro 39:784552c26163 1957 case 253:
yabahiro 39:784552c26163 1958 motor[TIRE_BR].pwm += 10;
yabahiro 39:784552c26163 1959 mtc=false;
yabahiro 39:784552c26163 1960 break;
yabahiro 39:784552c26163 1961 case 254:
yabahiro 39:784552c26163 1962 motor[TIRE_BR].pwm += 20;
yabahiro 39:784552c26163 1963 motor[TIRE_BL].pwm = 0;
yabahiro 39:784552c26163 1964 mtc=false;
yabahiro 39:784552c26163 1965 break;
yabahiro 39:784552c26163 1966 case 1:
yabahiro 39:784552c26163 1967 motor[TIRE_BL].pwm += 5;
yabahiro 39:784552c26163 1968 mtc=false;
yabahiro 39:784552c26163 1969 break;
yabahiro 39:784552c26163 1970 case 3:
yabahiro 39:784552c26163 1971 motor[TIRE_BL].pwm += 10;
yabahiro 39:784552c26163 1972 mtc=false;
yabahiro 39:784552c26163 1973 break;
yabahiro 39:784552c26163 1974 case 2:
yabahiro 39:784552c26163 1975 motor[TIRE_BL].pwm += 20;
yabahiro 39:784552c26163 1976 motor[TIRE_BR].pwm = 0;
yabahiro 39:784552c26163 1977 mtc=false;
yabahiro 39:784552c26163 1978 break;
yabahiro 39:784552c26163 1979 }
yabahiro 33:dd598bb23ad1 1980 }
yabahiro 51:a0bdbddd08f7 1981 break;
yabahiro 51:a0bdbddd08f7 1982 case 100:
yabahiro 54:4167f10ecad0 1983 motor[TIRE_FR].pwm = 47;
yabahiro 51:a0bdbddd08f7 1984 motor[TIRE_FR].dir = BACK;
yabahiro 51:a0bdbddd08f7 1985 motor[TIRE_FL].pwm = 50;
yabahiro 51:a0bdbddd08f7 1986 motor[TIRE_FL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1987 motor[TIRE_BR].pwm = 50;
yabahiro 51:a0bdbddd08f7 1988 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 1989 motor[TIRE_BL].pwm = 55;
yabahiro 51:a0bdbddd08f7 1990 motor[TIRE_BL].dir = FOR;
yabahiro 51:a0bdbddd08f7 1991 break;
yabahiro 51:a0bdbddd08f7 1992 default:
yabahiro 51:a0bdbddd08f7 1993 motor[TIRE_FR].pwm = 50;
yabahiro 51:a0bdbddd08f7 1994 motor[TIRE_FR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1995 motor[TIRE_FL].pwm = 50;
yabahiro 51:a0bdbddd08f7 1996 motor[TIRE_FL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1997 motor[TIRE_BR].pwm = 50;
yabahiro 51:a0bdbddd08f7 1998 motor[TIRE_BR].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 1999 motor[TIRE_BL].pwm = 50;
yabahiro 51:a0bdbddd08f7 2000 motor[TIRE_BL].dir = BRAKE;
yabahiro 51:a0bdbddd08f7 2001 break;
yabahiro 33:dd598bb23ad1 2002 }
kishibekairohan 13:b6e02d6261d7 2003 }
kishibekairohan 13:b6e02d6261d7 2004 #endif
kishibekairohan 13:b6e02d6261d7 2005
kishibekairohan 13:b6e02d6261d7 2006 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 2007 static void Process3()
kishibekairohan 13:b6e02d6261d7 2008 {
yabahiro 33:dd598bb23ad1 2009 //Clearly 赤ゾーン
kishibekairohan 35:33c6cdba5630 2010 LED::LED::TapeledMode(3);
yabahiro 58:bfc38c43dadb 2011 value=aime.read_ms();
yabahiro 33:dd598bb23ad1 2012 countss++;
yabahiro 54:4167f10ecad0 2013 switch(mode) {
yabahiro 54:4167f10ecad0 2014 case 1:
yabahiro 54:4167f10ecad0 2015 fast=40;
yabahiro 54:4167f10ecad0 2016 normal=30;
yabahiro 54:4167f10ecad0 2017 slow=20;
yabahiro 54:4167f10ecad0 2018 motor[TIRE_FR].pwm = normal+20;
yabahiro 54:4167f10ecad0 2019 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2020 motor[TIRE_FL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2021 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2022 motor[TIRE_BR].pwm = normal+20;
yabahiro 54:4167f10ecad0 2023 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2024 motor[TIRE_BL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2025 motor[TIRE_BL].dir = FOR;
yabahiro 58:bfc38c43dadb 2026 if(g[2]==0&&value>=1500) {
yabahiro 54:4167f10ecad0 2027 countss=100000;
yabahiro 54:4167f10ecad0 2028 cross=0;
yabahiro 54:4167f10ecad0 2029 mode=3;
yabahiro 54:4167f10ecad0 2030 }
yabahiro 54:4167f10ecad0 2031 break;
yabahiro 54:4167f10ecad0 2032 case 3:
yabahiro 54:4167f10ecad0 2033 motor[TIRE_FR].pwm = 255;
yabahiro 54:4167f10ecad0 2034 motor[TIRE_FR].dir = BRAKE;
yabahiro 54:4167f10ecad0 2035 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2036 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2037 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2038 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2039 motor[TIRE_BL].pwm = 255;
yabahiro 54:4167f10ecad0 2040 motor[TIRE_BL].dir = BRAKE;
yabahiro 54:4167f10ecad0 2041 if(g[0]==0) {
yabahiro 54:4167f10ecad0 2042 mode=10;
yabahiro 58:bfc38c43dadb 2043 aime.reset();
yabahiro 54:4167f10ecad0 2044 countss=100000;
yabahiro 56:61ebd4b759b4 2045 fast = 80;
yabahiro 56:61ebd4b759b4 2046 normal = 60;
yabahiro 56:61ebd4b759b4 2047 slow = 40;
yabahiro 54:4167f10ecad0 2048 }
yabahiro 54:4167f10ecad0 2049 break;
yabahiro 54:4167f10ecad0 2050 case 10:
yabahiro 54:4167f10ecad0 2051 switch(g[0]) {
yabahiro 54:4167f10ecad0 2052 case 98:
yabahiro 54:4167f10ecad0 2053 motor[TIRE_FR].pwm = normal+50;
yabahiro 54:4167f10ecad0 2054 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2055 motor[TIRE_FL].pwm = normal+50;
yabahiro 54:4167f10ecad0 2056 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2057 motor[TIRE_BR].pwm = normal+50;
yabahiro 54:4167f10ecad0 2058 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2059 motor[TIRE_BL].pwm = normal+50;
yabahiro 54:4167f10ecad0 2060 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2061 mtc=true;
yabahiro 58:bfc38c43dadb 2062 if(value>=1200) {
yabahiro 58:bfc38c43dadb 2063 cross++;
yabahiro 58:bfc38c43dadb 2064 countss=0;
yabahiro 58:bfc38c43dadb 2065 value=0;
yabahiro 58:bfc38c43dadb 2066 aime.reset();
yabahiro 58:bfc38c43dadb 2067 }
yabahiro 58:bfc38c43dadb 2068 /*
yabahiro 54:4167f10ecad0 2069 if(countss>80000) {
yabahiro 54:4167f10ecad0 2070 cross++;
yabahiro 54:4167f10ecad0 2071 countss=0;
yabahiro 54:4167f10ecad0 2072 }
yabahiro 58:bfc38c43dadb 2073 */
yabahiro 54:4167f10ecad0 2074 if(cross==2) {
yabahiro 54:4167f10ecad0 2075 fast = 10;
yabahiro 54:4167f10ecad0 2076 normal = 10;
yabahiro 54:4167f10ecad0 2077 slow = 5;
yabahiro 54:4167f10ecad0 2078 }
yabahiro 46:68296caae1af 2079
yabahiro 53:694dbd7dfa0e 2080 if(cross==3) {
yabahiro 54:4167f10ecad0 2081 fast=40;
yabahiro 54:4167f10ecad0 2082 normal=30;
yabahiro 54:4167f10ecad0 2083 slow=20;
yabahiro 54:4167f10ecad0 2084 mode=11;
yabahiro 58:bfc38c43dadb 2085 aime.reset();
yabahiro 53:694dbd7dfa0e 2086 countss=0;
yabahiro 53:694dbd7dfa0e 2087 cross=0;
yabahiro 54:4167f10ecad0 2088
yabahiro 53:694dbd7dfa0e 2089 }
yabahiro 54:4167f10ecad0 2090 break;
yabahiro 38:40e7fdf3c694 2091 case 0:
yabahiro 54:4167f10ecad0 2092 motor[TIRE_FR].pwm = fast+50;
yabahiro 54:4167f10ecad0 2093 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 2094 motor[TIRE_FL].pwm = fast+60;
yabahiro 54:4167f10ecad0 2095 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2096 motor[TIRE_BR].pwm = fast+50;
yabahiro 54:4167f10ecad0 2097 motor[TIRE_BR].dir = BACK;
yabahiro 56:61ebd4b759b4 2098 motor[TIRE_BL].pwm = fast+45;
yabahiro 54:4167f10ecad0 2099 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2100 mtc=true;
yabahiro 38:40e7fdf3c694 2101 break;
yabahiro 38:40e7fdf3c694 2102 case 255:
yabahiro 54:4167f10ecad0 2103 motor[TIRE_FR].pwm = fast+50;
yabahiro 54:4167f10ecad0 2104 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 2105 motor[TIRE_FL].pwm = slow+60;
yabahiro 54:4167f10ecad0 2106 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2107 motor[TIRE_BR].pwm = slow+50;
yabahiro 54:4167f10ecad0 2108 motor[TIRE_BR].dir = BACK;
yabahiro 56:61ebd4b759b4 2109 motor[TIRE_BL].pwm = fast+45;
yabahiro 54:4167f10ecad0 2110 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2111 mtc=true;
yabahiro 53:694dbd7dfa0e 2112 break;
yabahiro 53:694dbd7dfa0e 2113 case 253:
yabahiro 54:4167f10ecad0 2114 motor[TIRE_FR].pwm = slow+50;
yabahiro 54:4167f10ecad0 2115 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2116 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2117 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2118 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2119 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 2120 motor[TIRE_BL].pwm = slow+45;
yabahiro 54:4167f10ecad0 2121 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2122 mtc=true;
yabahiro 53:694dbd7dfa0e 2123 break;
yabahiro 53:694dbd7dfa0e 2124 case 254:
yabahiro 54:4167f10ecad0 2125 motor[TIRE_FR].pwm = normal+50;
yabahiro 54:4167f10ecad0 2126 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 2127 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2128 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2129 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2130 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 2131 motor[TIRE_BL].pwm = normal+45;
yabahiro 54:4167f10ecad0 2132 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2133 mtc=true;
yabahiro 53:694dbd7dfa0e 2134 break;
yabahiro 53:694dbd7dfa0e 2135 case 1:
yabahiro 54:4167f10ecad0 2136 motor[TIRE_FR].pwm = slow+50;
yabahiro 54:4167f10ecad0 2137 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 2138 motor[TIRE_FL].pwm = fast+60;
yabahiro 54:4167f10ecad0 2139 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2140 motor[TIRE_BR].pwm = fast+50;
yabahiro 54:4167f10ecad0 2141 motor[TIRE_BR].dir = BACK;
yabahiro 56:61ebd4b759b4 2142 motor[TIRE_BL].pwm = slow+45;
yabahiro 54:4167f10ecad0 2143 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2144 mtc=true;
yabahiro 53:694dbd7dfa0e 2145 break;
yabahiro 53:694dbd7dfa0e 2146 case 3:
yabahiro 54:4167f10ecad0 2147 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2148 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 2149 motor[TIRE_FL].pwm = slow+60;
yabahiro 54:4167f10ecad0 2150 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2151 motor[TIRE_BR].pwm = slow+50;
yabahiro 54:4167f10ecad0 2152 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2153 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2154 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2155 mtc=true;
yabahiro 53:694dbd7dfa0e 2156 break;
yabahiro 53:694dbd7dfa0e 2157 case 2:
yabahiro 54:4167f10ecad0 2158 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2159 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 2160 motor[TIRE_FL].pwm = normal+60;
yabahiro 54:4167f10ecad0 2161 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2162 motor[TIRE_BR].pwm = normal+50;
yabahiro 54:4167f10ecad0 2163 motor[TIRE_BR].dir = BACK;
yabahiro 53:694dbd7dfa0e 2164 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2165 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2166 mtc=true;
yabahiro 53:694dbd7dfa0e 2167 break;
yabahiro 53:694dbd7dfa0e 2168 }
yabahiro 54:4167f10ecad0 2169 if(g[0]!=98&&mtc==true) {
yabahiro 54:4167f10ecad0 2170 switch(g[1]) {
yabahiro 54:4167f10ecad0 2171 case 0:
yabahiro 54:4167f10ecad0 2172 mtc=false;
yabahiro 54:4167f10ecad0 2173 break;
yabahiro 54:4167f10ecad0 2174 case 255:
yabahiro 54:4167f10ecad0 2175 motor[TIRE_BR].pwm += 3;
yabahiro 54:4167f10ecad0 2176 mtc=false;
yabahiro 54:4167f10ecad0 2177 break;
yabahiro 54:4167f10ecad0 2178 case 253:
yabahiro 54:4167f10ecad0 2179 motor[TIRE_BR].pwm += 6;
yabahiro 54:4167f10ecad0 2180 mtc=false;
yabahiro 54:4167f10ecad0 2181 break;
yabahiro 54:4167f10ecad0 2182 case 254:
yabahiro 54:4167f10ecad0 2183 motor[TIRE_BR].pwm += 9;
yabahiro 54:4167f10ecad0 2184 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2185 mtc=false;
yabahiro 54:4167f10ecad0 2186 break;
yabahiro 54:4167f10ecad0 2187 case 1:
yabahiro 54:4167f10ecad0 2188 motor[TIRE_BL].pwm += 3;
yabahiro 54:4167f10ecad0 2189 mtc=false;
yabahiro 54:4167f10ecad0 2190 break;
yabahiro 54:4167f10ecad0 2191 case 3:
yabahiro 54:4167f10ecad0 2192 motor[TIRE_BL].pwm += 6;
yabahiro 54:4167f10ecad0 2193 mtc=false;
yabahiro 54:4167f10ecad0 2194 break;
yabahiro 54:4167f10ecad0 2195 case 2:
yabahiro 54:4167f10ecad0 2196 motor[TIRE_BL].pwm += 9;
yabahiro 54:4167f10ecad0 2197 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2198 mtc=false;
yabahiro 54:4167f10ecad0 2199 break;
yabahiro 54:4167f10ecad0 2200 }
yabahiro 54:4167f10ecad0 2201 }
yabahiro 54:4167f10ecad0 2202 break;
yabahiro 54:4167f10ecad0 2203 case 11:
yabahiro 54:4167f10ecad0 2204 motor[TIRE_FR].pwm = 15;
yabahiro 54:4167f10ecad0 2205 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2206 motor[TIRE_FL].pwm = normal;
yabahiro 54:4167f10ecad0 2207 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2208 motor[TIRE_BR].pwm = normal;
yabahiro 54:4167f10ecad0 2209 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2210 motor[TIRE_BL].pwm = 15;
yabahiro 54:4167f10ecad0 2211 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2212 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 53:694dbd7dfa0e 2213 countss=0;
yabahiro 54:4167f10ecad0 2214 cross=0;
yabahiro 54:4167f10ecad0 2215 mode=20;
yabahiro 53:694dbd7dfa0e 2216 }
yabahiro 54:4167f10ecad0 2217 break;
yabahiro 54:4167f10ecad0 2218 case 20:
yabahiro 54:4167f10ecad0 2219 solenoid.solenoid4= SOLENOID_ON;
yabahiro 53:694dbd7dfa0e 2220 switch(g[2]) {
yabahiro 54:4167f10ecad0 2221 case 98:
yabahiro 54:4167f10ecad0 2222 motor[TIRE_FR].pwm = normal+20;
yabahiro 54:4167f10ecad0 2223 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2224 motor[TIRE_FL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2225 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2226 motor[TIRE_BR].pwm = normal+20;
yabahiro 54:4167f10ecad0 2227 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2228 motor[TIRE_BL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2229 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2230 if(countss>30000) {
yabahiro 54:4167f10ecad0 2231 cross++;
yabahiro 54:4167f10ecad0 2232 countss=0;
yabahiro 54:4167f10ecad0 2233 }
yabahiro 54:4167f10ecad0 2234 mtc2=true;
yabahiro 54:4167f10ecad0 2235
yabahiro 54:4167f10ecad0 2236 if(YELLOW) {
yabahiro 54:4167f10ecad0 2237 if(cross==1) {
yabahiro 54:4167f10ecad0 2238 fast=20;
yabahiro 54:4167f10ecad0 2239 normal=15;
yabahiro 54:4167f10ecad0 2240 slow=15;
yabahiro 54:4167f10ecad0 2241 } else if(cross==2) {
yabahiro 54:4167f10ecad0 2242 mode=21;
yabahiro 54:4167f10ecad0 2243 countss=0;
yabahiro 54:4167f10ecad0 2244 cross=0;
yabahiro 54:4167f10ecad0 2245 }
yabahiro 54:4167f10ecad0 2246 } else {
yabahiro 54:4167f10ecad0 2247 if(cross==3) {
yabahiro 54:4167f10ecad0 2248 mode=21;
yabahiro 54:4167f10ecad0 2249 countss=0;
yabahiro 54:4167f10ecad0 2250 cross=0;
yabahiro 54:4167f10ecad0 2251 }
yabahiro 54:4167f10ecad0 2252 }
yabahiro 54:4167f10ecad0 2253 break;
yabahiro 53:694dbd7dfa0e 2254 case 0:
yabahiro 54:4167f10ecad0 2255 motor[TIRE_FR].pwm = fast+20;
yabahiro 54:4167f10ecad0 2256 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2257 motor[TIRE_FL].pwm = fast+20;
yabahiro 54:4167f10ecad0 2258 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2259 motor[TIRE_BR].pwm = fast+20;
yabahiro 54:4167f10ecad0 2260 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2261 motor[TIRE_BL].pwm = fast+20;
yabahiro 54:4167f10ecad0 2262 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2263 mtc2=true;
yabahiro 53:694dbd7dfa0e 2264 break;
yabahiro 53:694dbd7dfa0e 2265 case 255:
yabahiro 54:4167f10ecad0 2266 motor[TIRE_FR].pwm = normal+20;
yabahiro 54:4167f10ecad0 2267 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2268 motor[TIRE_FL].pwm = fast+20;
yabahiro 54:4167f10ecad0 2269 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2270 motor[TIRE_BR].pwm = fast+20;
yabahiro 54:4167f10ecad0 2271 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2272 motor[TIRE_BL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2273 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2274 mtc2=true;
yabahiro 38:40e7fdf3c694 2275 break;
yabahiro 38:40e7fdf3c694 2276 case 253:
yabahiro 54:4167f10ecad0 2277 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2278 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2279 motor[TIRE_FL].pwm = slow+20;
yabahiro 54:4167f10ecad0 2280 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2281 motor[TIRE_BR].pwm = slow+20;
yabahiro 54:4167f10ecad0 2282 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2283 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2284 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2285 mtc2=true;
yabahiro 38:40e7fdf3c694 2286 break;
yabahiro 38:40e7fdf3c694 2287 case 254:
yabahiro 54:4167f10ecad0 2288 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2289 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2290 motor[TIRE_FL].pwm = normal+20;
yabahiro 54:4167f10ecad0 2291 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2292 motor[TIRE_BR].pwm = normal+20;
yabahiro 53:694dbd7dfa0e 2293 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2294 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2295 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2296 mtc2=true;
yabahiro 53:694dbd7dfa0e 2297 break;
yabahiro 53:694dbd7dfa0e 2298 case 1:
yabahiro 54:4167f10ecad0 2299 motor[TIRE_FR].pwm = fast+20;
yabahiro 54:4167f10ecad0 2300 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2301 motor[TIRE_FL].pwm = slow+20;
yabahiro 54:4167f10ecad0 2302 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2303 motor[TIRE_BR].pwm = slow+20;
yabahiro 54:4167f10ecad0 2304 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2305 motor[TIRE_BL].pwm = fast+20;
yabahiro 54:4167f10ecad0 2306 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2307 mtc2=true;
yabahiro 53:694dbd7dfa0e 2308 break;
yabahiro 53:694dbd7dfa0e 2309 case 3:
yabahiro 54:4167f10ecad0 2310 motor[TIRE_FR].pwm = slow+20;
yabahiro 54:4167f10ecad0 2311 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2312 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2313 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2314 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2315 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2316 motor[TIRE_BL].pwm = slow+20;
yabahiro 54:4167f10ecad0 2317 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2318 mtc2=true;
yabahiro 53:694dbd7dfa0e 2319 break;
yabahiro 53:694dbd7dfa0e 2320 case 2:
yabahiro 54:4167f10ecad0 2321 motor[TIRE_FR].pwm = normal+20;
yabahiro 53:694dbd7dfa0e 2322 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 2323 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 2324 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 2325 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 2326 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2327 motor[TIRE_BL].pwm = normal+20;
yabahiro 38:40e7fdf3c694 2328 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2329 mtc2=true;
yabahiro 38:40e7fdf3c694 2330 break;
yabahiro 33:dd598bb23ad1 2331 }
yabahiro 54:4167f10ecad0 2332 if(g[2]!=98&&mtc2==true) {
yabahiro 54:4167f10ecad0 2333 switch(g[3]) {
yabahiro 54:4167f10ecad0 2334 case 0:
yabahiro 54:4167f10ecad0 2335 mtc2=false;
yabahiro 54:4167f10ecad0 2336 break;
yabahiro 54:4167f10ecad0 2337 case 255:
yabahiro 54:4167f10ecad0 2338 motor[TIRE_BL].pwm += 5;
yabahiro 54:4167f10ecad0 2339 mtc2=false;
yabahiro 54:4167f10ecad0 2340 break;
yabahiro 54:4167f10ecad0 2341 case 253:
yabahiro 54:4167f10ecad0 2342 motor[TIRE_BL].pwm += 10;
yabahiro 54:4167f10ecad0 2343 mtc2=false;
yabahiro 54:4167f10ecad0 2344 break;
yabahiro 54:4167f10ecad0 2345 case 254:
yabahiro 54:4167f10ecad0 2346 motor[TIRE_BL].pwm += 15;
yabahiro 54:4167f10ecad0 2347 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2348 mtc2=false;
yabahiro 54:4167f10ecad0 2349 break;
yabahiro 54:4167f10ecad0 2350 case 1:
yabahiro 54:4167f10ecad0 2351 motor[TIRE_FL].pwm += 5;
yabahiro 54:4167f10ecad0 2352 mtc2=false;
yabahiro 54:4167f10ecad0 2353 break;
yabahiro 54:4167f10ecad0 2354 case 3:
yabahiro 54:4167f10ecad0 2355 motor[TIRE_FL].pwm += 10;
yabahiro 54:4167f10ecad0 2356 mtc2=false;
yabahiro 54:4167f10ecad0 2357 break;
yabahiro 54:4167f10ecad0 2358 case 2:
yabahiro 54:4167f10ecad0 2359 motor[TIRE_FL].pwm += 15;
yabahiro 54:4167f10ecad0 2360 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2361 mtc2=false;
yabahiro 54:4167f10ecad0 2362 break;
yabahiro 54:4167f10ecad0 2363 }
yabahiro 38:40e7fdf3c694 2364 }
yabahiro 54:4167f10ecad0 2365 break;
yabahiro 54:4167f10ecad0 2366 case 21:
yabahiro 54:4167f10ecad0 2367 if(YELLOW) {
yabahiro 54:4167f10ecad0 2368 motor[TIRE_FR].pwm = 20;
yabahiro 54:4167f10ecad0 2369 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2370 motor[TIRE_FL].pwm = 20;
yabahiro 54:4167f10ecad0 2371 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2372 motor[TIRE_BR].pwm = 20;
yabahiro 54:4167f10ecad0 2373 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2374 motor[TIRE_BL].pwm = 20;
yabahiro 54:4167f10ecad0 2375 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2376 if(g[3]==98&&countss>=10000) {
yabahiro 54:4167f10ecad0 2377 mode=22;
yabahiro 58:bfc38c43dadb 2378 aime.reset();
yabahiro 54:4167f10ecad0 2379 countss=0;
yabahiro 54:4167f10ecad0 2380 fast=60;
yabahiro 54:4167f10ecad0 2381 normal=40;
yabahiro 54:4167f10ecad0 2382 slow=20;
yabahiro 54:4167f10ecad0 2383 }
yabahiro 54:4167f10ecad0 2384 } else {
yabahiro 38:40e7fdf3c694 2385 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2386 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2387 motor[TIRE_FL].pwm = 255;
yabahiro 54:4167f10ecad0 2388 motor[TIRE_FL].dir = BRAKE;
yabahiro 54:4167f10ecad0 2389 motor[TIRE_BR].pwm = 255;
yabahiro 54:4167f10ecad0 2390 motor[TIRE_BR].dir = BRAKE;
yabahiro 38:40e7fdf3c694 2391 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2392 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2393 if(g[1]==0) {
yabahiro 54:4167f10ecad0 2394 mode=22;
yabahiro 58:bfc38c43dadb 2395 aime.reset();
yabahiro 54:4167f10ecad0 2396 fast=60;
yabahiro 54:4167f10ecad0 2397 normal=40;
yabahiro 54:4167f10ecad0 2398 slow=25;
yabahiro 54:4167f10ecad0 2399 }
yabahiro 38:40e7fdf3c694 2400 }
yabahiro 54:4167f10ecad0 2401 break;
yabahiro 54:4167f10ecad0 2402 case 22:
yabahiro 54:4167f10ecad0 2403 if(YELLOW) {
yabahiro 54:4167f10ecad0 2404 if(g[4]==3||g[4]==2) {
yabahiro 38:40e7fdf3c694 2405 motor[TIRE_FR].pwm = 30;
yabahiro 38:40e7fdf3c694 2406 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2407 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2408 motor[TIRE_FL].dir = FOR;
yabahiro 38:40e7fdf3c694 2409 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2410 motor[TIRE_BR].dir = BACK;
yabahiro 38:40e7fdf3c694 2411 motor[TIRE_BL].pwm = 30;
yabahiro 38:40e7fdf3c694 2412 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2413 }
yabahiro 54:4167f10ecad0 2414 if(g[4]==253||g[4]==254) {
yabahiro 54:4167f10ecad0 2415 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2416 motor[TIRE_FR].dir = BACK;
yabahiro 38:40e7fdf3c694 2417 motor[TIRE_FL].pwm = 30;
yabahiro 38:40e7fdf3c694 2418 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2419 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2420 motor[TIRE_BR].dir = FOR;
yabahiro 53:694dbd7dfa0e 2421 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2422 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2423 }
yabahiro 54:4167f10ecad0 2424 switch(g[2]) {
yabahiro 54:4167f10ecad0 2425 case 0:
yabahiro 54:4167f10ecad0 2426 motor[TIRE_FR].pwm = 20;
yabahiro 54:4167f10ecad0 2427 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2428 motor[TIRE_BR].pwm = 20;
yabahiro 54:4167f10ecad0 2429 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2430 break;
yabahiro 54:4167f10ecad0 2431 case 255:
yabahiro 54:4167f10ecad0 2432 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2433 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2434 motor[TIRE_BR].pwm = 20;
yabahiro 54:4167f10ecad0 2435 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2436 break;
yabahiro 54:4167f10ecad0 2437 case 253:
yabahiro 54:4167f10ecad0 2438 motor[TIRE_FR].pwm = 40;
yabahiro 54:4167f10ecad0 2439 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2440 motor[TIRE_BR].pwm = 20;
yabahiro 54:4167f10ecad0 2441 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2442 break;
yabahiro 54:4167f10ecad0 2443 case 254:
yabahiro 54:4167f10ecad0 2444 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2445 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2446 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2447 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2448 break;
yabahiro 54:4167f10ecad0 2449 case 1:
yabahiro 54:4167f10ecad0 2450 motor[TIRE_FR].pwm = 20;
yabahiro 54:4167f10ecad0 2451 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2452 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2453 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2454 break;
yabahiro 54:4167f10ecad0 2455 case 3:
yabahiro 54:4167f10ecad0 2456 motor[TIRE_FR].pwm = 20;
yabahiro 54:4167f10ecad0 2457 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2458 motor[TIRE_BR].pwm = 40;
yabahiro 54:4167f10ecad0 2459 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2460 break;
yabahiro 54:4167f10ecad0 2461 case 2:
yabahiro 54:4167f10ecad0 2462 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2463 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2464 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2465 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2466 break;
yabahiro 54:4167f10ecad0 2467 }
yabahiro 54:4167f10ecad0 2468 switch(g[3]) {
yabahiro 54:4167f10ecad0 2469 case 0:
yabahiro 54:4167f10ecad0 2470 motor[TIRE_FL].pwm = 20;
yabahiro 54:4167f10ecad0 2471 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2472 motor[TIRE_BL].pwm = 20;
yabahiro 54:4167f10ecad0 2473 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2474 break;
yabahiro 54:4167f10ecad0 2475 case 255:
yabahiro 54:4167f10ecad0 2476 motor[TIRE_FL].pwm = 20;
yabahiro 54:4167f10ecad0 2477 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2478 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2479 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2480 case 253:
yabahiro 54:4167f10ecad0 2481 motor[TIRE_FL].pwm = 20;
yabahiro 54:4167f10ecad0 2482 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2483 motor[TIRE_BL].pwm = 40;
yabahiro 54:4167f10ecad0 2484 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2485 break;
yabahiro 54:4167f10ecad0 2486 case 254:
yabahiro 54:4167f10ecad0 2487 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2488 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2489 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2490 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2491 break;
yabahiro 54:4167f10ecad0 2492 case 1:
yabahiro 54:4167f10ecad0 2493 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2494 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2495 motor[TIRE_BL].pwm = 20;
yabahiro 54:4167f10ecad0 2496 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2497 break;
yabahiro 54:4167f10ecad0 2498 case 3:
yabahiro 54:4167f10ecad0 2499 motor[TIRE_FL].pwm = 40;
yabahiro 54:4167f10ecad0 2500 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2501 motor[TIRE_BL].pwm = 20;
yabahiro 54:4167f10ecad0 2502 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2503 break;
yabahiro 54:4167f10ecad0 2504 case 2:
yabahiro 54:4167f10ecad0 2505 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2506 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2507 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2508 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2509 break;
yabahiro 54:4167f10ecad0 2510 }
yabahiro 54:4167f10ecad0 2511 if(g[3]==0&&g[2]==0) {
yabahiro 54:4167f10ecad0 2512 mode=23;
yabahiro 58:bfc38c43dadb 2513 aime.reset();
yabahiro 54:4167f10ecad0 2514 countss=0;
yabahiro 56:61ebd4b759b4 2515 fast = 60;
yabahiro 56:61ebd4b759b4 2516 normal = 40;
yabahiro 54:4167f10ecad0 2517 slow = 30;
yabahiro 54:4167f10ecad0 2518 }
yabahiro 54:4167f10ecad0 2519 } else {
yabahiro 60:24ebb611f26e 2520 if(value>=2400) {
yabahiro 58:bfc38c43dadb 2521 slow=25;
yabahiro 58:bfc38c43dadb 2522 normal=30;
yabahiro 58:bfc38c43dadb 2523 fast=40;
yabahiro 58:bfc38c43dadb 2524 }
yabahiro 58:bfc38c43dadb 2525 /*
yabahiro 54:4167f10ecad0 2526 if(g[1]==98) {
yabahiro 54:4167f10ecad0 2527 slow=25;
yabahiro 54:4167f10ecad0 2528 normal=30;
yabahiro 54:4167f10ecad0 2529 fast=40;
yabahiro 54:4167f10ecad0 2530 }
yabahiro 60:24ebb611f26e 2531
yabahiro 58:bfc38c43dadb 2532 */
yabahiro 60:24ebb611f26e 2533 if(value>=3000) {
yabahiro 54:4167f10ecad0 2534 slow=25;
yabahiro 54:4167f10ecad0 2535 normal=40;
yabahiro 54:4167f10ecad0 2536 fast=60;
yabahiro 54:4167f10ecad0 2537 mode=23;
yabahiro 60:24ebb611f26e 2538 value=0;
yabahiro 60:24ebb611f26e 2539 aime.reset();
yabahiro 60:24ebb611f26e 2540 }
yabahiro 54:4167f10ecad0 2541 switch(g[1]) {
yabahiro 54:4167f10ecad0 2542 case 98:
yabahiro 54:4167f10ecad0 2543 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 54:4167f10ecad0 2544 fast=40;
yabahiro 54:4167f10ecad0 2545 normal=30;
yabahiro 54:4167f10ecad0 2546 slow=25;
yabahiro 54:4167f10ecad0 2547 motor[TIRE_FR].pwm = normal;
yabahiro 54:4167f10ecad0 2548 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2549 motor[TIRE_FL].pwm = normal;
yabahiro 54:4167f10ecad0 2550 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2551 motor[TIRE_BR].pwm = normal;
yabahiro 54:4167f10ecad0 2552 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2553 motor[TIRE_BL].pwm = normal;
yabahiro 54:4167f10ecad0 2554 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2555 break;
yabahiro 54:4167f10ecad0 2556 case 0:
yabahiro 54:4167f10ecad0 2557 motor[TIRE_FR].pwm = fast;
yabahiro 54:4167f10ecad0 2558 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2559 motor[TIRE_FL].pwm = fast;
yabahiro 54:4167f10ecad0 2560 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2561 motor[TIRE_BR].pwm = fast;
yabahiro 54:4167f10ecad0 2562 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2563 motor[TIRE_BL].pwm = fast;
yabahiro 54:4167f10ecad0 2564 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2565 mtc=true;
yabahiro 54:4167f10ecad0 2566 break;
yabahiro 54:4167f10ecad0 2567 case 255:
yabahiro 54:4167f10ecad0 2568 motor[TIRE_FR].pwm = fast;
yabahiro 54:4167f10ecad0 2569 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2570 motor[TIRE_FL].pwm = slow;
yabahiro 54:4167f10ecad0 2571 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2572 motor[TIRE_BR].pwm = slow;
yabahiro 54:4167f10ecad0 2573 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2574 motor[TIRE_BL].pwm = fast;
yabahiro 54:4167f10ecad0 2575 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2576 mtc=true;
yabahiro 54:4167f10ecad0 2577 break;
yabahiro 54:4167f10ecad0 2578 case 253:
yabahiro 54:4167f10ecad0 2579 motor[TIRE_FR].pwm = slow;
yabahiro 54:4167f10ecad0 2580 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2581 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2582 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2583 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2584 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2585 motor[TIRE_BL].pwm = slow;
yabahiro 54:4167f10ecad0 2586 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2587 mtc=true;
yabahiro 54:4167f10ecad0 2588 break;
yabahiro 54:4167f10ecad0 2589 case 254:
yabahiro 54:4167f10ecad0 2590 motor[TIRE_FR].pwm = normal;
yabahiro 54:4167f10ecad0 2591 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2592 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2593 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2594 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2595 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2596 motor[TIRE_BL].pwm = normal;
yabahiro 54:4167f10ecad0 2597 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2598 mtc=true;
yabahiro 54:4167f10ecad0 2599 break;
yabahiro 54:4167f10ecad0 2600 case 1:
yabahiro 54:4167f10ecad0 2601 motor[TIRE_FR].pwm = slow;
yabahiro 54:4167f10ecad0 2602 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2603 motor[TIRE_FL].pwm = fast;
yabahiro 54:4167f10ecad0 2604 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2605 motor[TIRE_BR].pwm = fast;
yabahiro 54:4167f10ecad0 2606 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2607 motor[TIRE_BL].pwm = slow;
yabahiro 54:4167f10ecad0 2608 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2609 mtc=true;
yabahiro 54:4167f10ecad0 2610 break;
yabahiro 54:4167f10ecad0 2611 case 3:
yabahiro 54:4167f10ecad0 2612 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2613 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2614 motor[TIRE_FL].pwm = slow;
yabahiro 54:4167f10ecad0 2615 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2616 motor[TIRE_BR].pwm = slow;
yabahiro 54:4167f10ecad0 2617 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2618 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2619 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2620 mtc=true;
yabahiro 54:4167f10ecad0 2621 break;
yabahiro 54:4167f10ecad0 2622 case 2:
yabahiro 54:4167f10ecad0 2623 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2624 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2625 motor[TIRE_FL].pwm = normal;
yabahiro 54:4167f10ecad0 2626 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2627 motor[TIRE_BR].pwm = normal;
yabahiro 54:4167f10ecad0 2628 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2629 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2630 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2631 mtc=true;
yabahiro 54:4167f10ecad0 2632 break;
yabahiro 54:4167f10ecad0 2633 }
yabahiro 54:4167f10ecad0 2634 if(g[1]!=98&&mtc==true) {
yabahiro 54:4167f10ecad0 2635 switch(g[0]) {
yabahiro 54:4167f10ecad0 2636 case 0:
yabahiro 54:4167f10ecad0 2637 mtc=false;
yabahiro 54:4167f10ecad0 2638 break;
yabahiro 54:4167f10ecad0 2639 case 255:
yabahiro 54:4167f10ecad0 2640 motor[TIRE_FL].pwm += 3;
yabahiro 54:4167f10ecad0 2641 mtc=false;
yabahiro 54:4167f10ecad0 2642 break;
yabahiro 54:4167f10ecad0 2643 case 253:
yabahiro 54:4167f10ecad0 2644 motor[TIRE_FL].pwm += 6;
yabahiro 54:4167f10ecad0 2645 mtc=false;
yabahiro 54:4167f10ecad0 2646 break;
yabahiro 54:4167f10ecad0 2647 case 254:
yabahiro 54:4167f10ecad0 2648 motor[TIRE_FL].pwm += 9;
yabahiro 54:4167f10ecad0 2649 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2650 mtc=false;
yabahiro 54:4167f10ecad0 2651 break;
yabahiro 54:4167f10ecad0 2652 case 1:
yabahiro 54:4167f10ecad0 2653 motor[TIRE_FR].pwm += 3;
yabahiro 54:4167f10ecad0 2654 mtc=false;
yabahiro 54:4167f10ecad0 2655 break;
yabahiro 54:4167f10ecad0 2656 case 3:
yabahiro 54:4167f10ecad0 2657 motor[TIRE_FR].pwm += 6;
yabahiro 54:4167f10ecad0 2658 mtc=false;
yabahiro 54:4167f10ecad0 2659 break;
yabahiro 54:4167f10ecad0 2660 case 2:
yabahiro 54:4167f10ecad0 2661 motor[TIRE_FR].pwm += 9;
yabahiro 54:4167f10ecad0 2662 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2663 mtc=false;
yabahiro 54:4167f10ecad0 2664 break;
yabahiro 54:4167f10ecad0 2665 }
yabahiro 54:4167f10ecad0 2666 }
yabahiro 54:4167f10ecad0 2667 }
yabahiro 54:4167f10ecad0 2668 break;
yabahiro 54:4167f10ecad0 2669 case 23:
yabahiro 54:4167f10ecad0 2670 if(YELLOW) {
yabahiro 54:4167f10ecad0 2671 switch(g[4]) {
yabahiro 54:4167f10ecad0 2672 case 98:
yabahiro 54:4167f10ecad0 2673 motor[TIRE_FR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2674 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2675 motor[TIRE_FL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2676 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2677 motor[TIRE_BR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2678 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2679 motor[TIRE_BL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2680 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2681 break;
yabahiro 54:4167f10ecad0 2682 case 99:
yabahiro 54:4167f10ecad0 2683 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2684 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2685 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2686 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2687 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2688 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2689 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2690 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2691 break;
yabahiro 54:4167f10ecad0 2692 case 0:
yabahiro 54:4167f10ecad0 2693 motor[TIRE_FR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2694 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2695 motor[TIRE_FL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2696 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2697 motor[TIRE_BR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2698 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2699 motor[TIRE_BL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2700 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2701 break;
yabahiro 54:4167f10ecad0 2702 case 255:
yabahiro 54:4167f10ecad0 2703 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2704 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2705 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2706 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2707 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2708 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2709 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2710 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2711 break;
yabahiro 54:4167f10ecad0 2712 case 253:
yabahiro 54:4167f10ecad0 2713 motor[TIRE_FR].pwm = slow+5;
yabahiro 54:4167f10ecad0 2714 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2715 motor[TIRE_FL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2716 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2717 motor[TIRE_BR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2718 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2719 motor[TIRE_BL].pwm = slow+5;
yabahiro 54:4167f10ecad0 2720 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2721 break;
yabahiro 54:4167f10ecad0 2722 case 254:
yabahiro 54:4167f10ecad0 2723 motor[TIRE_FR].pwm = normal+5;
yabahiro 54:4167f10ecad0 2724 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2725 motor[TIRE_FL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2726 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2727 motor[TIRE_BR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2728 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2729 motor[TIRE_BL].pwm = normal+5;
yabahiro 54:4167f10ecad0 2730 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2731 break;
yabahiro 54:4167f10ecad0 2732 case 1:
yabahiro 54:4167f10ecad0 2733 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 2734 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2735 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2736 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2737 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2738 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2739 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 2740 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2741 break;
yabahiro 54:4167f10ecad0 2742 case 3:
yabahiro 54:4167f10ecad0 2743 motor[TIRE_FR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2744 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2745 motor[TIRE_FL].pwm = slow+5;
yabahiro 54:4167f10ecad0 2746 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2747 motor[TIRE_BR].pwm = slow+5;
yabahiro 54:4167f10ecad0 2748 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2749 motor[TIRE_BL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2750 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2751 break;
yabahiro 54:4167f10ecad0 2752 case 2:
yabahiro 54:4167f10ecad0 2753 motor[TIRE_FR].pwm = fast-5;
yabahiro 54:4167f10ecad0 2754 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2755 motor[TIRE_FL].pwm = normal+5;
yabahiro 54:4167f10ecad0 2756 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2757 motor[TIRE_BR].pwm = normal+5;
yabahiro 54:4167f10ecad0 2758 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2759 motor[TIRE_BL].pwm = fast-5;
yabahiro 54:4167f10ecad0 2760 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2761 break;
yabahiro 54:4167f10ecad0 2762 }
yabahiro 60:24ebb611f26e 2763 if(value>=1800) {
yabahiro 58:bfc38c43dadb 2764 fast=20;
yabahiro 58:bfc38c43dadb 2765 normal=15;
yabahiro 54:4167f10ecad0 2766 slow=10;
yabahiro 54:4167f10ecad0 2767 }
yabahiro 60:24ebb611f26e 2768 if(value>=2600) {
yabahiro 54:4167f10ecad0 2769 mode=24;
yabahiro 54:4167f10ecad0 2770 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 56:61ebd4b759b4 2771 aime.reset();
yabahiro 54:4167f10ecad0 2772 countss=0;
yabahiro 54:4167f10ecad0 2773 fast=80;
yabahiro 54:4167f10ecad0 2774 normal=60;
yabahiro 54:4167f10ecad0 2775 slow=30;
yabahiro 54:4167f10ecad0 2776 }
yabahiro 58:bfc38c43dadb 2777
yabahiro 58:bfc38c43dadb 2778 /*
yabahiro 58:bfc38c43dadb 2779 if(value>=2500) {
yabahiro 58:bfc38c43dadb 2780 mode=24;
yabahiro 58:bfc38c43dadb 2781 aime.reset();
yabahiro 58:bfc38c43dadb 2782 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 58:bfc38c43dadb 2783 countss=0;
yabahiro 58:bfc38c43dadb 2784 fast=80;
yabahiro 58:bfc38c43dadb 2785 normal=60;
yabahiro 58:bfc38c43dadb 2786 slow=30;
yabahiro 58:bfc38c43dadb 2787 }
yabahiro 58:bfc38c43dadb 2788 if(g[1]==98) {
yabahiro 58:bfc38c43dadb 2789 fast=10;
yabahiro 58:bfc38c43dadb 2790 normal=10;
yabahiro 58:bfc38c43dadb 2791 slow=10;
yabahiro 58:bfc38c43dadb 2792 }
yabahiro 58:bfc38c43dadb 2793 */
yabahiro 54:4167f10ecad0 2794 } else {
yabahiro 54:4167f10ecad0 2795 if(UPLS||DOLS) {
yabahiro 54:4167f10ecad0 2796 if(UPLS||DOLS) {
yabahiro 54:4167f10ecad0 2797 motor[MOTOR_0].pwm = 200;
yabahiro 54:4167f10ecad0 2798 motor[MOTOR_0].dir = BRAKE;
yabahiro 54:4167f10ecad0 2799 fast=60;
yabahiro 54:4167f10ecad0 2800 normal=40;
yabahiro 54:4167f10ecad0 2801 slow=30;
yabahiro 54:4167f10ecad0 2802 countss=0;
yabahiro 54:4167f10ecad0 2803 mode=24;
yabahiro 54:4167f10ecad0 2804 }
yabahiro 54:4167f10ecad0 2805 } else {
yabahiro 54:4167f10ecad0 2806 motor[MOTOR_0].pwm = 100;
yabahiro 54:4167f10ecad0 2807 motor[MOTOR_0].dir = BACK;
yabahiro 54:4167f10ecad0 2808 }
yabahiro 54:4167f10ecad0 2809 motor[TIRE_FR].pwm = 100;
yabahiro 54:4167f10ecad0 2810 motor[TIRE_FR].dir = BRAKE;
yabahiro 54:4167f10ecad0 2811 motor[TIRE_FL].pwm = 100;
yabahiro 54:4167f10ecad0 2812 motor[TIRE_FL].dir = BRAKE;
yabahiro 54:4167f10ecad0 2813 motor[TIRE_BR].pwm = 100;
yabahiro 54:4167f10ecad0 2814 motor[TIRE_BR].dir = BRAKE;
yabahiro 54:4167f10ecad0 2815 motor[TIRE_BL].pwm = 100;
yabahiro 54:4167f10ecad0 2816 motor[TIRE_BL].dir = BRAKE;
yabahiro 54:4167f10ecad0 2817 }
yabahiro 54:4167f10ecad0 2818 break;
yabahiro 54:4167f10ecad0 2819 case 24:
yabahiro 54:4167f10ecad0 2820 if(YELLOW) {
yabahiro 60:24ebb611f26e 2821 /*
yabahiro 56:61ebd4b759b4 2822 value=aime.read_ms();
yabahiro 56:61ebd4b759b4 2823 if(value>=4000) {
yabahiro 56:61ebd4b759b4 2824 mode=25;
yabahiro 56:61ebd4b759b4 2825 }
yabahiro 54:4167f10ecad0 2826 if(g[4]==3||g[4]==2) {
yabahiro 54:4167f10ecad0 2827 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2828 motor[TIRE_FR].dir = FOR;
yabahiro 53:694dbd7dfa0e 2829 motor[TIRE_FL].pwm = 30;
yabahiro 53:694dbd7dfa0e 2830 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2831 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2832 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2833 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2834 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2835 }
yabahiro 54:4167f10ecad0 2836 if(g[4]==253||g[4]==254) {
yabahiro 54:4167f10ecad0 2837 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2838 motor[TIRE_FR].dir = BACK;
yabahiro 53:694dbd7dfa0e 2839 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2840 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2841 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2842 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 2843 motor[TIRE_BL].pwm = 30;
yabahiro 52:4394a1debceb 2844 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2845 }
yabahiro 54:4167f10ecad0 2846 switch(g[2]) {
yabahiro 54:4167f10ecad0 2847 case 0:
yabahiro 54:4167f10ecad0 2848 motor[TIRE_FR].pwm = 25;
yabahiro 54:4167f10ecad0 2849 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2850 motor[TIRE_BR].pwm = 25;
yabahiro 54:4167f10ecad0 2851 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2852 break;
yabahiro 54:4167f10ecad0 2853 case 255:
yabahiro 54:4167f10ecad0 2854 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2855 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2856 motor[TIRE_BR].pwm = 25;
yabahiro 54:4167f10ecad0 2857 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2858 break;
yabahiro 54:4167f10ecad0 2859 case 253:
yabahiro 54:4167f10ecad0 2860 motor[TIRE_FR].pwm = 40;
yabahiro 54:4167f10ecad0 2861 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2862 motor[TIRE_BR].pwm = 25;
yabahiro 54:4167f10ecad0 2863 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2864 break;
yabahiro 54:4167f10ecad0 2865 case 254:
yabahiro 54:4167f10ecad0 2866 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2867 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 2868 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2869 motor[TIRE_BR].dir = BACK;
yabahiro 54:4167f10ecad0 2870 break;
yabahiro 54:4167f10ecad0 2871 case 1:
yabahiro 54:4167f10ecad0 2872 motor[TIRE_FR].pwm = 25;
yabahiro 54:4167f10ecad0 2873 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2874 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2875 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2876 break;
yabahiro 54:4167f10ecad0 2877 case 3:
yabahiro 54:4167f10ecad0 2878 motor[TIRE_FR].pwm = 25;
yabahiro 54:4167f10ecad0 2879 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 2880 motor[TIRE_BR].pwm = 40;
yabahiro 54:4167f10ecad0 2881 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2882 break;
yabahiro 54:4167f10ecad0 2883 case 2:
yabahiro 54:4167f10ecad0 2884 motor[TIRE_FR].pwm = 30;
yabahiro 54:4167f10ecad0 2885 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2886 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 2887 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2888 break;
yabahiro 54:4167f10ecad0 2889 }
yabahiro 54:4167f10ecad0 2890 switch(g[3]) {
yabahiro 54:4167f10ecad0 2891 case 0:
yabahiro 54:4167f10ecad0 2892 motor[TIRE_FL].pwm = 25;
yabahiro 54:4167f10ecad0 2893 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2894 motor[TIRE_BL].pwm = 25;
yabahiro 54:4167f10ecad0 2895 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2896 break;
yabahiro 54:4167f10ecad0 2897 case 255:
yabahiro 54:4167f10ecad0 2898 motor[TIRE_FL].pwm = 25;
yabahiro 54:4167f10ecad0 2899 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2900 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2901 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2902 case 253:
yabahiro 54:4167f10ecad0 2903 motor[TIRE_FL].pwm = 25;
yabahiro 54:4167f10ecad0 2904 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2905 motor[TIRE_BL].pwm = 40;
yabahiro 54:4167f10ecad0 2906 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2907 break;
yabahiro 54:4167f10ecad0 2908 case 254:
yabahiro 54:4167f10ecad0 2909 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2910 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2911 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2912 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2913 break;
yabahiro 54:4167f10ecad0 2914 case 1:
yabahiro 54:4167f10ecad0 2915 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2916 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2917 motor[TIRE_BL].pwm = 25;
yabahiro 54:4167f10ecad0 2918 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2919 break;
yabahiro 54:4167f10ecad0 2920 case 3:
yabahiro 54:4167f10ecad0 2921 motor[TIRE_FL].pwm = 40;
yabahiro 54:4167f10ecad0 2922 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2923 motor[TIRE_BL].pwm = 25;
yabahiro 54:4167f10ecad0 2924 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 2925 break;
yabahiro 54:4167f10ecad0 2926 case 2:
yabahiro 54:4167f10ecad0 2927 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 2928 motor[TIRE_FL].dir = FOR;
yabahiro 54:4167f10ecad0 2929 motor[TIRE_BL].pwm = 30;
yabahiro 54:4167f10ecad0 2930 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 2931 break;
yabahiro 54:4167f10ecad0 2932 }
yabahiro 54:4167f10ecad0 2933 if(g[3]==0&&g[2]==0) {
yabahiro 54:4167f10ecad0 2934 mode=25;
yabahiro 54:4167f10ecad0 2935 }
yabahiro 60:24ebb611f26e 2936 */
yabahiro 60:24ebb611f26e 2937 mode=25;
yabahiro 54:4167f10ecad0 2938 } else {
yabahiro 60:24ebb611f26e 2939 /*
yabahiro 54:4167f10ecad0 2940 if(g[1]==98&&countss>=40000) {
yabahiro 54:4167f10ecad0 2941 mode=31;
yabahiro 54:4167f10ecad0 2942 fast = 60;
yabahiro 54:4167f10ecad0 2943 normal = 40;
yabahiro 54:4167f10ecad0 2944 slow = 30;
yabahiro 54:4167f10ecad0 2945 countss=0;
yabahiro 54:4167f10ecad0 2946 }
yabahiro 60:24ebb611f26e 2947 */
yabahiro 54:4167f10ecad0 2948 switch(g[1]) {
yabahiro 54:4167f10ecad0 2949 case 98:
yabahiro 54:4167f10ecad0 2950 fast=40;
yabahiro 54:4167f10ecad0 2951 normal=30;
yabahiro 54:4167f10ecad0 2952 slow=25;
yabahiro 54:4167f10ecad0 2953 motor[TIRE_FR].pwm = normal;
yabahiro 54:4167f10ecad0 2954 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2955 motor[TIRE_FL].pwm = normal;
yabahiro 54:4167f10ecad0 2956 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2957 motor[TIRE_BR].pwm = normal;
yabahiro 54:4167f10ecad0 2958 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2959 motor[TIRE_BL].pwm = normal;
yabahiro 54:4167f10ecad0 2960 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2961 break;
yabahiro 54:4167f10ecad0 2962 case 0:
yabahiro 54:4167f10ecad0 2963 motor[TIRE_FR].pwm = fast;
yabahiro 54:4167f10ecad0 2964 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2965 motor[TIRE_FL].pwm = fast;
yabahiro 54:4167f10ecad0 2966 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2967 motor[TIRE_BR].pwm = fast;
yabahiro 54:4167f10ecad0 2968 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2969 motor[TIRE_BL].pwm = fast;
yabahiro 54:4167f10ecad0 2970 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2971 mtc=true;
yabahiro 54:4167f10ecad0 2972 break;
yabahiro 54:4167f10ecad0 2973 case 255:
yabahiro 54:4167f10ecad0 2974 motor[TIRE_FR].pwm = fast;
yabahiro 54:4167f10ecad0 2975 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2976 motor[TIRE_FL].pwm = slow;
yabahiro 54:4167f10ecad0 2977 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 2978 motor[TIRE_BR].pwm = slow;
yabahiro 54:4167f10ecad0 2979 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 2980 motor[TIRE_BL].pwm = fast;
yabahiro 54:4167f10ecad0 2981 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2982 mtc=true;
yabahiro 54:4167f10ecad0 2983 break;
yabahiro 54:4167f10ecad0 2984 case 253:
yabahiro 54:4167f10ecad0 2985 motor[TIRE_FR].pwm = slow;
yabahiro 54:4167f10ecad0 2986 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2987 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2988 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 2989 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 2990 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 2991 motor[TIRE_BL].pwm = slow;
yabahiro 54:4167f10ecad0 2992 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 2993 mtc=true;
yabahiro 54:4167f10ecad0 2994 break;
yabahiro 54:4167f10ecad0 2995 case 254:
yabahiro 54:4167f10ecad0 2996 motor[TIRE_FR].pwm = normal;
yabahiro 54:4167f10ecad0 2997 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 2998 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 2999 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 3000 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 3001 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 3002 motor[TIRE_BL].pwm = normal;
yabahiro 54:4167f10ecad0 3003 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 3004 mtc=true;
yabahiro 54:4167f10ecad0 3005 break;
yabahiro 54:4167f10ecad0 3006 case 1:
yabahiro 54:4167f10ecad0 3007 motor[TIRE_FR].pwm = slow;
yabahiro 54:4167f10ecad0 3008 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 3009 motor[TIRE_FL].pwm = fast;
yabahiro 54:4167f10ecad0 3010 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3011 motor[TIRE_BR].pwm = fast;
yabahiro 54:4167f10ecad0 3012 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3013 motor[TIRE_BL].pwm = slow;
yabahiro 54:4167f10ecad0 3014 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 3015 mtc=true;
yabahiro 54:4167f10ecad0 3016 break;
yabahiro 54:4167f10ecad0 3017 case 3:
yabahiro 54:4167f10ecad0 3018 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3019 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 3020 motor[TIRE_FL].pwm = slow;
yabahiro 54:4167f10ecad0 3021 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3022 motor[TIRE_BR].pwm = slow;
yabahiro 54:4167f10ecad0 3023 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3024 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 3025 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 3026 mtc=true;
yabahiro 54:4167f10ecad0 3027 break;
yabahiro 54:4167f10ecad0 3028 case 2:
yabahiro 54:4167f10ecad0 3029 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3030 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 3031 motor[TIRE_FL].pwm = normal;
yabahiro 54:4167f10ecad0 3032 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3033 motor[TIRE_BR].pwm = normal;
yabahiro 54:4167f10ecad0 3034 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3035 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 3036 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 3037 mtc=true;
yabahiro 54:4167f10ecad0 3038 break;
yabahiro 54:4167f10ecad0 3039 }
yabahiro 54:4167f10ecad0 3040 if(g[1]!=98&&mtc==true) {
yabahiro 54:4167f10ecad0 3041 switch(g[0]) {
yabahiro 54:4167f10ecad0 3042 case 0:
yabahiro 54:4167f10ecad0 3043 mtc=false;
yabahiro 54:4167f10ecad0 3044 break;
yabahiro 54:4167f10ecad0 3045 case 255:
yabahiro 54:4167f10ecad0 3046 motor[TIRE_FL].pwm += 3;
yabahiro 54:4167f10ecad0 3047 mtc=false;
yabahiro 54:4167f10ecad0 3048 break;
yabahiro 54:4167f10ecad0 3049 case 253:
yabahiro 54:4167f10ecad0 3050 motor[TIRE_FL].pwm += 6;
yabahiro 54:4167f10ecad0 3051 mtc=false;
yabahiro 54:4167f10ecad0 3052 break;
yabahiro 54:4167f10ecad0 3053 case 254:
yabahiro 54:4167f10ecad0 3054 motor[TIRE_FL].pwm += 9;
yabahiro 54:4167f10ecad0 3055 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3056 mtc=false;
yabahiro 54:4167f10ecad0 3057 break;
yabahiro 54:4167f10ecad0 3058 case 1:
yabahiro 54:4167f10ecad0 3059 motor[TIRE_FR].pwm += 3;
yabahiro 54:4167f10ecad0 3060 mtc=false;
yabahiro 54:4167f10ecad0 3061 break;
yabahiro 54:4167f10ecad0 3062 case 3:
yabahiro 54:4167f10ecad0 3063 motor[TIRE_FR].pwm += 6;
yabahiro 54:4167f10ecad0 3064 mtc=false;
yabahiro 54:4167f10ecad0 3065 break;
yabahiro 54:4167f10ecad0 3066 case 2:
yabahiro 54:4167f10ecad0 3067 motor[TIRE_FR].pwm += 9;
yabahiro 54:4167f10ecad0 3068 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 3069 mtc=false;
yabahiro 54:4167f10ecad0 3070 break;
yabahiro 54:4167f10ecad0 3071 }
yabahiro 54:4167f10ecad0 3072 }
yabahiro 52:4394a1debceb 3073 }
yabahiro 54:4167f10ecad0 3074 break;
yabahiro 54:4167f10ecad0 3075 case 25:
kishibekairohan 47:a9cad8502999 3076 motor[TIRE_FR].pwm = 100;
kishibekairohan 47:a9cad8502999 3077 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 3078 motor[TIRE_FL].pwm = 100;
kishibekairohan 47:a9cad8502999 3079 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 47:a9cad8502999 3080 motor[TIRE_BR].pwm = 100;
kishibekairohan 47:a9cad8502999 3081 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 47:a9cad8502999 3082 motor[TIRE_BL].pwm = 100;
kishibekairohan 47:a9cad8502999 3083 motor[TIRE_BL].dir = BRAKE;
yabahiro 46:68296caae1af 3084 if(UPLS==true||DOLS==true) {
yabahiro 54:4167f10ecad0 3085 if(UPLS==true||DOLS==true) {
yabahiro 54:4167f10ecad0 3086 motor[MOTOR_0].pwm = 200;
yabahiro 54:4167f10ecad0 3087 motor[MOTOR_0].dir = BRAKE;
yabahiro 54:4167f10ecad0 3088 countss = 0;
yabahiro 54:4167f10ecad0 3089 mode=26;
yabahiro 54:4167f10ecad0 3090 }
yabahiro 46:68296caae1af 3091 } else {
yabahiro 46:68296caae1af 3092 motor[MOTOR_0].pwm = 100;
yabahiro 46:68296caae1af 3093 motor[MOTOR_0].dir = BACK;
yabahiro 46:68296caae1af 3094 }
yabahiro 54:4167f10ecad0 3095 break;
yabahiro 54:4167f10ecad0 3096 case 26:
yabahiro 56:61ebd4b759b4 3097 /*
yabahiro 56:61ebd4b759b4 3098 if (g[3] == 0&&countss>=30000) {
yabahiro 56:61ebd4b759b4 3099 mode = 40;
yabahiro 56:61ebd4b759b4 3100 }
yabahiro 56:61ebd4b759b4 3101 */
yabahiro 56:61ebd4b759b4 3102 if (g[1] != 99) {
yabahiro 54:4167f10ecad0 3103 mode = 31;
yabahiro 54:4167f10ecad0 3104 }
yabahiro 38:40e7fdf3c694 3105 switch(g[4]) {
yabahiro 38:40e7fdf3c694 3106 case 98:
yabahiro 38:40e7fdf3c694 3107 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 3108 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3109 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 3110 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3111 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 3112 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 3113 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 3114 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3115 break;
yabahiro 38:40e7fdf3c694 3116 case 99:
yabahiro 38:40e7fdf3c694 3117 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3118 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3119 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3120 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 3121 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 3122 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 3123 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3124 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3125 break;
yabahiro 38:40e7fdf3c694 3126 case 0:
yabahiro 38:40e7fdf3c694 3127 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 3128 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3129 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 3130 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3131 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 3132 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 3133 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 3134 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3135 break;
yabahiro 38:40e7fdf3c694 3136 case 255:
yabahiro 38:40e7fdf3c694 3137 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3138 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3139 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3140 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 3141 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 3142 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 3143 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3144 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3145 break;
yabahiro 38:40e7fdf3c694 3146 case 253:
yabahiro 46:68296caae1af 3147 motor[TIRE_FR].pwm = 25;
yabahiro 38:40e7fdf3c694 3148 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3149 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 3150 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3151 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 3152 motor[TIRE_BR].dir = FOR;
yabahiro 46:68296caae1af 3153 motor[TIRE_BL].pwm = 25;
yabahiro 38:40e7fdf3c694 3154 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3155 break;
yabahiro 38:40e7fdf3c694 3156 case 254:
yabahiro 38:40e7fdf3c694 3157 motor[TIRE_FR].pwm = 40;
yabahiro 38:40e7fdf3c694 3158 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3159 motor[TIRE_FL].pwm = 60;
yabahiro 38:40e7fdf3c694 3160 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3161 motor[TIRE_BR].pwm = 60;
yabahiro 38:40e7fdf3c694 3162 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 3163 motor[TIRE_BL].pwm = 40;
yabahiro 38:40e7fdf3c694 3164 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3165 break;
yabahiro 38:40e7fdf3c694 3166 case 1:
yabahiro 38:40e7fdf3c694 3167 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3168 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3169 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3170 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 3171 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 3172 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 3173 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3174 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3175 break;
yabahiro 38:40e7fdf3c694 3176 case 3:
yabahiro 38:40e7fdf3c694 3177 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 3178 motor[TIRE_FR].dir = FOR;
yabahiro 46:68296caae1af 3179 motor[TIRE_FL].pwm = 25;
yabahiro 38:40e7fdf3c694 3180 motor[TIRE_FL].dir = BACK;
yabahiro 46:68296caae1af 3181 motor[TIRE_BR].pwm = 25;
yabahiro 38:40e7fdf3c694 3182 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 3183 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 3184 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3185 break;
yabahiro 38:40e7fdf3c694 3186 case 2:
yabahiro 38:40e7fdf3c694 3187 motor[TIRE_FR].pwm = 60;
yabahiro 38:40e7fdf3c694 3188 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3189 motor[TIRE_FL].pwm = 40;
yabahiro 38:40e7fdf3c694 3190 motor[TIRE_FL].dir = BACK;
yabahiro 38:40e7fdf3c694 3191 motor[TIRE_BR].pwm = 40;
yabahiro 38:40e7fdf3c694 3192 motor[TIRE_BR].dir = FOR;
kishibekairohan 41:99a1158f3eca 3193 motor[TIRE_BL].pwm = 60;
yabahiro 38:40e7fdf3c694 3194 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3195 break;
yabahiro 38:40e7fdf3c694 3196 }
yabahiro 54:4167f10ecad0 3197 break;
yabahiro 54:4167f10ecad0 3198 case 31:
yabahiro 54:4167f10ecad0 3199 motor[TIRE_FR].pwm = 255;
yabahiro 54:4167f10ecad0 3200 motor[TIRE_FR].dir = BRAKE;
yabahiro 54:4167f10ecad0 3201 motor[TIRE_FL].pwm = 45;
yabahiro 54:4167f10ecad0 3202 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3203 motor[TIRE_BR].pwm = 45;
yabahiro 54:4167f10ecad0 3204 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3205 motor[TIRE_BL].pwm = 255;
yabahiro 54:4167f10ecad0 3206 motor[TIRE_BL].dir = BRAKE;
yabahiro 56:61ebd4b759b4 3207 if(g[3]==0) {
yabahiro 54:4167f10ecad0 3208 cross=0;
yabahiro 54:4167f10ecad0 3209 countss=0;
yabahiro 54:4167f10ecad0 3210 mode=40;
yabahiro 56:61ebd4b759b4 3211 fast = 70;
yabahiro 56:61ebd4b759b4 3212 normal = 50;
yabahiro 54:4167f10ecad0 3213 slow = 30;
yabahiro 54:4167f10ecad0 3214 }
yabahiro 54:4167f10ecad0 3215 break;
yabahiro 54:4167f10ecad0 3216 case 40:
yabahiro 54:4167f10ecad0 3217 if(YELLOW) {
yabahiro 54:4167f10ecad0 3218 if(cross==1) {
yabahiro 54:4167f10ecad0 3219 mode=41;
yabahiro 54:4167f10ecad0 3220 countss=0;
yabahiro 54:4167f10ecad0 3221 cross=0;
yabahiro 54:4167f10ecad0 3222 }
yabahiro 54:4167f10ecad0 3223 } else {
yabahiro 54:4167f10ecad0 3224 if(cross==3) {
yabahiro 54:4167f10ecad0 3225 fast = 60;
yabahiro 54:4167f10ecad0 3226 normal = 40;
yabahiro 54:4167f10ecad0 3227 slow = 30;
yabahiro 54:4167f10ecad0 3228 mode=41;
yabahiro 54:4167f10ecad0 3229 countss=0;
yabahiro 54:4167f10ecad0 3230 cross=0;
yabahiro 54:4167f10ecad0 3231 }
yabahiro 54:4167f10ecad0 3232 }
yabahiro 54:4167f10ecad0 3233 /*
yabahiro 54:4167f10ecad0 3234 if(LIF||LIB) {
yabahiro 54:4167f10ecad0 3235 mode=42;
yabahiro 33:dd598bb23ad1 3236 }
yabahiro 54:4167f10ecad0 3237 */
yabahiro 54:4167f10ecad0 3238 switch(g[3]) {
yabahiro 54:4167f10ecad0 3239 case 98:
yabahiro 54:4167f10ecad0 3240 motor[TIRE_FR].pwm = normal+15;
yabahiro 54:4167f10ecad0 3241 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3242 motor[TIRE_FL].pwm = normal+15;
yabahiro 54:4167f10ecad0 3243 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3244 motor[TIRE_BR].pwm = normal+15;
yabahiro 54:4167f10ecad0 3245 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3246 motor[TIRE_BL].pwm = normal+15;
yabahiro 54:4167f10ecad0 3247 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3248 if(countss>30000) {
yabahiro 54:4167f10ecad0 3249 cross++;
yabahiro 54:4167f10ecad0 3250 countss=0;
yabahiro 54:4167f10ecad0 3251 }
yabahiro 54:4167f10ecad0 3252 mtc2=true;
yabahiro 54:4167f10ecad0 3253 break;
yabahiro 54:4167f10ecad0 3254 case 0:
yabahiro 54:4167f10ecad0 3255 motor[TIRE_FR].pwm = fast+15;
yabahiro 54:4167f10ecad0 3256 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3257 motor[TIRE_FL].pwm = fast+15;
yabahiro 54:4167f10ecad0 3258 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3259 motor[TIRE_BR].pwm = fast+15;
yabahiro 54:4167f10ecad0 3260 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3261 motor[TIRE_BL].pwm = fast+15;
yabahiro 54:4167f10ecad0 3262 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3263 mtc2=true;
yabahiro 54:4167f10ecad0 3264 break;
yabahiro 54:4167f10ecad0 3265 case 255:
yabahiro 54:4167f10ecad0 3266 motor[TIRE_FR].pwm = slow+15;
yabahiro 54:4167f10ecad0 3267 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3268 motor[TIRE_FL].pwm = fast+15;
yabahiro 54:4167f10ecad0 3269 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3270 motor[TIRE_BR].pwm = fast+15;
yabahiro 54:4167f10ecad0 3271 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3272 motor[TIRE_BL].pwm = slow+15;
yabahiro 54:4167f10ecad0 3273 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3274 mtc2=true;
yabahiro 54:4167f10ecad0 3275 break;
yabahiro 54:4167f10ecad0 3276 case 253:
yabahiro 54:4167f10ecad0 3277 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3278 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 3279 motor[TIRE_FL].pwm = slow+15;
yabahiro 54:4167f10ecad0 3280 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3281 motor[TIRE_BR].pwm = slow+15;
yabahiro 54:4167f10ecad0 3282 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3283 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 3284 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 3285 mtc2=true;
yabahiro 54:4167f10ecad0 3286 break;
yabahiro 54:4167f10ecad0 3287 case 254:
yabahiro 54:4167f10ecad0 3288 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3289 motor[TIRE_FR].dir = FREE;
yabahiro 54:4167f10ecad0 3290 motor[TIRE_FL].pwm = normal+15;
yabahiro 54:4167f10ecad0 3291 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3292 motor[TIRE_BR].pwm = normal+15;
yabahiro 54:4167f10ecad0 3293 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3294 motor[TIRE_BL].pwm = 0;
yabahiro 54:4167f10ecad0 3295 motor[TIRE_BL].dir = FREE;
yabahiro 54:4167f10ecad0 3296 mtc2=true;
yabahiro 54:4167f10ecad0 3297 break;
yabahiro 54:4167f10ecad0 3298 case 1:
yabahiro 54:4167f10ecad0 3299 motor[TIRE_FR].pwm = fast+15;
yabahiro 54:4167f10ecad0 3300 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3301 motor[TIRE_FL].pwm = slow+15;
yabahiro 54:4167f10ecad0 3302 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3303 motor[TIRE_BR].pwm = slow+15;
yabahiro 54:4167f10ecad0 3304 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3305 motor[TIRE_BL].pwm = fast+15;
yabahiro 54:4167f10ecad0 3306 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3307 mtc2=true;
yabahiro 54:4167f10ecad0 3308 break;
yabahiro 54:4167f10ecad0 3309 case 3:
yabahiro 54:4167f10ecad0 3310 motor[TIRE_FR].pwm = slow+15;
yabahiro 54:4167f10ecad0 3311 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3312 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 3313 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 3314 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 3315 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 3316 motor[TIRE_BL].pwm = slow+15;
yabahiro 54:4167f10ecad0 3317 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3318 mtc2=true;
yabahiro 54:4167f10ecad0 3319 break;
yabahiro 54:4167f10ecad0 3320 case 2:
yabahiro 54:4167f10ecad0 3321 motor[TIRE_FR].pwm = normal+15;
yabahiro 54:4167f10ecad0 3322 motor[TIRE_FR].dir = BACK;
yabahiro 54:4167f10ecad0 3323 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 3324 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 3325 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 3326 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 3327 motor[TIRE_BL].pwm = normal+15;
yabahiro 54:4167f10ecad0 3328 motor[TIRE_BL].dir = FOR;
yabahiro 54:4167f10ecad0 3329 mtc2=true;
yabahiro 54:4167f10ecad0 3330 break;
yabahiro 54:4167f10ecad0 3331 }
yabahiro 54:4167f10ecad0 3332 if(g[2]!=98&&mtc2==true) {
yabahiro 54:4167f10ecad0 3333 switch(g[2]) {
yabahiro 54:4167f10ecad0 3334 case 0:
yabahiro 54:4167f10ecad0 3335 mtc2=false;
yabahiro 54:4167f10ecad0 3336 break;
yabahiro 54:4167f10ecad0 3337 case 255:
yabahiro 54:4167f10ecad0 3338 motor[TIRE_FR].pwm += 3;
yabahiro 54:4167f10ecad0 3339 mtc2=false;
yabahiro 54:4167f10ecad0 3340 break;
yabahiro 54:4167f10ecad0 3341 case 253:
yabahiro 54:4167f10ecad0 3342 motor[TIRE_FR].pwm += 6;
yabahiro 54:4167f10ecad0 3343 mtc2=false;
yabahiro 54:4167f10ecad0 3344 break;
yabahiro 54:4167f10ecad0 3345 case 254:
yabahiro 54:4167f10ecad0 3346 motor[TIRE_FR].pwm += 9;
yabahiro 54:4167f10ecad0 3347 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 3348 mtc2=false;
yabahiro 54:4167f10ecad0 3349 break;
yabahiro 54:4167f10ecad0 3350 case 1:
yabahiro 54:4167f10ecad0 3351 motor[TIRE_BR].pwm += 3;
yabahiro 54:4167f10ecad0 3352 mtc2=false;
yabahiro 54:4167f10ecad0 3353 break;
yabahiro 54:4167f10ecad0 3354 case 3:
yabahiro 54:4167f10ecad0 3355 motor[TIRE_BR].pwm += 6;
yabahiro 54:4167f10ecad0 3356 mtc2=false;
yabahiro 54:4167f10ecad0 3357 break;
yabahiro 54:4167f10ecad0 3358 case 2:
yabahiro 54:4167f10ecad0 3359 motor[TIRE_BR].pwm += 9;
yabahiro 54:4167f10ecad0 3360 motor[TIRE_FR].pwm = 0;
yabahiro 54:4167f10ecad0 3361 mtc2=false;
yabahiro 54:4167f10ecad0 3362 break;
yabahiro 54:4167f10ecad0 3363 }
yabahiro 54:4167f10ecad0 3364 }
yabahiro 54:4167f10ecad0 3365 break;
yabahiro 54:4167f10ecad0 3366 case 41:
yabahiro 54:4167f10ecad0 3367 /*
yabahiro 54:4167f10ecad0 3368 if(LIF||LIB) {
yabahiro 54:4167f10ecad0 3369 mode=42;
yabahiro 54:4167f10ecad0 3370 }
yabahiro 54:4167f10ecad0 3371 */
yabahiro 54:4167f10ecad0 3372 motor[TIRE_FR].pwm = 255;
yabahiro 54:4167f10ecad0 3373 motor[TIRE_FR].dir = BRAKE;
yabahiro 54:4167f10ecad0 3374 motor[TIRE_FL].pwm = 30;
yabahiro 54:4167f10ecad0 3375 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3376 motor[TIRE_BR].pwm = 30;
yabahiro 54:4167f10ecad0 3377 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3378 motor[TIRE_BL].pwm = 255;
yabahiro 54:4167f10ecad0 3379 motor[TIRE_BL].dir = BRAKE;
yabahiro 54:4167f10ecad0 3380 if(g[1]==0) {
yabahiro 54:4167f10ecad0 3381 countss=0;
yabahiro 54:4167f10ecad0 3382 mode=70;
yabahiro 56:61ebd4b759b4 3383 normal = 120;
yabahiro 56:61ebd4b759b4 3384 fast = 140;
yabahiro 56:61ebd4b759b4 3385 slow = 100;
yabahiro 54:4167f10ecad0 3386 }
yabahiro 54:4167f10ecad0 3387 break;
yabahiro 54:4167f10ecad0 3388 case 42:
yabahiro 54:4167f10ecad0 3389 motor[TIRE_FR].pwm = 40;
yabahiro 38:40e7fdf3c694 3390 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 3391 motor[TIRE_FL].pwm = 0;
yabahiro 54:4167f10ecad0 3392 motor[TIRE_FL].dir = FREE;
yabahiro 54:4167f10ecad0 3393 motor[TIRE_BR].pwm = 0;
yabahiro 54:4167f10ecad0 3394 motor[TIRE_BR].dir = FREE;
yabahiro 54:4167f10ecad0 3395 motor[TIRE_BL].pwm = 40;
yabahiro 38:40e7fdf3c694 3396 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3397 if(g[1]==0) {
yabahiro 54:4167f10ecad0 3398 countss=0;
yabahiro 54:4167f10ecad0 3399 mode=70;
yabahiro 53:694dbd7dfa0e 3400 }
yabahiro 54:4167f10ecad0 3401 break;
yabahiro 54:4167f10ecad0 3402 case 70:
yabahiro 38:40e7fdf3c694 3403 switch(g[1]) {
yabahiro 54:4167f10ecad0 3404 case 99:
yabahiro 54:4167f10ecad0 3405 if(countss>=100000) {
yabahiro 54:4167f10ecad0 3406 mode=100;
yabahiro 54:4167f10ecad0 3407 }
yabahiro 54:4167f10ecad0 3408 break;
yabahiro 38:40e7fdf3c694 3409 case 98:
yabahiro 38:40e7fdf3c694 3410 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 3411 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3412 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 3413 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3414 motor[TIRE_BR].pwm = normal+5;
yabahiro 38:40e7fdf3c694 3415 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3416 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 3417 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3418 break;
yabahiro 38:40e7fdf3c694 3419 case 0:
yabahiro 54:4167f10ecad0 3420 motor[TIRE_FR].pwm = 100;
yabahiro 38:40e7fdf3c694 3421 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 3422 motor[TIRE_FL].pwm = 100;
yabahiro 38:40e7fdf3c694 3423 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3424 motor[TIRE_BR].pwm = 105;
yabahiro 38:40e7fdf3c694 3425 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3426 motor[TIRE_BL].pwm = 100;
yabahiro 38:40e7fdf3c694 3427 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3428 mtc=true;
yabahiro 38:40e7fdf3c694 3429 break;
yabahiro 38:40e7fdf3c694 3430 case 255:
yabahiro 38:40e7fdf3c694 3431 motor[TIRE_FR].pwm = fast;
yabahiro 38:40e7fdf3c694 3432 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3433 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 3434 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3435 motor[TIRE_BR].pwm = slow+5;
yabahiro 38:40e7fdf3c694 3436 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3437 motor[TIRE_BL].pwm = fast;
yabahiro 38:40e7fdf3c694 3438 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3439 mtc=true;
yabahiro 38:40e7fdf3c694 3440 break;
yabahiro 38:40e7fdf3c694 3441 case 253:
yabahiro 38:40e7fdf3c694 3442 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 3443 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3444 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3445 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 3446 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 3447 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 3448 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 3449 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3450 mtc=true;
yabahiro 38:40e7fdf3c694 3451 break;
yabahiro 38:40e7fdf3c694 3452 case 254:
yabahiro 38:40e7fdf3c694 3453 motor[TIRE_FR].pwm = normal;
yabahiro 38:40e7fdf3c694 3454 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3455 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3456 motor[TIRE_FL].dir = FREE;
yabahiro 38:40e7fdf3c694 3457 motor[TIRE_BR].pwm = 0;
yabahiro 38:40e7fdf3c694 3458 motor[TIRE_BR].dir = FREE;
yabahiro 38:40e7fdf3c694 3459 motor[TIRE_BL].pwm = normal;
yabahiro 38:40e7fdf3c694 3460 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3461 mtc=true;
yabahiro 38:40e7fdf3c694 3462 break;
yabahiro 38:40e7fdf3c694 3463 case 1:
yabahiro 38:40e7fdf3c694 3464 motor[TIRE_FR].pwm = slow;
yabahiro 38:40e7fdf3c694 3465 motor[TIRE_FR].dir = FOR;
yabahiro 38:40e7fdf3c694 3466 motor[TIRE_FL].pwm = fast;
yabahiro 38:40e7fdf3c694 3467 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3468 motor[TIRE_BR].pwm = fast+5;
yabahiro 38:40e7fdf3c694 3469 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3470 motor[TIRE_BL].pwm = slow;
yabahiro 38:40e7fdf3c694 3471 motor[TIRE_BL].dir = BACK;
yabahiro 38:40e7fdf3c694 3472 mtc=true;
yabahiro 38:40e7fdf3c694 3473 break;
yabahiro 38:40e7fdf3c694 3474 case 3:
yabahiro 38:40e7fdf3c694 3475 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3476 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3477 motor[TIRE_FL].pwm = slow;
yabahiro 38:40e7fdf3c694 3478 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3479 motor[TIRE_BR].pwm = slow+5;
yabahiro 38:40e7fdf3c694 3480 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3481 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3482 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3483 mtc=true;
yabahiro 38:40e7fdf3c694 3484 break;
yabahiro 38:40e7fdf3c694 3485 case 2:
yabahiro 38:40e7fdf3c694 3486 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3487 motor[TIRE_FR].dir = FREE;
yabahiro 38:40e7fdf3c694 3488 motor[TIRE_FL].pwm = normal;
yabahiro 38:40e7fdf3c694 3489 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3490 motor[TIRE_BR].pwm = normal+5;
yabahiro 38:40e7fdf3c694 3491 motor[TIRE_BR].dir = FOR;
yabahiro 38:40e7fdf3c694 3492 motor[TIRE_BL].pwm = 0;
yabahiro 38:40e7fdf3c694 3493 motor[TIRE_BL].dir = FREE;
yabahiro 38:40e7fdf3c694 3494 mtc=true;
yabahiro 38:40e7fdf3c694 3495 break;
yabahiro 38:40e7fdf3c694 3496 }
yabahiro 38:40e7fdf3c694 3497 if(g[1]!=98&&mtc==true) {
yabahiro 38:40e7fdf3c694 3498 switch(g[0]) {
yabahiro 38:40e7fdf3c694 3499 case 0:
yabahiro 38:40e7fdf3c694 3500 mtc=false;
yabahiro 38:40e7fdf3c694 3501 break;
yabahiro 38:40e7fdf3c694 3502 case 255:
kishibekairohan 44:2aff0da5ebdb 3503 motor[TIRE_FL].pwm += 3;
yabahiro 38:40e7fdf3c694 3504 mtc=false;
yabahiro 38:40e7fdf3c694 3505 break;
yabahiro 38:40e7fdf3c694 3506 case 253:
kishibekairohan 44:2aff0da5ebdb 3507 motor[TIRE_FL].pwm += 6;
yabahiro 38:40e7fdf3c694 3508 mtc=false;
yabahiro 38:40e7fdf3c694 3509 break;
yabahiro 38:40e7fdf3c694 3510 case 254:
kishibekairohan 44:2aff0da5ebdb 3511 motor[TIRE_FL].pwm += 9;
yabahiro 38:40e7fdf3c694 3512 motor[TIRE_FR].pwm = 0;
yabahiro 38:40e7fdf3c694 3513 mtc=false;
yabahiro 38:40e7fdf3c694 3514 break;
yabahiro 38:40e7fdf3c694 3515 case 1:
kishibekairohan 44:2aff0da5ebdb 3516 motor[TIRE_FR].pwm += 3;
yabahiro 38:40e7fdf3c694 3517 mtc=false;
yabahiro 38:40e7fdf3c694 3518 break;
yabahiro 38:40e7fdf3c694 3519 case 3:
kishibekairohan 44:2aff0da5ebdb 3520 motor[TIRE_FR].pwm += 6;
yabahiro 38:40e7fdf3c694 3521 mtc=false;
yabahiro 38:40e7fdf3c694 3522 break;
yabahiro 38:40e7fdf3c694 3523 case 2:
kishibekairohan 44:2aff0da5ebdb 3524 motor[TIRE_FR].pwm += 9;
yabahiro 38:40e7fdf3c694 3525 motor[TIRE_FL].pwm = 0;
yabahiro 38:40e7fdf3c694 3526 mtc=false;
yabahiro 38:40e7fdf3c694 3527 break;
yabahiro 38:40e7fdf3c694 3528 }
yabahiro 33:dd598bb23ad1 3529 }
yabahiro 54:4167f10ecad0 3530 break;
yabahiro 54:4167f10ecad0 3531 case 100:
yabahiro 54:4167f10ecad0 3532 motor[TIRE_FR].pwm = 50;
yabahiro 54:4167f10ecad0 3533 motor[TIRE_FR].dir = FOR;
yabahiro 54:4167f10ecad0 3534 motor[TIRE_FL].pwm = 55;
yabahiro 54:4167f10ecad0 3535 motor[TIRE_FL].dir = BACK;
yabahiro 54:4167f10ecad0 3536 motor[TIRE_BR].pwm = 60;
yabahiro 54:4167f10ecad0 3537 motor[TIRE_BR].dir = FOR;
yabahiro 54:4167f10ecad0 3538 motor[TIRE_BL].pwm = 50;
yabahiro 54:4167f10ecad0 3539 motor[TIRE_BL].dir = BACK;
yabahiro 54:4167f10ecad0 3540 break;
yabahiro 54:4167f10ecad0 3541 default:
yabahiro 54:4167f10ecad0 3542 motor[TIRE_FR].pwm = 50;
yabahiro 54:4167f10ecad0 3543 motor[TIRE_FR].dir = BRAKE;
yabahiro 54:4167f10ecad0 3544 motor[TIRE_FL].pwm = 50;
yabahiro 54:4167f10ecad0 3545 motor[TIRE_FL].dir = BRAKE;
yabahiro 54:4167f10ecad0 3546 motor[TIRE_BR].pwm = 50;
yabahiro 54:4167f10ecad0 3547 motor[TIRE_BR].dir = BRAKE;
yabahiro 54:4167f10ecad0 3548 motor[TIRE_BL].pwm = 50;
yabahiro 54:4167f10ecad0 3549 motor[TIRE_BL].dir = BRAKE;
yabahiro 54:4167f10ecad0 3550 break;
yabahiro 53:694dbd7dfa0e 3551 }
kishibekairohan 13:b6e02d6261d7 3552 }
kishibekairohan 13:b6e02d6261d7 3553 #endif
kishibekairohan 13:b6e02d6261d7 3554
kishibekairohan 13:b6e02d6261d7 3555 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 3556 static void Process4()
M_souta 22:7d93f79a3686 3557 {
yabahiro 48:5562a99479e5 3558 //Oh So 決勝青ゾーン
yabahiro 48:5562a99479e5 3559 LED::LED::TapeledMode(2);
yabahiro 48:5562a99479e5 3560 countss++;
yabahiro 48:5562a99479e5 3561 if(mode==1) {//スタートゾーンから白線検知
yabahiro 48:5562a99479e5 3562 fast=40;
yabahiro 48:5562a99479e5 3563 normal=30;
yabahiro 48:5562a99479e5 3564 slow=20;
yabahiro 48:5562a99479e5 3565 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3566 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3567 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3568 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3569 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3570 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3571 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3572 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3573 if(g[2]==0&&countss>=100000) {
yabahiro 48:5562a99479e5 3574 countss=100000;
yabahiro 48:5562a99479e5 3575 cross=0;
yabahiro 48:5562a99479e5 3576 mode=3;
yabahiro 48:5562a99479e5 3577 }
yabahiro 48:5562a99479e5 3578 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 48:5562a99479e5 3579 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3580 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3581 motor[TIRE_FL].pwm = 255;
yabahiro 48:5562a99479e5 3582 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 3583 motor[TIRE_BR].pwm = 255;
yabahiro 48:5562a99479e5 3584 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 3585 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3586 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3587 if(g[1]==0) {
yabahiro 48:5562a99479e5 3588 mode=10;
yabahiro 48:5562a99479e5 3589 countss=100000;
yabahiro 48:5562a99479e5 3590 }
yabahiro 48:5562a99479e5 3591 } else if(mode==10) { //縦ライントレース
yabahiro 48:5562a99479e5 3592 switch(g[1]) {
yabahiro 48:5562a99479e5 3593 case 98:
yabahiro 48:5562a99479e5 3594 motor[TIRE_FR].pwm = normal+50;
yabahiro 48:5562a99479e5 3595 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3596 motor[TIRE_FL].pwm = normal+50;
yabahiro 48:5562a99479e5 3597 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3598 motor[TIRE_BR].pwm = normal+50;
yabahiro 48:5562a99479e5 3599 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3600 motor[TIRE_BL].pwm = normal+50;
yabahiro 48:5562a99479e5 3601 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3602 mtc=true;
yabahiro 49:697b4e9ce884 3603 if(YELLOW) {
yabahiro 49:697b4e9ce884 3604 if(countss>80000) {
yabahiro 49:697b4e9ce884 3605 cross++;
yabahiro 49:697b4e9ce884 3606 countss=0;
yabahiro 49:697b4e9ce884 3607 }
yabahiro 48:5562a99479e5 3608
yabahiro 49:697b4e9ce884 3609 if(cross==2) {
yabahiro 49:697b4e9ce884 3610 fast = 10;
yabahiro 49:697b4e9ce884 3611 normal = 10;
yabahiro 49:697b4e9ce884 3612 slow = 5;
yabahiro 49:697b4e9ce884 3613 }
yabahiro 48:5562a99479e5 3614
yabahiro 49:697b4e9ce884 3615 if(cross==3) {
yabahiro 49:697b4e9ce884 3616 fast=40;
yabahiro 49:697b4e9ce884 3617 normal=30;
yabahiro 49:697b4e9ce884 3618 slow=20;
yabahiro 49:697b4e9ce884 3619 mode=11;
yabahiro 49:697b4e9ce884 3620 countss=0;
yabahiro 49:697b4e9ce884 3621 cross=0;
yabahiro 49:697b4e9ce884 3622 }
yabahiro 49:697b4e9ce884 3623 } else {
yabahiro 48:5562a99479e5 3624 mode=11;
yabahiro 48:5562a99479e5 3625 }
yabahiro 48:5562a99479e5 3626 break;
yabahiro 48:5562a99479e5 3627 case 0:
yabahiro 48:5562a99479e5 3628 motor[TIRE_FR].pwm = fast+50;
yabahiro 48:5562a99479e5 3629 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3630 motor[TIRE_FL].pwm = fast+50;
yabahiro 48:5562a99479e5 3631 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3632 motor[TIRE_BR].pwm = fast+50;
yabahiro 48:5562a99479e5 3633 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3634 motor[TIRE_BL].pwm = fast+50;
yabahiro 48:5562a99479e5 3635 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3636 mtc=true;
yabahiro 48:5562a99479e5 3637 break;
yabahiro 48:5562a99479e5 3638 case 255:
yabahiro 48:5562a99479e5 3639 motor[TIRE_FR].pwm = fast+50;
yabahiro 48:5562a99479e5 3640 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3641 motor[TIRE_FL].pwm = slow+50;
yabahiro 48:5562a99479e5 3642 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3643 motor[TIRE_BR].pwm = slow+50;
yabahiro 48:5562a99479e5 3644 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3645 motor[TIRE_BL].pwm = fast+50;
yabahiro 48:5562a99479e5 3646 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3647 mtc=true;
yabahiro 48:5562a99479e5 3648 break;
yabahiro 48:5562a99479e5 3649 case 253:
yabahiro 48:5562a99479e5 3650 motor[TIRE_FR].pwm = slow+50;
yabahiro 48:5562a99479e5 3651 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3652 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3653 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3654 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3655 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3656 motor[TIRE_BL].pwm = slow+50;
yabahiro 48:5562a99479e5 3657 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3658 mtc=true;
yabahiro 48:5562a99479e5 3659 break;
yabahiro 48:5562a99479e5 3660 case 254:
yabahiro 48:5562a99479e5 3661 motor[TIRE_FR].pwm = normal+50;
yabahiro 48:5562a99479e5 3662 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3663 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3664 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3665 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3666 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3667 motor[TIRE_BL].pwm = normal+50;
yabahiro 48:5562a99479e5 3668 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3669 mtc=true;
yabahiro 48:5562a99479e5 3670 break;
yabahiro 48:5562a99479e5 3671 case 1:
yabahiro 48:5562a99479e5 3672 motor[TIRE_FR].pwm = slow+50;
yabahiro 48:5562a99479e5 3673 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3674 motor[TIRE_FL].pwm = fast+50;
yabahiro 48:5562a99479e5 3675 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3676 motor[TIRE_BR].pwm = fast+50;
yabahiro 48:5562a99479e5 3677 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3678 motor[TIRE_BL].pwm = slow+50;
yabahiro 48:5562a99479e5 3679 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3680 mtc=true;
yabahiro 48:5562a99479e5 3681 break;
yabahiro 48:5562a99479e5 3682 case 3:
yabahiro 48:5562a99479e5 3683 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3684 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3685 motor[TIRE_FL].pwm = slow+50;
yabahiro 48:5562a99479e5 3686 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3687 motor[TIRE_BR].pwm = slow+50;
yabahiro 48:5562a99479e5 3688 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3689 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3690 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3691 mtc=true;
yabahiro 48:5562a99479e5 3692 break;
yabahiro 48:5562a99479e5 3693 case 2:
yabahiro 48:5562a99479e5 3694 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3695 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3696 motor[TIRE_FL].pwm = normal+50;
yabahiro 48:5562a99479e5 3697 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3698 motor[TIRE_BR].pwm = normal+50;
yabahiro 48:5562a99479e5 3699 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3700 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3701 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3702 mtc=true;
yabahiro 48:5562a99479e5 3703 break;
yabahiro 48:5562a99479e5 3704 }
yabahiro 48:5562a99479e5 3705 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 3706 switch(g[0]) {
yabahiro 48:5562a99479e5 3707 case 0:
yabahiro 48:5562a99479e5 3708 mtc=false;
yabahiro 48:5562a99479e5 3709 break;
yabahiro 48:5562a99479e5 3710 case 255:
yabahiro 48:5562a99479e5 3711 motor[TIRE_FL].pwm += 3;
yabahiro 48:5562a99479e5 3712 mtc=false;
yabahiro 48:5562a99479e5 3713 break;
yabahiro 48:5562a99479e5 3714 case 253:
yabahiro 48:5562a99479e5 3715 motor[TIRE_FL].pwm += 6;
yabahiro 48:5562a99479e5 3716 mtc=false;
yabahiro 48:5562a99479e5 3717 break;
yabahiro 48:5562a99479e5 3718 case 254:
yabahiro 48:5562a99479e5 3719 motor[TIRE_FL].pwm += 9;
yabahiro 48:5562a99479e5 3720 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3721 mtc=false;
yabahiro 48:5562a99479e5 3722 break;
yabahiro 48:5562a99479e5 3723 case 1:
yabahiro 48:5562a99479e5 3724 motor[TIRE_FR].pwm += 3;
yabahiro 48:5562a99479e5 3725 mtc=false;
yabahiro 48:5562a99479e5 3726 break;
yabahiro 48:5562a99479e5 3727 case 3:
yabahiro 48:5562a99479e5 3728 motor[TIRE_FR].pwm += 6;
yabahiro 48:5562a99479e5 3729 mtc=false;
yabahiro 48:5562a99479e5 3730 break;
yabahiro 48:5562a99479e5 3731 case 2:
yabahiro 48:5562a99479e5 3732 motor[TIRE_FR].pwm += 9;
yabahiro 48:5562a99479e5 3733 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3734 mtc=false;
yabahiro 48:5562a99479e5 3735 break;
yabahiro 48:5562a99479e5 3736 }
yabahiro 48:5562a99479e5 3737 }
yabahiro 48:5562a99479e5 3738 } else if(mode==11) {
yabahiro 48:5562a99479e5 3739 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 3740 motor[TIRE_FR].dir = FOR;
yabahiro 49:697b4e9ce884 3741 motor[TIRE_FL].pwm = 50;
yabahiro 49:697b4e9ce884 3742 motor[TIRE_FL].dir = BRAKE;
yabahiro 49:697b4e9ce884 3743 motor[TIRE_BR].pwm = 50;
yabahiro 49:697b4e9ce884 3744 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 3745 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 3746 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3747 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 48:5562a99479e5 3748 countss=0;
yabahiro 48:5562a99479e5 3749 cross=0;
yabahiro 48:5562a99479e5 3750 mode=20;
yabahiro 48:5562a99479e5 3751 }
yabahiro 48:5562a99479e5 3752 } else if(mode==20) {
yabahiro 48:5562a99479e5 3753 switch(g[2]) {
yabahiro 48:5562a99479e5 3754 case 98:
yabahiro 48:5562a99479e5 3755 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3756 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3757 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3758 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3759 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3760 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3761 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3762 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3763 if(countss>30000) {
yabahiro 48:5562a99479e5 3764 cross++;
yabahiro 48:5562a99479e5 3765 countss=0;
yabahiro 48:5562a99479e5 3766 }
yabahiro 48:5562a99479e5 3767 mtc2=true;
yabahiro 48:5562a99479e5 3768
yabahiro 48:5562a99479e5 3769 if(YELLOW) {
yabahiro 49:697b4e9ce884 3770 solenoid.solenoid4= SOLENOID_ON;
yabahiro 48:5562a99479e5 3771 if(cross==1) {
yabahiro 48:5562a99479e5 3772 fast=20;
yabahiro 48:5562a99479e5 3773 normal=15;
yabahiro 48:5562a99479e5 3774 slow=15;
yabahiro 48:5562a99479e5 3775 } else if(cross==2) {
yabahiro 48:5562a99479e5 3776 mode=21;
yabahiro 48:5562a99479e5 3777 countss=0;
yabahiro 48:5562a99479e5 3778 cross=0;
yabahiro 48:5562a99479e5 3779 }
yabahiro 48:5562a99479e5 3780 } else {
yabahiro 49:697b4e9ce884 3781 if(cross==2) {
yabahiro 49:697b4e9ce884 3782 fast=20;
yabahiro 49:697b4e9ce884 3783 normal=15;
yabahiro 49:697b4e9ce884 3784 slow=15;
yabahiro 49:697b4e9ce884 3785 }
yabahiro 48:5562a99479e5 3786 if(cross==3) {
yabahiro 48:5562a99479e5 3787 mode=21;
yabahiro 48:5562a99479e5 3788 countss=0;
yabahiro 48:5562a99479e5 3789 cross=0;
yabahiro 48:5562a99479e5 3790 }
yabahiro 48:5562a99479e5 3791 }
yabahiro 48:5562a99479e5 3792 break;
yabahiro 48:5562a99479e5 3793 case 0:
yabahiro 48:5562a99479e5 3794 motor[TIRE_FR].pwm = fast+20;
yabahiro 48:5562a99479e5 3795 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3796 motor[TIRE_FL].pwm = fast+20;
yabahiro 48:5562a99479e5 3797 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3798 motor[TIRE_BR].pwm = fast+20;
yabahiro 48:5562a99479e5 3799 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3800 motor[TIRE_BL].pwm = fast+20;
yabahiro 48:5562a99479e5 3801 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3802 mtc2=true;
yabahiro 48:5562a99479e5 3803 break;
yabahiro 48:5562a99479e5 3804 case 255:
yabahiro 48:5562a99479e5 3805 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3806 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3807 motor[TIRE_FL].pwm = fast+20;
yabahiro 48:5562a99479e5 3808 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3809 motor[TIRE_BR].pwm = fast+20;
yabahiro 48:5562a99479e5 3810 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3811 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3812 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3813 mtc2=true;
yabahiro 48:5562a99479e5 3814 break;
yabahiro 48:5562a99479e5 3815 case 253:
yabahiro 48:5562a99479e5 3816 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3817 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3818 motor[TIRE_FL].pwm = slow+20;
yabahiro 48:5562a99479e5 3819 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3820 motor[TIRE_BR].pwm = slow+20;
yabahiro 48:5562a99479e5 3821 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3822 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3823 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3824 mtc2=true;
yabahiro 48:5562a99479e5 3825 break;
yabahiro 48:5562a99479e5 3826 case 254:
yabahiro 48:5562a99479e5 3827 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 3828 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3829 motor[TIRE_FL].pwm = normal+20;
yabahiro 48:5562a99479e5 3830 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3831 motor[TIRE_BR].pwm = normal+20;
yabahiro 48:5562a99479e5 3832 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3833 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3834 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3835 mtc2=true;
yabahiro 48:5562a99479e5 3836 break;
yabahiro 48:5562a99479e5 3837 case 1:
yabahiro 48:5562a99479e5 3838 motor[TIRE_FR].pwm = fast+20;
yabahiro 48:5562a99479e5 3839 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3840 motor[TIRE_FL].pwm = slow+20;
yabahiro 48:5562a99479e5 3841 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3842 motor[TIRE_BR].pwm = slow+20;
yabahiro 48:5562a99479e5 3843 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3844 motor[TIRE_BL].pwm = fast+20;
yabahiro 48:5562a99479e5 3845 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3846 mtc2=true;
yabahiro 48:5562a99479e5 3847 break;
yabahiro 48:5562a99479e5 3848 case 3:
yabahiro 48:5562a99479e5 3849 motor[TIRE_FR].pwm = slow+20;
yabahiro 48:5562a99479e5 3850 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3851 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3852 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3853 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3854 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3855 motor[TIRE_BL].pwm = slow+20;
yabahiro 48:5562a99479e5 3856 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3857 mtc2=true;
yabahiro 48:5562a99479e5 3858 break;
yabahiro 48:5562a99479e5 3859 case 2:
yabahiro 48:5562a99479e5 3860 motor[TIRE_FR].pwm = normal+20;
yabahiro 48:5562a99479e5 3861 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3862 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3863 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3864 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 3865 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3866 motor[TIRE_BL].pwm = normal+20;
yabahiro 48:5562a99479e5 3867 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3868 mtc2=true;
yabahiro 48:5562a99479e5 3869 break;
yabahiro 48:5562a99479e5 3870 }
yabahiro 48:5562a99479e5 3871 if(g[2]!=98&&mtc2==true) {
yabahiro 48:5562a99479e5 3872 switch(g[3]) {
yabahiro 48:5562a99479e5 3873 case 0:
yabahiro 48:5562a99479e5 3874 mtc2=false;
yabahiro 48:5562a99479e5 3875 break;
yabahiro 48:5562a99479e5 3876 case 255:
yabahiro 48:5562a99479e5 3877 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 3878 mtc2=false;
yabahiro 48:5562a99479e5 3879 break;
yabahiro 48:5562a99479e5 3880 case 253:
yabahiro 48:5562a99479e5 3881 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 3882 mtc2=false;
yabahiro 48:5562a99479e5 3883 break;
yabahiro 48:5562a99479e5 3884 case 254:
yabahiro 48:5562a99479e5 3885 motor[TIRE_BL].pwm += 15;
yabahiro 48:5562a99479e5 3886 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 3887 mtc2=false;
yabahiro 48:5562a99479e5 3888 break;
yabahiro 48:5562a99479e5 3889 case 1:
yabahiro 48:5562a99479e5 3890 motor[TIRE_FL].pwm += 5;
yabahiro 48:5562a99479e5 3891 mtc2=false;
yabahiro 48:5562a99479e5 3892 break;
yabahiro 48:5562a99479e5 3893 case 3:
yabahiro 48:5562a99479e5 3894 motor[TIRE_FL].pwm += 10;
yabahiro 48:5562a99479e5 3895 mtc2=false;
yabahiro 48:5562a99479e5 3896 break;
yabahiro 48:5562a99479e5 3897 case 2:
yabahiro 48:5562a99479e5 3898 motor[TIRE_FL].pwm += 15;
yabahiro 48:5562a99479e5 3899 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 3900 mtc2=false;
yabahiro 48:5562a99479e5 3901 break;
yabahiro 48:5562a99479e5 3902 }
yabahiro 48:5562a99479e5 3903 }
yabahiro 48:5562a99479e5 3904 }
yabahiro 48:5562a99479e5 3905 if(YELLOW) {
yabahiro 48:5562a99479e5 3906 if(mode==21) {
yabahiro 48:5562a99479e5 3907 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3908 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3909 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3910 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3911 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3912 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3913 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3914 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3915 if(g[3]==98&&countss>=10000) {
yabahiro 48:5562a99479e5 3916 mode=22;
yabahiro 48:5562a99479e5 3917 countss=0;
yabahiro 48:5562a99479e5 3918 fast=60;
yabahiro 48:5562a99479e5 3919 normal=40;
yabahiro 48:5562a99479e5 3920 slow=20;
yabahiro 48:5562a99479e5 3921 }
yabahiro 48:5562a99479e5 3922 } else if(mode==22) {
yabahiro 48:5562a99479e5 3923 if(g[4]==3||g[4]==2) {
yabahiro 48:5562a99479e5 3924 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3925 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3926 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3927 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 3928 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3929 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3930 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3931 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3932 }
yabahiro 48:5562a99479e5 3933 if(g[4]==253||g[4]==254) {
yabahiro 48:5562a99479e5 3934 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3935 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3936 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 3937 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 3938 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3939 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3940 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3941 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 3942 }
yabahiro 48:5562a99479e5 3943 switch(g[2]) {
yabahiro 48:5562a99479e5 3944 case 0:
yabahiro 48:5562a99479e5 3945 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3946 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3947 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3948 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3949 break;
yabahiro 48:5562a99479e5 3950 case 255:
yabahiro 48:5562a99479e5 3951 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3952 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3953 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3954 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3955 break;
yabahiro 48:5562a99479e5 3956 case 253:
yabahiro 48:5562a99479e5 3957 motor[TIRE_FR].pwm = 40;
yabahiro 48:5562a99479e5 3958 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3959 motor[TIRE_BR].pwm = 20;
yabahiro 48:5562a99479e5 3960 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 3961 break;
yabahiro 48:5562a99479e5 3962 case 254:
yabahiro 48:5562a99479e5 3963 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3964 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 3965 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3966 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 3967 break;
yabahiro 48:5562a99479e5 3968 case 1:
yabahiro 48:5562a99479e5 3969 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3970 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3971 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3972 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3973 break;
yabahiro 48:5562a99479e5 3974 case 3:
yabahiro 48:5562a99479e5 3975 motor[TIRE_FR].pwm = 20;
yabahiro 48:5562a99479e5 3976 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 3977 motor[TIRE_BR].pwm = 40;
yabahiro 48:5562a99479e5 3978 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3979 break;
yabahiro 48:5562a99479e5 3980 case 2:
yabahiro 48:5562a99479e5 3981 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 3982 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 3983 motor[TIRE_BR].pwm = 30;
yabahiro 48:5562a99479e5 3984 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 3985 break;
yabahiro 48:5562a99479e5 3986 }
yabahiro 48:5562a99479e5 3987 switch(g[3]) {
yabahiro 48:5562a99479e5 3988 case 0:
yabahiro 48:5562a99479e5 3989 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3990 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3991 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 3992 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 3993 break;
yabahiro 48:5562a99479e5 3994 case 255:
yabahiro 48:5562a99479e5 3995 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 3996 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 3997 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 3998 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 3999 case 253:
yabahiro 48:5562a99479e5 4000 motor[TIRE_FL].pwm = 20;
yabahiro 48:5562a99479e5 4001 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4002 motor[TIRE_BL].pwm = 40;
yabahiro 48:5562a99479e5 4003 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 4004 break;
yabahiro 48:5562a99479e5 4005 case 254:
yabahiro 48:5562a99479e5 4006 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 4007 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4008 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4009 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 4010 break;
yabahiro 48:5562a99479e5 4011 case 1:
yabahiro 48:5562a99479e5 4012 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 4013 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4014 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 4015 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4016 break;
yabahiro 48:5562a99479e5 4017 case 3:
yabahiro 48:5562a99479e5 4018 motor[TIRE_FL].pwm = 40;
yabahiro 48:5562a99479e5 4019 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4020 motor[TIRE_BL].pwm = 20;
yabahiro 48:5562a99479e5 4021 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4022 break;
yabahiro 48:5562a99479e5 4023 case 2:
yabahiro 48:5562a99479e5 4024 motor[TIRE_FL].pwm = 30;
yabahiro 48:5562a99479e5 4025 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4026 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4027 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4028 break;
yabahiro 48:5562a99479e5 4029 }
yabahiro 48:5562a99479e5 4030 if(g[3]==0&&g[2]==0) {
yabahiro 48:5562a99479e5 4031 mode=23;
yabahiro 48:5562a99479e5 4032 countss=0;
yabahiro 48:5562a99479e5 4033 }
yabahiro 48:5562a99479e5 4034 } else if(mode==23) {
yabahiro 48:5562a99479e5 4035 switch(g[4]) {
yabahiro 48:5562a99479e5 4036 case 98:
yabahiro 48:5562a99479e5 4037 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 4038 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4039 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 4040 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4041 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 4042 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4043 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 4044 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4045 break;
yabahiro 48:5562a99479e5 4046 case 99:
yabahiro 48:5562a99479e5 4047 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4048 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4049 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4050 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4051 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4052 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4053 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4054 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4055 break;
yabahiro 48:5562a99479e5 4056 case 0:
yabahiro 48:5562a99479e5 4057 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 4058 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4059 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 4060 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4061 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 4062 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4063 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 4064 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4065 break;
yabahiro 48:5562a99479e5 4066 case 255:
yabahiro 48:5562a99479e5 4067 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4068 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4069 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4070 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4071 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4072 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4073 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4074 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4075 break;
yabahiro 48:5562a99479e5 4076 case 253:
yabahiro 48:5562a99479e5 4077 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 4078 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4079 motor[TIRE_FL].pwm = slow-5;
yabahiro 48:5562a99479e5 4080 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4081 motor[TIRE_BR].pwm = slow-5;
yabahiro 48:5562a99479e5 4082 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4083 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 4084 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4085 break;
yabahiro 48:5562a99479e5 4086 case 254:
yabahiro 48:5562a99479e5 4087 motor[TIRE_FR].pwm = fast-5;
yabahiro 48:5562a99479e5 4088 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4089 motor[TIRE_FL].pwm = normal-5;
yabahiro 48:5562a99479e5 4090 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4091 motor[TIRE_BR].pwm = normal-5;
yabahiro 48:5562a99479e5 4092 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4093 motor[TIRE_BL].pwm = fast-5;
yabahiro 48:5562a99479e5 4094 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4095 break;
yabahiro 48:5562a99479e5 4096 case 1:
yabahiro 48:5562a99479e5 4097 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4098 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4099 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4100 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4101 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4102 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4103 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4104 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4105 break;
yabahiro 48:5562a99479e5 4106 case 3:
yabahiro 48:5562a99479e5 4107 motor[TIRE_FR].pwm = slow-5;
yabahiro 48:5562a99479e5 4108 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4109 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 4110 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4111 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 4112 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4113 motor[TIRE_BL].pwm = slow-5;
yabahiro 48:5562a99479e5 4114 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4115 break;
yabahiro 48:5562a99479e5 4116 case 2:
yabahiro 48:5562a99479e5 4117 motor[TIRE_FR].pwm = normal-5;
yabahiro 48:5562a99479e5 4118 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4119 motor[TIRE_FL].pwm = fast-5;
yabahiro 48:5562a99479e5 4120 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4121 motor[TIRE_BR].pwm = fast-5;
yabahiro 48:5562a99479e5 4122 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4123 motor[TIRE_BL].pwm = normal-5;
yabahiro 48:5562a99479e5 4124 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4125 break;
yabahiro 48:5562a99479e5 4126 }
yabahiro 48:5562a99479e5 4127 if(g[2]==254&&countss>=100000) {
yabahiro 48:5562a99479e5 4128 mode=24;
yabahiro 48:5562a99479e5 4129 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 48:5562a99479e5 4130 countss=0;
yabahiro 48:5562a99479e5 4131 fast=60;
yabahiro 48:5562a99479e5 4132 normal=40;
yabahiro 48:5562a99479e5 4133 slow=20;
yabahiro 48:5562a99479e5 4134 }
yabahiro 48:5562a99479e5 4135 } else if(mode==24) {
yabahiro 48:5562a99479e5 4136 motor[TIRE_FR].pwm = 60;
yabahiro 48:5562a99479e5 4137 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4138 motor[TIRE_FL].pwm = 60;
yabahiro 48:5562a99479e5 4139 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4140 motor[TIRE_BR].pwm = 60;
yabahiro 48:5562a99479e5 4141 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4142 motor[TIRE_BL].pwm = 60;
yabahiro 48:5562a99479e5 4143 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4144 if(g[0]==0) {
yabahiro 48:5562a99479e5 4145 mode=25;
yabahiro 48:5562a99479e5 4146 }
yabahiro 48:5562a99479e5 4147 } else if(mode==25) {
yabahiro 48:5562a99479e5 4148 motor[TIRE_FR].pwm = 100;
yabahiro 48:5562a99479e5 4149 motor[TIRE_FR].dir = BRAKE;
yabahiro 48:5562a99479e5 4150 motor[TIRE_FL].pwm = 100;
yabahiro 48:5562a99479e5 4151 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4152 motor[TIRE_BR].pwm = 100;
yabahiro 48:5562a99479e5 4153 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4154 motor[TIRE_BL].pwm = 100;
yabahiro 48:5562a99479e5 4155 motor[TIRE_BL].dir = BRAKE;
yabahiro 48:5562a99479e5 4156 if(DOLS==true||UPLS==true) {
yabahiro 48:5562a99479e5 4157 motor[MOTOR_0].pwm = 200;
yabahiro 48:5562a99479e5 4158 motor[MOTOR_0].dir = BRAKE;
yabahiro 48:5562a99479e5 4159 mode=26;
yabahiro 48:5562a99479e5 4160 countss=0;
yabahiro 48:5562a99479e5 4161 } else {
yabahiro 48:5562a99479e5 4162 motor[MOTOR_0].pwm = 100;
yabahiro 48:5562a99479e5 4163 motor[MOTOR_0].dir = BACK;
yabahiro 48:5562a99479e5 4164 }
yabahiro 48:5562a99479e5 4165 } else if(mode==26) {
yabahiro 48:5562a99479e5 4166 if(g[0]==98&&countss>=40000) {
yabahiro 48:5562a99479e5 4167 mode=31;
yabahiro 48:5562a99479e5 4168 countss=0;
yabahiro 48:5562a99479e5 4169 }
yabahiro 48:5562a99479e5 4170 switch(g[0]) {
yabahiro 48:5562a99479e5 4171 case 98:
yabahiro 48:5562a99479e5 4172 fast=40;
yabahiro 48:5562a99479e5 4173 normal=30;
yabahiro 48:5562a99479e5 4174 slow=25;
yabahiro 48:5562a99479e5 4175 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4176 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4177 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4178 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4179 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4180 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4181 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4182 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4183 break;
yabahiro 48:5562a99479e5 4184 case 0:
yabahiro 48:5562a99479e5 4185 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4186 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4187 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4188 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4189 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4190 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4191 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4192 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4193 mtc=true;
yabahiro 48:5562a99479e5 4194 break;
yabahiro 48:5562a99479e5 4195 case 255:
yabahiro 48:5562a99479e5 4196 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4197 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4198 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4199 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4200 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4201 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4202 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4203 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4204 mtc=true;
yabahiro 48:5562a99479e5 4205 break;
yabahiro 48:5562a99479e5 4206 case 253:
yabahiro 48:5562a99479e5 4207 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4208 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4209 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4210 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4211 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4212 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4213 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4214 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4215 mtc=true;
yabahiro 48:5562a99479e5 4216 break;
yabahiro 48:5562a99479e5 4217 case 254:
yabahiro 48:5562a99479e5 4218 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4219 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4220 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4221 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4222 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4223 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4224 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4225 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4226 mtc=true;
yabahiro 48:5562a99479e5 4227 break;
yabahiro 48:5562a99479e5 4228 case 1:
yabahiro 48:5562a99479e5 4229 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4230 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4231 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4232 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4233 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4234 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4235 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4236 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4237 mtc=true;
yabahiro 48:5562a99479e5 4238 break;
yabahiro 48:5562a99479e5 4239 case 3:
yabahiro 48:5562a99479e5 4240 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4241 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4242 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4243 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4244 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4245 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4246 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4247 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4248 mtc=true;
yabahiro 48:5562a99479e5 4249 break;
yabahiro 48:5562a99479e5 4250 case 2:
yabahiro 48:5562a99479e5 4251 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4252 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4253 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4254 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4255 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4256 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4257 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4258 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4259 mtc=true;
yabahiro 48:5562a99479e5 4260 break;
yabahiro 48:5562a99479e5 4261 }
yabahiro 48:5562a99479e5 4262 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4263 switch(g[1]) {
yabahiro 48:5562a99479e5 4264 case 0:
yabahiro 48:5562a99479e5 4265 mtc=false;
yabahiro 48:5562a99479e5 4266 break;
yabahiro 48:5562a99479e5 4267 case 255:
yabahiro 48:5562a99479e5 4268 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4269 mtc=false;
yabahiro 48:5562a99479e5 4270 break;
yabahiro 48:5562a99479e5 4271 case 253:
yabahiro 48:5562a99479e5 4272 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4273 mtc=false;
yabahiro 48:5562a99479e5 4274 break;
yabahiro 48:5562a99479e5 4275 case 254:
yabahiro 48:5562a99479e5 4276 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4277 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4278 mtc=false;
yabahiro 48:5562a99479e5 4279 break;
yabahiro 48:5562a99479e5 4280 case 1:
yabahiro 48:5562a99479e5 4281 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4282 mtc=false;
yabahiro 48:5562a99479e5 4283 break;
yabahiro 48:5562a99479e5 4284 case 3:
yabahiro 48:5562a99479e5 4285 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4286 mtc=false;
yabahiro 48:5562a99479e5 4287 break;
yabahiro 48:5562a99479e5 4288 case 2:
yabahiro 48:5562a99479e5 4289 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4290 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4291 mtc=false;
yabahiro 48:5562a99479e5 4292 break;
yabahiro 48:5562a99479e5 4293 }
yabahiro 48:5562a99479e5 4294 }
yabahiro 48:5562a99479e5 4295 }
yabahiro 48:5562a99479e5 4296 } else {
yabahiro 48:5562a99479e5 4297 if(mode==21) {
yabahiro 49:697b4e9ce884 4298 if(g[0]==98&&countss>=40000) {
yabahiro 48:5562a99479e5 4299 fast=60;
yabahiro 48:5562a99479e5 4300 normal=30;
yabahiro 48:5562a99479e5 4301 slow=25;
yabahiro 48:5562a99479e5 4302 mode=31;
yabahiro 48:5562a99479e5 4303 countss=0;
yabahiro 48:5562a99479e5 4304 }
yabahiro 48:5562a99479e5 4305 switch(g[0]) {
yabahiro 48:5562a99479e5 4306 case 98:
yabahiro 48:5562a99479e5 4307 fast=40;
yabahiro 48:5562a99479e5 4308 normal=30;
yabahiro 48:5562a99479e5 4309 slow=25;
yabahiro 48:5562a99479e5 4310 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4311 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4312 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4313 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4314 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4315 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4316 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4317 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4318 break;
yabahiro 48:5562a99479e5 4319 case 0:
yabahiro 48:5562a99479e5 4320 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4321 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4322 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4323 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4324 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4325 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4326 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4327 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4328 mtc=true;
yabahiro 48:5562a99479e5 4329 break;
yabahiro 48:5562a99479e5 4330 case 255:
yabahiro 48:5562a99479e5 4331 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4332 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4333 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4334 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4335 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4336 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4337 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4338 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4339 mtc=true;
yabahiro 48:5562a99479e5 4340 break;
yabahiro 48:5562a99479e5 4341 case 253:
yabahiro 48:5562a99479e5 4342 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4343 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4344 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4345 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4346 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4347 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4348 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4349 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4350 mtc=true;
yabahiro 48:5562a99479e5 4351 break;
yabahiro 48:5562a99479e5 4352 case 254:
yabahiro 48:5562a99479e5 4353 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4354 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4355 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4356 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4357 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4358 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4359 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4360 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4361 mtc=true;
yabahiro 48:5562a99479e5 4362 break;
yabahiro 48:5562a99479e5 4363 case 1:
yabahiro 48:5562a99479e5 4364 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4365 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4366 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4367 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4368 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4369 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4370 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4371 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4372 mtc=true;
yabahiro 48:5562a99479e5 4373 break;
yabahiro 48:5562a99479e5 4374 case 3:
yabahiro 48:5562a99479e5 4375 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4376 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4377 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4378 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4379 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4380 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4381 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4382 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4383 mtc=true;
yabahiro 48:5562a99479e5 4384 break;
yabahiro 48:5562a99479e5 4385 case 2:
yabahiro 48:5562a99479e5 4386 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4387 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4388 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4389 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4390 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4391 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4392 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4393 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4394 mtc=true;
yabahiro 48:5562a99479e5 4395 break;
yabahiro 48:5562a99479e5 4396 }
yabahiro 48:5562a99479e5 4397 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4398 switch(g[1]) {
yabahiro 48:5562a99479e5 4399 case 0:
yabahiro 48:5562a99479e5 4400 mtc=false;
yabahiro 48:5562a99479e5 4401 break;
yabahiro 48:5562a99479e5 4402 case 255:
yabahiro 48:5562a99479e5 4403 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4404 mtc=false;
yabahiro 48:5562a99479e5 4405 break;
yabahiro 48:5562a99479e5 4406 case 253:
yabahiro 48:5562a99479e5 4407 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4408 mtc=false;
yabahiro 48:5562a99479e5 4409 break;
yabahiro 48:5562a99479e5 4410 case 254:
yabahiro 48:5562a99479e5 4411 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4412 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4413 mtc=false;
yabahiro 48:5562a99479e5 4414 break;
yabahiro 48:5562a99479e5 4415 case 1:
yabahiro 48:5562a99479e5 4416 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4417 mtc=false;
yabahiro 48:5562a99479e5 4418 break;
yabahiro 48:5562a99479e5 4419 case 3:
yabahiro 48:5562a99479e5 4420 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4421 mtc=false;
yabahiro 48:5562a99479e5 4422 break;
yabahiro 48:5562a99479e5 4423 case 2:
yabahiro 48:5562a99479e5 4424 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4425 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4426 mtc=false;
yabahiro 48:5562a99479e5 4427 break;
yabahiro 48:5562a99479e5 4428 }
yabahiro 48:5562a99479e5 4429 }
yabahiro 48:5562a99479e5 4430 }
yabahiro 48:5562a99479e5 4431 }
yabahiro 48:5562a99479e5 4432
yabahiro 48:5562a99479e5 4433 /*
yabahiro 48:5562a99479e5 4434 if(mode==31) {
yabahiro 48:5562a99479e5 4435 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4436 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4437 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4438 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4439 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4440 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4441 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4442 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4443 if(countss>=5000) {
yabahiro 48:5562a99479e5 4444 countss=0;
yabahiro 48:5562a99479e5 4445 mode=33;
yabahiro 48:5562a99479e5 4446 }
yabahiro 48:5562a99479e5 4447 } else if(mode==33) {
yabahiro 48:5562a99479e5 4448 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 4449 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4450 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4451 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4452 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4453 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4454 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4455 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4456 if(g[3]==0) {
yabahiro 48:5562a99479e5 4457 cross=0;
yabahiro 48:5562a99479e5 4458 countss=0;
yabahiro 48:5562a99479e5 4459 mode=40;
yabahiro 48:5562a99479e5 4460 }
yabahiro 48:5562a99479e5 4461 } else
yabahiro 48:5562a99479e5 4462 */
yabahiro 48:5562a99479e5 4463 if(mode==31) {
yabahiro 48:5562a99479e5 4464 motor[TIRE_FR].pwm = 45;
yabahiro 48:5562a99479e5 4465 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4466 motor[TIRE_FL].pwm = 255;
yabahiro 48:5562a99479e5 4467 motor[TIRE_FL].dir = BRAKE;
yabahiro 48:5562a99479e5 4468 motor[TIRE_BR].pwm = 255;
yabahiro 48:5562a99479e5 4469 motor[TIRE_BR].dir = BRAKE;
yabahiro 48:5562a99479e5 4470 motor[TIRE_BL].pwm = 45;
yabahiro 48:5562a99479e5 4471 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4472 if(g[3]==0) {
yabahiro 48:5562a99479e5 4473 cross=0;
yabahiro 48:5562a99479e5 4474 countss=0;
yabahiro 48:5562a99479e5 4475 mode=40;
yabahiro 48:5562a99479e5 4476 }
yabahiro 48:5562a99479e5 4477 } else if(mode==40) {
yabahiro 48:5562a99479e5 4478 switch(g[3]) {
yabahiro 48:5562a99479e5 4479 case 98:
yabahiro 48:5562a99479e5 4480 motor[TIRE_FR].pwm = normal+15;
yabahiro 48:5562a99479e5 4481 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4482 motor[TIRE_FL].pwm = normal+15;
yabahiro 48:5562a99479e5 4483 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4484 motor[TIRE_BR].pwm = normal+15;
yabahiro 48:5562a99479e5 4485 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4486 motor[TIRE_BL].pwm = normal+15;
yabahiro 48:5562a99479e5 4487 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4488 if(countss>10000) {
yabahiro 48:5562a99479e5 4489 cross++;
yabahiro 48:5562a99479e5 4490 countss=0;
yabahiro 48:5562a99479e5 4491 }
yabahiro 48:5562a99479e5 4492 mtc2=true;
yabahiro 48:5562a99479e5 4493 if(cross==1) {
yabahiro 48:5562a99479e5 4494 mode=41;
yabahiro 48:5562a99479e5 4495 countss=0;
yabahiro 48:5562a99479e5 4496 cross=0;
yabahiro 48:5562a99479e5 4497 }
yabahiro 48:5562a99479e5 4498 break;
yabahiro 48:5562a99479e5 4499 case 0:
yabahiro 48:5562a99479e5 4500 motor[TIRE_FR].pwm = fast+15;
yabahiro 48:5562a99479e5 4501 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4502 motor[TIRE_FL].pwm = fast+15;
yabahiro 48:5562a99479e5 4503 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4504 motor[TIRE_BR].pwm = fast+15;
yabahiro 48:5562a99479e5 4505 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4506 motor[TIRE_BL].pwm = fast+15;
yabahiro 48:5562a99479e5 4507 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4508 mtc2=true;
yabahiro 48:5562a99479e5 4509 break;
yabahiro 48:5562a99479e5 4510 case 255:
yabahiro 48:5562a99479e5 4511 motor[TIRE_FR].pwm = slow+15;
yabahiro 48:5562a99479e5 4512 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4513 motor[TIRE_FL].pwm = fast+15;
yabahiro 48:5562a99479e5 4514 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4515 motor[TIRE_BR].pwm = fast+15;
yabahiro 48:5562a99479e5 4516 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4517 motor[TIRE_BL].pwm = slow+15;
yabahiro 48:5562a99479e5 4518 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4519 mtc2=true;
yabahiro 48:5562a99479e5 4520 break;
yabahiro 48:5562a99479e5 4521 case 253:
yabahiro 48:5562a99479e5 4522 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4523 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4524 motor[TIRE_FL].pwm = slow+15;
yabahiro 48:5562a99479e5 4525 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4526 motor[TIRE_BR].pwm = slow+15;
yabahiro 48:5562a99479e5 4527 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4528 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4529 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4530 mtc2=true;
yabahiro 48:5562a99479e5 4531 break;
yabahiro 48:5562a99479e5 4532 case 254:
yabahiro 48:5562a99479e5 4533 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4534 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4535 motor[TIRE_FL].pwm = normal+15;
yabahiro 48:5562a99479e5 4536 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4537 motor[TIRE_BR].pwm = normal+15;
yabahiro 48:5562a99479e5 4538 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4539 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4540 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4541 mtc2=true;
yabahiro 48:5562a99479e5 4542 break;
yabahiro 48:5562a99479e5 4543 case 1:
yabahiro 48:5562a99479e5 4544 motor[TIRE_FR].pwm = fast+15;
yabahiro 48:5562a99479e5 4545 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4546 motor[TIRE_FL].pwm = slow+15;
yabahiro 48:5562a99479e5 4547 motor[TIRE_FL].dir = BACK;
yabahiro 48:5562a99479e5 4548 motor[TIRE_BR].pwm = slow+15;
yabahiro 48:5562a99479e5 4549 motor[TIRE_BR].dir = FOR;
yabahiro 48:5562a99479e5 4550 motor[TIRE_BL].pwm = fast+15;
yabahiro 48:5562a99479e5 4551 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4552 mtc2=true;
yabahiro 48:5562a99479e5 4553 break;
yabahiro 48:5562a99479e5 4554 case 3:
yabahiro 48:5562a99479e5 4555 motor[TIRE_FR].pwm = slow+15;
yabahiro 48:5562a99479e5 4556 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4557 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4558 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4559 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4560 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4561 motor[TIRE_BL].pwm = slow+15;
yabahiro 48:5562a99479e5 4562 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4563 mtc2=true;
yabahiro 48:5562a99479e5 4564 break;
yabahiro 48:5562a99479e5 4565 case 2:
yabahiro 48:5562a99479e5 4566 motor[TIRE_FR].pwm = normal+15;
yabahiro 48:5562a99479e5 4567 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4568 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4569 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4570 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4571 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4572 motor[TIRE_BL].pwm = normal+15;
yabahiro 48:5562a99479e5 4573 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4574 mtc2=true;
yabahiro 48:5562a99479e5 4575 break;
yabahiro 48:5562a99479e5 4576 }
yabahiro 48:5562a99479e5 4577 if(g[2]!=98&&mtc2==true) {
yabahiro 48:5562a99479e5 4578 switch(g[2]) {
yabahiro 48:5562a99479e5 4579 case 0:
yabahiro 48:5562a99479e5 4580 mtc2=false;
yabahiro 48:5562a99479e5 4581 break;
yabahiro 48:5562a99479e5 4582 case 255:
yabahiro 48:5562a99479e5 4583 motor[TIRE_FL].pwm += 5;
yabahiro 48:5562a99479e5 4584 mtc2=false;
yabahiro 48:5562a99479e5 4585 break;
yabahiro 48:5562a99479e5 4586 case 253:
yabahiro 48:5562a99479e5 4587 motor[TIRE_FL].pwm += 10;
yabahiro 48:5562a99479e5 4588 mtc2=false;
yabahiro 48:5562a99479e5 4589 break;
yabahiro 48:5562a99479e5 4590 case 254:
yabahiro 48:5562a99479e5 4591 motor[TIRE_FL].pwm += 20;
yabahiro 48:5562a99479e5 4592 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4593 mtc2=false;
yabahiro 48:5562a99479e5 4594 break;
yabahiro 48:5562a99479e5 4595 case 1:
yabahiro 48:5562a99479e5 4596 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4597 mtc2=false;
yabahiro 48:5562a99479e5 4598 break;
yabahiro 48:5562a99479e5 4599 case 3:
yabahiro 48:5562a99479e5 4600 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4601 mtc2=false;
yabahiro 48:5562a99479e5 4602 break;
yabahiro 48:5562a99479e5 4603 case 2:
yabahiro 48:5562a99479e5 4604 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4605 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4606 mtc2=false;
yabahiro 48:5562a99479e5 4607 break;
yabahiro 48:5562a99479e5 4608 }
yabahiro 48:5562a99479e5 4609 }
yabahiro 48:5562a99479e5 4610 } else if(mode==41) {
yabahiro 48:5562a99479e5 4611 if(LIF||LIB) {
yabahiro 48:5562a99479e5 4612 mode=42;
yabahiro 48:5562a99479e5 4613 }
yabahiro 48:5562a99479e5 4614 motor[TIRE_FR].pwm = 30;
yabahiro 48:5562a99479e5 4615 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4616 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4617 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4618 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4619 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4620 motor[TIRE_BL].pwm = 30;
yabahiro 48:5562a99479e5 4621 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4622 if(g[1]==0) {
yabahiro 48:5562a99479e5 4623 countss=0;
yabahiro 48:5562a99479e5 4624 mode=70;
yabahiro 48:5562a99479e5 4625 normal=60;
yabahiro 48:5562a99479e5 4626 fast=70;
yabahiro 48:5562a99479e5 4627 slow=40;
yabahiro 48:5562a99479e5 4628 }
yabahiro 48:5562a99479e5 4629 } else if(mode==42) {
yabahiro 48:5562a99479e5 4630 motor[TIRE_FR].pwm = 40;
yabahiro 48:5562a99479e5 4631 motor[TIRE_FR].dir = FOR;
yabahiro 48:5562a99479e5 4632 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4633 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4634 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4635 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4636 motor[TIRE_BL].pwm = 40;
yabahiro 48:5562a99479e5 4637 motor[TIRE_BL].dir = BACK;
yabahiro 48:5562a99479e5 4638 if(g[1]==0) {
yabahiro 48:5562a99479e5 4639 countss=0;
yabahiro 48:5562a99479e5 4640 mode=70;
yabahiro 48:5562a99479e5 4641 }
yabahiro 48:5562a99479e5 4642 } else if(mode==70) {
yabahiro 48:5562a99479e5 4643 switch(g[0]) {
yabahiro 48:5562a99479e5 4644 case 99:
yabahiro 48:5562a99479e5 4645 if(countss>=100000) {
yabahiro 48:5562a99479e5 4646 mode=100;
yabahiro 48:5562a99479e5 4647 }
yabahiro 48:5562a99479e5 4648 break;
yabahiro 48:5562a99479e5 4649 case 98:
yabahiro 48:5562a99479e5 4650 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4651 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4652 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4653 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4654 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4655 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4656 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4657 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4658 break;
yabahiro 48:5562a99479e5 4659 case 0:
yabahiro 48:5562a99479e5 4660 motor[TIRE_FR].pwm = 80;
yabahiro 48:5562a99479e5 4661 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4662 motor[TIRE_FL].pwm = 80;
yabahiro 48:5562a99479e5 4663 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4664 motor[TIRE_BR].pwm = 80;
yabahiro 48:5562a99479e5 4665 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4666 motor[TIRE_BL].pwm = 80;
yabahiro 48:5562a99479e5 4667 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4668 mtc=true;
yabahiro 48:5562a99479e5 4669 break;
yabahiro 48:5562a99479e5 4670 case 255:
yabahiro 48:5562a99479e5 4671 motor[TIRE_FR].pwm = fast;
yabahiro 48:5562a99479e5 4672 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4673 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4674 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4675 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4676 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4677 motor[TIRE_BL].pwm = fast;
yabahiro 48:5562a99479e5 4678 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4679 mtc=true;
yabahiro 48:5562a99479e5 4680 break;
yabahiro 48:5562a99479e5 4681 case 253:
yabahiro 48:5562a99479e5 4682 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4683 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4684 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4685 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4686 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4687 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4688 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4689 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4690 mtc=true;
yabahiro 48:5562a99479e5 4691 break;
yabahiro 48:5562a99479e5 4692 case 254:
yabahiro 48:5562a99479e5 4693 motor[TIRE_FR].pwm = normal;
yabahiro 48:5562a99479e5 4694 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4695 motor[TIRE_FL].pwm = 0;
yabahiro 48:5562a99479e5 4696 motor[TIRE_FL].dir = FREE;
yabahiro 48:5562a99479e5 4697 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4698 motor[TIRE_BR].dir = FREE;
yabahiro 48:5562a99479e5 4699 motor[TIRE_BL].pwm = normal;
yabahiro 48:5562a99479e5 4700 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4701 mtc=true;
yabahiro 48:5562a99479e5 4702 break;
yabahiro 48:5562a99479e5 4703 case 1:
yabahiro 48:5562a99479e5 4704 motor[TIRE_FR].pwm = slow;
yabahiro 48:5562a99479e5 4705 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4706 motor[TIRE_FL].pwm = fast;
yabahiro 48:5562a99479e5 4707 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4708 motor[TIRE_BR].pwm = fast;
yabahiro 48:5562a99479e5 4709 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4710 motor[TIRE_BL].pwm = slow;
yabahiro 48:5562a99479e5 4711 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4712 mtc=true;
yabahiro 48:5562a99479e5 4713 break;
yabahiro 48:5562a99479e5 4714 case 3:
yabahiro 48:5562a99479e5 4715 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4716 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4717 motor[TIRE_FL].pwm = slow;
yabahiro 48:5562a99479e5 4718 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4719 motor[TIRE_BR].pwm = slow;
yabahiro 48:5562a99479e5 4720 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4721 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4722 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4723 mtc=true;
yabahiro 48:5562a99479e5 4724 break;
yabahiro 48:5562a99479e5 4725 case 2:
yabahiro 48:5562a99479e5 4726 motor[TIRE_FR].pwm = 0;
yabahiro 48:5562a99479e5 4727 motor[TIRE_FR].dir = FREE;
yabahiro 48:5562a99479e5 4728 motor[TIRE_FL].pwm = normal;
yabahiro 48:5562a99479e5 4729 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4730 motor[TIRE_BR].pwm = normal;
yabahiro 48:5562a99479e5 4731 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4732 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4733 motor[TIRE_BL].dir = FREE;
yabahiro 48:5562a99479e5 4734 mtc=true;
yabahiro 48:5562a99479e5 4735 break;
yabahiro 48:5562a99479e5 4736 }
yabahiro 48:5562a99479e5 4737 if(g[1]!=98&&mtc==true) {
yabahiro 48:5562a99479e5 4738 switch(g[1]) {
yabahiro 48:5562a99479e5 4739 case 0:
yabahiro 48:5562a99479e5 4740 mtc=false;
yabahiro 48:5562a99479e5 4741 break;
yabahiro 48:5562a99479e5 4742 case 255:
yabahiro 48:5562a99479e5 4743 motor[TIRE_BR].pwm += 5;
yabahiro 48:5562a99479e5 4744 mtc=false;
yabahiro 48:5562a99479e5 4745 break;
yabahiro 48:5562a99479e5 4746 case 253:
yabahiro 48:5562a99479e5 4747 motor[TIRE_BR].pwm += 10;
yabahiro 48:5562a99479e5 4748 mtc=false;
yabahiro 48:5562a99479e5 4749 break;
yabahiro 48:5562a99479e5 4750 case 254:
yabahiro 48:5562a99479e5 4751 motor[TIRE_BR].pwm += 20;
yabahiro 48:5562a99479e5 4752 motor[TIRE_BL].pwm = 0;
yabahiro 48:5562a99479e5 4753 mtc=false;
yabahiro 48:5562a99479e5 4754 break;
yabahiro 48:5562a99479e5 4755 case 1:
yabahiro 48:5562a99479e5 4756 motor[TIRE_BL].pwm += 5;
yabahiro 48:5562a99479e5 4757 mtc=false;
yabahiro 48:5562a99479e5 4758 break;
yabahiro 48:5562a99479e5 4759 case 3:
yabahiro 48:5562a99479e5 4760 motor[TIRE_BL].pwm += 10;
yabahiro 48:5562a99479e5 4761 mtc=false;
yabahiro 48:5562a99479e5 4762 break;
yabahiro 48:5562a99479e5 4763 case 2:
yabahiro 48:5562a99479e5 4764 motor[TIRE_BL].pwm += 20;
yabahiro 48:5562a99479e5 4765 motor[TIRE_BR].pwm = 0;
yabahiro 48:5562a99479e5 4766 mtc=false;
yabahiro 48:5562a99479e5 4767 break;
yabahiro 48:5562a99479e5 4768 }
yabahiro 48:5562a99479e5 4769 }
yabahiro 48:5562a99479e5 4770 } else if(mode==100) {
yabahiro 48:5562a99479e5 4771 motor[TIRE_FR].pwm = 50;
yabahiro 48:5562a99479e5 4772 motor[TIRE_FR].dir = BACK;
yabahiro 48:5562a99479e5 4773 motor[TIRE_FL].pwm = 50;
yabahiro 48:5562a99479e5 4774 motor[TIRE_FL].dir = FOR;
yabahiro 48:5562a99479e5 4775 motor[TIRE_BR].pwm = 50;
yabahiro 48:5562a99479e5 4776 motor[TIRE_BR].dir = BACK;
yabahiro 48:5562a99479e5 4777 motor[TIRE_BL].pwm = 50;
yabahiro 48:5562a99479e5 4778 motor[TIRE_BL].dir = FOR;
yabahiro 48:5562a99479e5 4779 } else {
yabahiro 48:5562a99479e5 4780 }
yabahiro 33:dd598bb23ad1 4781
kishibekairohan 13:b6e02d6261d7 4782 }
kishibekairohan 13:b6e02d6261d7 4783 #endif
kishibekairohan 13:b6e02d6261d7 4784
kishibekairohan 13:b6e02d6261d7 4785 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 4786 static void Process5()
Ryosei 27:dd9f27fce7d1 4787 {
yabahiro 56:61ebd4b759b4 4788 countss++;
yabahiro 56:61ebd4b759b4 4789 switch(mode) {
yabahiro 56:61ebd4b759b4 4790 case 1:
yabahiro 56:61ebd4b759b4 4791 mode=2;
yabahiro 56:61ebd4b759b4 4792 countss=0;
yabahiro 56:61ebd4b759b4 4793 fast = 60;
yabahiro 56:61ebd4b759b4 4794 normal = 40;
yabahiro 56:61ebd4b759b4 4795 slow = 30;
yabahiro 56:61ebd4b759b4 4796 break;
yabahiro 56:61ebd4b759b4 4797 case 2:
yabahiro 56:61ebd4b759b4 4798 if(YELLOW) {
yabahiro 56:61ebd4b759b4 4799 switch(g[4]) {
yabahiro 56:61ebd4b759b4 4800 case 98:
yabahiro 56:61ebd4b759b4 4801 motor[TIRE_FR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4802 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4803 motor[TIRE_FL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4804 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4805 motor[TIRE_BR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4806 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4807 motor[TIRE_BL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4808 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4809 break;
yabahiro 56:61ebd4b759b4 4810 case 99:
yabahiro 56:61ebd4b759b4 4811 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 4812 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4813 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 4814 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 4815 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 4816 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4817 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 4818 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 4819 break;
yabahiro 56:61ebd4b759b4 4820 case 0:
yabahiro 56:61ebd4b759b4 4821 motor[TIRE_FR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4822 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4823 motor[TIRE_FL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4824 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4825 motor[TIRE_BR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4826 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4827 motor[TIRE_BL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4828 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4829 break;
yabahiro 56:61ebd4b759b4 4830 case 255:
yabahiro 56:61ebd4b759b4 4831 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 4832 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4833 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 4834 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 4835 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 4836 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4837 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 4838 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 4839 break;
yabahiro 56:61ebd4b759b4 4840 case 253:
yabahiro 56:61ebd4b759b4 4841 motor[TIRE_FR].pwm = slow+5;
yabahiro 56:61ebd4b759b4 4842 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4843 motor[TIRE_FL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4844 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4845 motor[TIRE_BR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4846 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4847 motor[TIRE_BL].pwm = slow+5;
yabahiro 56:61ebd4b759b4 4848 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4849 break;
yabahiro 56:61ebd4b759b4 4850 case 254:
yabahiro 56:61ebd4b759b4 4851 motor[TIRE_FR].pwm = normal+5;
yabahiro 56:61ebd4b759b4 4852 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4853 motor[TIRE_FL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4854 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4855 motor[TIRE_BR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4856 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4857 motor[TIRE_BL].pwm = normal+5;
yabahiro 56:61ebd4b759b4 4858 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4859 break;
yabahiro 56:61ebd4b759b4 4860 case 1:
yabahiro 56:61ebd4b759b4 4861 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 4862 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4863 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 4864 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 4865 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 4866 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4867 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 4868 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 4869 break;
yabahiro 56:61ebd4b759b4 4870 case 3:
yabahiro 56:61ebd4b759b4 4871 motor[TIRE_FR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4872 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4873 motor[TIRE_FL].pwm = slow+5;
yabahiro 56:61ebd4b759b4 4874 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4875 motor[TIRE_BR].pwm = slow+5;
yabahiro 56:61ebd4b759b4 4876 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4877 motor[TIRE_BL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4878 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4879 break;
yabahiro 56:61ebd4b759b4 4880 case 2:
yabahiro 56:61ebd4b759b4 4881 motor[TIRE_FR].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4882 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4883 motor[TIRE_FL].pwm = normal+5;
yabahiro 56:61ebd4b759b4 4884 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4885 motor[TIRE_BR].pwm = normal+5;
yabahiro 56:61ebd4b759b4 4886 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4887 motor[TIRE_BL].pwm = fast-5;
yabahiro 56:61ebd4b759b4 4888 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4889 break;
yabahiro 56:61ebd4b759b4 4890 }
yabahiro 56:61ebd4b759b4 4891 if(g[1]==98||g[1]==0||g[1]==1||g[1]==2||g[1]==3||g[1]==254||g[1]==253||g[1]==255) {
yabahiro 56:61ebd4b759b4 4892 fast=10;
yabahiro 56:61ebd4b759b4 4893 normal=10;
yabahiro 56:61ebd4b759b4 4894 slow=10;
yabahiro 56:61ebd4b759b4 4895 }
yabahiro 56:61ebd4b759b4 4896 if(g[3]==254&&countss>=100000) {
yabahiro 56:61ebd4b759b4 4897 mode=3;
yabahiro 56:61ebd4b759b4 4898 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 56:61ebd4b759b4 4899 aime.start();
yabahiro 56:61ebd4b759b4 4900 aime.reset();
yabahiro 56:61ebd4b759b4 4901 countss=0;
yabahiro 56:61ebd4b759b4 4902 fast=80;
yabahiro 56:61ebd4b759b4 4903 normal=60;
yabahiro 56:61ebd4b759b4 4904 slow=30;
yabahiro 56:61ebd4b759b4 4905 }
yabahiro 56:61ebd4b759b4 4906 } else {
yabahiro 56:61ebd4b759b4 4907 if(UPLS||DOLS) {
yabahiro 56:61ebd4b759b4 4908 if(UPLS||DOLS) {
yabahiro 56:61ebd4b759b4 4909 motor[MOTOR_0].pwm = 200;
yabahiro 56:61ebd4b759b4 4910 motor[MOTOR_0].dir = BRAKE;
yabahiro 56:61ebd4b759b4 4911 fast=60;
yabahiro 56:61ebd4b759b4 4912 normal=40;
yabahiro 56:61ebd4b759b4 4913 slow=30;
yabahiro 56:61ebd4b759b4 4914 countss=0;
yabahiro 56:61ebd4b759b4 4915 mode=24;
yabahiro 56:61ebd4b759b4 4916 }
yabahiro 56:61ebd4b759b4 4917 } else {
yabahiro 56:61ebd4b759b4 4918 motor[MOTOR_0].pwm = 100;
yabahiro 56:61ebd4b759b4 4919 motor[MOTOR_0].dir = BACK;
yabahiro 56:61ebd4b759b4 4920 }
yabahiro 56:61ebd4b759b4 4921 motor[TIRE_FR].pwm = 100;
yabahiro 56:61ebd4b759b4 4922 motor[TIRE_FR].dir = BRAKE;
yabahiro 56:61ebd4b759b4 4923 motor[TIRE_FL].pwm = 100;
yabahiro 56:61ebd4b759b4 4924 motor[TIRE_FL].dir = BRAKE;
yabahiro 56:61ebd4b759b4 4925 motor[TIRE_BR].pwm = 100;
yabahiro 56:61ebd4b759b4 4926 motor[TIRE_BR].dir = BRAKE;
yabahiro 56:61ebd4b759b4 4927 motor[TIRE_BL].pwm = 100;
yabahiro 56:61ebd4b759b4 4928 motor[TIRE_BL].dir = BRAKE;
yabahiro 56:61ebd4b759b4 4929 }
yabahiro 56:61ebd4b759b4 4930 break;
yabahiro 56:61ebd4b759b4 4931 case 3:
yabahiro 56:61ebd4b759b4 4932 if(YELLOW) {
yabahiro 56:61ebd4b759b4 4933 value=aime.read_ms();
yabahiro 56:61ebd4b759b4 4934 if(value>=5000) {
yabahiro 56:61ebd4b759b4 4935 mode=4;
yabahiro 56:61ebd4b759b4 4936 }
yabahiro 56:61ebd4b759b4 4937 if(g[4]==3||g[4]==2) {
yabahiro 56:61ebd4b759b4 4938 motor[TIRE_FR].pwm = 20;
yabahiro 56:61ebd4b759b4 4939 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4940 motor[TIRE_FL].pwm = 30;
yabahiro 56:61ebd4b759b4 4941 motor[TIRE_FL].dir = FOR;
yabahiro 56:61ebd4b759b4 4942 motor[TIRE_BR].pwm = 30;
yabahiro 56:61ebd4b759b4 4943 motor[TIRE_BR].dir = BACK;
yabahiro 56:61ebd4b759b4 4944 motor[TIRE_BL].pwm = 20;
yabahiro 56:61ebd4b759b4 4945 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 4946 }
yabahiro 56:61ebd4b759b4 4947 if(g[4]==253||g[4]==254) {
yabahiro 56:61ebd4b759b4 4948 motor[TIRE_FR].pwm = 30;
yabahiro 56:61ebd4b759b4 4949 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 4950 motor[TIRE_FL].pwm = 20;
yabahiro 56:61ebd4b759b4 4951 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 4952 motor[TIRE_BR].pwm = 20;
yabahiro 56:61ebd4b759b4 4953 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4954 motor[TIRE_BL].pwm = 30;
yabahiro 56:61ebd4b759b4 4955 motor[TIRE_BL].dir = FOR;
yabahiro 56:61ebd4b759b4 4956 }
yabahiro 56:61ebd4b759b4 4957 switch(g[2]) {
yabahiro 56:61ebd4b759b4 4958 case 0:
yabahiro 56:61ebd4b759b4 4959 motor[TIRE_FR].pwm = 25;
yabahiro 56:61ebd4b759b4 4960 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4961 motor[TIRE_BR].pwm = 25;
yabahiro 56:61ebd4b759b4 4962 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4963 break;
yabahiro 56:61ebd4b759b4 4964 case 255:
yabahiro 56:61ebd4b759b4 4965 motor[TIRE_FR].pwm = 30;
yabahiro 56:61ebd4b759b4 4966 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 4967 motor[TIRE_BR].pwm = 25;
yabahiro 56:61ebd4b759b4 4968 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4969 break;
yabahiro 56:61ebd4b759b4 4970 case 253:
yabahiro 56:61ebd4b759b4 4971 motor[TIRE_FR].pwm = 40;
yabahiro 56:61ebd4b759b4 4972 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 4973 motor[TIRE_BR].pwm = 25;
yabahiro 56:61ebd4b759b4 4974 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 4975 break;
yabahiro 56:61ebd4b759b4 4976 case 254:
yabahiro 56:61ebd4b759b4 4977 motor[TIRE_FR].pwm = 30;
yabahiro 56:61ebd4b759b4 4978 motor[TIRE_FR].dir = BACK;
yabahiro 56:61ebd4b759b4 4979 motor[TIRE_BR].pwm = 30;
yabahiro 56:61ebd4b759b4 4980 motor[TIRE_BR].dir = BACK;
yabahiro 56:61ebd4b759b4 4981 break;
yabahiro 56:61ebd4b759b4 4982 case 1:
yabahiro 56:61ebd4b759b4 4983 motor[TIRE_FR].pwm = 25;
yabahiro 56:61ebd4b759b4 4984 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4985 motor[TIRE_BR].pwm = 30;
yabahiro 56:61ebd4b759b4 4986 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4987 break;
yabahiro 56:61ebd4b759b4 4988 case 3:
yabahiro 56:61ebd4b759b4 4989 motor[TIRE_FR].pwm = 25;
yabahiro 56:61ebd4b759b4 4990 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 4991 motor[TIRE_BR].pwm = 40;
yabahiro 56:61ebd4b759b4 4992 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4993 break;
yabahiro 56:61ebd4b759b4 4994 case 2:
yabahiro 56:61ebd4b759b4 4995 motor[TIRE_FR].pwm = 30;
yabahiro 56:61ebd4b759b4 4996 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 4997 motor[TIRE_BR].pwm = 30;
yabahiro 56:61ebd4b759b4 4998 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 4999 break;
yabahiro 56:61ebd4b759b4 5000 }
yabahiro 56:61ebd4b759b4 5001 switch(g[3]) {
yabahiro 56:61ebd4b759b4 5002 case 0:
yabahiro 56:61ebd4b759b4 5003 motor[TIRE_FL].pwm = 25;
yabahiro 56:61ebd4b759b4 5004 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5005 motor[TIRE_BL].pwm = 25;
yabahiro 56:61ebd4b759b4 5006 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5007 break;
yabahiro 56:61ebd4b759b4 5008 case 255:
yabahiro 56:61ebd4b759b4 5009 motor[TIRE_FL].pwm = 25;
yabahiro 56:61ebd4b759b4 5010 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5011 motor[TIRE_BL].pwm = 30;
yabahiro 56:61ebd4b759b4 5012 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5013 case 253:
yabahiro 56:61ebd4b759b4 5014 motor[TIRE_FL].pwm = 25;
yabahiro 56:61ebd4b759b4 5015 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5016 motor[TIRE_BL].pwm = 40;
yabahiro 56:61ebd4b759b4 5017 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5018 break;
yabahiro 56:61ebd4b759b4 5019 case 254:
yabahiro 56:61ebd4b759b4 5020 motor[TIRE_FL].pwm = 30;
yabahiro 56:61ebd4b759b4 5021 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5022 motor[TIRE_BL].pwm = 30;
yabahiro 56:61ebd4b759b4 5023 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5024 break;
yabahiro 56:61ebd4b759b4 5025 case 1:
yabahiro 56:61ebd4b759b4 5026 motor[TIRE_FL].pwm = 30;
yabahiro 56:61ebd4b759b4 5027 motor[TIRE_FL].dir = FOR;
yabahiro 56:61ebd4b759b4 5028 motor[TIRE_BL].pwm = 25;
yabahiro 56:61ebd4b759b4 5029 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5030 break;
yabahiro 56:61ebd4b759b4 5031 case 3:
yabahiro 56:61ebd4b759b4 5032 motor[TIRE_FL].pwm = 40;
yabahiro 56:61ebd4b759b4 5033 motor[TIRE_FL].dir = FOR;
yabahiro 56:61ebd4b759b4 5034 motor[TIRE_BL].pwm = 25;
yabahiro 56:61ebd4b759b4 5035 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5036 break;
yabahiro 56:61ebd4b759b4 5037 case 2:
yabahiro 56:61ebd4b759b4 5038 motor[TIRE_FL].pwm = 30;
yabahiro 56:61ebd4b759b4 5039 motor[TIRE_FL].dir = FOR;
yabahiro 56:61ebd4b759b4 5040 motor[TIRE_BL].pwm = 30;
yabahiro 56:61ebd4b759b4 5041 motor[TIRE_BL].dir = FOR;
yabahiro 56:61ebd4b759b4 5042 break;
yabahiro 56:61ebd4b759b4 5043 }
yabahiro 56:61ebd4b759b4 5044 if(g[3]==0&&g[2]==0) {
yabahiro 56:61ebd4b759b4 5045 mode=25;
yabahiro 56:61ebd4b759b4 5046 }
yabahiro 56:61ebd4b759b4 5047 } else {
yabahiro 56:61ebd4b759b4 5048 if(g[1]==98&&countss>=40000) {
yabahiro 56:61ebd4b759b4 5049 mode=31;
yabahiro 56:61ebd4b759b4 5050 fast = 60;
yabahiro 56:61ebd4b759b4 5051 normal = 40;
yabahiro 56:61ebd4b759b4 5052 slow = 30;
yabahiro 56:61ebd4b759b4 5053 countss=0;
yabahiro 56:61ebd4b759b4 5054 }
yabahiro 56:61ebd4b759b4 5055 switch(g[1]) {
yabahiro 56:61ebd4b759b4 5056 case 98:
yabahiro 56:61ebd4b759b4 5057 fast=40;
yabahiro 56:61ebd4b759b4 5058 normal=30;
yabahiro 56:61ebd4b759b4 5059 slow=25;
yabahiro 56:61ebd4b759b4 5060 motor[TIRE_FR].pwm = normal;
yabahiro 56:61ebd4b759b4 5061 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5062 motor[TIRE_FL].pwm = normal;
yabahiro 56:61ebd4b759b4 5063 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5064 motor[TIRE_BR].pwm = normal;
yabahiro 56:61ebd4b759b4 5065 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5066 motor[TIRE_BL].pwm = normal;
yabahiro 56:61ebd4b759b4 5067 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5068 break;
yabahiro 56:61ebd4b759b4 5069 case 0:
yabahiro 56:61ebd4b759b4 5070 motor[TIRE_FR].pwm = fast;
yabahiro 56:61ebd4b759b4 5071 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5072 motor[TIRE_FL].pwm = fast;
yabahiro 56:61ebd4b759b4 5073 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5074 motor[TIRE_BR].pwm = fast;
yabahiro 56:61ebd4b759b4 5075 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5076 motor[TIRE_BL].pwm = fast;
yabahiro 56:61ebd4b759b4 5077 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5078 mtc=true;
yabahiro 56:61ebd4b759b4 5079 break;
yabahiro 56:61ebd4b759b4 5080 case 255:
yabahiro 56:61ebd4b759b4 5081 motor[TIRE_FR].pwm = fast;
yabahiro 56:61ebd4b759b4 5082 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5083 motor[TIRE_FL].pwm = slow;
yabahiro 56:61ebd4b759b4 5084 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5085 motor[TIRE_BR].pwm = slow;
yabahiro 56:61ebd4b759b4 5086 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5087 motor[TIRE_BL].pwm = fast;
yabahiro 56:61ebd4b759b4 5088 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5089 mtc=true;
yabahiro 56:61ebd4b759b4 5090 break;
yabahiro 56:61ebd4b759b4 5091 case 253:
yabahiro 56:61ebd4b759b4 5092 motor[TIRE_FR].pwm = slow;
yabahiro 56:61ebd4b759b4 5093 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5094 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5095 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5096 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 5097 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 5098 motor[TIRE_BL].pwm = slow;
yabahiro 56:61ebd4b759b4 5099 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5100 mtc=true;
yabahiro 56:61ebd4b759b4 5101 break;
yabahiro 56:61ebd4b759b4 5102 case 254:
yabahiro 56:61ebd4b759b4 5103 motor[TIRE_FR].pwm = normal;
yabahiro 56:61ebd4b759b4 5104 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5105 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5106 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5107 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 5108 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 5109 motor[TIRE_BL].pwm = normal;
yabahiro 56:61ebd4b759b4 5110 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5111 mtc=true;
yabahiro 56:61ebd4b759b4 5112 break;
yabahiro 56:61ebd4b759b4 5113 case 1:
yabahiro 56:61ebd4b759b4 5114 motor[TIRE_FR].pwm = slow;
yabahiro 56:61ebd4b759b4 5115 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5116 motor[TIRE_FL].pwm = fast;
yabahiro 56:61ebd4b759b4 5117 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5118 motor[TIRE_BR].pwm = fast;
yabahiro 56:61ebd4b759b4 5119 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5120 motor[TIRE_BL].pwm = slow;
yabahiro 56:61ebd4b759b4 5121 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5122 mtc=true;
yabahiro 56:61ebd4b759b4 5123 break;
yabahiro 56:61ebd4b759b4 5124 case 3:
yabahiro 56:61ebd4b759b4 5125 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5126 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 5127 motor[TIRE_FL].pwm = slow;
yabahiro 56:61ebd4b759b4 5128 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5129 motor[TIRE_BR].pwm = slow;
yabahiro 56:61ebd4b759b4 5130 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5131 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 5132 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5133 mtc=true;
yabahiro 56:61ebd4b759b4 5134 break;
yabahiro 56:61ebd4b759b4 5135 case 2:
yabahiro 56:61ebd4b759b4 5136 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5137 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 5138 motor[TIRE_FL].pwm = normal;
yabahiro 56:61ebd4b759b4 5139 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5140 motor[TIRE_BR].pwm = normal;
yabahiro 56:61ebd4b759b4 5141 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5142 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 5143 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5144 mtc=true;
yabahiro 56:61ebd4b759b4 5145 break;
yabahiro 56:61ebd4b759b4 5146 }
yabahiro 56:61ebd4b759b4 5147 if(g[1]!=98&&mtc==true) {
yabahiro 56:61ebd4b759b4 5148 switch(g[0]) {
yabahiro 56:61ebd4b759b4 5149 case 0:
yabahiro 56:61ebd4b759b4 5150 mtc=false;
yabahiro 56:61ebd4b759b4 5151 break;
yabahiro 56:61ebd4b759b4 5152 case 255:
yabahiro 56:61ebd4b759b4 5153 motor[TIRE_FL].pwm += 3;
yabahiro 56:61ebd4b759b4 5154 mtc=false;
yabahiro 56:61ebd4b759b4 5155 break;
yabahiro 56:61ebd4b759b4 5156 case 253:
yabahiro 56:61ebd4b759b4 5157 motor[TIRE_FL].pwm += 6;
yabahiro 56:61ebd4b759b4 5158 mtc=false;
yabahiro 56:61ebd4b759b4 5159 break;
yabahiro 56:61ebd4b759b4 5160 case 254:
yabahiro 56:61ebd4b759b4 5161 motor[TIRE_FL].pwm += 9;
yabahiro 56:61ebd4b759b4 5162 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5163 mtc=false;
yabahiro 56:61ebd4b759b4 5164 break;
yabahiro 56:61ebd4b759b4 5165 case 1:
yabahiro 56:61ebd4b759b4 5166 motor[TIRE_FR].pwm += 3;
yabahiro 56:61ebd4b759b4 5167 mtc=false;
yabahiro 56:61ebd4b759b4 5168 break;
yabahiro 56:61ebd4b759b4 5169 case 3:
yabahiro 56:61ebd4b759b4 5170 motor[TIRE_FR].pwm += 6;
yabahiro 56:61ebd4b759b4 5171 mtc=false;
yabahiro 56:61ebd4b759b4 5172 break;
yabahiro 56:61ebd4b759b4 5173 case 2:
yabahiro 56:61ebd4b759b4 5174 motor[TIRE_FR].pwm += 9;
yabahiro 56:61ebd4b759b4 5175 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5176 mtc=false;
yabahiro 56:61ebd4b759b4 5177 break;
yabahiro 56:61ebd4b759b4 5178 }
yabahiro 56:61ebd4b759b4 5179 }
yabahiro 56:61ebd4b759b4 5180 }
yabahiro 56:61ebd4b759b4 5181 break;
yabahiro 56:61ebd4b759b4 5182 case 4:
yabahiro 56:61ebd4b759b4 5183 motor[TIRE_FR].pwm = 100;
yabahiro 56:61ebd4b759b4 5184 motor[TIRE_FR].dir = BRAKE;
yabahiro 56:61ebd4b759b4 5185 motor[TIRE_FL].pwm = 100;
yabahiro 56:61ebd4b759b4 5186 motor[TIRE_FL].dir = BRAKE;
yabahiro 56:61ebd4b759b4 5187 motor[TIRE_BR].pwm = 100;
yabahiro 56:61ebd4b759b4 5188 motor[TIRE_BR].dir = BRAKE;
yabahiro 56:61ebd4b759b4 5189 motor[TIRE_BL].pwm = 100;
yabahiro 56:61ebd4b759b4 5190 motor[TIRE_BL].dir = BRAKE;
yabahiro 56:61ebd4b759b4 5191 if(UPLS==true||DOLS==true) {
yabahiro 56:61ebd4b759b4 5192 if(UPLS==true||DOLS==true) {
yabahiro 56:61ebd4b759b4 5193 motor[MOTOR_0].pwm = 200;
yabahiro 56:61ebd4b759b4 5194 motor[MOTOR_0].dir = BRAKE;
yabahiro 56:61ebd4b759b4 5195 countss = 0;
yabahiro 56:61ebd4b759b4 5196 mode=26;
yabahiro 56:61ebd4b759b4 5197 }
yabahiro 56:61ebd4b759b4 5198 } else {
yabahiro 56:61ebd4b759b4 5199 motor[MOTOR_0].pwm = 100;
yabahiro 56:61ebd4b759b4 5200 motor[MOTOR_0].dir = BACK;
yabahiro 56:61ebd4b759b4 5201 }
yabahiro 56:61ebd4b759b4 5202 break;
yabahiro 56:61ebd4b759b4 5203 case 5:
yabahiro 56:61ebd4b759b4 5204 /*
yabahiro 56:61ebd4b759b4 5205 if (g[3] == 0&&countss>=30000) {
yabahiro 56:61ebd4b759b4 5206 mode = 40;
yabahiro 56:61ebd4b759b4 5207 }
yabahiro 56:61ebd4b759b4 5208 */
yabahiro 56:61ebd4b759b4 5209 if (g[1] != 99) {
yabahiro 56:61ebd4b759b4 5210 mode = 31;
yabahiro 56:61ebd4b759b4 5211 }
yabahiro 56:61ebd4b759b4 5212 switch(g[4]) {
yabahiro 56:61ebd4b759b4 5213 case 98:
yabahiro 56:61ebd4b759b4 5214 motor[TIRE_FR].pwm = 60;
yabahiro 56:61ebd4b759b4 5215 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5216 motor[TIRE_FL].pwm = 60;
yabahiro 56:61ebd4b759b4 5217 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5218 motor[TIRE_BR].pwm = 60;
yabahiro 56:61ebd4b759b4 5219 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5220 motor[TIRE_BL].pwm = 60;
yabahiro 56:61ebd4b759b4 5221 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5222 break;
yabahiro 56:61ebd4b759b4 5223 case 99:
yabahiro 56:61ebd4b759b4 5224 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5225 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 5226 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5227 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5228 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 5229 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 5230 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 5231 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5232 break;
yabahiro 56:61ebd4b759b4 5233 case 0:
yabahiro 56:61ebd4b759b4 5234 motor[TIRE_FR].pwm = 60;
yabahiro 56:61ebd4b759b4 5235 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5236 motor[TIRE_FL].pwm = 60;
yabahiro 56:61ebd4b759b4 5237 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5238 motor[TIRE_BR].pwm = 60;
yabahiro 56:61ebd4b759b4 5239 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5240 motor[TIRE_BL].pwm = 60;
yabahiro 56:61ebd4b759b4 5241 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5242 break;
yabahiro 56:61ebd4b759b4 5243 case 255:
yabahiro 56:61ebd4b759b4 5244 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5245 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 5246 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5247 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5248 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 5249 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 5250 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 5251 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5252 break;
yabahiro 56:61ebd4b759b4 5253 case 253:
yabahiro 56:61ebd4b759b4 5254 motor[TIRE_FR].pwm = 25;
yabahiro 56:61ebd4b759b4 5255 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5256 motor[TIRE_FL].pwm = 60;
yabahiro 56:61ebd4b759b4 5257 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5258 motor[TIRE_BR].pwm = 60;
yabahiro 56:61ebd4b759b4 5259 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5260 motor[TIRE_BL].pwm = 25;
yabahiro 56:61ebd4b759b4 5261 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5262 break;
yabahiro 56:61ebd4b759b4 5263 case 254:
yabahiro 56:61ebd4b759b4 5264 motor[TIRE_FR].pwm = 40;
yabahiro 56:61ebd4b759b4 5265 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5266 motor[TIRE_FL].pwm = 60;
yabahiro 56:61ebd4b759b4 5267 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5268 motor[TIRE_BR].pwm = 60;
yabahiro 56:61ebd4b759b4 5269 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5270 motor[TIRE_BL].pwm = 40;
yabahiro 56:61ebd4b759b4 5271 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5272 break;
yabahiro 56:61ebd4b759b4 5273 case 1:
yabahiro 56:61ebd4b759b4 5274 motor[TIRE_FR].pwm = 0;
yabahiro 56:61ebd4b759b4 5275 motor[TIRE_FR].dir = FREE;
yabahiro 56:61ebd4b759b4 5276 motor[TIRE_FL].pwm = 0;
yabahiro 56:61ebd4b759b4 5277 motor[TIRE_FL].dir = FREE;
yabahiro 56:61ebd4b759b4 5278 motor[TIRE_BR].pwm = 0;
yabahiro 56:61ebd4b759b4 5279 motor[TIRE_BR].dir = FREE;
yabahiro 56:61ebd4b759b4 5280 motor[TIRE_BL].pwm = 0;
yabahiro 56:61ebd4b759b4 5281 motor[TIRE_BL].dir = FREE;
yabahiro 56:61ebd4b759b4 5282 break;
yabahiro 56:61ebd4b759b4 5283 case 3:
yabahiro 56:61ebd4b759b4 5284 motor[TIRE_FR].pwm = 60;
yabahiro 56:61ebd4b759b4 5285 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5286 motor[TIRE_FL].pwm = 25;
yabahiro 56:61ebd4b759b4 5287 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5288 motor[TIRE_BR].pwm = 25;
yabahiro 56:61ebd4b759b4 5289 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5290 motor[TIRE_BL].pwm = 60;
yabahiro 56:61ebd4b759b4 5291 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5292 break;
yabahiro 56:61ebd4b759b4 5293 case 2:
yabahiro 56:61ebd4b759b4 5294 motor[TIRE_FR].pwm = 60;
yabahiro 56:61ebd4b759b4 5295 motor[TIRE_FR].dir = FOR;
yabahiro 56:61ebd4b759b4 5296 motor[TIRE_FL].pwm = 40;
yabahiro 56:61ebd4b759b4 5297 motor[TIRE_FL].dir = BACK;
yabahiro 56:61ebd4b759b4 5298 motor[TIRE_BR].pwm = 40;
yabahiro 56:61ebd4b759b4 5299 motor[TIRE_BR].dir = FOR;
yabahiro 56:61ebd4b759b4 5300 motor[TIRE_BL].pwm = 60;
yabahiro 56:61ebd4b759b4 5301 motor[TIRE_BL].dir = BACK;
yabahiro 56:61ebd4b759b4 5302 break;
yabahiro 56:61ebd4b759b4 5303 }
yabahiro 56:61ebd4b759b4 5304 break;
yabahiro 56:61ebd4b759b4 5305 }
yabahiro 46:68296caae1af 5306
kishibekairohan 13:b6e02d6261d7 5307 }
kishibekairohan 13:b6e02d6261d7 5308 #endif
kishibekairohan 13:b6e02d6261d7 5309
kishibekairohan 13:b6e02d6261d7 5310 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 5311 static void Process6()
kishibekairohan 13:b6e02d6261d7 5312 {
yabahiro 59:5cd3875fa96a 5313 //Oh So 青ゾーン
yabahiro 59:5cd3875fa96a 5314 value=aime.read_ms();
yabahiro 60:24ebb611f26e 5315 LED::LED::TapeledMode(3);//4
yabahiro 59:5cd3875fa96a 5316 countss++;
yabahiro 59:5cd3875fa96a 5317 switch(mode) {
yabahiro 59:5cd3875fa96a 5318 case 1:
yabahiro 59:5cd3875fa96a 5319 fast=40;
yabahiro 59:5cd3875fa96a 5320 normal=30;
yabahiro 59:5cd3875fa96a 5321 slow=20;
yabahiro 59:5cd3875fa96a 5322 motor[TIRE_FR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5323 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5324 motor[TIRE_FL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5325 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5326 motor[TIRE_BR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5327 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5328 motor[TIRE_BL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5329 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5330 if(g[2]==0&&value>=1500) {
yabahiro 59:5cd3875fa96a 5331 countss=100000;
yabahiro 59:5cd3875fa96a 5332 aime.reset();
yabahiro 59:5cd3875fa96a 5333 cross=0;
yabahiro 59:5cd3875fa96a 5334 mode=3;
yabahiro 59:5cd3875fa96a 5335 }
yabahiro 59:5cd3875fa96a 5336 break;
yabahiro 59:5cd3875fa96a 5337 case 3:
yabahiro 59:5cd3875fa96a 5338 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5339 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5340 motor[TIRE_FL].pwm = 255;
yabahiro 59:5cd3875fa96a 5341 motor[TIRE_FL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 5342 motor[TIRE_BR].pwm = 255;
yabahiro 59:5cd3875fa96a 5343 motor[TIRE_BR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 5344 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5345 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5346 if(g[1]==0) {
yabahiro 59:5cd3875fa96a 5347 mode=10;
yabahiro 59:5cd3875fa96a 5348 aime.reset();
yabahiro 59:5cd3875fa96a 5349 countss=100000;
yabahiro 59:5cd3875fa96a 5350 fast=80;
yabahiro 59:5cd3875fa96a 5351 normal=60;
yabahiro 59:5cd3875fa96a 5352 slow=40;
yabahiro 59:5cd3875fa96a 5353 }
yabahiro 59:5cd3875fa96a 5354 break;
yabahiro 59:5cd3875fa96a 5355 case 10:
yabahiro 59:5cd3875fa96a 5356 switch(g[1]) {
yabahiro 59:5cd3875fa96a 5357 case 98:
yabahiro 59:5cd3875fa96a 5358 motor[TIRE_FR].pwm = normal+55;
yabahiro 59:5cd3875fa96a 5359 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5360 motor[TIRE_FL].pwm = normal+50;
yabahiro 59:5cd3875fa96a 5361 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5362 motor[TIRE_BR].pwm = normal+50;
yabahiro 59:5cd3875fa96a 5363 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5364 motor[TIRE_BL].pwm = normal+45;
yabahiro 59:5cd3875fa96a 5365 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5366 mtc=true;
yabahiro 60:24ebb611f26e 5367 if(value>=1000) {
yabahiro 59:5cd3875fa96a 5368 cross++;
yabahiro 59:5cd3875fa96a 5369 countss=0;
yabahiro 59:5cd3875fa96a 5370 value=0;
yabahiro 59:5cd3875fa96a 5371 aime.reset();
yabahiro 59:5cd3875fa96a 5372 }
Ryosei 27:dd9f27fce7d1 5373
yabahiro 59:5cd3875fa96a 5374 if(cross==2) {
yabahiro 59:5cd3875fa96a 5375 fast = 10;
yabahiro 59:5cd3875fa96a 5376 normal = 10;
yabahiro 59:5cd3875fa96a 5377 slow = 5;
yabahiro 59:5cd3875fa96a 5378 }
yabahiro 59:5cd3875fa96a 5379
yabahiro 59:5cd3875fa96a 5380 if(cross==3) {
yabahiro 59:5cd3875fa96a 5381 fast=40;
yabahiro 59:5cd3875fa96a 5382 normal=30;
yabahiro 59:5cd3875fa96a 5383 slow=20;
yabahiro 59:5cd3875fa96a 5384 mode=11;
yabahiro 59:5cd3875fa96a 5385 aime.reset();
yabahiro 59:5cd3875fa96a 5386 countss=0;
yabahiro 59:5cd3875fa96a 5387 cross=0;
yabahiro 59:5cd3875fa96a 5388 }
yabahiro 59:5cd3875fa96a 5389 break;
yabahiro 59:5cd3875fa96a 5390 case 0:
yabahiro 59:5cd3875fa96a 5391 motor[TIRE_FR].pwm = fast+55;
yabahiro 59:5cd3875fa96a 5392 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5393 motor[TIRE_FL].pwm = fast+50;
yabahiro 59:5cd3875fa96a 5394 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5395 motor[TIRE_BR].pwm = fast+50;
yabahiro 59:5cd3875fa96a 5396 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5397 motor[TIRE_BL].pwm = fast+45;
yabahiro 59:5cd3875fa96a 5398 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5399 mtc=true;
yabahiro 59:5cd3875fa96a 5400 break;
yabahiro 59:5cd3875fa96a 5401 case 255:
yabahiro 59:5cd3875fa96a 5402 motor[TIRE_FR].pwm = fast+55;
yabahiro 59:5cd3875fa96a 5403 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5404 motor[TIRE_FL].pwm = slow+50;
yabahiro 59:5cd3875fa96a 5405 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5406 motor[TIRE_BR].pwm = slow+50;
yabahiro 59:5cd3875fa96a 5407 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5408 motor[TIRE_BL].pwm = fast+45;
yabahiro 59:5cd3875fa96a 5409 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5410 mtc=true;
yabahiro 59:5cd3875fa96a 5411 break;
yabahiro 59:5cd3875fa96a 5412 case 253:
yabahiro 59:5cd3875fa96a 5413 motor[TIRE_FR].pwm = slow+55;
yabahiro 59:5cd3875fa96a 5414 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5415 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5416 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5417 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5418 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5419 motor[TIRE_BL].pwm = slow+45;
yabahiro 59:5cd3875fa96a 5420 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5421 mtc=true;
yabahiro 59:5cd3875fa96a 5422 break;
yabahiro 59:5cd3875fa96a 5423 case 254:
yabahiro 59:5cd3875fa96a 5424 motor[TIRE_FR].pwm = normal+55;
yabahiro 59:5cd3875fa96a 5425 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5426 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5427 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5428 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5429 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5430 motor[TIRE_BL].pwm = normal+45;
yabahiro 59:5cd3875fa96a 5431 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5432 mtc=true;
yabahiro 59:5cd3875fa96a 5433 break;
yabahiro 59:5cd3875fa96a 5434 case 1:
yabahiro 59:5cd3875fa96a 5435 motor[TIRE_FR].pwm = slow+55;
yabahiro 59:5cd3875fa96a 5436 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5437 motor[TIRE_FL].pwm = fast+50;
yabahiro 59:5cd3875fa96a 5438 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5439 motor[TIRE_BR].pwm = fast+50;
yabahiro 59:5cd3875fa96a 5440 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5441 motor[TIRE_BL].pwm = slow+45;
yabahiro 59:5cd3875fa96a 5442 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5443 mtc=true;
yabahiro 59:5cd3875fa96a 5444 break;
yabahiro 59:5cd3875fa96a 5445 case 3:
yabahiro 59:5cd3875fa96a 5446 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5447 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5448 motor[TIRE_FL].pwm = slow+50;
yabahiro 59:5cd3875fa96a 5449 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5450 motor[TIRE_BR].pwm = slow+50;
yabahiro 59:5cd3875fa96a 5451 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5452 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5453 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5454 mtc=true;
yabahiro 59:5cd3875fa96a 5455 break;
yabahiro 59:5cd3875fa96a 5456 case 2:
yabahiro 59:5cd3875fa96a 5457 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5458 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5459 motor[TIRE_FL].pwm = normal+50;
yabahiro 59:5cd3875fa96a 5460 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5461 motor[TIRE_BR].pwm = normal+50;
yabahiro 59:5cd3875fa96a 5462 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5463 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5464 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5465 mtc=true;
yabahiro 59:5cd3875fa96a 5466 break;
yabahiro 59:5cd3875fa96a 5467 }
yabahiro 59:5cd3875fa96a 5468 if(g[1]!=98&&mtc==true) {
yabahiro 59:5cd3875fa96a 5469 switch(g[0]) {
yabahiro 59:5cd3875fa96a 5470 case 0:
yabahiro 59:5cd3875fa96a 5471 mtc=false;
yabahiro 59:5cd3875fa96a 5472 break;
yabahiro 59:5cd3875fa96a 5473 case 255:
yabahiro 59:5cd3875fa96a 5474 motor[TIRE_FL].pwm += 3;
yabahiro 59:5cd3875fa96a 5475 mtc=false;
yabahiro 59:5cd3875fa96a 5476 break;
yabahiro 59:5cd3875fa96a 5477 case 253:
yabahiro 59:5cd3875fa96a 5478 motor[TIRE_FL].pwm += 6;
yabahiro 59:5cd3875fa96a 5479 mtc=false;
yabahiro 59:5cd3875fa96a 5480 break;
yabahiro 59:5cd3875fa96a 5481 case 254:
yabahiro 59:5cd3875fa96a 5482 motor[TIRE_FL].pwm += 9;
yabahiro 59:5cd3875fa96a 5483 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5484 mtc=false;
yabahiro 59:5cd3875fa96a 5485 break;
yabahiro 59:5cd3875fa96a 5486 case 1:
yabahiro 59:5cd3875fa96a 5487 motor[TIRE_FR].pwm += 3;
yabahiro 59:5cd3875fa96a 5488 mtc=false;
yabahiro 59:5cd3875fa96a 5489 break;
yabahiro 59:5cd3875fa96a 5490 case 3:
yabahiro 59:5cd3875fa96a 5491 motor[TIRE_FR].pwm += 6;
yabahiro 59:5cd3875fa96a 5492 mtc=false;
yabahiro 59:5cd3875fa96a 5493 break;
yabahiro 59:5cd3875fa96a 5494 case 2:
yabahiro 59:5cd3875fa96a 5495 motor[TIRE_FR].pwm += 9;
yabahiro 59:5cd3875fa96a 5496 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5497 mtc=false;
yabahiro 59:5cd3875fa96a 5498 break;
yabahiro 59:5cd3875fa96a 5499 }
yabahiro 59:5cd3875fa96a 5500 }
yabahiro 59:5cd3875fa96a 5501 break;
yabahiro 59:5cd3875fa96a 5502 case 11:
yabahiro 59:5cd3875fa96a 5503 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 5504 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5505 motor[TIRE_FL].pwm = 15;
yabahiro 59:5cd3875fa96a 5506 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5507 motor[TIRE_BR].pwm = 15;
yabahiro 59:5cd3875fa96a 5508 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5509 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 5510 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5511 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 59:5cd3875fa96a 5512 countss=0;
yabahiro 59:5cd3875fa96a 5513 cross=0;
yabahiro 59:5cd3875fa96a 5514 mode=20;
yabahiro 59:5cd3875fa96a 5515 }
yabahiro 59:5cd3875fa96a 5516 break;
yabahiro 59:5cd3875fa96a 5517 case 20:
yabahiro 59:5cd3875fa96a 5518 solenoid.solenoid2= SOLENOID_ON;
yabahiro 59:5cd3875fa96a 5519 switch(g[2]) {
yabahiro 59:5cd3875fa96a 5520 case 98:
yabahiro 59:5cd3875fa96a 5521 motor[TIRE_FR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5522 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5523 motor[TIRE_FL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5524 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5525 motor[TIRE_BR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5526 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5527 motor[TIRE_BL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5528 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5529 if(countss>30000) {
yabahiro 59:5cd3875fa96a 5530 cross++;
yabahiro 59:5cd3875fa96a 5531 countss=0;
yabahiro 59:5cd3875fa96a 5532 }
yabahiro 59:5cd3875fa96a 5533 mtc2=true;
yabahiro 59:5cd3875fa96a 5534 if(YELLOW) {
yabahiro 59:5cd3875fa96a 5535 if(cross==1) {
yabahiro 59:5cd3875fa96a 5536 fast=20;
yabahiro 59:5cd3875fa96a 5537 normal=15;
yabahiro 59:5cd3875fa96a 5538 slow=15;
yabahiro 59:5cd3875fa96a 5539 }
yabahiro 59:5cd3875fa96a 5540 if(cross==2) {
yabahiro 59:5cd3875fa96a 5541 mode=21;
yabahiro 59:5cd3875fa96a 5542 countss=0;
yabahiro 59:5cd3875fa96a 5543 cross=0;
yabahiro 59:5cd3875fa96a 5544 }
yabahiro 59:5cd3875fa96a 5545 } else {
yabahiro 59:5cd3875fa96a 5546 if(cross==3) {
yabahiro 59:5cd3875fa96a 5547 mode=21;
yabahiro 59:5cd3875fa96a 5548 countss=0;
yabahiro 59:5cd3875fa96a 5549 cross=0;
yabahiro 59:5cd3875fa96a 5550 }
yabahiro 59:5cd3875fa96a 5551 }
yabahiro 59:5cd3875fa96a 5552 break;
yabahiro 59:5cd3875fa96a 5553 case 0:
yabahiro 59:5cd3875fa96a 5554 motor[TIRE_FR].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5555 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5556 motor[TIRE_FL].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5557 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5558 motor[TIRE_BR].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5559 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5560 motor[TIRE_BL].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5561 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5562 mtc2=true;
yabahiro 59:5cd3875fa96a 5563 break;
yabahiro 59:5cd3875fa96a 5564 case 255:
yabahiro 59:5cd3875fa96a 5565 motor[TIRE_FR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5566 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5567 motor[TIRE_FL].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5568 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5569 motor[TIRE_BR].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5570 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5571 motor[TIRE_BL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5572 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5573 mtc2=true;
yabahiro 59:5cd3875fa96a 5574 break;
yabahiro 59:5cd3875fa96a 5575 case 253:
yabahiro 59:5cd3875fa96a 5576 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5577 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5578 motor[TIRE_FL].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5579 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5580 motor[TIRE_BR].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5581 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5582 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5583 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5584 mtc2=true;
yabahiro 59:5cd3875fa96a 5585 break;
yabahiro 59:5cd3875fa96a 5586 case 254:
yabahiro 59:5cd3875fa96a 5587 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5588 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5589 motor[TIRE_FL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5590 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5591 motor[TIRE_BR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5592 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5593 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5594 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5595 mtc2=true;
yabahiro 59:5cd3875fa96a 5596 break;
yabahiro 59:5cd3875fa96a 5597 case 1:
yabahiro 59:5cd3875fa96a 5598 motor[TIRE_FR].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5599 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5600 motor[TIRE_FL].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5601 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5602 motor[TIRE_BR].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5603 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5604 motor[TIRE_BL].pwm = fast+20;
yabahiro 59:5cd3875fa96a 5605 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5606 mtc2=true;
yabahiro 59:5cd3875fa96a 5607 break;
yabahiro 59:5cd3875fa96a 5608 case 3:
yabahiro 59:5cd3875fa96a 5609 motor[TIRE_FR].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5610 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5611 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5612 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5613 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5614 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5615 motor[TIRE_BL].pwm = slow+20;
yabahiro 59:5cd3875fa96a 5616 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5617 mtc2=true;
yabahiro 59:5cd3875fa96a 5618 break;
yabahiro 59:5cd3875fa96a 5619 case 2:
yabahiro 59:5cd3875fa96a 5620 motor[TIRE_FR].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5621 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5622 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5623 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5624 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5625 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5626 motor[TIRE_BL].pwm = normal+20;
yabahiro 59:5cd3875fa96a 5627 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5628 mtc2=true;
yabahiro 59:5cd3875fa96a 5629 break;
yabahiro 59:5cd3875fa96a 5630 }
yabahiro 59:5cd3875fa96a 5631 if(g[2]!=98&&mtc2==true) {
yabahiro 59:5cd3875fa96a 5632 switch(g[3]) {
yabahiro 59:5cd3875fa96a 5633 case 0:
yabahiro 59:5cd3875fa96a 5634 mtc2=false;
yabahiro 59:5cd3875fa96a 5635 break;
yabahiro 59:5cd3875fa96a 5636 case 255:
yabahiro 59:5cd3875fa96a 5637 motor[TIRE_BL].pwm += 5;
yabahiro 59:5cd3875fa96a 5638 mtc2=false;
yabahiro 59:5cd3875fa96a 5639 break;
yabahiro 59:5cd3875fa96a 5640 case 253:
yabahiro 59:5cd3875fa96a 5641 motor[TIRE_BL].pwm += 10;
yabahiro 59:5cd3875fa96a 5642 mtc2=false;
yabahiro 59:5cd3875fa96a 5643 break;
yabahiro 59:5cd3875fa96a 5644 case 254:
yabahiro 59:5cd3875fa96a 5645 motor[TIRE_BL].pwm += 15;
yabahiro 59:5cd3875fa96a 5646 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5647 mtc2=false;
yabahiro 59:5cd3875fa96a 5648 break;
yabahiro 59:5cd3875fa96a 5649 case 1:
yabahiro 59:5cd3875fa96a 5650 motor[TIRE_FL].pwm += 5;
yabahiro 59:5cd3875fa96a 5651 mtc2=false;
yabahiro 59:5cd3875fa96a 5652 break;
yabahiro 59:5cd3875fa96a 5653 case 3:
yabahiro 59:5cd3875fa96a 5654 motor[TIRE_FL].pwm += 10;
yabahiro 59:5cd3875fa96a 5655 mtc2=false;
yabahiro 59:5cd3875fa96a 5656 break;
yabahiro 59:5cd3875fa96a 5657 case 2:
yabahiro 59:5cd3875fa96a 5658 motor[TIRE_FL].pwm += 15;
yabahiro 59:5cd3875fa96a 5659 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5660 mtc2=false;
yabahiro 59:5cd3875fa96a 5661 break;
yabahiro 59:5cd3875fa96a 5662 }
yabahiro 59:5cd3875fa96a 5663 }
yabahiro 59:5cd3875fa96a 5664 break;
yabahiro 59:5cd3875fa96a 5665 case 21:
yabahiro 59:5cd3875fa96a 5666 if(YELLOW) {
yabahiro 59:5cd3875fa96a 5667 motor[TIRE_FR].pwm = 20;
yabahiro 59:5cd3875fa96a 5668 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5669 motor[TIRE_FL].pwm = 20;
yabahiro 59:5cd3875fa96a 5670 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5671 motor[TIRE_BR].pwm = 20;
yabahiro 59:5cd3875fa96a 5672 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5673 motor[TIRE_BL].pwm = 20;
yabahiro 59:5cd3875fa96a 5674 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5675 if(g[3]==98&&countss>=10000) {
yabahiro 59:5cd3875fa96a 5676 mode=22;
yabahiro 59:5cd3875fa96a 5677 countss=0;
yabahiro 59:5cd3875fa96a 5678 fast=60;
yabahiro 59:5cd3875fa96a 5679 normal=40;
yabahiro 59:5cd3875fa96a 5680 slow=20;
yabahiro 59:5cd3875fa96a 5681 }
yabahiro 59:5cd3875fa96a 5682 } else {
yabahiro 59:5cd3875fa96a 5683 motor[TIRE_FR].pwm = 255;
yabahiro 59:5cd3875fa96a 5684 motor[TIRE_FR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 5685 motor[TIRE_FL].pwm = 30;
yabahiro 59:5cd3875fa96a 5686 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5687 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5688 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5689 motor[TIRE_BL].pwm = 255;
yabahiro 59:5cd3875fa96a 5690 motor[TIRE_BL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 5691 if(g[0]==0) {
yabahiro 59:5cd3875fa96a 5692 mode=22;
yabahiro 59:5cd3875fa96a 5693 fast=60;
yabahiro 59:5cd3875fa96a 5694 normal=40;
yabahiro 59:5cd3875fa96a 5695 slow=25;
yabahiro 59:5cd3875fa96a 5696 value=0;
yabahiro 59:5cd3875fa96a 5697 aime.reset();
yabahiro 59:5cd3875fa96a 5698 }
yabahiro 59:5cd3875fa96a 5699 }
yabahiro 59:5cd3875fa96a 5700 break;
yabahiro 59:5cd3875fa96a 5701 case 22:
yabahiro 59:5cd3875fa96a 5702 if(YELLOW) {
yabahiro 59:5cd3875fa96a 5703 if(g[4]==3||g[4]==2) {
yabahiro 59:5cd3875fa96a 5704 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5705 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5706 motor[TIRE_FL].pwm = 30;
yabahiro 59:5cd3875fa96a 5707 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5708 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5709 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5710 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5711 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5712 }
yabahiro 59:5cd3875fa96a 5713 if(g[4]==253||g[4]==254) {
yabahiro 59:5cd3875fa96a 5714 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5715 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5716 motor[TIRE_FL].pwm = 30;
yabahiro 59:5cd3875fa96a 5717 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5718 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5719 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5720 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5721 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5722 }
yabahiro 59:5cd3875fa96a 5723 switch(g[2]) {
yabahiro 59:5cd3875fa96a 5724 case 0:
yabahiro 59:5cd3875fa96a 5725 motor[TIRE_FR].pwm = 20;
yabahiro 59:5cd3875fa96a 5726 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5727 motor[TIRE_BR].pwm = 20;
yabahiro 59:5cd3875fa96a 5728 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5729 break;
yabahiro 59:5cd3875fa96a 5730 case 255:
yabahiro 59:5cd3875fa96a 5731 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5732 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5733 motor[TIRE_BR].pwm = 20;
yabahiro 59:5cd3875fa96a 5734 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5735 break;
yabahiro 59:5cd3875fa96a 5736 case 253:
yabahiro 59:5cd3875fa96a 5737 motor[TIRE_FR].pwm = 40;
yabahiro 59:5cd3875fa96a 5738 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5739 motor[TIRE_BR].pwm = 20;
yabahiro 59:5cd3875fa96a 5740 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5741 break;
yabahiro 59:5cd3875fa96a 5742 case 254:
yabahiro 59:5cd3875fa96a 5743 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5744 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5745 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5746 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5747 break;
yabahiro 59:5cd3875fa96a 5748 case 1:
yabahiro 59:5cd3875fa96a 5749 motor[TIRE_FR].pwm = 20;
yabahiro 59:5cd3875fa96a 5750 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5751 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5752 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5753 break;
yabahiro 59:5cd3875fa96a 5754 case 3:
yabahiro 59:5cd3875fa96a 5755 motor[TIRE_FR].pwm = 20;
yabahiro 59:5cd3875fa96a 5756 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5757 motor[TIRE_BR].pwm = 40;
yabahiro 59:5cd3875fa96a 5758 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5759 break;
yabahiro 59:5cd3875fa96a 5760 case 2:
yabahiro 59:5cd3875fa96a 5761 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 5762 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 5763 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 5764 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 5765 break;
yabahiro 59:5cd3875fa96a 5766 }
yabahiro 59:5cd3875fa96a 5767 switch(g[3]) {
yabahiro 59:5cd3875fa96a 5768 case 0:
yabahiro 59:5cd3875fa96a 5769 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 5770 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5771 motor[TIRE_BL].pwm = 20;
yabahiro 59:5cd3875fa96a 5772 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5773 break;
yabahiro 59:5cd3875fa96a 5774 case 255:
yabahiro 59:5cd3875fa96a 5775 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 5776 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5777 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5778 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5779 case 253:
yabahiro 59:5cd3875fa96a 5780 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 5781 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5782 motor[TIRE_BL].pwm = 40;
yabahiro 59:5cd3875fa96a 5783 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5784 break;
yabahiro 59:5cd3875fa96a 5785 case 254:
yabahiro 59:5cd3875fa96a 5786 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 5787 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 5788 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5789 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 5790 break;
yabahiro 59:5cd3875fa96a 5791 case 1:
yabahiro 59:5cd3875fa96a 5792 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 5793 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5794 motor[TIRE_BL].pwm = 20;
yabahiro 59:5cd3875fa96a 5795 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5796 break;
yabahiro 59:5cd3875fa96a 5797 case 3:
yabahiro 59:5cd3875fa96a 5798 motor[TIRE_FL].pwm = 45;
yabahiro 59:5cd3875fa96a 5799 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5800 motor[TIRE_BL].pwm = 20;
yabahiro 59:5cd3875fa96a 5801 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5802 break;
yabahiro 59:5cd3875fa96a 5803 case 2:
yabahiro 59:5cd3875fa96a 5804 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 5805 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5806 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 5807 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5808 break;
yabahiro 59:5cd3875fa96a 5809 }
yabahiro 59:5cd3875fa96a 5810 if(g[3]==0&&g[2]==0) {
yabahiro 59:5cd3875fa96a 5811 aime.reset();
yabahiro 59:5cd3875fa96a 5812 mode=23;
yabahiro 59:5cd3875fa96a 5813 countss=0;
yabahiro 59:5cd3875fa96a 5814 fast=80;
yabahiro 59:5cd3875fa96a 5815 normal=60;
yabahiro 59:5cd3875fa96a 5816 slow=30;
yabahiro 59:5cd3875fa96a 5817 }
yabahiro 59:5cd3875fa96a 5818 } else {
yabahiro 59:5cd3875fa96a 5819 if(value>=2400) {
yabahiro 59:5cd3875fa96a 5820 slow=25;
yabahiro 59:5cd3875fa96a 5821 normal=30;
yabahiro 59:5cd3875fa96a 5822 fast=30;
yabahiro 59:5cd3875fa96a 5823 }
yabahiro 59:5cd3875fa96a 5824 if(value>=3000) {
yabahiro 59:5cd3875fa96a 5825 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 59:5cd3875fa96a 5826 slow=25;
yabahiro 59:5cd3875fa96a 5827 normal=40;
yabahiro 59:5cd3875fa96a 5828 fast=60;
yabahiro 59:5cd3875fa96a 5829 aime.reset();
yabahiro 59:5cd3875fa96a 5830 mode=23;
yabahiro 59:5cd3875fa96a 5831 }
yabahiro 59:5cd3875fa96a 5832 switch(g[0]) {
yabahiro 59:5cd3875fa96a 5833 case 98:
yabahiro 59:5cd3875fa96a 5834 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 5835 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5836 motor[TIRE_FL].pwm = normal+5;
yabahiro 59:5cd3875fa96a 5837 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5838 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 5839 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5840 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 5841 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5842 break;
yabahiro 59:5cd3875fa96a 5843 case 0:
yabahiro 59:5cd3875fa96a 5844 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 5845 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5846 motor[TIRE_FL].pwm = fast+5;
yabahiro 59:5cd3875fa96a 5847 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5848 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 5849 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5850 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 5851 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5852 mtc=true;
yabahiro 59:5cd3875fa96a 5853 break;
yabahiro 59:5cd3875fa96a 5854 case 255:
yabahiro 59:5cd3875fa96a 5855 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 5856 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5857 motor[TIRE_FL].pwm = slow+5;
yabahiro 59:5cd3875fa96a 5858 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5859 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 5860 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5861 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 5862 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5863 mtc=true;
yabahiro 59:5cd3875fa96a 5864 break;
yabahiro 59:5cd3875fa96a 5865 case 253:
yabahiro 59:5cd3875fa96a 5866 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 5867 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5868 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5869 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5870 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5871 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5872 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 5873 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5874 mtc=true;
yabahiro 59:5cd3875fa96a 5875 break;
yabahiro 59:5cd3875fa96a 5876 case 254:
yabahiro 59:5cd3875fa96a 5877 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 5878 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5879 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5880 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5881 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5882 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5883 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 5884 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5885 mtc=true;
yabahiro 59:5cd3875fa96a 5886 break;
yabahiro 59:5cd3875fa96a 5887 case 1:
yabahiro 59:5cd3875fa96a 5888 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 5889 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5890 motor[TIRE_FL].pwm = fast+5;
yabahiro 59:5cd3875fa96a 5891 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5892 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 5893 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5894 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 5895 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5896 mtc=true;
yabahiro 59:5cd3875fa96a 5897 break;
yabahiro 59:5cd3875fa96a 5898 case 3:
yabahiro 59:5cd3875fa96a 5899 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5900 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5901 motor[TIRE_FL].pwm = slow+5;
yabahiro 59:5cd3875fa96a 5902 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5903 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 5904 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5905 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5906 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5907 mtc=true;
yabahiro 59:5cd3875fa96a 5908 break;
yabahiro 59:5cd3875fa96a 5909 case 2:
yabahiro 59:5cd3875fa96a 5910 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5911 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5912 motor[TIRE_FL].pwm = normal+5;
yabahiro 59:5cd3875fa96a 5913 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5914 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 5915 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5916 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5917 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5918 mtc=true;
yabahiro 59:5cd3875fa96a 5919 break;
yabahiro 59:5cd3875fa96a 5920 }
yabahiro 59:5cd3875fa96a 5921 if(g[0]!=98&&mtc==true) {
yabahiro 59:5cd3875fa96a 5922 switch(g[0]) {
yabahiro 59:5cd3875fa96a 5923 case 0:
yabahiro 59:5cd3875fa96a 5924 mtc=false;
yabahiro 59:5cd3875fa96a 5925 break;
yabahiro 59:5cd3875fa96a 5926 case 255:
yabahiro 59:5cd3875fa96a 5927 motor[TIRE_BR].pwm += 5;
yabahiro 59:5cd3875fa96a 5928 mtc=false;
yabahiro 59:5cd3875fa96a 5929 break;
yabahiro 59:5cd3875fa96a 5930 case 253:
yabahiro 59:5cd3875fa96a 5931 motor[TIRE_BR].pwm += 10;
yabahiro 59:5cd3875fa96a 5932 mtc=false;
yabahiro 59:5cd3875fa96a 5933 break;
yabahiro 59:5cd3875fa96a 5934 case 254:
yabahiro 59:5cd3875fa96a 5935 motor[TIRE_BR].pwm += 20;
yabahiro 59:5cd3875fa96a 5936 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5937 mtc=false;
yabahiro 59:5cd3875fa96a 5938 break;
yabahiro 59:5cd3875fa96a 5939 case 1:
yabahiro 59:5cd3875fa96a 5940 motor[TIRE_BL].pwm += 5;
yabahiro 59:5cd3875fa96a 5941 mtc=false;
yabahiro 59:5cd3875fa96a 5942 break;
yabahiro 59:5cd3875fa96a 5943 case 3:
yabahiro 59:5cd3875fa96a 5944 motor[TIRE_BL].pwm += 10;
yabahiro 59:5cd3875fa96a 5945 mtc=false;
yabahiro 59:5cd3875fa96a 5946 break;
yabahiro 59:5cd3875fa96a 5947 case 2:
yabahiro 59:5cd3875fa96a 5948 motor[TIRE_BL].pwm += 20;
yabahiro 59:5cd3875fa96a 5949 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5950 mtc=false;
yabahiro 59:5cd3875fa96a 5951 break;
yabahiro 59:5cd3875fa96a 5952 }
yabahiro 59:5cd3875fa96a 5953 }
yabahiro 59:5cd3875fa96a 5954 }
yabahiro 59:5cd3875fa96a 5955 break;
yabahiro 59:5cd3875fa96a 5956 case 23:
yabahiro 59:5cd3875fa96a 5957 if(YELLOW) {
yabahiro 59:5cd3875fa96a 5958 switch(g[4]) {
yabahiro 59:5cd3875fa96a 5959 case 98:
yabahiro 59:5cd3875fa96a 5960 motor[TIRE_FR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5961 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5962 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 5963 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5964 motor[TIRE_BR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5965 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5966 motor[TIRE_BL].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5967 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5968 break;
yabahiro 59:5cd3875fa96a 5969 case 99:
yabahiro 59:5cd3875fa96a 5970 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5971 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5972 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5973 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5974 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5975 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5976 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5977 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5978 break;
yabahiro 59:5cd3875fa96a 5979 case 0:
yabahiro 59:5cd3875fa96a 5980 motor[TIRE_FR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5981 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 5982 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 5983 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 5984 motor[TIRE_BR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5985 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 5986 motor[TIRE_BL].pwm = fast-5;
yabahiro 59:5cd3875fa96a 5987 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 5988 break;
yabahiro 59:5cd3875fa96a 5989 case 255:
yabahiro 59:5cd3875fa96a 5990 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 5991 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 5992 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 5993 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 5994 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 5995 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 5996 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 5997 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 5998 break;
yabahiro 59:5cd3875fa96a 5999 case 253:
yabahiro 59:5cd3875fa96a 6000 motor[TIRE_FR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6001 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6002 motor[TIRE_FL].pwm = slow;
yabahiro 59:5cd3875fa96a 6003 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6004 motor[TIRE_BR].pwm = slow-5;
yabahiro 59:5cd3875fa96a 6005 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6006 motor[TIRE_BL].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6007 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6008 break;
yabahiro 59:5cd3875fa96a 6009 case 254:
yabahiro 59:5cd3875fa96a 6010 motor[TIRE_FR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6011 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6012 motor[TIRE_FL].pwm = normal;
yabahiro 59:5cd3875fa96a 6013 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6014 motor[TIRE_BR].pwm = normal-5;
yabahiro 59:5cd3875fa96a 6015 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6016 motor[TIRE_BL].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6017 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6018 break;
yabahiro 59:5cd3875fa96a 6019 case 1:
yabahiro 59:5cd3875fa96a 6020 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6021 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6022 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6023 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6024 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6025 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6026 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6027 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6028 break;
yabahiro 59:5cd3875fa96a 6029 case 3:
yabahiro 59:5cd3875fa96a 6030 motor[TIRE_FR].pwm = slow-5;
yabahiro 59:5cd3875fa96a 6031 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6032 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 6033 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6034 motor[TIRE_BR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6035 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6036 motor[TIRE_BL].pwm = slow-5;
yabahiro 59:5cd3875fa96a 6037 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6038 break;
yabahiro 59:5cd3875fa96a 6039 case 2:
yabahiro 59:5cd3875fa96a 6040 motor[TIRE_FR].pwm = normal-5;
yabahiro 59:5cd3875fa96a 6041 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6042 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 6043 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6044 motor[TIRE_BR].pwm = fast-5;
yabahiro 59:5cd3875fa96a 6045 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6046 motor[TIRE_BL].pwm = normal-5;
yabahiro 59:5cd3875fa96a 6047 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6048 break;
yabahiro 59:5cd3875fa96a 6049 }
yabahiro 59:5cd3875fa96a 6050
yabahiro 59:5cd3875fa96a 6051
yabahiro 59:5cd3875fa96a 6052 /*
yabahiro 59:5cd3875fa96a 6053 if(value>=3000) {
yabahiro 59:5cd3875fa96a 6054 mode=24;
yabahiro 59:5cd3875fa96a 6055 aime.reset();
yabahiro 59:5cd3875fa96a 6056 value=0;
yabahiro 59:5cd3875fa96a 6057 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 59:5cd3875fa96a 6058 countss=0;
yabahiro 59:5cd3875fa96a 6059 fast=80;
yabahiro 59:5cd3875fa96a 6060 normal=60;
yabahiro 59:5cd3875fa96a 6061 slow=30;
yabahiro 59:5cd3875fa96a 6062 }
yabahiro 59:5cd3875fa96a 6063 if(g[0]==98) {
yabahiro 59:5cd3875fa96a 6064 fast=20;
yabahiro 59:5cd3875fa96a 6065 normal=15;
yabahiro 59:5cd3875fa96a 6066 slow=10;
yabahiro 59:5cd3875fa96a 6067 }
yabahiro 59:5cd3875fa96a 6068 */
yabahiro 59:5cd3875fa96a 6069 if(value>=1800) {
yabahiro 59:5cd3875fa96a 6070 fast=20;
yabahiro 59:5cd3875fa96a 6071 normal=15;
yabahiro 59:5cd3875fa96a 6072 slow=10;
yabahiro 59:5cd3875fa96a 6073 }
yabahiro 59:5cd3875fa96a 6074 if(value>=2600) {
yabahiro 59:5cd3875fa96a 6075 mode=24;
yabahiro 59:5cd3875fa96a 6076 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 59:5cd3875fa96a 6077 countss=0;
yabahiro 59:5cd3875fa96a 6078 value=0;
yabahiro 59:5cd3875fa96a 6079 aime.reset();
yabahiro 59:5cd3875fa96a 6080 fast=80;
yabahiro 59:5cd3875fa96a 6081 normal=60;
yabahiro 59:5cd3875fa96a 6082 slow=30;
yabahiro 59:5cd3875fa96a 6083 }
yabahiro 59:5cd3875fa96a 6084 } else {
yabahiro 59:5cd3875fa96a 6085 if(UPLS==true||DOLS==true) {
yabahiro 59:5cd3875fa96a 6086 if(UPLS==true||DOLS==true) {
yabahiro 59:5cd3875fa96a 6087 motor[MOTOR_0].pwm = 200;
yabahiro 59:5cd3875fa96a 6088 motor[MOTOR_0].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6089 fast=60;
yabahiro 59:5cd3875fa96a 6090 normal=40;
yabahiro 59:5cd3875fa96a 6091 slow=30;
yabahiro 59:5cd3875fa96a 6092 countss=0;
yabahiro 59:5cd3875fa96a 6093 value=0;
yabahiro 59:5cd3875fa96a 6094 aime.reset();
yabahiro 59:5cd3875fa96a 6095 mode=24;
yabahiro 59:5cd3875fa96a 6096 }
yabahiro 59:5cd3875fa96a 6097 } else {
yabahiro 59:5cd3875fa96a 6098 motor[MOTOR_0].pwm = 100;
yabahiro 59:5cd3875fa96a 6099 motor[MOTOR_0].dir = BACK;
yabahiro 59:5cd3875fa96a 6100 }
yabahiro 59:5cd3875fa96a 6101 motor[TIRE_FR].pwm = 100;
yabahiro 59:5cd3875fa96a 6102 motor[TIRE_FR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6103 motor[TIRE_FL].pwm = 100;
yabahiro 59:5cd3875fa96a 6104 motor[TIRE_FL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6105 motor[TIRE_BR].pwm = 100;
yabahiro 59:5cd3875fa96a 6106 motor[TIRE_BR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6107 motor[TIRE_BL].pwm = 100;
yabahiro 59:5cd3875fa96a 6108 motor[TIRE_BL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6109 }
yabahiro 59:5cd3875fa96a 6110 break;
yabahiro 59:5cd3875fa96a 6111 case 24:
yabahiro 59:5cd3875fa96a 6112 if(YELLOW) {
yabahiro 59:5cd3875fa96a 6113 /*
yabahiro 59:5cd3875fa96a 6114 if(g[4]==3||g[4]==2) {
yabahiro 59:5cd3875fa96a 6115 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6116 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 6117 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 6118 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6119 motor[TIRE_BR].pwm = 25;
yabahiro 59:5cd3875fa96a 6120 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6121 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6122 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 6123 }
yabahiro 59:5cd3875fa96a 6124 if(g[4]==253||g[4]==254) {
yabahiro 59:5cd3875fa96a 6125 motor[TIRE_FR].pwm = 25;
yabahiro 59:5cd3875fa96a 6126 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6127 motor[TIRE_FL].pwm = 30;
yabahiro 59:5cd3875fa96a 6128 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6129 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 6130 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6131 motor[TIRE_BL].pwm = 25;
yabahiro 59:5cd3875fa96a 6132 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6133 }
yabahiro 59:5cd3875fa96a 6134 switch(g[2]) {
yabahiro 59:5cd3875fa96a 6135 case 0:
yabahiro 59:5cd3875fa96a 6136 motor[TIRE_FR].pwm = 25;
yabahiro 59:5cd3875fa96a 6137 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6138 motor[TIRE_BR].pwm = 25;
yabahiro 59:5cd3875fa96a 6139 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6140 break;
yabahiro 59:5cd3875fa96a 6141 case 255:
yabahiro 59:5cd3875fa96a 6142 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6143 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6144 motor[TIRE_BR].pwm = 25;
yabahiro 59:5cd3875fa96a 6145 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6146 break;
yabahiro 59:5cd3875fa96a 6147 case 253:
yabahiro 59:5cd3875fa96a 6148 motor[TIRE_FR].pwm = 40;
yabahiro 59:5cd3875fa96a 6149 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6150 motor[TIRE_BR].pwm = 25;
yabahiro 59:5cd3875fa96a 6151 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6152 break;
yabahiro 59:5cd3875fa96a 6153 case 254:
yabahiro 59:5cd3875fa96a 6154 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6155 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6156 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 6157 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6158 break;
yabahiro 59:5cd3875fa96a 6159 case 1:
yabahiro 59:5cd3875fa96a 6160 motor[TIRE_FR].pwm = 25;
yabahiro 59:5cd3875fa96a 6161 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6162 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 6163 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6164 break;
yabahiro 59:5cd3875fa96a 6165 case 3:
yabahiro 59:5cd3875fa96a 6166 motor[TIRE_FR].pwm = 25;
yabahiro 59:5cd3875fa96a 6167 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6168 motor[TIRE_BR].pwm = 40;
yabahiro 59:5cd3875fa96a 6169 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6170 break;
yabahiro 59:5cd3875fa96a 6171 case 2:
yabahiro 59:5cd3875fa96a 6172 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6173 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 6174 motor[TIRE_BR].pwm = 30;
yabahiro 59:5cd3875fa96a 6175 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6176 break;
yabahiro 59:5cd3875fa96a 6177 }
yabahiro 59:5cd3875fa96a 6178 switch(g[3]) {
yabahiro 59:5cd3875fa96a 6179 case 0:
yabahiro 59:5cd3875fa96a 6180 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 6181 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6182 motor[TIRE_BL].pwm = 25;
yabahiro 59:5cd3875fa96a 6183 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6184 break;
yabahiro 59:5cd3875fa96a 6185 case 255:
yabahiro 59:5cd3875fa96a 6186 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 6187 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6188 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6189 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 6190 case 253:
yabahiro 59:5cd3875fa96a 6191 motor[TIRE_FL].pwm = 25;
yabahiro 59:5cd3875fa96a 6192 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6193 motor[TIRE_BL].pwm = 40;
yabahiro 59:5cd3875fa96a 6194 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 6195 break;
yabahiro 59:5cd3875fa96a 6196 case 254:
yabahiro 59:5cd3875fa96a 6197 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 6198 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6199 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6200 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 6201 break;
yabahiro 59:5cd3875fa96a 6202 case 1:
yabahiro 59:5cd3875fa96a 6203 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 6204 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6205 motor[TIRE_BL].pwm = 25;
yabahiro 59:5cd3875fa96a 6206 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6207 break;
yabahiro 59:5cd3875fa96a 6208 case 3:
yabahiro 59:5cd3875fa96a 6209 motor[TIRE_FL].pwm = 45;
yabahiro 59:5cd3875fa96a 6210 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6211 motor[TIRE_BL].pwm = 25;
yabahiro 59:5cd3875fa96a 6212 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6213 break;
yabahiro 59:5cd3875fa96a 6214 case 2:
yabahiro 59:5cd3875fa96a 6215 motor[TIRE_FL].pwm = 35;
yabahiro 59:5cd3875fa96a 6216 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6217 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6218 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6219 break;
yabahiro 59:5cd3875fa96a 6220 }
yabahiro 59:5cd3875fa96a 6221
yabahiro 59:5cd3875fa96a 6222 if(g[3]==0&&g[2]==0) {
yabahiro 59:5cd3875fa96a 6223 mode=25;
yabahiro 59:5cd3875fa96a 6224 }
yabahiro 59:5cd3875fa96a 6225 */
yabahiro 59:5cd3875fa96a 6226 mode=25;
yabahiro 59:5cd3875fa96a 6227 } else {
yabahiro 59:5cd3875fa96a 6228 /*
yabahiro 59:5cd3875fa96a 6229 if(g[0]==98&&value>=2000) {
yabahiro 59:5cd3875fa96a 6230 mode=31;
yabahiro 59:5cd3875fa96a 6231 fast=60;
yabahiro 59:5cd3875fa96a 6232 normal=40;
yabahiro 59:5cd3875fa96a 6233 slow=30;
yabahiro 59:5cd3875fa96a 6234 countss=0;
yabahiro 59:5cd3875fa96a 6235 }
yabahiro 59:5cd3875fa96a 6236 */
yabahiro 59:5cd3875fa96a 6237 switch(g[0]) {
yabahiro 59:5cd3875fa96a 6238 case 98:
yabahiro 59:5cd3875fa96a 6239 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6240 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6241 motor[TIRE_FL].pwm = normal+5;
yabahiro 59:5cd3875fa96a 6242 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6243 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6244 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6245 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6246 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6247 break;
yabahiro 59:5cd3875fa96a 6248 case 0:
yabahiro 59:5cd3875fa96a 6249 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 6250 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6251 motor[TIRE_FL].pwm = fast+5;
yabahiro 59:5cd3875fa96a 6252 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6253 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 6254 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6255 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 6256 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6257 mtc=true;
yabahiro 59:5cd3875fa96a 6258 break;
yabahiro 59:5cd3875fa96a 6259 case 255:
yabahiro 59:5cd3875fa96a 6260 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 6261 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6262 motor[TIRE_FL].pwm = slow+5;
yabahiro 59:5cd3875fa96a 6263 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6264 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6265 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6266 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 6267 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6268 mtc=true;
yabahiro 59:5cd3875fa96a 6269 break;
yabahiro 59:5cd3875fa96a 6270 case 253:
yabahiro 59:5cd3875fa96a 6271 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6272 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6273 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6274 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6275 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6276 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6277 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6278 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6279 mtc=true;
yabahiro 59:5cd3875fa96a 6280 break;
yabahiro 59:5cd3875fa96a 6281 case 254:
yabahiro 59:5cd3875fa96a 6282 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6283 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6284 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6285 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6286 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6287 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6288 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6289 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6290 mtc=true;
yabahiro 59:5cd3875fa96a 6291 break;
yabahiro 59:5cd3875fa96a 6292 case 1:
yabahiro 59:5cd3875fa96a 6293 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6294 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6295 motor[TIRE_FL].pwm = fast+5;
yabahiro 59:5cd3875fa96a 6296 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6297 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 6298 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6299 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6300 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6301 mtc=true;
yabahiro 59:5cd3875fa96a 6302 break;
yabahiro 59:5cd3875fa96a 6303 case 3:
yabahiro 59:5cd3875fa96a 6304 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6305 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6306 motor[TIRE_FL].pwm = slow+5;
yabahiro 59:5cd3875fa96a 6307 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6308 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6309 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6310 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6311 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6312 mtc=true;
yabahiro 59:5cd3875fa96a 6313 break;
yabahiro 59:5cd3875fa96a 6314 case 2:
yabahiro 59:5cd3875fa96a 6315 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6316 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6317 motor[TIRE_FL].pwm = normal+5;
yabahiro 59:5cd3875fa96a 6318 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6319 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6320 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6321 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6322 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6323 mtc=true;
yabahiro 59:5cd3875fa96a 6324 break;
yabahiro 59:5cd3875fa96a 6325 }
yabahiro 59:5cd3875fa96a 6326 if(g[1]!=98&&mtc==true) {
yabahiro 59:5cd3875fa96a 6327 switch(g[1]) {
yabahiro 59:5cd3875fa96a 6328 case 0:
yabahiro 59:5cd3875fa96a 6329 mtc=false;
yabahiro 59:5cd3875fa96a 6330 break;
yabahiro 59:5cd3875fa96a 6331 case 255:
yabahiro 59:5cd3875fa96a 6332 motor[TIRE_BR].pwm += 5;
yabahiro 59:5cd3875fa96a 6333 mtc=false;
yabahiro 59:5cd3875fa96a 6334 break;
yabahiro 59:5cd3875fa96a 6335 case 253:
yabahiro 59:5cd3875fa96a 6336 motor[TIRE_BR].pwm += 10;
yabahiro 59:5cd3875fa96a 6337 mtc=false;
yabahiro 59:5cd3875fa96a 6338 break;
yabahiro 59:5cd3875fa96a 6339 case 254:
yabahiro 59:5cd3875fa96a 6340 motor[TIRE_BR].pwm += 20;
yabahiro 59:5cd3875fa96a 6341 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6342 mtc=false;
yabahiro 59:5cd3875fa96a 6343 break;
yabahiro 59:5cd3875fa96a 6344 case 1:
yabahiro 59:5cd3875fa96a 6345 motor[TIRE_BL].pwm += 5;
yabahiro 59:5cd3875fa96a 6346 mtc=false;
yabahiro 59:5cd3875fa96a 6347 break;
yabahiro 59:5cd3875fa96a 6348 case 3:
yabahiro 59:5cd3875fa96a 6349 motor[TIRE_BL].pwm += 10;
yabahiro 59:5cd3875fa96a 6350 mtc=false;
yabahiro 59:5cd3875fa96a 6351 break;
yabahiro 59:5cd3875fa96a 6352 case 2:
yabahiro 59:5cd3875fa96a 6353 motor[TIRE_BL].pwm += 20;
yabahiro 59:5cd3875fa96a 6354 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6355 mtc=false;
yabahiro 59:5cd3875fa96a 6356 break;
yabahiro 59:5cd3875fa96a 6357 }
yabahiro 59:5cd3875fa96a 6358 }
yabahiro 59:5cd3875fa96a 6359 }
yabahiro 59:5cd3875fa96a 6360 break;
yabahiro 59:5cd3875fa96a 6361 case 25:
yabahiro 59:5cd3875fa96a 6362 motor[TIRE_FR].pwm = 100;
yabahiro 59:5cd3875fa96a 6363 motor[TIRE_FR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6364 motor[TIRE_FL].pwm = 100;
yabahiro 59:5cd3875fa96a 6365 motor[TIRE_FL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6366 motor[TIRE_BR].pwm = 100;
yabahiro 59:5cd3875fa96a 6367 motor[TIRE_BR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6368 motor[TIRE_BL].pwm = 100;
yabahiro 59:5cd3875fa96a 6369 motor[TIRE_BL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6370 if(DOLS==true||UPLS==true) {
yabahiro 59:5cd3875fa96a 6371 if(DOLS==true||UPLS==true) {
yabahiro 59:5cd3875fa96a 6372 motor[MOTOR_0].pwm = 200;
yabahiro 59:5cd3875fa96a 6373 motor[MOTOR_0].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6374 countss=0;
yabahiro 59:5cd3875fa96a 6375 mode=26;
yabahiro 59:5cd3875fa96a 6376 value=0;
yabahiro 59:5cd3875fa96a 6377 aime.reset();
yabahiro 59:5cd3875fa96a 6378 }
yabahiro 59:5cd3875fa96a 6379 } else {
yabahiro 59:5cd3875fa96a 6380 motor[MOTOR_0].pwm = 100;
yabahiro 59:5cd3875fa96a 6381 motor[MOTOR_0].dir = BACK;
yabahiro 59:5cd3875fa96a 6382 }
yabahiro 59:5cd3875fa96a 6383 break;
yabahiro 59:5cd3875fa96a 6384 case 26:
yabahiro 59:5cd3875fa96a 6385 /*
yabahiro 59:5cd3875fa96a 6386 if(g[0]==98&&value>=2500) {
yabahiro 59:5cd3875fa96a 6387 mode=31;
yabahiro 59:5cd3875fa96a 6388 }
yabahiro 59:5cd3875fa96a 6389 */
yabahiro 59:5cd3875fa96a 6390 switch(g[4]) {
yabahiro 59:5cd3875fa96a 6391 case 98:
yabahiro 59:5cd3875fa96a 6392 motor[TIRE_FR].pwm = 60;
yabahiro 59:5cd3875fa96a 6393 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6394 motor[TIRE_FL].pwm = 65;
yabahiro 59:5cd3875fa96a 6395 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6396 motor[TIRE_BR].pwm = 60;
yabahiro 59:5cd3875fa96a 6397 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6398 motor[TIRE_BL].pwm = 60;
yabahiro 59:5cd3875fa96a 6399 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6400 break;
yabahiro 59:5cd3875fa96a 6401 case 99:
yabahiro 59:5cd3875fa96a 6402 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6403 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6404 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6405 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6406 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6407 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6408 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6409 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6410 break;
yabahiro 59:5cd3875fa96a 6411 case 0:
yabahiro 59:5cd3875fa96a 6412 motor[TIRE_FR].pwm = 60;
yabahiro 59:5cd3875fa96a 6413 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6414 motor[TIRE_FL].pwm = 65;
yabahiro 59:5cd3875fa96a 6415 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6416 motor[TIRE_BR].pwm = 60;
yabahiro 59:5cd3875fa96a 6417 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6418 motor[TIRE_BL].pwm = 60;
yabahiro 59:5cd3875fa96a 6419 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6420 break;
yabahiro 59:5cd3875fa96a 6421 case 255:
yabahiro 59:5cd3875fa96a 6422 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6423 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6424 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6425 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6426 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6427 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6428 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6429 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6430 break;
yabahiro 59:5cd3875fa96a 6431 case 253:
yabahiro 59:5cd3875fa96a 6432 motor[TIRE_FR].pwm = 60;
yabahiro 59:5cd3875fa96a 6433 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6434 motor[TIRE_FL].pwm = 30;
yabahiro 59:5cd3875fa96a 6435 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6436 motor[TIRE_BR].pwm = 25;
yabahiro 59:5cd3875fa96a 6437 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6438 motor[TIRE_BL].pwm = 60;
yabahiro 59:5cd3875fa96a 6439 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6440 break;
yabahiro 59:5cd3875fa96a 6441 case 254:
yabahiro 59:5cd3875fa96a 6442 motor[TIRE_FR].pwm = 60;
yabahiro 59:5cd3875fa96a 6443 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6444 motor[TIRE_FL].pwm = 45;
yabahiro 59:5cd3875fa96a 6445 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6446 motor[TIRE_BR].pwm = 40;
yabahiro 59:5cd3875fa96a 6447 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6448 motor[TIRE_BL].pwm = 60;
yabahiro 59:5cd3875fa96a 6449 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6450 break;
yabahiro 59:5cd3875fa96a 6451 case 1:
yabahiro 59:5cd3875fa96a 6452 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6453 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6454 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6455 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6456 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6457 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6458 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6459 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6460 break;
yabahiro 59:5cd3875fa96a 6461 case 3:
yabahiro 59:5cd3875fa96a 6462 motor[TIRE_FR].pwm = 25;
yabahiro 59:5cd3875fa96a 6463 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6464 motor[TIRE_FL].pwm = 65;
yabahiro 59:5cd3875fa96a 6465 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6466 motor[TIRE_BR].pwm = 60;
yabahiro 59:5cd3875fa96a 6467 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6468 motor[TIRE_BL].pwm = 25;
yabahiro 59:5cd3875fa96a 6469 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6470 break;
yabahiro 59:5cd3875fa96a 6471 case 2:
yabahiro 59:5cd3875fa96a 6472 motor[TIRE_FR].pwm = 40;
yabahiro 59:5cd3875fa96a 6473 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6474 motor[TIRE_FL].pwm = 65;
yabahiro 59:5cd3875fa96a 6475 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6476 motor[TIRE_BR].pwm = 60;
yabahiro 59:5cd3875fa96a 6477 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6478 motor[TIRE_BL].pwm = 40;
yabahiro 59:5cd3875fa96a 6479 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6480 break;
yabahiro 59:5cd3875fa96a 6481 }
yabahiro 59:5cd3875fa96a 6482 break;
yabahiro 59:5cd3875fa96a 6483 case 31:
yabahiro 59:5cd3875fa96a 6484 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6485 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6486 motor[TIRE_FL].pwm = 5;
yabahiro 59:5cd3875fa96a 6487 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6488 motor[TIRE_BR].pwm = 5;
yabahiro 59:5cd3875fa96a 6489 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6490 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6491 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6492 if(g[2]==0) {
yabahiro 59:5cd3875fa96a 6493 cross=0;
yabahiro 59:5cd3875fa96a 6494 countss=0;
yabahiro 59:5cd3875fa96a 6495 mode=40;
yabahiro 59:5cd3875fa96a 6496 fast=70;
yabahiro 59:5cd3875fa96a 6497 normal=50;
yabahiro 59:5cd3875fa96a 6498 slow=40;
yabahiro 59:5cd3875fa96a 6499 }
yabahiro 59:5cd3875fa96a 6500 break;
yabahiro 59:5cd3875fa96a 6501 case 40:
yabahiro 59:5cd3875fa96a 6502 switch(g[3]) {
yabahiro 59:5cd3875fa96a 6503 case 98:
yabahiro 59:5cd3875fa96a 6504 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6505 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6506 motor[TIRE_FL].pwm = normal;
yabahiro 59:5cd3875fa96a 6507 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6508 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6509 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6510 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6511 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6512 if(countss>30000) {
yabahiro 59:5cd3875fa96a 6513 cross++;
yabahiro 59:5cd3875fa96a 6514 countss=0;
yabahiro 59:5cd3875fa96a 6515 }
yabahiro 59:5cd3875fa96a 6516 mtc2=true;
yabahiro 59:5cd3875fa96a 6517 if(YELLOW) {
yabahiro 59:5cd3875fa96a 6518 if(cross==1) {
yabahiro 59:5cd3875fa96a 6519 mode=41;
yabahiro 59:5cd3875fa96a 6520 countss=0;
yabahiro 59:5cd3875fa96a 6521 cross=0;
yabahiro 59:5cd3875fa96a 6522 }
yabahiro 59:5cd3875fa96a 6523 } else {
yabahiro 59:5cd3875fa96a 6524 if(cross==3) {
yabahiro 59:5cd3875fa96a 6525 fast=60;
yabahiro 59:5cd3875fa96a 6526 normal=40;
yabahiro 59:5cd3875fa96a 6527 slow=30;
yabahiro 59:5cd3875fa96a 6528 mode=41;
yabahiro 59:5cd3875fa96a 6529 countss=0;
yabahiro 59:5cd3875fa96a 6530 cross=0;
yabahiro 59:5cd3875fa96a 6531 }
yabahiro 59:5cd3875fa96a 6532 }
yabahiro 59:5cd3875fa96a 6533 break;
yabahiro 59:5cd3875fa96a 6534 case 0:
yabahiro 59:5cd3875fa96a 6535 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 6536 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6537 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 6538 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6539 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 6540 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6541 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 6542 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6543 mtc2=true;
yabahiro 59:5cd3875fa96a 6544 break;
yabahiro 59:5cd3875fa96a 6545 case 255:
yabahiro 59:5cd3875fa96a 6546 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6547 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6548 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 6549 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6550 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6551 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6552 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6553 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6554 mtc2=true;
yabahiro 59:5cd3875fa96a 6555 break;
yabahiro 59:5cd3875fa96a 6556 case 253:
yabahiro 59:5cd3875fa96a 6557 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6558 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6559 motor[TIRE_FL].pwm = slow;
yabahiro 59:5cd3875fa96a 6560 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6561 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6562 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6563 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6564 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6565 mtc2=true;
yabahiro 59:5cd3875fa96a 6566 break;
yabahiro 59:5cd3875fa96a 6567 case 254:
yabahiro 59:5cd3875fa96a 6568 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6569 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6570 motor[TIRE_FL].pwm = normal;
yabahiro 59:5cd3875fa96a 6571 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6572 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6573 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6574 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6575 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6576 mtc2=true;
yabahiro 59:5cd3875fa96a 6577 break;
yabahiro 59:5cd3875fa96a 6578 case 1:
yabahiro 59:5cd3875fa96a 6579 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6580 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6581 motor[TIRE_FL].pwm = slow;
yabahiro 59:5cd3875fa96a 6582 motor[TIRE_FL].dir = BACK;
yabahiro 59:5cd3875fa96a 6583 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6584 motor[TIRE_BR].dir = FOR;
yabahiro 59:5cd3875fa96a 6585 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 6586 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6587 mtc2=true;
yabahiro 59:5cd3875fa96a 6588 break;
yabahiro 59:5cd3875fa96a 6589 case 3:
yabahiro 59:5cd3875fa96a 6590 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6591 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6592 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6593 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6594 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6595 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6596 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6597 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6598 mtc2=true;
yabahiro 59:5cd3875fa96a 6599 break;
yabahiro 59:5cd3875fa96a 6600 case 2:
yabahiro 59:5cd3875fa96a 6601 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6602 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6603 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6604 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6605 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6606 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6607 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6608 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6609 mtc2=true;
yabahiro 59:5cd3875fa96a 6610 break;
yabahiro 59:5cd3875fa96a 6611 }
yabahiro 59:5cd3875fa96a 6612 if(g[2]!=98&&mtc2==true) {
yabahiro 59:5cd3875fa96a 6613 switch(g[2]) {
yabahiro 59:5cd3875fa96a 6614 case 0:
yabahiro 59:5cd3875fa96a 6615 mtc2=false;
yabahiro 59:5cd3875fa96a 6616 break;
yabahiro 59:5cd3875fa96a 6617 case 255:
yabahiro 59:5cd3875fa96a 6618 motor[TIRE_FL].pwm += 5;
yabahiro 59:5cd3875fa96a 6619 mtc2=false;
yabahiro 59:5cd3875fa96a 6620 break;
yabahiro 59:5cd3875fa96a 6621 case 253:
yabahiro 59:5cd3875fa96a 6622 motor[TIRE_FL].pwm += 10;
yabahiro 59:5cd3875fa96a 6623 mtc2=false;
yabahiro 59:5cd3875fa96a 6624 break;
yabahiro 59:5cd3875fa96a 6625 case 254:
yabahiro 59:5cd3875fa96a 6626 motor[TIRE_FL].pwm += 20;
yabahiro 59:5cd3875fa96a 6627 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6628 mtc2=false;
yabahiro 59:5cd3875fa96a 6629 break;
yabahiro 59:5cd3875fa96a 6630 case 1:
yabahiro 59:5cd3875fa96a 6631 motor[TIRE_BL].pwm += 5;
yabahiro 59:5cd3875fa96a 6632 mtc2=false;
yabahiro 59:5cd3875fa96a 6633 break;
yabahiro 59:5cd3875fa96a 6634 case 3:
yabahiro 59:5cd3875fa96a 6635 motor[TIRE_BL].pwm += 10;
yabahiro 59:5cd3875fa96a 6636 mtc2=false;
yabahiro 59:5cd3875fa96a 6637 break;
yabahiro 59:5cd3875fa96a 6638 case 2:
yabahiro 59:5cd3875fa96a 6639 motor[TIRE_BL].pwm += 20;
yabahiro 59:5cd3875fa96a 6640 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6641 mtc2=false;
yabahiro 59:5cd3875fa96a 6642 break;
yabahiro 59:5cd3875fa96a 6643 }
yabahiro 59:5cd3875fa96a 6644 }
yabahiro 59:5cd3875fa96a 6645 break;
yabahiro 59:5cd3875fa96a 6646 case 41:
yabahiro 59:5cd3875fa96a 6647 /*
yabahiro 59:5cd3875fa96a 6648 if(LIF||LIB) {
yabahiro 59:5cd3875fa96a 6649 mode=42;
yabahiro 59:5cd3875fa96a 6650 }
yabahiro 59:5cd3875fa96a 6651 */
yabahiro 59:5cd3875fa96a 6652 motor[TIRE_FR].pwm = 30;
yabahiro 59:5cd3875fa96a 6653 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6654 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6655 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6656 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6657 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6658 motor[TIRE_BL].pwm = 30;
yabahiro 59:5cd3875fa96a 6659 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6660 if(g[1]==0) {
yabahiro 59:5cd3875fa96a 6661 countss=0;
yabahiro 59:5cd3875fa96a 6662 mode=70;
yabahiro 59:5cd3875fa96a 6663 normal=120;
yabahiro 59:5cd3875fa96a 6664 fast=140;
yabahiro 59:5cd3875fa96a 6665 slow=100;
yabahiro 59:5cd3875fa96a 6666 }
yabahiro 59:5cd3875fa96a 6667 break;
yabahiro 59:5cd3875fa96a 6668 case 42:
yabahiro 59:5cd3875fa96a 6669 motor[TIRE_FR].pwm = 40;
yabahiro 59:5cd3875fa96a 6670 motor[TIRE_FR].dir = FOR;
yabahiro 59:5cd3875fa96a 6671 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6672 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6673 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6674 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6675 motor[TIRE_BL].pwm = 40;
yabahiro 59:5cd3875fa96a 6676 motor[TIRE_BL].dir = BACK;
yabahiro 59:5cd3875fa96a 6677 if(g[1]==0) {
yabahiro 59:5cd3875fa96a 6678 countss=0;
yabahiro 59:5cd3875fa96a 6679 mode=70;
yabahiro 59:5cd3875fa96a 6680 }
yabahiro 59:5cd3875fa96a 6681 break;
yabahiro 59:5cd3875fa96a 6682 case 70:
yabahiro 59:5cd3875fa96a 6683 switch(g[0]) {
yabahiro 59:5cd3875fa96a 6684 case 99:
yabahiro 59:5cd3875fa96a 6685 if(countss>=100000) {
yabahiro 59:5cd3875fa96a 6686 mode=100;
yabahiro 59:5cd3875fa96a 6687 }
yabahiro 59:5cd3875fa96a 6688 break;
yabahiro 59:5cd3875fa96a 6689 case 98:
yabahiro 59:5cd3875fa96a 6690 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6691 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6692 motor[TIRE_FL].pwm = normal;
yabahiro 59:5cd3875fa96a 6693 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6694 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6695 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6696 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6697 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6698 break;
yabahiro 59:5cd3875fa96a 6699 case 0:
yabahiro 59:5cd3875fa96a 6700 motor[TIRE_FR].pwm = 80;
yabahiro 59:5cd3875fa96a 6701 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6702 motor[TIRE_FL].pwm = 80;
yabahiro 59:5cd3875fa96a 6703 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6704 motor[TIRE_BR].pwm = 80;
yabahiro 59:5cd3875fa96a 6705 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6706 motor[TIRE_BL].pwm = 80;
yabahiro 59:5cd3875fa96a 6707 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6708 mtc=true;
yabahiro 59:5cd3875fa96a 6709 break;
yabahiro 59:5cd3875fa96a 6710 case 255:
yabahiro 59:5cd3875fa96a 6711 motor[TIRE_FR].pwm = fast;
yabahiro 59:5cd3875fa96a 6712 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6713 motor[TIRE_FL].pwm = slow;
yabahiro 59:5cd3875fa96a 6714 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6715 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6716 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6717 motor[TIRE_BL].pwm = fast;
yabahiro 59:5cd3875fa96a 6718 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6719 mtc=true;
yabahiro 59:5cd3875fa96a 6720 break;
yabahiro 59:5cd3875fa96a 6721 case 253:
yabahiro 59:5cd3875fa96a 6722 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6723 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6724 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6725 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6726 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6727 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6728 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6729 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6730 mtc=true;
yabahiro 59:5cd3875fa96a 6731 break;
yabahiro 59:5cd3875fa96a 6732 case 254:
yabahiro 59:5cd3875fa96a 6733 motor[TIRE_FR].pwm = normal;
yabahiro 59:5cd3875fa96a 6734 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6735 motor[TIRE_FL].pwm = 0;
yabahiro 59:5cd3875fa96a 6736 motor[TIRE_FL].dir = FREE;
yabahiro 59:5cd3875fa96a 6737 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6738 motor[TIRE_BR].dir = FREE;
yabahiro 59:5cd3875fa96a 6739 motor[TIRE_BL].pwm = normal;
yabahiro 59:5cd3875fa96a 6740 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6741 mtc=true;
yabahiro 59:5cd3875fa96a 6742 break;
yabahiro 59:5cd3875fa96a 6743 case 1:
yabahiro 59:5cd3875fa96a 6744 motor[TIRE_FR].pwm = slow;
yabahiro 59:5cd3875fa96a 6745 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6746 motor[TIRE_FL].pwm = fast;
yabahiro 59:5cd3875fa96a 6747 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6748 motor[TIRE_BR].pwm = fast;
yabahiro 59:5cd3875fa96a 6749 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6750 motor[TIRE_BL].pwm = slow;
yabahiro 59:5cd3875fa96a 6751 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6752 mtc=true;
yabahiro 59:5cd3875fa96a 6753 break;
yabahiro 59:5cd3875fa96a 6754 case 3:
yabahiro 59:5cd3875fa96a 6755 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6756 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6757 motor[TIRE_FL].pwm = slow;
yabahiro 59:5cd3875fa96a 6758 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6759 motor[TIRE_BR].pwm = slow;
yabahiro 59:5cd3875fa96a 6760 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6761 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6762 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6763 mtc=true;
yabahiro 59:5cd3875fa96a 6764 break;
yabahiro 59:5cd3875fa96a 6765 case 2:
yabahiro 59:5cd3875fa96a 6766 motor[TIRE_FR].pwm = 0;
yabahiro 59:5cd3875fa96a 6767 motor[TIRE_FR].dir = FREE;
yabahiro 59:5cd3875fa96a 6768 motor[TIRE_FL].pwm = normal;
yabahiro 59:5cd3875fa96a 6769 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6770 motor[TIRE_BR].pwm = normal;
yabahiro 59:5cd3875fa96a 6771 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6772 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6773 motor[TIRE_BL].dir = FREE;
yabahiro 59:5cd3875fa96a 6774 mtc=true;
yabahiro 59:5cd3875fa96a 6775 break;
yabahiro 59:5cd3875fa96a 6776 }
yabahiro 59:5cd3875fa96a 6777 if(g[1]!=98&&mtc==true) {
yabahiro 59:5cd3875fa96a 6778 switch(g[1]) {
yabahiro 59:5cd3875fa96a 6779 case 0:
yabahiro 59:5cd3875fa96a 6780 mtc=false;
yabahiro 59:5cd3875fa96a 6781 break;
yabahiro 59:5cd3875fa96a 6782 case 255:
yabahiro 59:5cd3875fa96a 6783 motor[TIRE_BR].pwm += 5;
yabahiro 59:5cd3875fa96a 6784 mtc=false;
yabahiro 59:5cd3875fa96a 6785 break;
yabahiro 59:5cd3875fa96a 6786 case 253:
yabahiro 59:5cd3875fa96a 6787 motor[TIRE_BR].pwm += 10;
yabahiro 59:5cd3875fa96a 6788 mtc=false;
yabahiro 59:5cd3875fa96a 6789 break;
yabahiro 59:5cd3875fa96a 6790 case 254:
yabahiro 59:5cd3875fa96a 6791 motor[TIRE_BR].pwm += 20;
yabahiro 59:5cd3875fa96a 6792 motor[TIRE_BL].pwm = 0;
yabahiro 59:5cd3875fa96a 6793 mtc=false;
yabahiro 59:5cd3875fa96a 6794 break;
yabahiro 59:5cd3875fa96a 6795 case 1:
yabahiro 59:5cd3875fa96a 6796 motor[TIRE_BL].pwm += 5;
yabahiro 59:5cd3875fa96a 6797 mtc=false;
yabahiro 59:5cd3875fa96a 6798 break;
yabahiro 59:5cd3875fa96a 6799 case 3:
yabahiro 59:5cd3875fa96a 6800 motor[TIRE_BL].pwm += 10;
yabahiro 59:5cd3875fa96a 6801 mtc=false;
yabahiro 59:5cd3875fa96a 6802 break;
yabahiro 59:5cd3875fa96a 6803 case 2:
yabahiro 59:5cd3875fa96a 6804 motor[TIRE_BL].pwm += 20;
yabahiro 59:5cd3875fa96a 6805 motor[TIRE_BR].pwm = 0;
yabahiro 59:5cd3875fa96a 6806 mtc=false;
yabahiro 59:5cd3875fa96a 6807 break;
yabahiro 59:5cd3875fa96a 6808 }
yabahiro 59:5cd3875fa96a 6809 }
yabahiro 59:5cd3875fa96a 6810 break;
yabahiro 59:5cd3875fa96a 6811 case 100:
yabahiro 59:5cd3875fa96a 6812 motor[TIRE_FR].pwm = 47;
yabahiro 59:5cd3875fa96a 6813 motor[TIRE_FR].dir = BACK;
yabahiro 59:5cd3875fa96a 6814 motor[TIRE_FL].pwm = 50;
yabahiro 59:5cd3875fa96a 6815 motor[TIRE_FL].dir = FOR;
yabahiro 59:5cd3875fa96a 6816 motor[TIRE_BR].pwm = 50;
yabahiro 59:5cd3875fa96a 6817 motor[TIRE_BR].dir = BACK;
yabahiro 59:5cd3875fa96a 6818 motor[TIRE_BL].pwm = 55;
yabahiro 59:5cd3875fa96a 6819 motor[TIRE_BL].dir = FOR;
yabahiro 59:5cd3875fa96a 6820 break;
yabahiro 59:5cd3875fa96a 6821 default:
yabahiro 59:5cd3875fa96a 6822 motor[TIRE_FR].pwm = 50;
yabahiro 59:5cd3875fa96a 6823 motor[TIRE_FR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6824 motor[TIRE_FL].pwm = 50;
yabahiro 59:5cd3875fa96a 6825 motor[TIRE_FL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6826 motor[TIRE_BR].pwm = 50;
yabahiro 59:5cd3875fa96a 6827 motor[TIRE_BR].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6828 motor[TIRE_BL].pwm = 50;
yabahiro 59:5cd3875fa96a 6829 motor[TIRE_BL].dir = BRAKE;
yabahiro 59:5cd3875fa96a 6830 break;
yabahiro 59:5cd3875fa96a 6831 }
kishibekairohan 13:b6e02d6261d7 6832 }
kishibekairohan 13:b6e02d6261d7 6833 #endif
kishibekairohan 13:b6e02d6261d7 6834
kishibekairohan 13:b6e02d6261d7 6835 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 6836 static void Process7()
kishibekairohan 13:b6e02d6261d7 6837 {
yabahiro 60:24ebb611f26e 6838 //Clearly 赤ゾーン
yabahiro 60:24ebb611f26e 6839 LED::LED::TapeledMode(4);//3
yabahiro 60:24ebb611f26e 6840 value=aime.read_ms();
yabahiro 60:24ebb611f26e 6841 countss++;
yabahiro 60:24ebb611f26e 6842 switch(mode) {
yabahiro 60:24ebb611f26e 6843 case 1:
yabahiro 60:24ebb611f26e 6844 fast=40;
yabahiro 60:24ebb611f26e 6845 normal=30;
yabahiro 60:24ebb611f26e 6846 slow=20;
yabahiro 60:24ebb611f26e 6847 motor[TIRE_FR].pwm = normal+20;
yabahiro 60:24ebb611f26e 6848 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6849 motor[TIRE_FL].pwm = normal+20;
yabahiro 60:24ebb611f26e 6850 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6851 motor[TIRE_BR].pwm = normal+20;
yabahiro 60:24ebb611f26e 6852 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6853 motor[TIRE_BL].pwm = normal+20;
yabahiro 60:24ebb611f26e 6854 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6855 if(g[2]==0&&value>=1500) {
yabahiro 60:24ebb611f26e 6856 countss=100000;
yabahiro 60:24ebb611f26e 6857 cross=0;
yabahiro 60:24ebb611f26e 6858 mode=3;
yabahiro 60:24ebb611f26e 6859 }
yabahiro 60:24ebb611f26e 6860 break;
yabahiro 60:24ebb611f26e 6861 case 3:
yabahiro 60:24ebb611f26e 6862 motor[TIRE_FR].pwm = 255;
yabahiro 60:24ebb611f26e 6863 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 6864 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 6865 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6866 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 6867 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6868 motor[TIRE_BL].pwm = 255;
yabahiro 60:24ebb611f26e 6869 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 6870 if(g[0]==0) {
yabahiro 60:24ebb611f26e 6871 mode=10;
yabahiro 60:24ebb611f26e 6872 aime.reset();
yabahiro 60:24ebb611f26e 6873 countss=100000;
yabahiro 60:24ebb611f26e 6874 fast = 80;
yabahiro 60:24ebb611f26e 6875 normal = 60;
yabahiro 60:24ebb611f26e 6876 slow = 40;
yabahiro 60:24ebb611f26e 6877 }
yabahiro 60:24ebb611f26e 6878 break;
yabahiro 60:24ebb611f26e 6879 case 10:
yabahiro 60:24ebb611f26e 6880 switch(g[0]) {
yabahiro 60:24ebb611f26e 6881 case 98:
yabahiro 60:24ebb611f26e 6882 motor[TIRE_FR].pwm = normal+50;
yabahiro 60:24ebb611f26e 6883 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6884 motor[TIRE_FL].pwm = normal+50;
yabahiro 60:24ebb611f26e 6885 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6886 motor[TIRE_BR].pwm = normal+50;
yabahiro 60:24ebb611f26e 6887 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6888 motor[TIRE_BL].pwm = normal+50;
yabahiro 60:24ebb611f26e 6889 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6890 mtc=true;
yabahiro 60:24ebb611f26e 6891 if(value>=1200) {
yabahiro 60:24ebb611f26e 6892 cross++;
yabahiro 60:24ebb611f26e 6893 countss=0;
yabahiro 60:24ebb611f26e 6894 value=0;
yabahiro 60:24ebb611f26e 6895 aime.reset();
yabahiro 60:24ebb611f26e 6896 }
yabahiro 60:24ebb611f26e 6897 /*
yabahiro 60:24ebb611f26e 6898 if(countss>80000) {
yabahiro 60:24ebb611f26e 6899 cross++;
yabahiro 60:24ebb611f26e 6900 countss=0;
yabahiro 60:24ebb611f26e 6901 }
yabahiro 60:24ebb611f26e 6902 */
yabahiro 60:24ebb611f26e 6903 if(cross==2) {
yabahiro 60:24ebb611f26e 6904 fast = 10;
yabahiro 60:24ebb611f26e 6905 normal = 10;
yabahiro 60:24ebb611f26e 6906 slow = 5;
yabahiro 60:24ebb611f26e 6907 }
yabahiro 60:24ebb611f26e 6908
yabahiro 60:24ebb611f26e 6909 if(cross==3) {
yabahiro 60:24ebb611f26e 6910 fast=40;
yabahiro 60:24ebb611f26e 6911 normal=30;
yabahiro 60:24ebb611f26e 6912 slow=20;
yabahiro 60:24ebb611f26e 6913 mode=11;
yabahiro 60:24ebb611f26e 6914 aime.reset();
yabahiro 60:24ebb611f26e 6915 countss=0;
yabahiro 60:24ebb611f26e 6916 cross=0;
yabahiro 60:24ebb611f26e 6917
yabahiro 60:24ebb611f26e 6918 }
yabahiro 60:24ebb611f26e 6919 break;
yabahiro 60:24ebb611f26e 6920 case 0:
yabahiro 60:24ebb611f26e 6921 motor[TIRE_FR].pwm = fast+50;
yabahiro 60:24ebb611f26e 6922 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6923 motor[TIRE_FL].pwm = fast+60;
yabahiro 60:24ebb611f26e 6924 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6925 motor[TIRE_BR].pwm = fast+50;
yabahiro 60:24ebb611f26e 6926 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6927 motor[TIRE_BL].pwm = fast+45;
yabahiro 60:24ebb611f26e 6928 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6929 mtc=true;
yabahiro 60:24ebb611f26e 6930 break;
yabahiro 60:24ebb611f26e 6931 case 255:
yabahiro 60:24ebb611f26e 6932 motor[TIRE_FR].pwm = fast+50;
yabahiro 60:24ebb611f26e 6933 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6934 motor[TIRE_FL].pwm = slow+60;
yabahiro 60:24ebb611f26e 6935 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6936 motor[TIRE_BR].pwm = slow+50;
yabahiro 60:24ebb611f26e 6937 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6938 motor[TIRE_BL].pwm = fast+45;
yabahiro 60:24ebb611f26e 6939 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6940 mtc=true;
yabahiro 60:24ebb611f26e 6941 break;
yabahiro 60:24ebb611f26e 6942 case 253:
yabahiro 60:24ebb611f26e 6943 motor[TIRE_FR].pwm = slow+50;
yabahiro 60:24ebb611f26e 6944 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6945 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 6946 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 6947 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 6948 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 6949 motor[TIRE_BL].pwm = slow+45;
yabahiro 60:24ebb611f26e 6950 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6951 mtc=true;
yabahiro 60:24ebb611f26e 6952 break;
yabahiro 60:24ebb611f26e 6953 case 254:
yabahiro 60:24ebb611f26e 6954 motor[TIRE_FR].pwm = normal+50;
yabahiro 60:24ebb611f26e 6955 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6956 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 6957 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 6958 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 6959 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 6960 motor[TIRE_BL].pwm = normal+45;
yabahiro 60:24ebb611f26e 6961 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6962 mtc=true;
yabahiro 60:24ebb611f26e 6963 break;
yabahiro 60:24ebb611f26e 6964 case 1:
yabahiro 60:24ebb611f26e 6965 motor[TIRE_FR].pwm = slow+50;
yabahiro 60:24ebb611f26e 6966 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 6967 motor[TIRE_FL].pwm = fast+60;
yabahiro 60:24ebb611f26e 6968 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6969 motor[TIRE_BR].pwm = fast+50;
yabahiro 60:24ebb611f26e 6970 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6971 motor[TIRE_BL].pwm = slow+45;
yabahiro 60:24ebb611f26e 6972 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 6973 mtc=true;
yabahiro 60:24ebb611f26e 6974 break;
yabahiro 60:24ebb611f26e 6975 case 3:
yabahiro 60:24ebb611f26e 6976 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 6977 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 6978 motor[TIRE_FL].pwm = slow+60;
yabahiro 60:24ebb611f26e 6979 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6980 motor[TIRE_BR].pwm = slow+50;
yabahiro 60:24ebb611f26e 6981 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6982 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 6983 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 6984 mtc=true;
yabahiro 60:24ebb611f26e 6985 break;
yabahiro 60:24ebb611f26e 6986 case 2:
yabahiro 60:24ebb611f26e 6987 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 6988 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 6989 motor[TIRE_FL].pwm = normal+60;
yabahiro 60:24ebb611f26e 6990 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 6991 motor[TIRE_BR].pwm = normal+50;
yabahiro 60:24ebb611f26e 6992 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 6993 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 6994 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 6995 mtc=true;
yabahiro 60:24ebb611f26e 6996 break;
yabahiro 60:24ebb611f26e 6997 }
yabahiro 60:24ebb611f26e 6998 if(g[0]!=98&&mtc==true) {
yabahiro 60:24ebb611f26e 6999 switch(g[1]) {
yabahiro 60:24ebb611f26e 7000 case 0:
yabahiro 60:24ebb611f26e 7001 mtc=false;
yabahiro 60:24ebb611f26e 7002 break;
yabahiro 60:24ebb611f26e 7003 case 255:
yabahiro 60:24ebb611f26e 7004 motor[TIRE_BR].pwm += 3;
yabahiro 60:24ebb611f26e 7005 mtc=false;
yabahiro 60:24ebb611f26e 7006 break;
yabahiro 60:24ebb611f26e 7007 case 253:
yabahiro 60:24ebb611f26e 7008 motor[TIRE_BR].pwm += 6;
yabahiro 60:24ebb611f26e 7009 mtc=false;
yabahiro 60:24ebb611f26e 7010 break;
yabahiro 60:24ebb611f26e 7011 case 254:
yabahiro 60:24ebb611f26e 7012 motor[TIRE_BR].pwm += 9;
yabahiro 60:24ebb611f26e 7013 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7014 mtc=false;
yabahiro 60:24ebb611f26e 7015 break;
yabahiro 60:24ebb611f26e 7016 case 1:
yabahiro 60:24ebb611f26e 7017 motor[TIRE_BL].pwm += 3;
yabahiro 60:24ebb611f26e 7018 mtc=false;
yabahiro 60:24ebb611f26e 7019 break;
yabahiro 60:24ebb611f26e 7020 case 3:
yabahiro 60:24ebb611f26e 7021 motor[TIRE_BL].pwm += 6;
yabahiro 60:24ebb611f26e 7022 mtc=false;
yabahiro 60:24ebb611f26e 7023 break;
yabahiro 60:24ebb611f26e 7024 case 2:
yabahiro 60:24ebb611f26e 7025 motor[TIRE_BL].pwm += 9;
yabahiro 60:24ebb611f26e 7026 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7027 mtc=false;
yabahiro 60:24ebb611f26e 7028 break;
yabahiro 60:24ebb611f26e 7029 }
yabahiro 60:24ebb611f26e 7030 }
yabahiro 60:24ebb611f26e 7031 break;
yabahiro 60:24ebb611f26e 7032 case 11:
yabahiro 60:24ebb611f26e 7033 motor[TIRE_FR].pwm = 15;
yabahiro 60:24ebb611f26e 7034 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7035 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 7036 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7037 motor[TIRE_BR].pwm = normal;
yabahiro 60:24ebb611f26e 7038 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7039 motor[TIRE_BL].pwm = 15;
yabahiro 60:24ebb611f26e 7040 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7041 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
yabahiro 60:24ebb611f26e 7042 countss=0;
yabahiro 60:24ebb611f26e 7043 cross=0;
yabahiro 60:24ebb611f26e 7044 mode=20;
yabahiro 60:24ebb611f26e 7045 }
yabahiro 60:24ebb611f26e 7046 break;
yabahiro 60:24ebb611f26e 7047 case 20:
yabahiro 60:24ebb611f26e 7048 solenoid.solenoid4= SOLENOID_ON;
yabahiro 60:24ebb611f26e 7049 switch(g[2]) {
yabahiro 60:24ebb611f26e 7050 case 98:
yabahiro 60:24ebb611f26e 7051 motor[TIRE_FR].pwm = normal+20;
yabahiro 60:24ebb611f26e 7052 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7053 motor[TIRE_FL].pwm = normal+20;
yabahiro 60:24ebb611f26e 7054 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7055 motor[TIRE_BR].pwm = normal+20;
yabahiro 60:24ebb611f26e 7056 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7057 motor[TIRE_BL].pwm = normal+20;
yabahiro 60:24ebb611f26e 7058 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7059 if(countss>30000) {
yabahiro 60:24ebb611f26e 7060 cross++;
yabahiro 60:24ebb611f26e 7061 countss=0;
yabahiro 60:24ebb611f26e 7062 }
yabahiro 60:24ebb611f26e 7063 mtc2=true;
yabahiro 60:24ebb611f26e 7064
yabahiro 60:24ebb611f26e 7065 if(YELLOW) {
yabahiro 60:24ebb611f26e 7066 if(cross==1) {
yabahiro 60:24ebb611f26e 7067 fast=20;
yabahiro 60:24ebb611f26e 7068 normal=15;
yabahiro 60:24ebb611f26e 7069 slow=15;
yabahiro 60:24ebb611f26e 7070 } else if(cross==2) {
yabahiro 60:24ebb611f26e 7071 mode=21;
yabahiro 60:24ebb611f26e 7072 countss=0;
yabahiro 60:24ebb611f26e 7073 cross=0;
yabahiro 60:24ebb611f26e 7074 }
yabahiro 60:24ebb611f26e 7075 } else {
yabahiro 60:24ebb611f26e 7076 if(cross==3) {
yabahiro 60:24ebb611f26e 7077 mode=21;
yabahiro 60:24ebb611f26e 7078 countss=0;
yabahiro 60:24ebb611f26e 7079 cross=0;
yabahiro 60:24ebb611f26e 7080 }
yabahiro 60:24ebb611f26e 7081 }
yabahiro 60:24ebb611f26e 7082 break;
yabahiro 60:24ebb611f26e 7083 case 0:
yabahiro 60:24ebb611f26e 7084 motor[TIRE_FR].pwm = fast+20;
yabahiro 60:24ebb611f26e 7085 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7086 motor[TIRE_FL].pwm = fast+20;
yabahiro 60:24ebb611f26e 7087 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7088 motor[TIRE_BR].pwm = fast+20;
yabahiro 60:24ebb611f26e 7089 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7090 motor[TIRE_BL].pwm = fast+20;
yabahiro 60:24ebb611f26e 7091 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7092 mtc2=true;
yabahiro 60:24ebb611f26e 7093 break;
yabahiro 60:24ebb611f26e 7094 case 255:
yabahiro 60:24ebb611f26e 7095 motor[TIRE_FR].pwm = normal+20;
yabahiro 60:24ebb611f26e 7096 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7097 motor[TIRE_FL].pwm = fast+20;
yabahiro 60:24ebb611f26e 7098 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7099 motor[TIRE_BR].pwm = fast+20;
yabahiro 60:24ebb611f26e 7100 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7101 motor[TIRE_BL].pwm = normal+20;
yabahiro 60:24ebb611f26e 7102 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7103 mtc2=true;
yabahiro 60:24ebb611f26e 7104 break;
yabahiro 60:24ebb611f26e 7105 case 253:
yabahiro 60:24ebb611f26e 7106 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7107 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7108 motor[TIRE_FL].pwm = slow+20;
yabahiro 60:24ebb611f26e 7109 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7110 motor[TIRE_BR].pwm = slow+20;
yabahiro 60:24ebb611f26e 7111 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7112 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7113 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7114 mtc2=true;
yabahiro 60:24ebb611f26e 7115 break;
yabahiro 60:24ebb611f26e 7116 case 254:
yabahiro 60:24ebb611f26e 7117 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7118 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7119 motor[TIRE_FL].pwm = normal+20;
yabahiro 60:24ebb611f26e 7120 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7121 motor[TIRE_BR].pwm = normal+20;
yabahiro 60:24ebb611f26e 7122 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7123 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7124 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7125 mtc2=true;
yabahiro 60:24ebb611f26e 7126 break;
yabahiro 60:24ebb611f26e 7127 case 1:
yabahiro 60:24ebb611f26e 7128 motor[TIRE_FR].pwm = fast+20;
yabahiro 60:24ebb611f26e 7129 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7130 motor[TIRE_FL].pwm = slow+20;
yabahiro 60:24ebb611f26e 7131 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7132 motor[TIRE_BR].pwm = slow+20;
yabahiro 60:24ebb611f26e 7133 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7134 motor[TIRE_BL].pwm = fast+20;
yabahiro 60:24ebb611f26e 7135 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7136 mtc2=true;
yabahiro 60:24ebb611f26e 7137 break;
yabahiro 60:24ebb611f26e 7138 case 3:
yabahiro 60:24ebb611f26e 7139 motor[TIRE_FR].pwm = slow+20;
yabahiro 60:24ebb611f26e 7140 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7141 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7142 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7143 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7144 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7145 motor[TIRE_BL].pwm = slow+20;
yabahiro 60:24ebb611f26e 7146 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7147 mtc2=true;
yabahiro 60:24ebb611f26e 7148 break;
yabahiro 60:24ebb611f26e 7149 case 2:
yabahiro 60:24ebb611f26e 7150 motor[TIRE_FR].pwm = normal+20;
yabahiro 60:24ebb611f26e 7151 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7152 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7153 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7154 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7155 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7156 motor[TIRE_BL].pwm = normal+20;
yabahiro 60:24ebb611f26e 7157 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7158 mtc2=true;
yabahiro 60:24ebb611f26e 7159 break;
yabahiro 60:24ebb611f26e 7160 }
yabahiro 60:24ebb611f26e 7161 if(g[2]!=98&&mtc2==true) {
yabahiro 60:24ebb611f26e 7162 switch(g[3]) {
yabahiro 60:24ebb611f26e 7163 case 0:
yabahiro 60:24ebb611f26e 7164 mtc2=false;
yabahiro 60:24ebb611f26e 7165 break;
yabahiro 60:24ebb611f26e 7166 case 255:
yabahiro 60:24ebb611f26e 7167 motor[TIRE_BL].pwm += 5;
yabahiro 60:24ebb611f26e 7168 mtc2=false;
yabahiro 60:24ebb611f26e 7169 break;
yabahiro 60:24ebb611f26e 7170 case 253:
yabahiro 60:24ebb611f26e 7171 motor[TIRE_BL].pwm += 10;
yabahiro 60:24ebb611f26e 7172 mtc2=false;
yabahiro 60:24ebb611f26e 7173 break;
yabahiro 60:24ebb611f26e 7174 case 254:
yabahiro 60:24ebb611f26e 7175 motor[TIRE_BL].pwm += 15;
yabahiro 60:24ebb611f26e 7176 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7177 mtc2=false;
yabahiro 60:24ebb611f26e 7178 break;
yabahiro 60:24ebb611f26e 7179 case 1:
yabahiro 60:24ebb611f26e 7180 motor[TIRE_FL].pwm += 5;
yabahiro 60:24ebb611f26e 7181 mtc2=false;
yabahiro 60:24ebb611f26e 7182 break;
yabahiro 60:24ebb611f26e 7183 case 3:
yabahiro 60:24ebb611f26e 7184 motor[TIRE_FL].pwm += 10;
yabahiro 60:24ebb611f26e 7185 mtc2=false;
yabahiro 60:24ebb611f26e 7186 break;
yabahiro 60:24ebb611f26e 7187 case 2:
yabahiro 60:24ebb611f26e 7188 motor[TIRE_FL].pwm += 15;
yabahiro 60:24ebb611f26e 7189 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7190 mtc2=false;
yabahiro 60:24ebb611f26e 7191 break;
yabahiro 60:24ebb611f26e 7192 }
yabahiro 60:24ebb611f26e 7193 }
yabahiro 60:24ebb611f26e 7194 break;
yabahiro 60:24ebb611f26e 7195 case 21:
yabahiro 60:24ebb611f26e 7196 if(YELLOW) {
yabahiro 60:24ebb611f26e 7197 motor[TIRE_FR].pwm = 20;
yabahiro 60:24ebb611f26e 7198 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7199 motor[TIRE_FL].pwm = 20;
yabahiro 60:24ebb611f26e 7200 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7201 motor[TIRE_BR].pwm = 20;
yabahiro 60:24ebb611f26e 7202 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7203 motor[TIRE_BL].pwm = 20;
yabahiro 60:24ebb611f26e 7204 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 7205 if(g[3]==98&&countss>=10000) {
yabahiro 60:24ebb611f26e 7206 mode=22;
yabahiro 60:24ebb611f26e 7207 aime.reset();
yabahiro 60:24ebb611f26e 7208 countss=0;
yabahiro 60:24ebb611f26e 7209 fast=60;
yabahiro 60:24ebb611f26e 7210 normal=40;
yabahiro 60:24ebb611f26e 7211 slow=20;
yabahiro 60:24ebb611f26e 7212 }
yabahiro 60:24ebb611f26e 7213 } else {
yabahiro 60:24ebb611f26e 7214 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7215 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7216 motor[TIRE_FL].pwm = 255;
yabahiro 60:24ebb611f26e 7217 motor[TIRE_FL].dir = BRAKE;
yabahiro 60:24ebb611f26e 7218 motor[TIRE_BR].pwm = 255;
yabahiro 60:24ebb611f26e 7219 motor[TIRE_BR].dir = BRAKE;
yabahiro 60:24ebb611f26e 7220 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7221 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7222 if(g[1]==0) {
yabahiro 60:24ebb611f26e 7223 mode=22;
yabahiro 60:24ebb611f26e 7224 aime.reset();
yabahiro 60:24ebb611f26e 7225 fast=60;
yabahiro 60:24ebb611f26e 7226 normal=40;
yabahiro 60:24ebb611f26e 7227 slow=25;
yabahiro 60:24ebb611f26e 7228 }
yabahiro 60:24ebb611f26e 7229 }
yabahiro 60:24ebb611f26e 7230 break;
yabahiro 60:24ebb611f26e 7231 case 22:
yabahiro 60:24ebb611f26e 7232 if(YELLOW) {
yabahiro 60:24ebb611f26e 7233 if(g[4]==3||g[4]==2) {
yabahiro 60:24ebb611f26e 7234 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7235 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7236 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7237 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7238 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7239 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7240 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7241 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7242 }
yabahiro 60:24ebb611f26e 7243 if(g[4]==253||g[4]==254) {
yabahiro 60:24ebb611f26e 7244 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7245 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7246 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7247 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7248 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7249 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7250 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7251 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 7252 }
yabahiro 60:24ebb611f26e 7253 switch(g[2]) {
yabahiro 60:24ebb611f26e 7254 case 0:
yabahiro 60:24ebb611f26e 7255 motor[TIRE_FR].pwm = 20;
yabahiro 60:24ebb611f26e 7256 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7257 motor[TIRE_BR].pwm = 20;
yabahiro 60:24ebb611f26e 7258 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7259 break;
yabahiro 60:24ebb611f26e 7260 case 255:
yabahiro 60:24ebb611f26e 7261 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7262 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7263 motor[TIRE_BR].pwm = 20;
yabahiro 60:24ebb611f26e 7264 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7265 break;
yabahiro 60:24ebb611f26e 7266 case 253:
yabahiro 60:24ebb611f26e 7267 motor[TIRE_FR].pwm = 40;
yabahiro 60:24ebb611f26e 7268 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7269 motor[TIRE_BR].pwm = 20;
yabahiro 60:24ebb611f26e 7270 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7271 break;
yabahiro 60:24ebb611f26e 7272 case 254:
yabahiro 60:24ebb611f26e 7273 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7274 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7275 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7276 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7277 break;
yabahiro 60:24ebb611f26e 7278 case 1:
yabahiro 60:24ebb611f26e 7279 motor[TIRE_FR].pwm = 20;
yabahiro 60:24ebb611f26e 7280 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7281 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7282 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7283 break;
yabahiro 60:24ebb611f26e 7284 case 3:
yabahiro 60:24ebb611f26e 7285 motor[TIRE_FR].pwm = 20;
yabahiro 60:24ebb611f26e 7286 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7287 motor[TIRE_BR].pwm = 40;
yabahiro 60:24ebb611f26e 7288 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7289 break;
yabahiro 60:24ebb611f26e 7290 case 2:
yabahiro 60:24ebb611f26e 7291 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7292 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7293 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7294 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7295 break;
yabahiro 60:24ebb611f26e 7296 }
yabahiro 60:24ebb611f26e 7297 switch(g[3]) {
yabahiro 60:24ebb611f26e 7298 case 0:
yabahiro 60:24ebb611f26e 7299 motor[TIRE_FL].pwm = 20;
yabahiro 60:24ebb611f26e 7300 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7301 motor[TIRE_BL].pwm = 20;
yabahiro 60:24ebb611f26e 7302 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7303 break;
yabahiro 60:24ebb611f26e 7304 case 255:
yabahiro 60:24ebb611f26e 7305 motor[TIRE_FL].pwm = 20;
yabahiro 60:24ebb611f26e 7306 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7307 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7308 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7309 case 253:
yabahiro 60:24ebb611f26e 7310 motor[TIRE_FL].pwm = 20;
yabahiro 60:24ebb611f26e 7311 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7312 motor[TIRE_BL].pwm = 40;
yabahiro 60:24ebb611f26e 7313 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7314 break;
yabahiro 60:24ebb611f26e 7315 case 254:
yabahiro 60:24ebb611f26e 7316 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7317 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7318 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7319 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7320 break;
yabahiro 60:24ebb611f26e 7321 case 1:
yabahiro 60:24ebb611f26e 7322 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7323 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7324 motor[TIRE_BL].pwm = 20;
yabahiro 60:24ebb611f26e 7325 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7326 break;
yabahiro 60:24ebb611f26e 7327 case 3:
yabahiro 60:24ebb611f26e 7328 motor[TIRE_FL].pwm = 40;
yabahiro 60:24ebb611f26e 7329 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7330 motor[TIRE_BL].pwm = 20;
yabahiro 60:24ebb611f26e 7331 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7332 break;
yabahiro 60:24ebb611f26e 7333 case 2:
yabahiro 60:24ebb611f26e 7334 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7335 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7336 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7337 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 7338 break;
yabahiro 60:24ebb611f26e 7339 }
yabahiro 60:24ebb611f26e 7340 if(g[3]==0&&g[2]==0) {
yabahiro 60:24ebb611f26e 7341 mode=23;
yabahiro 60:24ebb611f26e 7342 aime.reset();
yabahiro 60:24ebb611f26e 7343 countss=0;
yabahiro 60:24ebb611f26e 7344 fast = 60;
yabahiro 60:24ebb611f26e 7345 normal = 40;
yabahiro 60:24ebb611f26e 7346 slow = 30;
yabahiro 60:24ebb611f26e 7347 }
yabahiro 60:24ebb611f26e 7348 } else {
yabahiro 60:24ebb611f26e 7349 if(value>=2400) {
yabahiro 60:24ebb611f26e 7350 slow=25;
yabahiro 60:24ebb611f26e 7351 normal=30;
yabahiro 60:24ebb611f26e 7352 fast=40;
yabahiro 60:24ebb611f26e 7353 }
yabahiro 60:24ebb611f26e 7354 /*
yabahiro 60:24ebb611f26e 7355 if(g[1]==98) {
yabahiro 60:24ebb611f26e 7356 slow=25;
yabahiro 60:24ebb611f26e 7357 normal=30;
yabahiro 60:24ebb611f26e 7358 fast=40;
yabahiro 60:24ebb611f26e 7359 }
yabahiro 60:24ebb611f26e 7360
yabahiro 60:24ebb611f26e 7361 */
yabahiro 60:24ebb611f26e 7362 if(value>=3000) {
yabahiro 60:24ebb611f26e 7363 slow=25;
yabahiro 60:24ebb611f26e 7364 normal=40;
yabahiro 60:24ebb611f26e 7365 fast=60;
yabahiro 60:24ebb611f26e 7366 mode=23;
yabahiro 60:24ebb611f26e 7367 value=0;
yabahiro 60:24ebb611f26e 7368 aime.reset();
yabahiro 60:24ebb611f26e 7369 }
yabahiro 60:24ebb611f26e 7370 switch(g[1]) {
yabahiro 60:24ebb611f26e 7371 case 98:
yabahiro 60:24ebb611f26e 7372 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 60:24ebb611f26e 7373 fast=40;
yabahiro 60:24ebb611f26e 7374 normal=30;
yabahiro 60:24ebb611f26e 7375 slow=25;
yabahiro 60:24ebb611f26e 7376 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 7377 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7378 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 7379 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7380 motor[TIRE_BR].pwm = normal;
yabahiro 60:24ebb611f26e 7381 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7382 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 7383 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7384 break;
yabahiro 60:24ebb611f26e 7385 case 0:
yabahiro 60:24ebb611f26e 7386 motor[TIRE_FR].pwm = fast;
yabahiro 60:24ebb611f26e 7387 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7388 motor[TIRE_FL].pwm = fast;
yabahiro 60:24ebb611f26e 7389 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7390 motor[TIRE_BR].pwm = fast;
yabahiro 60:24ebb611f26e 7391 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7392 motor[TIRE_BL].pwm = fast;
yabahiro 60:24ebb611f26e 7393 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7394 mtc=true;
yabahiro 60:24ebb611f26e 7395 break;
yabahiro 60:24ebb611f26e 7396 case 255:
yabahiro 60:24ebb611f26e 7397 motor[TIRE_FR].pwm = fast;
yabahiro 60:24ebb611f26e 7398 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7399 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 7400 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7401 motor[TIRE_BR].pwm = slow;
yabahiro 60:24ebb611f26e 7402 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7403 motor[TIRE_BL].pwm = fast;
yabahiro 60:24ebb611f26e 7404 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7405 mtc=true;
yabahiro 60:24ebb611f26e 7406 break;
yabahiro 60:24ebb611f26e 7407 case 253:
yabahiro 60:24ebb611f26e 7408 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 7409 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7410 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7411 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7412 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7413 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7414 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 7415 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7416 mtc=true;
yabahiro 60:24ebb611f26e 7417 break;
yabahiro 60:24ebb611f26e 7418 case 254:
yabahiro 60:24ebb611f26e 7419 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 7420 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7421 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7422 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7423 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7424 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7425 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 7426 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7427 mtc=true;
yabahiro 60:24ebb611f26e 7428 break;
yabahiro 60:24ebb611f26e 7429 case 1:
yabahiro 60:24ebb611f26e 7430 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 7431 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7432 motor[TIRE_FL].pwm = fast;
yabahiro 60:24ebb611f26e 7433 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7434 motor[TIRE_BR].pwm = fast;
yabahiro 60:24ebb611f26e 7435 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7436 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 7437 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7438 mtc=true;
yabahiro 60:24ebb611f26e 7439 break;
yabahiro 60:24ebb611f26e 7440 case 3:
yabahiro 60:24ebb611f26e 7441 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7442 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7443 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 7444 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7445 motor[TIRE_BR].pwm = slow;
yabahiro 60:24ebb611f26e 7446 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7447 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7448 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7449 mtc=true;
yabahiro 60:24ebb611f26e 7450 break;
yabahiro 60:24ebb611f26e 7451 case 2:
yabahiro 60:24ebb611f26e 7452 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7453 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7454 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 7455 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7456 motor[TIRE_BR].pwm = normal;
yabahiro 60:24ebb611f26e 7457 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7458 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7459 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7460 mtc=true;
yabahiro 60:24ebb611f26e 7461 break;
yabahiro 60:24ebb611f26e 7462 }
yabahiro 60:24ebb611f26e 7463 if(g[1]!=98&&mtc==true) {
yabahiro 60:24ebb611f26e 7464 switch(g[0]) {
yabahiro 60:24ebb611f26e 7465 case 0:
yabahiro 60:24ebb611f26e 7466 mtc=false;
yabahiro 60:24ebb611f26e 7467 break;
yabahiro 60:24ebb611f26e 7468 case 255:
yabahiro 60:24ebb611f26e 7469 motor[TIRE_FL].pwm += 3;
yabahiro 60:24ebb611f26e 7470 mtc=false;
yabahiro 60:24ebb611f26e 7471 break;
yabahiro 60:24ebb611f26e 7472 case 253:
yabahiro 60:24ebb611f26e 7473 motor[TIRE_FL].pwm += 6;
yabahiro 60:24ebb611f26e 7474 mtc=false;
yabahiro 60:24ebb611f26e 7475 break;
yabahiro 60:24ebb611f26e 7476 case 254:
yabahiro 60:24ebb611f26e 7477 motor[TIRE_FL].pwm += 9;
yabahiro 60:24ebb611f26e 7478 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7479 mtc=false;
yabahiro 60:24ebb611f26e 7480 break;
yabahiro 60:24ebb611f26e 7481 case 1:
yabahiro 60:24ebb611f26e 7482 motor[TIRE_FR].pwm += 3;
yabahiro 60:24ebb611f26e 7483 mtc=false;
yabahiro 60:24ebb611f26e 7484 break;
yabahiro 60:24ebb611f26e 7485 case 3:
yabahiro 60:24ebb611f26e 7486 motor[TIRE_FR].pwm += 6;
yabahiro 60:24ebb611f26e 7487 mtc=false;
yabahiro 60:24ebb611f26e 7488 break;
yabahiro 60:24ebb611f26e 7489 case 2:
yabahiro 60:24ebb611f26e 7490 motor[TIRE_FR].pwm += 9;
yabahiro 60:24ebb611f26e 7491 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7492 mtc=false;
yabahiro 60:24ebb611f26e 7493 break;
yabahiro 60:24ebb611f26e 7494 }
yabahiro 60:24ebb611f26e 7495 }
yabahiro 60:24ebb611f26e 7496 }
yabahiro 60:24ebb611f26e 7497 break;
yabahiro 60:24ebb611f26e 7498 case 23:
yabahiro 60:24ebb611f26e 7499 if(YELLOW) {
yabahiro 60:24ebb611f26e 7500 switch(g[4]) {
yabahiro 60:24ebb611f26e 7501 case 98:
yabahiro 60:24ebb611f26e 7502 motor[TIRE_FR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7503 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7504 motor[TIRE_FL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7505 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7506 motor[TIRE_BR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7507 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7508 motor[TIRE_BL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7509 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7510 break;
yabahiro 60:24ebb611f26e 7511 case 99:
yabahiro 60:24ebb611f26e 7512 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7513 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7514 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7515 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7516 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7517 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7518 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7519 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7520 break;
yabahiro 60:24ebb611f26e 7521 case 0:
yabahiro 60:24ebb611f26e 7522 motor[TIRE_FR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7523 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7524 motor[TIRE_FL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7525 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7526 motor[TIRE_BR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7527 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7528 motor[TIRE_BL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7529 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7530 break;
yabahiro 60:24ebb611f26e 7531 case 255:
yabahiro 60:24ebb611f26e 7532 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7533 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7534 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7535 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7536 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7537 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7538 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7539 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7540 break;
yabahiro 60:24ebb611f26e 7541 case 253:
yabahiro 60:24ebb611f26e 7542 motor[TIRE_FR].pwm = slow+5;
yabahiro 60:24ebb611f26e 7543 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7544 motor[TIRE_FL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7545 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7546 motor[TIRE_BR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7547 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7548 motor[TIRE_BL].pwm = slow+5;
yabahiro 60:24ebb611f26e 7549 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7550 break;
yabahiro 60:24ebb611f26e 7551 case 254:
yabahiro 60:24ebb611f26e 7552 motor[TIRE_FR].pwm = normal+5;
yabahiro 60:24ebb611f26e 7553 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7554 motor[TIRE_FL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7555 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7556 motor[TIRE_BR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7557 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7558 motor[TIRE_BL].pwm = normal+5;
yabahiro 60:24ebb611f26e 7559 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7560 break;
yabahiro 60:24ebb611f26e 7561 case 1:
yabahiro 60:24ebb611f26e 7562 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7563 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7564 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7565 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7566 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7567 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7568 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7569 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7570 break;
yabahiro 60:24ebb611f26e 7571 case 3:
yabahiro 60:24ebb611f26e 7572 motor[TIRE_FR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7573 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7574 motor[TIRE_FL].pwm = slow+5;
yabahiro 60:24ebb611f26e 7575 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7576 motor[TIRE_BR].pwm = slow+5;
yabahiro 60:24ebb611f26e 7577 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7578 motor[TIRE_BL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7579 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7580 break;
yabahiro 60:24ebb611f26e 7581 case 2:
yabahiro 60:24ebb611f26e 7582 motor[TIRE_FR].pwm = fast-5;
yabahiro 60:24ebb611f26e 7583 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7584 motor[TIRE_FL].pwm = normal+5;
yabahiro 60:24ebb611f26e 7585 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7586 motor[TIRE_BR].pwm = normal+5;
yabahiro 60:24ebb611f26e 7587 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7588 motor[TIRE_BL].pwm = fast-5;
yabahiro 60:24ebb611f26e 7589 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7590 break;
yabahiro 60:24ebb611f26e 7591 }
yabahiro 60:24ebb611f26e 7592 if(value>=1800) {
yabahiro 60:24ebb611f26e 7593 fast=20;
yabahiro 60:24ebb611f26e 7594 normal=15;
yabahiro 60:24ebb611f26e 7595 slow=10;
yabahiro 60:24ebb611f26e 7596 }
yabahiro 60:24ebb611f26e 7597 if(value>=2600) {
yabahiro 60:24ebb611f26e 7598 mode=24;
yabahiro 60:24ebb611f26e 7599 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 60:24ebb611f26e 7600 aime.reset();
yabahiro 60:24ebb611f26e 7601 countss=0;
yabahiro 60:24ebb611f26e 7602 fast=80;
yabahiro 60:24ebb611f26e 7603 normal=60;
yabahiro 60:24ebb611f26e 7604 slow=30;
yabahiro 60:24ebb611f26e 7605 }
yabahiro 60:24ebb611f26e 7606
yabahiro 60:24ebb611f26e 7607 /*
yabahiro 60:24ebb611f26e 7608 if(value>=2500) {
yabahiro 60:24ebb611f26e 7609 mode=24;
yabahiro 60:24ebb611f26e 7610 aime.reset();
yabahiro 60:24ebb611f26e 7611 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 60:24ebb611f26e 7612 countss=0;
yabahiro 60:24ebb611f26e 7613 fast=80;
yabahiro 60:24ebb611f26e 7614 normal=60;
yabahiro 60:24ebb611f26e 7615 slow=30;
yabahiro 60:24ebb611f26e 7616 }
yabahiro 60:24ebb611f26e 7617 if(g[1]==98) {
yabahiro 60:24ebb611f26e 7618 fast=10;
yabahiro 60:24ebb611f26e 7619 normal=10;
yabahiro 60:24ebb611f26e 7620 slow=10;
yabahiro 60:24ebb611f26e 7621 }
yabahiro 60:24ebb611f26e 7622 */
yabahiro 60:24ebb611f26e 7623 } else {
yabahiro 60:24ebb611f26e 7624 if(UPLS||DOLS) {
yabahiro 60:24ebb611f26e 7625 if(UPLS||DOLS) {
yabahiro 60:24ebb611f26e 7626 motor[MOTOR_0].pwm = 200;
yabahiro 60:24ebb611f26e 7627 motor[MOTOR_0].dir = BRAKE;
yabahiro 60:24ebb611f26e 7628 fast=60;
yabahiro 60:24ebb611f26e 7629 normal=40;
yabahiro 60:24ebb611f26e 7630 slow=30;
yabahiro 60:24ebb611f26e 7631 countss=0;
yabahiro 60:24ebb611f26e 7632 mode=24;
yabahiro 60:24ebb611f26e 7633 }
yabahiro 60:24ebb611f26e 7634 } else {
yabahiro 60:24ebb611f26e 7635 motor[MOTOR_0].pwm = 100;
yabahiro 60:24ebb611f26e 7636 motor[MOTOR_0].dir = BACK;
yabahiro 60:24ebb611f26e 7637 }
yabahiro 60:24ebb611f26e 7638 motor[TIRE_FR].pwm = 100;
yabahiro 60:24ebb611f26e 7639 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 7640 motor[TIRE_FL].pwm = 100;
yabahiro 60:24ebb611f26e 7641 motor[TIRE_FL].dir = BRAKE;
yabahiro 60:24ebb611f26e 7642 motor[TIRE_BR].pwm = 100;
yabahiro 60:24ebb611f26e 7643 motor[TIRE_BR].dir = BRAKE;
yabahiro 60:24ebb611f26e 7644 motor[TIRE_BL].pwm = 100;
yabahiro 60:24ebb611f26e 7645 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 7646 }
yabahiro 60:24ebb611f26e 7647 break;
yabahiro 60:24ebb611f26e 7648 case 24:
yabahiro 60:24ebb611f26e 7649 if(YELLOW) {
yabahiro 60:24ebb611f26e 7650 /*
yabahiro 60:24ebb611f26e 7651 value=aime.read_ms();
yabahiro 60:24ebb611f26e 7652 if(value>=4000) {
yabahiro 60:24ebb611f26e 7653 mode=25;
yabahiro 60:24ebb611f26e 7654 }
yabahiro 60:24ebb611f26e 7655 if(g[4]==3||g[4]==2) {
yabahiro 60:24ebb611f26e 7656 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7657 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7658 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7659 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7660 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7661 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7662 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7663 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7664 }
yabahiro 60:24ebb611f26e 7665 if(g[4]==253||g[4]==254) {
yabahiro 60:24ebb611f26e 7666 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7667 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7668 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7669 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7670 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7671 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7672 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7673 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 7674 }
yabahiro 60:24ebb611f26e 7675 switch(g[2]) {
yabahiro 60:24ebb611f26e 7676 case 0:
yabahiro 60:24ebb611f26e 7677 motor[TIRE_FR].pwm = 25;
yabahiro 60:24ebb611f26e 7678 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7679 motor[TIRE_BR].pwm = 25;
yabahiro 60:24ebb611f26e 7680 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7681 break;
yabahiro 60:24ebb611f26e 7682 case 255:
yabahiro 60:24ebb611f26e 7683 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7684 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7685 motor[TIRE_BR].pwm = 25;
yabahiro 60:24ebb611f26e 7686 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7687 break;
yabahiro 60:24ebb611f26e 7688 case 253:
yabahiro 60:24ebb611f26e 7689 motor[TIRE_FR].pwm = 40;
yabahiro 60:24ebb611f26e 7690 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7691 motor[TIRE_BR].pwm = 25;
yabahiro 60:24ebb611f26e 7692 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7693 break;
yabahiro 60:24ebb611f26e 7694 case 254:
yabahiro 60:24ebb611f26e 7695 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7696 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 7697 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7698 motor[TIRE_BR].dir = BACK;
yabahiro 60:24ebb611f26e 7699 break;
yabahiro 60:24ebb611f26e 7700 case 1:
yabahiro 60:24ebb611f26e 7701 motor[TIRE_FR].pwm = 25;
yabahiro 60:24ebb611f26e 7702 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7703 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7704 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7705 break;
yabahiro 60:24ebb611f26e 7706 case 3:
yabahiro 60:24ebb611f26e 7707 motor[TIRE_FR].pwm = 25;
yabahiro 60:24ebb611f26e 7708 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7709 motor[TIRE_BR].pwm = 40;
yabahiro 60:24ebb611f26e 7710 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7711 break;
yabahiro 60:24ebb611f26e 7712 case 2:
yabahiro 60:24ebb611f26e 7713 motor[TIRE_FR].pwm = 30;
yabahiro 60:24ebb611f26e 7714 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7715 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 7716 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7717 break;
yabahiro 60:24ebb611f26e 7718 }
yabahiro 60:24ebb611f26e 7719 switch(g[3]) {
yabahiro 60:24ebb611f26e 7720 case 0:
yabahiro 60:24ebb611f26e 7721 motor[TIRE_FL].pwm = 25;
yabahiro 60:24ebb611f26e 7722 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7723 motor[TIRE_BL].pwm = 25;
yabahiro 60:24ebb611f26e 7724 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7725 break;
yabahiro 60:24ebb611f26e 7726 case 255:
yabahiro 60:24ebb611f26e 7727 motor[TIRE_FL].pwm = 25;
yabahiro 60:24ebb611f26e 7728 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7729 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7730 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7731 case 253:
yabahiro 60:24ebb611f26e 7732 motor[TIRE_FL].pwm = 25;
yabahiro 60:24ebb611f26e 7733 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7734 motor[TIRE_BL].pwm = 40;
yabahiro 60:24ebb611f26e 7735 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7736 break;
yabahiro 60:24ebb611f26e 7737 case 254:
yabahiro 60:24ebb611f26e 7738 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7739 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7740 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7741 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7742 break;
yabahiro 60:24ebb611f26e 7743 case 1:
yabahiro 60:24ebb611f26e 7744 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7745 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7746 motor[TIRE_BL].pwm = 25;
yabahiro 60:24ebb611f26e 7747 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7748 break;
yabahiro 60:24ebb611f26e 7749 case 3:
yabahiro 60:24ebb611f26e 7750 motor[TIRE_FL].pwm = 40;
yabahiro 60:24ebb611f26e 7751 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7752 motor[TIRE_BL].pwm = 25;
yabahiro 60:24ebb611f26e 7753 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7754 break;
yabahiro 60:24ebb611f26e 7755 case 2:
yabahiro 60:24ebb611f26e 7756 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 7757 motor[TIRE_FL].dir = FOR;
yabahiro 60:24ebb611f26e 7758 motor[TIRE_BL].pwm = 30;
yabahiro 60:24ebb611f26e 7759 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 7760 break;
yabahiro 60:24ebb611f26e 7761 }
yabahiro 60:24ebb611f26e 7762 if(g[3]==0&&g[2]==0) {
yabahiro 60:24ebb611f26e 7763 mode=25;
yabahiro 60:24ebb611f26e 7764 }
yabahiro 60:24ebb611f26e 7765 */
yabahiro 60:24ebb611f26e 7766 mode=25;
yabahiro 60:24ebb611f26e 7767 } else {
yabahiro 60:24ebb611f26e 7768 /*
yabahiro 60:24ebb611f26e 7769 if(g[1]==98&&countss>=40000) {
yabahiro 60:24ebb611f26e 7770 mode=31;
yabahiro 60:24ebb611f26e 7771 fast = 60;
yabahiro 60:24ebb611f26e 7772 normal = 40;
yabahiro 60:24ebb611f26e 7773 slow = 30;
yabahiro 60:24ebb611f26e 7774 countss=0;
yabahiro 60:24ebb611f26e 7775 }
yabahiro 60:24ebb611f26e 7776 */
yabahiro 60:24ebb611f26e 7777 switch(g[1]) {
yabahiro 60:24ebb611f26e 7778 case 98:
yabahiro 60:24ebb611f26e 7779 fast=40;
yabahiro 60:24ebb611f26e 7780 normal=30;
yabahiro 60:24ebb611f26e 7781 slow=25;
yabahiro 60:24ebb611f26e 7782 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 7783 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7784 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 7785 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7786 motor[TIRE_BR].pwm = normal;
yabahiro 60:24ebb611f26e 7787 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7788 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 7789 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7790 break;
yabahiro 60:24ebb611f26e 7791 case 0:
yabahiro 60:24ebb611f26e 7792 motor[TIRE_FR].pwm = fast;
yabahiro 60:24ebb611f26e 7793 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7794 motor[TIRE_FL].pwm = fast;
yabahiro 60:24ebb611f26e 7795 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7796 motor[TIRE_BR].pwm = fast;
yabahiro 60:24ebb611f26e 7797 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7798 motor[TIRE_BL].pwm = fast;
yabahiro 60:24ebb611f26e 7799 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7800 mtc=true;
yabahiro 60:24ebb611f26e 7801 break;
yabahiro 60:24ebb611f26e 7802 case 255:
yabahiro 60:24ebb611f26e 7803 motor[TIRE_FR].pwm = fast;
yabahiro 60:24ebb611f26e 7804 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7805 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 7806 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7807 motor[TIRE_BR].pwm = slow;
yabahiro 60:24ebb611f26e 7808 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7809 motor[TIRE_BL].pwm = fast;
yabahiro 60:24ebb611f26e 7810 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7811 mtc=true;
yabahiro 60:24ebb611f26e 7812 break;
yabahiro 60:24ebb611f26e 7813 case 253:
yabahiro 60:24ebb611f26e 7814 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 7815 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7816 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7817 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7818 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7819 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7820 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 7821 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7822 mtc=true;
yabahiro 60:24ebb611f26e 7823 break;
yabahiro 60:24ebb611f26e 7824 case 254:
yabahiro 60:24ebb611f26e 7825 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 7826 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7827 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7828 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7829 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7830 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7831 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 7832 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7833 mtc=true;
yabahiro 60:24ebb611f26e 7834 break;
yabahiro 60:24ebb611f26e 7835 case 1:
yabahiro 60:24ebb611f26e 7836 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 7837 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7838 motor[TIRE_FL].pwm = fast;
yabahiro 60:24ebb611f26e 7839 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7840 motor[TIRE_BR].pwm = fast;
yabahiro 60:24ebb611f26e 7841 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7842 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 7843 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7844 mtc=true;
yabahiro 60:24ebb611f26e 7845 break;
yabahiro 60:24ebb611f26e 7846 case 3:
yabahiro 60:24ebb611f26e 7847 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7848 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7849 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 7850 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7851 motor[TIRE_BR].pwm = slow;
yabahiro 60:24ebb611f26e 7852 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7853 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7854 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7855 mtc=true;
yabahiro 60:24ebb611f26e 7856 break;
yabahiro 60:24ebb611f26e 7857 case 2:
yabahiro 60:24ebb611f26e 7858 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7859 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7860 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 7861 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7862 motor[TIRE_BR].pwm = normal;
yabahiro 60:24ebb611f26e 7863 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7864 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7865 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7866 mtc=true;
yabahiro 60:24ebb611f26e 7867 break;
yabahiro 60:24ebb611f26e 7868 }
yabahiro 60:24ebb611f26e 7869 if(g[1]!=98&&mtc==true) {
yabahiro 60:24ebb611f26e 7870 switch(g[0]) {
yabahiro 60:24ebb611f26e 7871 case 0:
yabahiro 60:24ebb611f26e 7872 mtc=false;
yabahiro 60:24ebb611f26e 7873 break;
yabahiro 60:24ebb611f26e 7874 case 255:
yabahiro 60:24ebb611f26e 7875 motor[TIRE_FL].pwm += 3;
yabahiro 60:24ebb611f26e 7876 mtc=false;
yabahiro 60:24ebb611f26e 7877 break;
yabahiro 60:24ebb611f26e 7878 case 253:
yabahiro 60:24ebb611f26e 7879 motor[TIRE_FL].pwm += 6;
yabahiro 60:24ebb611f26e 7880 mtc=false;
yabahiro 60:24ebb611f26e 7881 break;
yabahiro 60:24ebb611f26e 7882 case 254:
yabahiro 60:24ebb611f26e 7883 motor[TIRE_FL].pwm += 9;
yabahiro 60:24ebb611f26e 7884 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7885 mtc=false;
yabahiro 60:24ebb611f26e 7886 break;
yabahiro 60:24ebb611f26e 7887 case 1:
yabahiro 60:24ebb611f26e 7888 motor[TIRE_FR].pwm += 3;
yabahiro 60:24ebb611f26e 7889 mtc=false;
yabahiro 60:24ebb611f26e 7890 break;
yabahiro 60:24ebb611f26e 7891 case 3:
yabahiro 60:24ebb611f26e 7892 motor[TIRE_FR].pwm += 6;
yabahiro 60:24ebb611f26e 7893 mtc=false;
yabahiro 60:24ebb611f26e 7894 break;
yabahiro 60:24ebb611f26e 7895 case 2:
yabahiro 60:24ebb611f26e 7896 motor[TIRE_FR].pwm += 9;
yabahiro 60:24ebb611f26e 7897 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7898 mtc=false;
yabahiro 60:24ebb611f26e 7899 break;
yabahiro 60:24ebb611f26e 7900 }
yabahiro 60:24ebb611f26e 7901 }
yabahiro 60:24ebb611f26e 7902 }
yabahiro 60:24ebb611f26e 7903 break;
yabahiro 60:24ebb611f26e 7904 case 25:
yabahiro 60:24ebb611f26e 7905 motor[TIRE_FR].pwm = 100;
yabahiro 60:24ebb611f26e 7906 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 7907 motor[TIRE_FL].pwm = 100;
yabahiro 60:24ebb611f26e 7908 motor[TIRE_FL].dir = BRAKE;
yabahiro 60:24ebb611f26e 7909 motor[TIRE_BR].pwm = 100;
yabahiro 60:24ebb611f26e 7910 motor[TIRE_BR].dir = BRAKE;
yabahiro 60:24ebb611f26e 7911 motor[TIRE_BL].pwm = 100;
yabahiro 60:24ebb611f26e 7912 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 7913 if(UPLS==true||DOLS==true) {
yabahiro 60:24ebb611f26e 7914 if(UPLS==true||DOLS==true) {
yabahiro 60:24ebb611f26e 7915 motor[MOTOR_0].pwm = 200;
yabahiro 60:24ebb611f26e 7916 motor[MOTOR_0].dir = BRAKE;
yabahiro 60:24ebb611f26e 7917 countss = 0;
yabahiro 60:24ebb611f26e 7918 mode=26;
yabahiro 60:24ebb611f26e 7919 }
yabahiro 60:24ebb611f26e 7920 } else {
yabahiro 60:24ebb611f26e 7921 motor[MOTOR_0].pwm = 100;
yabahiro 60:24ebb611f26e 7922 motor[MOTOR_0].dir = BACK;
yabahiro 60:24ebb611f26e 7923 }
yabahiro 60:24ebb611f26e 7924 break;
yabahiro 60:24ebb611f26e 7925 case 26:
yabahiro 60:24ebb611f26e 7926 /*
yabahiro 60:24ebb611f26e 7927 if (g[3] == 0&&countss>=30000) {
yabahiro 60:24ebb611f26e 7928 mode = 40;
yabahiro 60:24ebb611f26e 7929 }
yabahiro 60:24ebb611f26e 7930 */
yabahiro 60:24ebb611f26e 7931 if (g[1] != 99) {
yabahiro 60:24ebb611f26e 7932 mode = 31;
yabahiro 60:24ebb611f26e 7933 }
yabahiro 60:24ebb611f26e 7934 switch(g[4]) {
yabahiro 60:24ebb611f26e 7935 case 98:
yabahiro 60:24ebb611f26e 7936 motor[TIRE_FR].pwm = 60;
yabahiro 60:24ebb611f26e 7937 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7938 motor[TIRE_FL].pwm = 60;
yabahiro 60:24ebb611f26e 7939 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7940 motor[TIRE_BR].pwm = 60;
yabahiro 60:24ebb611f26e 7941 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7942 motor[TIRE_BL].pwm = 60;
yabahiro 60:24ebb611f26e 7943 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7944 break;
yabahiro 60:24ebb611f26e 7945 case 99:
yabahiro 60:24ebb611f26e 7946 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7947 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7948 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7949 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7950 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7951 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7952 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7953 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7954 break;
yabahiro 60:24ebb611f26e 7955 case 0:
yabahiro 60:24ebb611f26e 7956 motor[TIRE_FR].pwm = 60;
yabahiro 60:24ebb611f26e 7957 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7958 motor[TIRE_FL].pwm = 60;
yabahiro 60:24ebb611f26e 7959 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7960 motor[TIRE_BR].pwm = 60;
yabahiro 60:24ebb611f26e 7961 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7962 motor[TIRE_BL].pwm = 60;
yabahiro 60:24ebb611f26e 7963 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7964 break;
yabahiro 60:24ebb611f26e 7965 case 255:
yabahiro 60:24ebb611f26e 7966 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7967 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7968 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7969 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 7970 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 7971 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 7972 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 7973 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 7974 break;
yabahiro 60:24ebb611f26e 7975 case 253:
yabahiro 60:24ebb611f26e 7976 motor[TIRE_FR].pwm = 25;
yabahiro 60:24ebb611f26e 7977 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7978 motor[TIRE_FL].pwm = 60;
yabahiro 60:24ebb611f26e 7979 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7980 motor[TIRE_BR].pwm = 60;
yabahiro 60:24ebb611f26e 7981 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7982 motor[TIRE_BL].pwm = 25;
yabahiro 60:24ebb611f26e 7983 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7984 break;
yabahiro 60:24ebb611f26e 7985 case 254:
yabahiro 60:24ebb611f26e 7986 motor[TIRE_FR].pwm = 40;
yabahiro 60:24ebb611f26e 7987 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 7988 motor[TIRE_FL].pwm = 60;
yabahiro 60:24ebb611f26e 7989 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 7990 motor[TIRE_BR].pwm = 60;
yabahiro 60:24ebb611f26e 7991 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 7992 motor[TIRE_BL].pwm = 40;
yabahiro 60:24ebb611f26e 7993 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 7994 break;
yabahiro 60:24ebb611f26e 7995 case 1:
yabahiro 60:24ebb611f26e 7996 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 7997 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 7998 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 7999 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8000 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8001 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8002 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 8003 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 8004 break;
yabahiro 60:24ebb611f26e 8005 case 3:
yabahiro 60:24ebb611f26e 8006 motor[TIRE_FR].pwm = 60;
yabahiro 60:24ebb611f26e 8007 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8008 motor[TIRE_FL].pwm = 25;
yabahiro 60:24ebb611f26e 8009 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8010 motor[TIRE_BR].pwm = 25;
yabahiro 60:24ebb611f26e 8011 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8012 motor[TIRE_BL].pwm = 60;
yabahiro 60:24ebb611f26e 8013 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8014 break;
yabahiro 60:24ebb611f26e 8015 case 2:
yabahiro 60:24ebb611f26e 8016 motor[TIRE_FR].pwm = 60;
yabahiro 60:24ebb611f26e 8017 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8018 motor[TIRE_FL].pwm = 40;
yabahiro 60:24ebb611f26e 8019 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8020 motor[TIRE_BR].pwm = 40;
yabahiro 60:24ebb611f26e 8021 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8022 motor[TIRE_BL].pwm = 60;
yabahiro 60:24ebb611f26e 8023 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8024 break;
yabahiro 60:24ebb611f26e 8025 }
yabahiro 60:24ebb611f26e 8026 break;
yabahiro 60:24ebb611f26e 8027 case 31:
yabahiro 60:24ebb611f26e 8028 motor[TIRE_FR].pwm = 255;
yabahiro 60:24ebb611f26e 8029 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 8030 motor[TIRE_FL].pwm = 45;
yabahiro 60:24ebb611f26e 8031 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8032 motor[TIRE_BR].pwm = 45;
yabahiro 60:24ebb611f26e 8033 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8034 motor[TIRE_BL].pwm = 255;
yabahiro 60:24ebb611f26e 8035 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 8036 if(g[3]==0) {
yabahiro 60:24ebb611f26e 8037 cross=0;
yabahiro 60:24ebb611f26e 8038 countss=0;
yabahiro 60:24ebb611f26e 8039 mode=40;
yabahiro 60:24ebb611f26e 8040 fast = 70;
yabahiro 60:24ebb611f26e 8041 normal = 50;
yabahiro 60:24ebb611f26e 8042 slow = 30;
yabahiro 60:24ebb611f26e 8043 }
yabahiro 60:24ebb611f26e 8044 break;
yabahiro 60:24ebb611f26e 8045 case 40:
yabahiro 60:24ebb611f26e 8046 if(YELLOW) {
yabahiro 60:24ebb611f26e 8047 if(cross==1) {
yabahiro 60:24ebb611f26e 8048 mode=41;
yabahiro 60:24ebb611f26e 8049 countss=0;
yabahiro 60:24ebb611f26e 8050 cross=0;
yabahiro 60:24ebb611f26e 8051 }
yabahiro 60:24ebb611f26e 8052 } else {
yabahiro 60:24ebb611f26e 8053 if(cross==3) {
yabahiro 60:24ebb611f26e 8054 fast = 60;
yabahiro 60:24ebb611f26e 8055 normal = 40;
yabahiro 60:24ebb611f26e 8056 slow = 30;
yabahiro 60:24ebb611f26e 8057 mode=41;
yabahiro 60:24ebb611f26e 8058 countss=0;
yabahiro 60:24ebb611f26e 8059 cross=0;
yabahiro 60:24ebb611f26e 8060 }
yabahiro 60:24ebb611f26e 8061 }
yabahiro 60:24ebb611f26e 8062 /*
yabahiro 60:24ebb611f26e 8063 if(LIF||LIB) {
yabahiro 60:24ebb611f26e 8064 mode=42;
yabahiro 60:24ebb611f26e 8065 }
yabahiro 60:24ebb611f26e 8066 */
yabahiro 60:24ebb611f26e 8067 switch(g[3]) {
yabahiro 60:24ebb611f26e 8068 case 98:
yabahiro 60:24ebb611f26e 8069 motor[TIRE_FR].pwm = normal+15;
yabahiro 60:24ebb611f26e 8070 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8071 motor[TIRE_FL].pwm = normal+15;
yabahiro 60:24ebb611f26e 8072 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8073 motor[TIRE_BR].pwm = normal+15;
yabahiro 60:24ebb611f26e 8074 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8075 motor[TIRE_BL].pwm = normal+15;
yabahiro 60:24ebb611f26e 8076 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8077 if(countss>30000) {
yabahiro 60:24ebb611f26e 8078 cross++;
yabahiro 60:24ebb611f26e 8079 countss=0;
yabahiro 60:24ebb611f26e 8080 }
yabahiro 60:24ebb611f26e 8081 mtc2=true;
yabahiro 60:24ebb611f26e 8082 break;
yabahiro 60:24ebb611f26e 8083 case 0:
yabahiro 60:24ebb611f26e 8084 motor[TIRE_FR].pwm = fast+15;
yabahiro 60:24ebb611f26e 8085 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8086 motor[TIRE_FL].pwm = fast+15;
yabahiro 60:24ebb611f26e 8087 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8088 motor[TIRE_BR].pwm = fast+15;
yabahiro 60:24ebb611f26e 8089 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8090 motor[TIRE_BL].pwm = fast+15;
yabahiro 60:24ebb611f26e 8091 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8092 mtc2=true;
yabahiro 60:24ebb611f26e 8093 break;
yabahiro 60:24ebb611f26e 8094 case 255:
yabahiro 60:24ebb611f26e 8095 motor[TIRE_FR].pwm = slow+15;
yabahiro 60:24ebb611f26e 8096 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8097 motor[TIRE_FL].pwm = fast+15;
yabahiro 60:24ebb611f26e 8098 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8099 motor[TIRE_BR].pwm = fast+15;
yabahiro 60:24ebb611f26e 8100 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8101 motor[TIRE_BL].pwm = slow+15;
yabahiro 60:24ebb611f26e 8102 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8103 mtc2=true;
yabahiro 60:24ebb611f26e 8104 break;
yabahiro 60:24ebb611f26e 8105 case 253:
yabahiro 60:24ebb611f26e 8106 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8107 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 8108 motor[TIRE_FL].pwm = slow+15;
yabahiro 60:24ebb611f26e 8109 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8110 motor[TIRE_BR].pwm = slow+15;
yabahiro 60:24ebb611f26e 8111 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8112 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 8113 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 8114 mtc2=true;
yabahiro 60:24ebb611f26e 8115 break;
yabahiro 60:24ebb611f26e 8116 case 254:
yabahiro 60:24ebb611f26e 8117 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8118 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 8119 motor[TIRE_FL].pwm = normal+15;
yabahiro 60:24ebb611f26e 8120 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8121 motor[TIRE_BR].pwm = normal+15;
yabahiro 60:24ebb611f26e 8122 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8123 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 8124 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 8125 mtc2=true;
yabahiro 60:24ebb611f26e 8126 break;
yabahiro 60:24ebb611f26e 8127 case 1:
yabahiro 60:24ebb611f26e 8128 motor[TIRE_FR].pwm = fast+15;
yabahiro 60:24ebb611f26e 8129 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8130 motor[TIRE_FL].pwm = slow+15;
yabahiro 60:24ebb611f26e 8131 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8132 motor[TIRE_BR].pwm = slow+15;
yabahiro 60:24ebb611f26e 8133 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8134 motor[TIRE_BL].pwm = fast+15;
yabahiro 60:24ebb611f26e 8135 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8136 mtc2=true;
yabahiro 60:24ebb611f26e 8137 break;
yabahiro 60:24ebb611f26e 8138 case 3:
yabahiro 60:24ebb611f26e 8139 motor[TIRE_FR].pwm = slow+15;
yabahiro 60:24ebb611f26e 8140 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8141 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8142 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8143 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8144 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8145 motor[TIRE_BL].pwm = slow+15;
yabahiro 60:24ebb611f26e 8146 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8147 mtc2=true;
yabahiro 60:24ebb611f26e 8148 break;
yabahiro 60:24ebb611f26e 8149 case 2:
yabahiro 60:24ebb611f26e 8150 motor[TIRE_FR].pwm = normal+15;
yabahiro 60:24ebb611f26e 8151 motor[TIRE_FR].dir = BACK;
yabahiro 60:24ebb611f26e 8152 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8153 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8154 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8155 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8156 motor[TIRE_BL].pwm = normal+15;
yabahiro 60:24ebb611f26e 8157 motor[TIRE_BL].dir = FOR;
yabahiro 60:24ebb611f26e 8158 mtc2=true;
yabahiro 60:24ebb611f26e 8159 break;
yabahiro 60:24ebb611f26e 8160 }
yabahiro 60:24ebb611f26e 8161 if(g[2]!=98&&mtc2==true) {
yabahiro 60:24ebb611f26e 8162 switch(g[2]) {
yabahiro 60:24ebb611f26e 8163 case 0:
yabahiro 60:24ebb611f26e 8164 mtc2=false;
yabahiro 60:24ebb611f26e 8165 break;
yabahiro 60:24ebb611f26e 8166 case 255:
yabahiro 60:24ebb611f26e 8167 motor[TIRE_FR].pwm += 3;
yabahiro 60:24ebb611f26e 8168 mtc2=false;
yabahiro 60:24ebb611f26e 8169 break;
yabahiro 60:24ebb611f26e 8170 case 253:
yabahiro 60:24ebb611f26e 8171 motor[TIRE_FR].pwm += 6;
yabahiro 60:24ebb611f26e 8172 mtc2=false;
yabahiro 60:24ebb611f26e 8173 break;
yabahiro 60:24ebb611f26e 8174 case 254:
yabahiro 60:24ebb611f26e 8175 motor[TIRE_FR].pwm += 9;
yabahiro 60:24ebb611f26e 8176 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8177 mtc2=false;
yabahiro 60:24ebb611f26e 8178 break;
yabahiro 60:24ebb611f26e 8179 case 1:
yabahiro 60:24ebb611f26e 8180 motor[TIRE_BR].pwm += 3;
yabahiro 60:24ebb611f26e 8181 mtc2=false;
yabahiro 60:24ebb611f26e 8182 break;
yabahiro 60:24ebb611f26e 8183 case 3:
yabahiro 60:24ebb611f26e 8184 motor[TIRE_BR].pwm += 6;
yabahiro 60:24ebb611f26e 8185 mtc2=false;
yabahiro 60:24ebb611f26e 8186 break;
yabahiro 60:24ebb611f26e 8187 case 2:
yabahiro 60:24ebb611f26e 8188 motor[TIRE_BR].pwm += 9;
yabahiro 60:24ebb611f26e 8189 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8190 mtc2=false;
yabahiro 60:24ebb611f26e 8191 break;
yabahiro 60:24ebb611f26e 8192 }
yabahiro 60:24ebb611f26e 8193 }
yabahiro 60:24ebb611f26e 8194 break;
yabahiro 60:24ebb611f26e 8195 case 41:
yabahiro 60:24ebb611f26e 8196 /*
yabahiro 60:24ebb611f26e 8197 if(LIF||LIB) {
yabahiro 60:24ebb611f26e 8198 mode=42;
yabahiro 60:24ebb611f26e 8199 }
yabahiro 60:24ebb611f26e 8200 */
yabahiro 60:24ebb611f26e 8201 motor[TIRE_FR].pwm = 255;
yabahiro 60:24ebb611f26e 8202 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 8203 motor[TIRE_FL].pwm = 30;
yabahiro 60:24ebb611f26e 8204 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8205 motor[TIRE_BR].pwm = 30;
yabahiro 60:24ebb611f26e 8206 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8207 motor[TIRE_BL].pwm = 255;
yabahiro 60:24ebb611f26e 8208 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 8209 if(g[1]==0) {
yabahiro 60:24ebb611f26e 8210 countss=0;
yabahiro 60:24ebb611f26e 8211 mode=70;
yabahiro 60:24ebb611f26e 8212 normal = 120;
yabahiro 60:24ebb611f26e 8213 fast = 140;
yabahiro 60:24ebb611f26e 8214 slow = 100;
yabahiro 60:24ebb611f26e 8215 }
yabahiro 60:24ebb611f26e 8216 break;
yabahiro 60:24ebb611f26e 8217 case 42:
yabahiro 60:24ebb611f26e 8218 motor[TIRE_FR].pwm = 40;
yabahiro 60:24ebb611f26e 8219 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8220 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8221 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8222 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8223 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8224 motor[TIRE_BL].pwm = 40;
yabahiro 60:24ebb611f26e 8225 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8226 if(g[1]==0) {
yabahiro 60:24ebb611f26e 8227 countss=0;
yabahiro 60:24ebb611f26e 8228 mode=70;
yabahiro 60:24ebb611f26e 8229 }
yabahiro 60:24ebb611f26e 8230 break;
yabahiro 60:24ebb611f26e 8231 case 70:
yabahiro 60:24ebb611f26e 8232 switch(g[1]) {
yabahiro 60:24ebb611f26e 8233 case 99:
yabahiro 60:24ebb611f26e 8234 if(countss>=100000) {
yabahiro 60:24ebb611f26e 8235 mode=100;
yabahiro 60:24ebb611f26e 8236 }
yabahiro 60:24ebb611f26e 8237 break;
yabahiro 60:24ebb611f26e 8238 case 98:
yabahiro 60:24ebb611f26e 8239 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 8240 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8241 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 8242 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8243 motor[TIRE_BR].pwm = normal+5;
yabahiro 60:24ebb611f26e 8244 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8245 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 8246 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8247 break;
yabahiro 60:24ebb611f26e 8248 case 0:
yabahiro 60:24ebb611f26e 8249 motor[TIRE_FR].pwm = 100;
yabahiro 60:24ebb611f26e 8250 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8251 motor[TIRE_FL].pwm = 100;
yabahiro 60:24ebb611f26e 8252 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8253 motor[TIRE_BR].pwm = 105;
yabahiro 60:24ebb611f26e 8254 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8255 motor[TIRE_BL].pwm = 100;
yabahiro 60:24ebb611f26e 8256 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8257 mtc=true;
yabahiro 60:24ebb611f26e 8258 break;
yabahiro 60:24ebb611f26e 8259 case 255:
yabahiro 60:24ebb611f26e 8260 motor[TIRE_FR].pwm = fast;
yabahiro 60:24ebb611f26e 8261 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8262 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 8263 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8264 motor[TIRE_BR].pwm = slow+5;
yabahiro 60:24ebb611f26e 8265 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8266 motor[TIRE_BL].pwm = fast;
yabahiro 60:24ebb611f26e 8267 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8268 mtc=true;
yabahiro 60:24ebb611f26e 8269 break;
yabahiro 60:24ebb611f26e 8270 case 253:
yabahiro 60:24ebb611f26e 8271 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 8272 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8273 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8274 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8275 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8276 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8277 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 8278 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8279 mtc=true;
yabahiro 60:24ebb611f26e 8280 break;
yabahiro 60:24ebb611f26e 8281 case 254:
yabahiro 60:24ebb611f26e 8282 motor[TIRE_FR].pwm = normal;
yabahiro 60:24ebb611f26e 8283 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8284 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8285 motor[TIRE_FL].dir = FREE;
yabahiro 60:24ebb611f26e 8286 motor[TIRE_BR].pwm = 0;
yabahiro 60:24ebb611f26e 8287 motor[TIRE_BR].dir = FREE;
yabahiro 60:24ebb611f26e 8288 motor[TIRE_BL].pwm = normal;
yabahiro 60:24ebb611f26e 8289 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8290 mtc=true;
yabahiro 60:24ebb611f26e 8291 break;
yabahiro 60:24ebb611f26e 8292 case 1:
yabahiro 60:24ebb611f26e 8293 motor[TIRE_FR].pwm = slow;
yabahiro 60:24ebb611f26e 8294 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8295 motor[TIRE_FL].pwm = fast;
yabahiro 60:24ebb611f26e 8296 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8297 motor[TIRE_BR].pwm = fast+5;
yabahiro 60:24ebb611f26e 8298 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8299 motor[TIRE_BL].pwm = slow;
yabahiro 60:24ebb611f26e 8300 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8301 mtc=true;
yabahiro 60:24ebb611f26e 8302 break;
yabahiro 60:24ebb611f26e 8303 case 3:
yabahiro 60:24ebb611f26e 8304 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8305 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 8306 motor[TIRE_FL].pwm = slow;
yabahiro 60:24ebb611f26e 8307 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8308 motor[TIRE_BR].pwm = slow+5;
yabahiro 60:24ebb611f26e 8309 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8310 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 8311 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 8312 mtc=true;
yabahiro 60:24ebb611f26e 8313 break;
yabahiro 60:24ebb611f26e 8314 case 2:
yabahiro 60:24ebb611f26e 8315 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8316 motor[TIRE_FR].dir = FREE;
yabahiro 60:24ebb611f26e 8317 motor[TIRE_FL].pwm = normal;
yabahiro 60:24ebb611f26e 8318 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8319 motor[TIRE_BR].pwm = normal+5;
yabahiro 60:24ebb611f26e 8320 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8321 motor[TIRE_BL].pwm = 0;
yabahiro 60:24ebb611f26e 8322 motor[TIRE_BL].dir = FREE;
yabahiro 60:24ebb611f26e 8323 mtc=true;
yabahiro 60:24ebb611f26e 8324 break;
yabahiro 60:24ebb611f26e 8325 }
yabahiro 60:24ebb611f26e 8326 if(g[1]!=98&&mtc==true) {
yabahiro 60:24ebb611f26e 8327 switch(g[0]) {
yabahiro 60:24ebb611f26e 8328 case 0:
yabahiro 60:24ebb611f26e 8329 mtc=false;
yabahiro 60:24ebb611f26e 8330 break;
yabahiro 60:24ebb611f26e 8331 case 255:
yabahiro 60:24ebb611f26e 8332 motor[TIRE_FL].pwm += 3;
yabahiro 60:24ebb611f26e 8333 mtc=false;
yabahiro 60:24ebb611f26e 8334 break;
yabahiro 60:24ebb611f26e 8335 case 253:
yabahiro 60:24ebb611f26e 8336 motor[TIRE_FL].pwm += 6;
yabahiro 60:24ebb611f26e 8337 mtc=false;
yabahiro 60:24ebb611f26e 8338 break;
yabahiro 60:24ebb611f26e 8339 case 254:
yabahiro 60:24ebb611f26e 8340 motor[TIRE_FL].pwm += 9;
yabahiro 60:24ebb611f26e 8341 motor[TIRE_FR].pwm = 0;
yabahiro 60:24ebb611f26e 8342 mtc=false;
yabahiro 60:24ebb611f26e 8343 break;
yabahiro 60:24ebb611f26e 8344 case 1:
yabahiro 60:24ebb611f26e 8345 motor[TIRE_FR].pwm += 3;
yabahiro 60:24ebb611f26e 8346 mtc=false;
yabahiro 60:24ebb611f26e 8347 break;
yabahiro 60:24ebb611f26e 8348 case 3:
yabahiro 60:24ebb611f26e 8349 motor[TIRE_FR].pwm += 6;
yabahiro 60:24ebb611f26e 8350 mtc=false;
yabahiro 60:24ebb611f26e 8351 break;
yabahiro 60:24ebb611f26e 8352 case 2:
yabahiro 60:24ebb611f26e 8353 motor[TIRE_FR].pwm += 9;
yabahiro 60:24ebb611f26e 8354 motor[TIRE_FL].pwm = 0;
yabahiro 60:24ebb611f26e 8355 mtc=false;
yabahiro 60:24ebb611f26e 8356 break;
yabahiro 60:24ebb611f26e 8357 }
yabahiro 60:24ebb611f26e 8358 }
yabahiro 60:24ebb611f26e 8359 break;
yabahiro 60:24ebb611f26e 8360 case 100:
yabahiro 60:24ebb611f26e 8361 motor[TIRE_FR].pwm = 50;
yabahiro 60:24ebb611f26e 8362 motor[TIRE_FR].dir = FOR;
yabahiro 60:24ebb611f26e 8363 motor[TIRE_FL].pwm = 55;
yabahiro 60:24ebb611f26e 8364 motor[TIRE_FL].dir = BACK;
yabahiro 60:24ebb611f26e 8365 motor[TIRE_BR].pwm = 60;
yabahiro 60:24ebb611f26e 8366 motor[TIRE_BR].dir = FOR;
yabahiro 60:24ebb611f26e 8367 motor[TIRE_BL].pwm = 50;
yabahiro 60:24ebb611f26e 8368 motor[TIRE_BL].dir = BACK;
yabahiro 60:24ebb611f26e 8369 break;
yabahiro 60:24ebb611f26e 8370 default:
yabahiro 60:24ebb611f26e 8371 motor[TIRE_FR].pwm = 50;
yabahiro 60:24ebb611f26e 8372 motor[TIRE_FR].dir = BRAKE;
yabahiro 60:24ebb611f26e 8373 motor[TIRE_FL].pwm = 50;
yabahiro 60:24ebb611f26e 8374 motor[TIRE_FL].dir = BRAKE;
yabahiro 60:24ebb611f26e 8375 motor[TIRE_BR].pwm = 50;
yabahiro 60:24ebb611f26e 8376 motor[TIRE_BR].dir = BRAKE;
yabahiro 60:24ebb611f26e 8377 motor[TIRE_BL].pwm = 50;
yabahiro 60:24ebb611f26e 8378 motor[TIRE_BL].dir = BRAKE;
yabahiro 60:24ebb611f26e 8379 break;
yabahiro 60:24ebb611f26e 8380 }
yabahiro 33:dd598bb23ad1 8381
kishibekairohan 12:c09b3e08a316 8382 }
kishibekairohan 12:c09b3e08a316 8383 #endif
kishibekairohan 12:c09b3e08a316 8384
Ryosei 27:dd9f27fce7d1 8385 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 8386 static void Process8()
kishibekairohan 12:c09b3e08a316 8387 {
yabahiro 33:dd598bb23ad1 8388
kishibekairohan 12:c09b3e08a316 8389 }
kishibekairohan 12:c09b3e08a316 8390 #endif
kishibekairohan 12:c09b3e08a316 8391
kishibekairohan 13:b6e02d6261d7 8392 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 8393 static void Process9()
kishibekairohan 12:c09b3e08a316 8394 {
Ryosei 27:dd9f27fce7d1 8395
t_yamamoto 0:669ef71cba68 8396 }
t_yamamoto 0:669ef71cba68 8397 #endif
t_yamamoto 0:669ef71cba68 8398 #endif
t_yamamoto 0:669ef71cba68 8399 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 8400
t_yamamoto 0:669ef71cba68 8401 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 8402 {
yabahiro 33:dd598bb23ad1 8403 #ifdef USE_SOLENOID
yabahiro 33:dd598bb23ad1 8404 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 8405 #endif
yabahiro 33:dd598bb23ad1 8406
yabahiro 33:dd598bb23ad1 8407 #ifdef USE_MOTOR
yabahiro 33:dd598bb23ad1 8408 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 33:dd598bb23ad1 8409 motor[i].dir = FREE;
yabahiro 33:dd598bb23ad1 8410 motor[i].pwm = 0;
yabahiro 33:dd598bb23ad1 8411 }
yabahiro 33:dd598bb23ad1 8412 #endif
t_yamamoto 0:669ef71cba68 8413 }
t_yamamoto 0:669ef71cba68 8414
kishibekairohan 16:3f2c2d89372b 8415 #pragma region USER-DEFINED-FUNCTIONS
M_souta 29:44d5454ce8fa 8416
M_souta 29:44d5454ce8fa 8417 uint8_t SetStatus(int pwmVal)
Ryosei 27:dd9f27fce7d1 8418 {
M_souta 29:44d5454ce8fa 8419 if (pwmVal < 0) return BACK;
M_souta 29:44d5454ce8fa 8420 else if (pwmVal > 0) return FOR;
M_souta 29:44d5454ce8fa 8421 else if (pwmVal == 0) return BRAKE;
M_souta 29:44d5454ce8fa 8422 else return BRAKE;
M_souta 22:7d93f79a3686 8423 }
kishibekairohan 16:3f2c2d89372b 8424
M_souta 29:44d5454ce8fa 8425 uint8_t SetPWM(int pwmVal)
Ryosei 27:dd9f27fce7d1 8426 {
M_souta 29:44d5454ce8fa 8427 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 29:44d5454ce8fa 8428 else return abs(pwmVal);
M_souta 22:7d93f79a3686 8429 }
yabahiro 55:844ac0974740 8430 /*
yabahiro 55:844ac0974740 8431 void Count(){
yabahiro 55:844ac0974740 8432 num++;
yabahiro 55:844ac0974740 8433 }
yabahiro 55:844ac0974740 8434 */
t_yamamoto 0:669ef71cba68 8435 #pragma endregion