大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@32:90f018b10ad6, 2019-09-21 (annotated)

Committer:
kishibekairohan
Date:
Sat Sep 21 11:28:06 2019 +0000
Revision:
32:90f018b10ad6
Parent:
31:45f97698857b
Child:
33:656aa14904a5
help me;

Who changed what in which revision?

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