大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@33:656aa14904a5, 2019-09-21 (annotated)

Committer:
kishibekairohan
Date:
Sat Sep 21 13:14:12 2019 +0000
Revision:
33:656aa14904a5
Parent:
32:90f018b10ad6
Child:
34:cef6c124a52d
aaa

Who changed what in which revision?

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