大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@31:45f97698857b, 2019-09-21 (annotated)

Committer:
yabahiro
Date:
Sat Sep 21 04:28:31 2019 +0000
Revision:
31:45f97698857b
Parent:
30:9450dc1d53cc
Child:
32:90f018b10ad6
aa;

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