大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@29:c78a46f51209, 2019-09-19 (annotated)

Committer:
yabahiro
Date:
Thu Sep 19 05:42:23 2019 +0000
Revision:
29:c78a46f51209
Parent:
28:9cd36721cf83
Child:
30:9450dc1d53cc
asdsa

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