大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_20

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@30:9450dc1d53cc, 2019-09-20 (annotated)

Committer:
yabahiro
Date:
Fri Sep 20 02:47:02 2019 +0000
Revision:
30:9450dc1d53cc
Parent:
29:c78a46f51209 
aa

Who changed what in which revision?

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