大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_18_ugoku

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@27:545148741493, 2019-09-18 (annotated)

Committer:
yabahiro
Date:
Wed Sep 18 03:07:18 2019 +0000
Revision:
27:545148741493
Parent:
26:be5121da0f77 
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
yabahiro 21:1f1e9c585da8 8 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
kishibekairohan 16:3f2c2d89372b 13 #include "../../Input/Encoder/Encoder.h"
t_yamamoto 0:669ef71cba68 14 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 15 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 16 #include "../Using.h"
t_yamamoto 0:669ef71cba68 17
t_yamamoto 0:669ef71cba68 18 using namespace SWITCH;
7ka884 4:ba9df71868df 19 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 20 using namespace ENCODER;
yabahiro 21:1f1e9c585da8 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 24 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static bool lock;
t_yamamoto 0:669ef71cba68 28 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 29 static int current;
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 34 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 35 #endif
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 40
kishibekairohan 2:c015739085d3 41 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 42
kishibekairohan 16:3f2c2d89372b 43 //**************Encoder***************
kishibekairohan 16:3f2c2d89372b 44 const int PerRev = 256;
kishibekairohan 16:3f2c2d89372b 45 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 46 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 47 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 48 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 49 //**************Encoder***************
kishibekairohan 7:e88c5d47a3be 50
kishibekairohan 16:3f2c2d89372b 51 //**************Buzzer****************
kishibekairohan 11:028a150943b5 52 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 53 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 54 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 55 bool EMGflag = false;
yabahiro 21:1f1e9c585da8 56 //PWMOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 57 //**************Buzzer****************
kishibekairohan 11:028a150943b5 58
kishibekairohan 11:028a150943b5 59 //************TapeLed*****************
kishibekairohan 11:028a150943b5 60 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 61 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 62 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 63 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 64 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 65 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 66
M_souta 18:c694bae76e51 67 float tireProRPM[4];
M_souta 18:c694bae76e51 68 float tireTarRPM[4];
yabahiro 21:1f1e9c585da8 69 float tirepwm[4];
yabahiro 21:1f1e9c585da8 70
yabahiro 21:1f1e9c585da8 71 const int omni[15][15] = {
yabahiro 21:1f1e9c585da8 72 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 73 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 74 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 75 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
yabahiro 21:1f1e9c585da8 76 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
yabahiro 21:1f1e9c585da8 77 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
yabahiro 21:1f1e9c585da8 78 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
yabahiro 21:1f1e9c585da8 79 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
yabahiro 21:1f1e9c585da8 80 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
yabahiro 21:1f1e9c585da8 81 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
yabahiro 21:1f1e9c585da8 82 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
yabahiro 21:1f1e9c585da8 83 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
yabahiro 21:1f1e9c585da8 84 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
yabahiro 21:1f1e9c585da8 85 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
yabahiro 21:1f1e9c585da8 86 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
yabahiro 21:1f1e9c585da8 87 };
yabahiro 21:1f1e9c585da8 88
yabahiro 21:1f1e9c585da8 89 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
yabahiro 21:1f1e9c585da8 90 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
yabahiro 21:1f1e9c585da8 91
yabahiro 21:1f1e9c585da8 92 uint8_t SetStatus(int);
yabahiro 21:1f1e9c585da8 93 uint8_t SetStatus(int pwmVal)
yabahiro 21:1f1e9c585da8 94 {
yabahiro 21:1f1e9c585da8 95 if (pwmVal < 0) return BACK;
yabahiro 21:1f1e9c585da8 96 else if (pwmVal > 0) return FOR;
yabahiro 21:1f1e9c585da8 97 else if (pwmVal == 0) return BRAKE;
yabahiro 21:1f1e9c585da8 98 else return BRAKE;
yabahiro 21:1f1e9c585da8 99 }
yabahiro 21:1f1e9c585da8 100 uint8_t Setpwm(int);
yabahiro 21:1f1e9c585da8 101 uint8_t Setpwm(int pwmVal)
yabahiro 21:1f1e9c585da8 102 {
yabahiro 21:1f1e9c585da8 103 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
yabahiro 21:1f1e9c585da8 104 else return abs(pwmVal);
yabahiro 21:1f1e9c585da8 105 }
M_souta 18:c694bae76e51 106
t_yamamoto 0:669ef71cba68 107 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 108
t_yamamoto 0:669ef71cba68 109 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 110 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 111 static void Process0(void);
t_yamamoto 0:669ef71cba68 112 #endif
t_yamamoto 0:669ef71cba68 113 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 114 static void Process1(void);
t_yamamoto 0:669ef71cba68 115 #endif
t_yamamoto 0:669ef71cba68 116 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 117 static void Process2(void);
t_yamamoto 0:669ef71cba68 118 #endif
t_yamamoto 0:669ef71cba68 119 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 120 static void Process3(void);
t_yamamoto 0:669ef71cba68 121 #endif
t_yamamoto 0:669ef71cba68 122 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 123 static void Process4(void);
t_yamamoto 0:669ef71cba68 124 #endif
t_yamamoto 0:669ef71cba68 125 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 126 static void Process5(void);
t_yamamoto 0:669ef71cba68 127 #endif
t_yamamoto 0:669ef71cba68 128 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 129 static void Process6(void);
t_yamamoto 0:669ef71cba68 130 #endif
t_yamamoto 0:669ef71cba68 131 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 132 static void Process7(void);
t_yamamoto 0:669ef71cba68 133 #endif
t_yamamoto 0:669ef71cba68 134 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 135 static void Process8(void);
t_yamamoto 0:669ef71cba68 136 #endif
t_yamamoto 0:669ef71cba68 137 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 138 static void Process9(void);
t_yamamoto 0:669ef71cba68 139 #endif
t_yamamoto 0:669ef71cba68 140 #endif
t_yamamoto 0:669ef71cba68 141
t_yamamoto 0:669ef71cba68 142 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 143 {
yabahiro 21:1f1e9c585da8 144 #pragma region USER-DEFINED_VARIABLE_INIT
yabahiro 21:1f1e9c585da8 145 /*Replace here with the initialization code of your variables.*/
yabahiro 21:1f1e9c585da8 146 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 147
yabahiro 21:1f1e9c585da8 148 lock = true;
yabahiro 21:1f1e9c585da8 149 processChangeComp = true;
yabahiro 21:1f1e9c585da8 150 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 151
yabahiro 21:1f1e9c585da8 152 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 153 #if USE_PROCESS_NUM>0
yabahiro 21:1f1e9c585da8 154 Process[0] = Process0;
yabahiro 21:1f1e9c585da8 155 #endif
yabahiro 21:1f1e9c585da8 156 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 157 Process[1] = Process1;
yabahiro 21:1f1e9c585da8 158 #endif
yabahiro 21:1f1e9c585da8 159 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 160 Process[2] = Process2;
yabahiro 21:1f1e9c585da8 161 #endif
yabahiro 21:1f1e9c585da8 162 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 163 Process[3] = Process3;
yabahiro 21:1f1e9c585da8 164 #endif
yabahiro 21:1f1e9c585da8 165 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 166 Process[4] = Process4;
yabahiro 21:1f1e9c585da8 167 #endif
yabahiro 21:1f1e9c585da8 168 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 169 Process[5] = Process5;
yabahiro 21:1f1e9c585da8 170 #endif
yabahiro 21:1f1e9c585da8 171 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 172 Process[6] = Process6;
yabahiro 21:1f1e9c585da8 173 #endif
yabahiro 21:1f1e9c585da8 174 #if USE_PROCESS_NUM>7
yabahiro 21:1f1e9c585da8 175 Process[7] = Process7;
yabahiro 21:1f1e9c585da8 176 #endif
yabahiro 21:1f1e9c585da8 177 #if USE_PROCESS_NUM>8
yabahiro 21:1f1e9c585da8 178 Process[8] = Process8;
yabahiro 21:1f1e9c585da8 179 #endif
yabahiro 21:1f1e9c585da8 180 #if USE_PROCESS_NUM>9
yabahiro 21:1f1e9c585da8 181 Process[9] = Process9;
yabahiro 21:1f1e9c585da8 182 #endif
yabahiro 21:1f1e9c585da8 183 #endif
t_yamamoto 0:669ef71cba68 184 }
t_yamamoto 0:669ef71cba68 185
t_yamamoto 0:669ef71cba68 186 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 187 {
yabahiro 21:1f1e9c585da8 188 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 189 if(controller->Button.HOME) lock = false;
yabahiro 21:1f1e9c585da8 190
yabahiro 21:1f1e9c585da8 191 if(controller->Button.START && processChangeComp) {
yabahiro 21:1f1e9c585da8 192 current++;
yabahiro 21:1f1e9c585da8 193 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
yabahiro 21:1f1e9c585da8 194 processChangeComp = false;
yabahiro 21:1f1e9c585da8 195 } else if(controller->Button.SELECT && processChangeComp) {
yabahiro 21:1f1e9c585da8 196 current--;
yabahiro 21:1f1e9c585da8 197 if (current < 0) current = 0;
yabahiro 21:1f1e9c585da8 198 processChangeComp = false;
yabahiro 21:1f1e9c585da8 199 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
yabahiro 21:1f1e9c585da8 200 #endif
yabahiro 21:1f1e9c585da8 201
yabahiro 21:1f1e9c585da8 202 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 203 ACTUATORHUB::MOTOR::Motor::Update(motor);
yabahiro 21:1f1e9c585da8 204 #endif
yabahiro 21:1f1e9c585da8 205
yabahiro 21:1f1e9c585da8 206 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 207 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
yabahiro 21:1f1e9c585da8 208 #endif
yabahiro 21:1f1e9c585da8 209
yabahiro 21:1f1e9c585da8 210 #ifdef USE_RS485
yabahiro 21:1f1e9c585da8 211 ACTUATORHUB::ActuatorHub::Update();
yabahiro 21:1f1e9c585da8 212 #endif
yabahiro 21:1f1e9c585da8 213
yabahiro 21:1f1e9c585da8 214 }
yabahiro 21:1f1e9c585da8 215
yabahiro 21:1f1e9c585da8 216 int g[8];
yabahiro 21:1f1e9c585da8 217
yabahiro 21:1f1e9c585da8 218 void SystemProcess()
yabahiro 21:1f1e9c585da8 219 {
yabahiro 21:1f1e9c585da8 220 SystemProcessInitialize();
yabahiro 21:1f1e9c585da8 221
yabahiro 21:1f1e9c585da8 222 while(1) {
yabahiro 21:1f1e9c585da8 223 for(int i = 0; i < 8; i++) {
yabahiro 21:1f1e9c585da8 224 g[i] = LineHub::GetPara(i);
yabahiro 21:1f1e9c585da8 225 }
t_yamamoto 0:669ef71cba68 226
yabahiro 21:1f1e9c585da8 227 #ifdef USE_MU
yabahiro 21:1f1e9c585da8 228 controller = CONTROLLER::Controller::GetData();
yabahiro 21:1f1e9c585da8 229 #endif
yabahiro 21:1f1e9c585da8 230
yabahiro 21:1f1e9c585da8 231 #ifdef USE_ERRORCHECK
yabahiro 21:1f1e9c585da8 232 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
yabahiro 21:1f1e9c585da8 233 CONTROLLER::Controller::DataReset();
yabahiro 21:1f1e9c585da8 234 AllActuatorReset();
yabahiro 21:1f1e9c585da8 235 lock = true;
yabahiro 21:1f1e9c585da8 236 } else
yabahiro 21:1f1e9c585da8 237 #endif
yabahiro 21:1f1e9c585da8 238 {
yabahiro 21:1f1e9c585da8 239
yabahiro 21:1f1e9c585da8 240 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 241 if(!lock) {
yabahiro 21:1f1e9c585da8 242 Process[current]();
yabahiro 21:1f1e9c585da8 243 } else
yabahiro 21:1f1e9c585da8 244 #endif
yabahiro 21:1f1e9c585da8 245 {
yabahiro 21:1f1e9c585da8 246 //ロック時の処理
yabahiro 21:1f1e9c585da8 247 }
yabahiro 21:1f1e9c585da8 248 }
yabahiro 21:1f1e9c585da8 249 /*
yabahiro 21:1f1e9c585da8 250 //Emergency!
yabahiro 21:1f1e9c585da8 251 if(!EMG_0 && !EMG_1 && !EMGflag) {
yabahiro 21:1f1e9c585da8 252 buzzer = 0;
yabahiro 21:1f1e9c585da8 253 BuzzerTimer.attach(BuzzerTimer_func, 1);
yabahiro 21:1f1e9c585da8 254 EMGflag = true;
yabahiro 21:1f1e9c585da8 255 LED_DEBUG0 = 1;
yabahiro 21:1f1e9c585da8 256 }
yabahiro 21:1f1e9c585da8 257 if(EMG_0 && EMG_1 && EMGflag) {
yabahiro 21:1f1e9c585da8 258 buzzer = 1;
yabahiro 21:1f1e9c585da8 259 BuzzerTimer.detach();
yabahiro 21:1f1e9c585da8 260 EMGflag = false;
yabahiro 21:1f1e9c585da8 261 }
yabahiro 21:1f1e9c585da8 262 */
yabahiro 21:1f1e9c585da8 263 SystemProcessUpdate();
yabahiro 21:1f1e9c585da8 264 }
t_yamamoto 0:669ef71cba68 265 }
t_yamamoto 0:669ef71cba68 266
kishibekairohan 2:c015739085d3 267
yabahiro 25:38d4964e8aad 268 int Twsh;
kishibekairohan 2:c015739085d3 269
t_yamamoto 0:669ef71cba68 270 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 271 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 272 #if USE_PROCESS_NUM>0
yabahiro 27:545148741493 273
yabahiro 21:1f1e9c585da8 274 static void Process0()
yabahiro 21:1f1e9c585da8 275 {
yabahiro 25:38d4964e8aad 276 if(StertSW) {
yabahiro 25:38d4964e8aad 277 if(ZoneSW) {
yabahiro 25:38d4964e8aad 278 current = 1;
yabahiro 25:38d4964e8aad 279 Twsh=2;
yabahiro 27:545148741493 280 } else {
yabahiro 25:38d4964e8aad 281 current = 1;
yabahiro 25:38d4964e8aad 282 Twsh=3;
yabahiro 25:38d4964e8aad 283 }
yabahiro 25:38d4964e8aad 284 }
t_yamamoto 0:669ef71cba68 285 }
t_yamamoto 0:669ef71cba68 286 #endif
t_yamamoto 0:669ef71cba68 287
t_yamamoto 0:669ef71cba68 288 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 289
yabahiro 21:1f1e9c585da8 290 bool dz1=true;
yabahiro 21:1f1e9c585da8 291 bool dz1i=false;
yabahiro 21:1f1e9c585da8 292 bool dz2=true;
yabahiro 21:1f1e9c585da8 293 bool dz2i=false;
yabahiro 21:1f1e9c585da8 294 bool dz3=true;
yabahiro 21:1f1e9c585da8 295 bool dz3i=false;
yabahiro 21:1f1e9c585da8 296 bool dz4=true;
yabahiro 21:1f1e9c585da8 297 bool dz4i=false;
yabahiro 21:1f1e9c585da8 298
yabahiro 27:545148741493 299 int mode =1;
yabahiro 27:545148741493 300 int cross=0;//十字数える用
yabahiro 27:545148741493 301 int count=100000;//wait代替え
yabahiro 21:1f1e9c585da8 302
kishibekairohan 22:c47f29caded6 303
kishibekairohan 16:3f2c2d89372b 304 static void Process1()
t_yamamoto 0:669ef71cba68 305 {
yabahiro 27:545148741493 306 count=100000;
yabahiro 25:38d4964e8aad 307 mode=1;
yabahiro 27:545148741493 308 cross=0;
yabahiro 21:1f1e9c585da8 309 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 310 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 311 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 312 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 313
yabahiro 21:1f1e9c585da8 314 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 315 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 316 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 317 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 318
yabahiro 21:1f1e9c585da8 319 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 320
yabahiro 21:1f1e9c585da8 321 if(controller->Button.UP) {
yabahiro 21:1f1e9c585da8 322 motor[MOTOR_0].pwm = 110;
yabahiro 21:1f1e9c585da8 323 motor[MOTOR_0].dir = FOR;
yabahiro 21:1f1e9c585da8 324 }
yabahiro 21:1f1e9c585da8 325 if(controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 326 motor[MOTOR_0].pwm = 50;
yabahiro 21:1f1e9c585da8 327 motor[MOTOR_0].dir = BACK;
yabahiro 21:1f1e9c585da8 328 }
yabahiro 21:1f1e9c585da8 329 } else {
yabahiro 21:1f1e9c585da8 330 motor[MOTOR_0].pwm = 0;
yabahiro 21:1f1e9c585da8 331 motor[MOTOR_0].dir = BRAKE;
yabahiro 21:1f1e9c585da8 332 }
yabahiro 21:1f1e9c585da8 333 if(controller->Button.X) {
yabahiro 21:1f1e9c585da8 334 if(dz1==true) {
yabahiro 21:1f1e9c585da8 335 if(dz1i==false) {
yabahiro 21:1f1e9c585da8 336 solenoid.solenoid1 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 337 dz1i=true;
yabahiro 21:1f1e9c585da8 338 } else {
yabahiro 21:1f1e9c585da8 339 solenoid.solenoid1 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 340 dz1i=false;
yabahiro 21:1f1e9c585da8 341 }
yabahiro 21:1f1e9c585da8 342 dz1=false;
yabahiro 21:1f1e9c585da8 343 }
yabahiro 21:1f1e9c585da8 344 } else {
yabahiro 21:1f1e9c585da8 345 dz1=true;
yabahiro 21:1f1e9c585da8 346 }
yabahiro 21:1f1e9c585da8 347
yabahiro 21:1f1e9c585da8 348 if(controller->Button.Y) {
yabahiro 21:1f1e9c585da8 349 if(dz2==true) {
yabahiro 21:1f1e9c585da8 350 if(dz2i==false) {
yabahiro 21:1f1e9c585da8 351 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 352 dz2i=true;
yabahiro 21:1f1e9c585da8 353 } else {
yabahiro 21:1f1e9c585da8 354 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 355 dz2i=false;
yabahiro 21:1f1e9c585da8 356 }
yabahiro 21:1f1e9c585da8 357 dz2=false;
yabahiro 21:1f1e9c585da8 358 }
yabahiro 21:1f1e9c585da8 359 } else {
yabahiro 21:1f1e9c585da8 360 dz2=true;
yabahiro 21:1f1e9c585da8 361 }
yabahiro 21:1f1e9c585da8 362
yabahiro 21:1f1e9c585da8 363 if(controller->Button.A) {
yabahiro 21:1f1e9c585da8 364 if(dz3==true) {
yabahiro 21:1f1e9c585da8 365 if(dz3i==false) {
yabahiro 21:1f1e9c585da8 366 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 367 dz3i=true;
yabahiro 21:1f1e9c585da8 368 } else {
yabahiro 21:1f1e9c585da8 369 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 370 dz3i=false;
yabahiro 21:1f1e9c585da8 371 }
yabahiro 21:1f1e9c585da8 372 dz3=false;
yabahiro 21:1f1e9c585da8 373 }
yabahiro 21:1f1e9c585da8 374 } else {
yabahiro 21:1f1e9c585da8 375 dz3=true;
yabahiro 21:1f1e9c585da8 376 }
yabahiro 21:1f1e9c585da8 377
yabahiro 21:1f1e9c585da8 378 if(controller->Button.B) {
yabahiro 21:1f1e9c585da8 379 if(dz4==true) {
yabahiro 21:1f1e9c585da8 380 if(dz4i==false) {
yabahiro 21:1f1e9c585da8 381 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 382 dz4i=true;
yabahiro 21:1f1e9c585da8 383 } else {
yabahiro 21:1f1e9c585da8 384 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 385 dz4i=false;
yabahiro 21:1f1e9c585da8 386 }
yabahiro 21:1f1e9c585da8 387 dz4=false;
yabahiro 21:1f1e9c585da8 388 }
yabahiro 21:1f1e9c585da8 389 } else {
yabahiro 21:1f1e9c585da8 390 dz4=true;
yabahiro 21:1f1e9c585da8 391 }
yabahiro 21:1f1e9c585da8 392 /*
yabahiro 21:1f1e9c585da8 393 if(controller->Button.RIGHT){
yabahiro 21:1f1e9c585da8 394 motor[MOTOR_1].dir = FOR;
yabahiro 21:1f1e9c585da8 395 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 396 if (ARM_1){
yabahiro 21:1f1e9c585da8 397 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 398 }
yabahiro 21:1f1e9c585da8 399 }
yabahiro 21:1f1e9c585da8 400 else if(controller->Button.LEFT){
yabahiro 21:1f1e9c585da8 401 motor[MOTOR_1].dir = BACK;
yabahiro 21:1f1e9c585da8 402 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 403 if (ARM_0){
yabahiro 21:1f1e9c585da8 404 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 405 }
yabahiro 21:1f1e9c585da8 406 }
yabahiro 21:1f1e9c585da8 407 */
t_yamamoto 0:669ef71cba68 408 }
t_yamamoto 0:669ef71cba68 409 #endif
t_yamamoto 0:669ef71cba68 410
t_yamamoto 0:669ef71cba68 411 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 412 bool mtc=false;
kishibekairohan 22:c47f29caded6 413 //追加
kishibekairohan 22:c47f29caded6 414 bool mtc2 = false;
yabahiro 21:1f1e9c585da8 415
kishibekairohan 16:3f2c2d89372b 416 static void Process2()
yabahiro 21:1f1e9c585da8 417 {
yabahiro 21:1f1e9c585da8 418 // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
yabahiro 27:545148741493 419 count++;
yabahiro 27:545148741493 420 if(mode==1) {//スタートゾーンから白線検知
kishibekairohan 26:be5121da0f77 421 motor[TIRE_FR].pwm = 30;
yabahiro 25:38d4964e8aad 422 motor[TIRE_FR].dir = BACK;
kishibekairohan 26:be5121da0f77 423 motor[TIRE_FL].pwm = 30;
yabahiro 25:38d4964e8aad 424 motor[TIRE_FL].dir = FOR;
kishibekairohan 26:be5121da0f77 425 motor[TIRE_BR].pwm = 30;
yabahiro 25:38d4964e8aad 426 motor[TIRE_BR].dir = BACK;
kishibekairohan 26:be5121da0f77 427 motor[TIRE_BL].pwm = 30;
yabahiro 25:38d4964e8aad 428 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 429 if(g[2]==0) {
yabahiro 27:545148741493 430 count=100000;
yabahiro 27:545148741493 431 cross=0;
yabahiro 25:38d4964e8aad 432 mode=2;
yabahiro 25:38d4964e8aad 433 }
yabahiro 27:545148741493 434 } else if(mode==2) {//横移動
yabahiro 27:545148741493 435 if(g[2]==0) {
yabahiro 27:545148741493 436 count=100000;
yabahiro 27:545148741493 437 cross=0;
yabahiro 27:545148741493 438 mode=3;
yabahiro 27:545148741493 439 mtc2=true;
yabahiro 27:545148741493 440 } else {
yabahiro 27:545148741493 441 motor[TIRE_FR].pwm = 30;
yabahiro 27:545148741493 442 motor[TIRE_FR].dir = FOR;
yabahiro 27:545148741493 443 motor[TIRE_FL].pwm = 30;
yabahiro 27:545148741493 444 motor[TIRE_FL].dir = FOR;
yabahiro 27:545148741493 445 motor[TIRE_BR].pwm = 30;
yabahiro 27:545148741493 446 motor[TIRE_BR].dir = BACK;
yabahiro 27:545148741493 447 motor[TIRE_BL].pwm = 30;
yabahiro 27:545148741493 448 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 449 }
yabahiro 27:545148741493 450 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 25:38d4964e8aad 451 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 452 motor[TIRE_FR].dir = FREE;
yabahiro 27:545148741493 453 motor[TIRE_FL].pwm = 20;
yabahiro 25:38d4964e8aad 454 motor[TIRE_FL].dir = FOR;
yabahiro 27:545148741493 455 motor[TIRE_BR].pwm = 20;
yabahiro 25:38d4964e8aad 456 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 457 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 458 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 459 if(count>100000&&g[0]==0) {
yabahiro 27:545148741493 460 count=0;
yabahiro 27:545148741493 461 mode=10;
yabahiro 25:38d4964e8aad 462 }
yabahiro 27:545148741493 463 } else if(mode==4) { //横ライントレースから縦ライントレースへ
yabahiro 25:38d4964e8aad 464 motor[TIRE_FR].pwm = 15;
yabahiro 25:38d4964e8aad 465 motor[TIRE_FR].dir = BACK;
yabahiro 25:38d4964e8aad 466 motor[TIRE_FL].pwm = 15;
yabahiro 25:38d4964e8aad 467 motor[TIRE_FL].dir = BACK;
yabahiro 25:38d4964e8aad 468 motor[TIRE_BR].pwm = 15;
yabahiro 25:38d4964e8aad 469 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 470 motor[TIRE_BL].pwm = 15;
yabahiro 25:38d4964e8aad 471 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 472 if(count>30000&&g[0]==0) {
yabahiro 27:545148741493 473 count=0;
yabahiro 27:545148741493 474 cross=0;
yabahiro 25:38d4964e8aad 475 mode=10;
yabahiro 25:38d4964e8aad 476 }
yabahiro 27:545148741493 477 } else if(g[0]!=99&&mode==10) { //縦ライントレース
yabahiro 21:1f1e9c585da8 478 switch(g[0]) {
yabahiro 21:1f1e9c585da8 479 case 98:
yabahiro 21:1f1e9c585da8 480 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 481 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 482 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 483 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 484 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 485 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 486 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 487 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 488 if(count>100000) {
yabahiro 27:545148741493 489 cross++;
yabahiro 27:545148741493 490 count=0;
yabahiro 21:1f1e9c585da8 491 }
yabahiro 21:1f1e9c585da8 492 mtc=true;
yabahiro 27:545148741493 493 if(cross==1) {
yabahiro 23:694ecfa97839 494 mode=11;
yabahiro 27:545148741493 495 count=0;
yabahiro 21:1f1e9c585da8 496 }
yabahiro 21:1f1e9c585da8 497 break;
yabahiro 21:1f1e9c585da8 498 case 0:
yabahiro 21:1f1e9c585da8 499 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 500 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 501 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 502 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 503 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 504 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 505 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 506 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 507 mtc=true;
yabahiro 21:1f1e9c585da8 508 break;
yabahiro 21:1f1e9c585da8 509 case 255:
yabahiro 21:1f1e9c585da8 510 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 511 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 512 motor[TIRE_FL].pwm = 40;
yabahiro 21:1f1e9c585da8 513 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 514 motor[TIRE_BR].pwm = 40;
yabahiro 21:1f1e9c585da8 515 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 516 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 517 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 518 mtc=true;
yabahiro 21:1f1e9c585da8 519 break;
yabahiro 21:1f1e9c585da8 520 case 253:
yabahiro 21:1f1e9c585da8 521 motor[TIRE_FR].pwm = 20;
yabahiro 21:1f1e9c585da8 522 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 523 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 524 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 525 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 526 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 527 motor[TIRE_BL].pwm = 20;
yabahiro 21:1f1e9c585da8 528 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 529 mtc=true;
yabahiro 21:1f1e9c585da8 530 break;
yabahiro 21:1f1e9c585da8 531 case 254:
yabahiro 21:1f1e9c585da8 532 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 533 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 534 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 535 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 536 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 537 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 538 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 539 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 540 mtc=true;
yabahiro 21:1f1e9c585da8 541 break;
yabahiro 21:1f1e9c585da8 542 case 1:
yabahiro 21:1f1e9c585da8 543 motor[TIRE_FR].pwm = 40;
yabahiro 21:1f1e9c585da8 544 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 545 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 546 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 547 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 548 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 549 motor[TIRE_BL].pwm = 40;
yabahiro 21:1f1e9c585da8 550 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 551 mtc=true;
yabahiro 21:1f1e9c585da8 552 break;
yabahiro 21:1f1e9c585da8 553 case 3:
yabahiro 21:1f1e9c585da8 554 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 555 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 556 motor[TIRE_FL].pwm = 20;
yabahiro 21:1f1e9c585da8 557 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 558 motor[TIRE_BR].pwm = 20;
yabahiro 21:1f1e9c585da8 559 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 560 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 561 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 562 mtc=true;
yabahiro 21:1f1e9c585da8 563 break;
yabahiro 21:1f1e9c585da8 564 case 2:
yabahiro 21:1f1e9c585da8 565 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 566 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 567 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 568 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 569 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 570 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 571 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 572 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 573 mtc=true;
yabahiro 21:1f1e9c585da8 574 break;
yabahiro 21:1f1e9c585da8 575 }
yabahiro 21:1f1e9c585da8 576 if(g[0]!=98&&mtc==true) {
yabahiro 21:1f1e9c585da8 577 switch(g[1]) {
yabahiro 21:1f1e9c585da8 578 case 0:
yabahiro 21:1f1e9c585da8 579 mtc=false;
yabahiro 21:1f1e9c585da8 580 break;
yabahiro 21:1f1e9c585da8 581 case 255:
yabahiro 21:1f1e9c585da8 582 motor[TIRE_BR].pwm += 5;
yabahiro 21:1f1e9c585da8 583 mtc=false;
yabahiro 21:1f1e9c585da8 584 break;
yabahiro 21:1f1e9c585da8 585 case 253:
yabahiro 21:1f1e9c585da8 586 motor[TIRE_BR].pwm += 10;
yabahiro 21:1f1e9c585da8 587 mtc=false;
yabahiro 21:1f1e9c585da8 588 break;
yabahiro 21:1f1e9c585da8 589 case 254:
yabahiro 21:1f1e9c585da8 590 motor[TIRE_BR].pwm += 20;
yabahiro 21:1f1e9c585da8 591 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 592 mtc=false;
yabahiro 21:1f1e9c585da8 593 break;
yabahiro 21:1f1e9c585da8 594 case 1:
yabahiro 21:1f1e9c585da8 595 motor[TIRE_BL].pwm += 5;
yabahiro 21:1f1e9c585da8 596 mtc=false;
yabahiro 21:1f1e9c585da8 597 break;
yabahiro 21:1f1e9c585da8 598 case 3:
yabahiro 21:1f1e9c585da8 599 motor[TIRE_BL].pwm += 10;
yabahiro 21:1f1e9c585da8 600 mtc=false;
yabahiro 21:1f1e9c585da8 601 break;
yabahiro 21:1f1e9c585da8 602 case 2:
yabahiro 21:1f1e9c585da8 603 motor[TIRE_BL].pwm += 20;
yabahiro 21:1f1e9c585da8 604 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 605 mtc=false;
yabahiro 21:1f1e9c585da8 606 break;
yabahiro 21:1f1e9c585da8 607 }
yabahiro 21:1f1e9c585da8 608 }
yabahiro 23:694ecfa97839 609 } else if(mode==11) {
yabahiro 25:38d4964e8aad 610 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 611 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 612 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 613 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 614 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 615 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 616 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 617 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 618 if(count>5000&&g[2]==0) {
yabahiro 27:545148741493 619 count=0;
yabahiro 23:694ecfa97839 620 mode=12;
yabahiro 21:1f1e9c585da8 621 }
yabahiro 23:694ecfa97839 622 } else if(mode==12) {
yabahiro 21:1f1e9c585da8 623 motor[TIRE_FR].pwm = 15;
yabahiro 21:1f1e9c585da8 624 motor[TIRE_FR].dir = FOR;
yabahiro 21:1f1e9c585da8 625 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 626 motor[TIRE_FL].dir = BACK;
yabahiro 21:1f1e9c585da8 627 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 628 motor[TIRE_BR].dir = FOR;
yabahiro 21:1f1e9c585da8 629 motor[TIRE_BL].pwm = 15;
yabahiro 21:1f1e9c585da8 630 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 631 if(count>10000&&g[2]==0) {
yabahiro 27:545148741493 632 count=0;
yabahiro 27:545148741493 633 cross=0;
yabahiro 23:694ecfa97839 634 mode=20;
yabahiro 21:1f1e9c585da8 635 }
yabahiro 23:694ecfa97839 636 } else if(mode==20) {
yabahiro 23:694ecfa97839 637 switch(g[2]) {
kishibekairohan 22:c47f29caded6 638 case 98:
kishibekairohan 22:c47f29caded6 639 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 640 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 641 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 642 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 643 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 644 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 645 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 646 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 647 if(count>50000) {
yabahiro 27:545148741493 648 cross++;
yabahiro 27:545148741493 649 count=0;
kishibekairohan 22:c47f29caded6 650 }
kishibekairohan 22:c47f29caded6 651 mtc2=true;
yabahiro 27:545148741493 652 if(cross==3) {
yabahiro 23:694ecfa97839 653 mode=21;
yabahiro 27:545148741493 654 count=0;
yabahiro 27:545148741493 655 cross=0;
kishibekairohan 22:c47f29caded6 656 }
kishibekairohan 22:c47f29caded6 657 break;
kishibekairohan 22:c47f29caded6 658 case 0:
kishibekairohan 22:c47f29caded6 659 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 660 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 661 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 662 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 663 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 664 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 665 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 666 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 667 mtc2=true;
kishibekairohan 22:c47f29caded6 668 break;
kishibekairohan 22:c47f29caded6 669 case 255:
kishibekairohan 22:c47f29caded6 670 motor[TIRE_FR].pwm = 40;
kishibekairohan 22:c47f29caded6 671 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 672 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 673 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 674 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 675 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 676 motor[TIRE_BL].pwm = 40;
kishibekairohan 22:c47f29caded6 677 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 678 mtc2=true;
kishibekairohan 22:c47f29caded6 679 break;
kishibekairohan 22:c47f29caded6 680 case 253:
kishibekairohan 22:c47f29caded6 681 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 682 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 683 motor[TIRE_FL].pwm = 20;
kishibekairohan 22:c47f29caded6 684 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 685 motor[TIRE_BR].pwm = 20;
kishibekairohan 22:c47f29caded6 686 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 687 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 688 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 689 mtc2=true;
kishibekairohan 22:c47f29caded6 690 break;
kishibekairohan 22:c47f29caded6 691 case 254:
kishibekairohan 22:c47f29caded6 692 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 693 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 694 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 695 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 696 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 697 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 698 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 699 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 700 mtc2=true;
kishibekairohan 22:c47f29caded6 701 break;
kishibekairohan 22:c47f29caded6 702 case 1:
kishibekairohan 22:c47f29caded6 703 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 704 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 705 motor[TIRE_FL].pwm = 40;
kishibekairohan 22:c47f29caded6 706 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 707 motor[TIRE_BR].pwm = 40;
kishibekairohan 22:c47f29caded6 708 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 709 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 710 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 711 mtc2=true;
kishibekairohan 22:c47f29caded6 712 break;
kishibekairohan 22:c47f29caded6 713 case 3:
kishibekairohan 22:c47f29caded6 714 motor[TIRE_FR].pwm = 20;
kishibekairohan 22:c47f29caded6 715 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 716 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 717 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 718 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 719 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 720 motor[TIRE_BL].pwm = 20;
kishibekairohan 22:c47f29caded6 721 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 722 mtc2=true;
kishibekairohan 22:c47f29caded6 723 break;
kishibekairohan 22:c47f29caded6 724 case 2:
kishibekairohan 22:c47f29caded6 725 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 726 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 727 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 728 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 729 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 730 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 731 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 732 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 733 mtc2=true;
kishibekairohan 22:c47f29caded6 734 break;
kishibekairohan 22:c47f29caded6 735 }
kishibekairohan 22:c47f29caded6 736 if(g[2]!=98&&mtc2==true) {
kishibekairohan 22:c47f29caded6 737 switch(g[3]) {
kishibekairohan 22:c47f29caded6 738 case 0:
kishibekairohan 22:c47f29caded6 739 mtc2=false;
kishibekairohan 22:c47f29caded6 740 break;
kishibekairohan 22:c47f29caded6 741 case 255:
kishibekairohan 22:c47f29caded6 742 motor[TIRE_BL].pwm += 5;
kishibekairohan 22:c47f29caded6 743 mtc2=false;
kishibekairohan 22:c47f29caded6 744 break;
kishibekairohan 22:c47f29caded6 745 case 253:
kishibekairohan 22:c47f29caded6 746 motor[TIRE_BL].pwm += 10;
kishibekairohan 22:c47f29caded6 747 mtc2=false;
kishibekairohan 22:c47f29caded6 748 break;
kishibekairohan 22:c47f29caded6 749 case 254:
kishibekairohan 22:c47f29caded6 750 motor[TIRE_BL].pwm += 20;
kishibekairohan 22:c47f29caded6 751 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 752 mtc2=false;
kishibekairohan 22:c47f29caded6 753 break;
kishibekairohan 22:c47f29caded6 754 case 1:
kishibekairohan 22:c47f29caded6 755 motor[TIRE_FL].pwm += 5;
kishibekairohan 22:c47f29caded6 756 mtc2=false;
kishibekairohan 22:c47f29caded6 757 break;
kishibekairohan 22:c47f29caded6 758 case 3:
kishibekairohan 22:c47f29caded6 759 motor[TIRE_FL].pwm += 10;
kishibekairohan 22:c47f29caded6 760 mtc2=false;
kishibekairohan 22:c47f29caded6 761 break;
kishibekairohan 22:c47f29caded6 762 case 2:
kishibekairohan 22:c47f29caded6 763 motor[TIRE_FL].pwm += 20;
kishibekairohan 22:c47f29caded6 764 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 765 mtc2=false;
kishibekairohan 22:c47f29caded6 766 break;
kishibekairohan 22:c47f29caded6 767 }
kishibekairohan 22:c47f29caded6 768 }
yabahiro 23:694ecfa97839 769 } else if(mode==21) {
yabahiro 25:38d4964e8aad 770 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 771 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 772 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 773 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 774 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 775 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 776 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 777 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 778 if(count>5000&&g[0]==0) {
yabahiro 27:545148741493 779 count=0;
yabahiro 23:694ecfa97839 780 mode=22;
yabahiro 23:694ecfa97839 781 }
yabahiro 23:694ecfa97839 782 } else if(mode==22) {
yabahiro 23:694ecfa97839 783 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 784 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 785 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 786 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 787 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 788 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 789 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 790 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 791 if(count>10000&&g[0]==0) {
yabahiro 27:545148741493 792 count=0;
yabahiro 27:545148741493 793 cross=0;
yabahiro 23:694ecfa97839 794 mode=30;
yabahiro 23:694ecfa97839 795 }
yabahiro 23:694ecfa97839 796 } else if(mode==30) {
yabahiro 23:694ecfa97839 797 switch(g[0]) {
yabahiro 23:694ecfa97839 798 case 98:
yabahiro 23:694ecfa97839 799 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 800 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 801 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 802 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 803 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 804 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 805 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 806 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 807 if(count>100000) {
yabahiro 27:545148741493 808 cross++;
yabahiro 27:545148741493 809 count=0;
yabahiro 23:694ecfa97839 810 }
yabahiro 23:694ecfa97839 811 mtc=true;
yabahiro 27:545148741493 812 if(cross==1) {
yabahiro 23:694ecfa97839 813 mode=31;
yabahiro 27:545148741493 814 count=0;
yabahiro 23:694ecfa97839 815 }
yabahiro 23:694ecfa97839 816 break;
yabahiro 23:694ecfa97839 817 case 0:
yabahiro 23:694ecfa97839 818 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 819 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 820 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 821 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 822 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 823 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 824 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 825 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 826 mtc=true;
yabahiro 23:694ecfa97839 827 break;
yabahiro 23:694ecfa97839 828 case 255:
yabahiro 23:694ecfa97839 829 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 830 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 831 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 832 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 833 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 834 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 835 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 836 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 837 mtc=true;
yabahiro 23:694ecfa97839 838 break;
yabahiro 23:694ecfa97839 839 case 253:
yabahiro 23:694ecfa97839 840 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 841 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 842 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 843 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 844 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 845 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 846 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 847 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 848 mtc=true;
yabahiro 23:694ecfa97839 849 break;
yabahiro 23:694ecfa97839 850 case 254:
yabahiro 23:694ecfa97839 851 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 852 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 853 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 854 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 855 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 856 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 857 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 858 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 859 mtc=true;
yabahiro 23:694ecfa97839 860 break;
yabahiro 23:694ecfa97839 861 case 1:
yabahiro 23:694ecfa97839 862 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 863 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 864 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 865 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 866 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 867 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 868 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 869 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 870 mtc=true;
yabahiro 23:694ecfa97839 871 break;
yabahiro 23:694ecfa97839 872 case 3:
yabahiro 23:694ecfa97839 873 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 874 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 875 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 876 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 877 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 878 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 879 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 880 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 881 mtc=true;
yabahiro 23:694ecfa97839 882 break;
yabahiro 23:694ecfa97839 883 case 2:
yabahiro 23:694ecfa97839 884 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 885 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 886 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 887 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 888 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 889 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 890 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 891 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 892 mtc=true;
yabahiro 23:694ecfa97839 893 break;
yabahiro 23:694ecfa97839 894 }
yabahiro 23:694ecfa97839 895 if(g[0]!=98&&mtc==true) {
yabahiro 23:694ecfa97839 896 switch(g[1]) {
yabahiro 23:694ecfa97839 897 case 0:
yabahiro 23:694ecfa97839 898 mtc=false;
yabahiro 23:694ecfa97839 899 break;
yabahiro 23:694ecfa97839 900 case 255:
yabahiro 23:694ecfa97839 901 motor[TIRE_BR].pwm += 5;
yabahiro 23:694ecfa97839 902 mtc=false;
yabahiro 23:694ecfa97839 903 break;
yabahiro 23:694ecfa97839 904 case 253:
yabahiro 23:694ecfa97839 905 motor[TIRE_BR].pwm += 10;
yabahiro 23:694ecfa97839 906 mtc=false;
yabahiro 23:694ecfa97839 907 break;
yabahiro 23:694ecfa97839 908 case 254:
yabahiro 23:694ecfa97839 909 motor[TIRE_BR].pwm += 20;
yabahiro 23:694ecfa97839 910 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 911 mtc=false;
yabahiro 23:694ecfa97839 912 break;
yabahiro 23:694ecfa97839 913 case 1:
yabahiro 23:694ecfa97839 914 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 915 mtc=false;
yabahiro 23:694ecfa97839 916 break;
yabahiro 23:694ecfa97839 917 case 3:
yabahiro 23:694ecfa97839 918 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 919 mtc=false;
yabahiro 23:694ecfa97839 920 break;
yabahiro 23:694ecfa97839 921 case 2:
yabahiro 23:694ecfa97839 922 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 923 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 924 mtc=false;
yabahiro 23:694ecfa97839 925 break;
yabahiro 23:694ecfa97839 926 }
yabahiro 23:694ecfa97839 927 }
yabahiro 23:694ecfa97839 928 } else if(mode==31) {
yabahiro 23:694ecfa97839 929 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 930 motor[TIRE_FR].dir = BACK;
yabahiro 25:38d4964e8aad 931 motor[TIRE_FL].pwm = 0;
yabahiro 25:38d4964e8aad 932 motor[TIRE_FL].dir = FREE;
yabahiro 25:38d4964e8aad 933 motor[TIRE_BR].pwm = 0;
yabahiro 25:38d4964e8aad 934 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 935 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 936 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 937 if(count>5000&&g[3]==0) {
yabahiro 27:545148741493 938 count=0;
yabahiro 23:694ecfa97839 939 mode=32;
yabahiro 23:694ecfa97839 940 }
yabahiro 23:694ecfa97839 941 } else if(mode==32) {
yabahiro 23:694ecfa97839 942 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 943 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 944 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 945 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 946 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 947 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 948 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 949 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 950 if(count>5000&&g[3]==0) {
yabahiro 27:545148741493 951 count=0;
yabahiro 27:545148741493 952 cross=0;
yabahiro 23:694ecfa97839 953 mode=40;
yabahiro 23:694ecfa97839 954 }
yabahiro 23:694ecfa97839 955 } else if(mode==40) {
yabahiro 23:694ecfa97839 956 switch(g[3]) {
yabahiro 23:694ecfa97839 957 case 98:
yabahiro 23:694ecfa97839 958 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 959 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 960 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 961 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 962 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 963 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 964 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 965 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 966 if(count>30000) {
yabahiro 27:545148741493 967 cross++;
yabahiro 27:545148741493 968 count=0;
yabahiro 23:694ecfa97839 969 }
yabahiro 23:694ecfa97839 970 mtc2=true;
yabahiro 27:545148741493 971 if(cross==1) {
yabahiro 23:694ecfa97839 972 mode=41;
yabahiro 27:545148741493 973 count=0;
yabahiro 27:545148741493 974 cross=0;
yabahiro 23:694ecfa97839 975 }
yabahiro 23:694ecfa97839 976 break;
yabahiro 23:694ecfa97839 977 case 0:
yabahiro 23:694ecfa97839 978 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 979 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 980 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 981 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 982 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 983 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 984 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 985 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 986 mtc2=true;
yabahiro 23:694ecfa97839 987 break;
yabahiro 23:694ecfa97839 988 case 255:
yabahiro 23:694ecfa97839 989 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 990 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 991 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 992 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 993 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 994 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 995 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 996 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 997 mtc2=true;
yabahiro 23:694ecfa97839 998 break;
yabahiro 23:694ecfa97839 999 case 253:
yabahiro 23:694ecfa97839 1000 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1001 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1002 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 1003 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1004 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 1005 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1006 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1007 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1008 mtc2=true;
yabahiro 23:694ecfa97839 1009 break;
yabahiro 23:694ecfa97839 1010 case 254:
yabahiro 23:694ecfa97839 1011 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1012 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1013 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1014 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1015 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1016 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1017 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1018 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1019 mtc2=true;
yabahiro 23:694ecfa97839 1020 break;
yabahiro 23:694ecfa97839 1021 case 1:
yabahiro 23:694ecfa97839 1022 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 1023 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1024 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 1025 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1026 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 1027 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1028 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 1029 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1030 mtc2=true;
yabahiro 23:694ecfa97839 1031 break;
yabahiro 23:694ecfa97839 1032 case 3:
yabahiro 23:694ecfa97839 1033 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 1034 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1035 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1036 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1037 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1038 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1039 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 1040 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1041 mtc2=true;
yabahiro 23:694ecfa97839 1042 break;
yabahiro 23:694ecfa97839 1043 case 2:
yabahiro 23:694ecfa97839 1044 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 1045 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1046 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1047 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1048 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1049 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1050 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 1051 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1052 mtc2=true;
yabahiro 23:694ecfa97839 1053 break;
yabahiro 23:694ecfa97839 1054 }
yabahiro 23:694ecfa97839 1055 if(g[2]!=98&&mtc2==true) {
yabahiro 23:694ecfa97839 1056 switch(g[2]) {
yabahiro 23:694ecfa97839 1057 case 0:
yabahiro 23:694ecfa97839 1058 mtc2=false;
yabahiro 23:694ecfa97839 1059 break;
yabahiro 23:694ecfa97839 1060 case 255:
yabahiro 23:694ecfa97839 1061 motor[TIRE_FL].pwm += 5;
yabahiro 23:694ecfa97839 1062 mtc2=false;
yabahiro 23:694ecfa97839 1063 break;
yabahiro 23:694ecfa97839 1064 case 253:
yabahiro 23:694ecfa97839 1065 motor[TIRE_FL].pwm += 10;
yabahiro 23:694ecfa97839 1066 mtc2=false;
yabahiro 23:694ecfa97839 1067 break;
yabahiro 23:694ecfa97839 1068 case 254:
yabahiro 23:694ecfa97839 1069 motor[TIRE_FL].pwm += 20;
yabahiro 23:694ecfa97839 1070 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1071 mtc2=false;
yabahiro 23:694ecfa97839 1072 break;
yabahiro 23:694ecfa97839 1073 case 1:
yabahiro 23:694ecfa97839 1074 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 1075 mtc2=false;
yabahiro 23:694ecfa97839 1076 break;
yabahiro 23:694ecfa97839 1077 case 3:
yabahiro 23:694ecfa97839 1078 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 1079 mtc2=false;
yabahiro 23:694ecfa97839 1080 break;
yabahiro 23:694ecfa97839 1081 case 2:
yabahiro 23:694ecfa97839 1082 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 1083 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1084 mtc2=false;
yabahiro 23:694ecfa97839 1085 break;
yabahiro 23:694ecfa97839 1086 }
yabahiro 23:694ecfa97839 1087 }
yabahiro 23:694ecfa97839 1088 } else if(mode==41) {
yabahiro 25:38d4964e8aad 1089 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1090 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1091 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1092 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1093 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1094 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1095 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1096 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 1097 if(count>5000&&g[1]==0) {
yabahiro 27:545148741493 1098 count=0;
yabahiro 23:694ecfa97839 1099 mode=42;
yabahiro 23:694ecfa97839 1100 }
yabahiro 23:694ecfa97839 1101 } else if(mode==42) {
yabahiro 23:694ecfa97839 1102 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1103 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1104 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1105 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 1106 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1107 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 1108 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1109 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1110 if(count>5000&&g[1]==0) {
yabahiro 27:545148741493 1111 count=0;
yabahiro 23:694ecfa97839 1112 mode=50;
yabahiro 24:41c23c9a5058 1113 }
yabahiro 24:41c23c9a5058 1114 } else if(mode==50) {
yabahiro 24:41c23c9a5058 1115 switch(g[1]) {
yabahiro 24:41c23c9a5058 1116 case 98:
yabahiro 24:41c23c9a5058 1117 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1118 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1119 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1120 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1121 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1122 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1123 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1124 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1125 if(count>100000) {
yabahiro 27:545148741493 1126 cross++;
yabahiro 27:545148741493 1127 count=0;
yabahiro 24:41c23c9a5058 1128 }
yabahiro 24:41c23c9a5058 1129 mtc=true;
yabahiro 27:545148741493 1130 if(cross==1) {
yabahiro 24:41c23c9a5058 1131 mode=51;
yabahiro 27:545148741493 1132 count=0;
yabahiro 24:41c23c9a5058 1133 }
yabahiro 24:41c23c9a5058 1134 break;
yabahiro 24:41c23c9a5058 1135 case 0:
yabahiro 24:41c23c9a5058 1136 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1137 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1138 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1139 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1140 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1141 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1142 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1143 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1144 mtc=true;
yabahiro 24:41c23c9a5058 1145 break;
yabahiro 24:41c23c9a5058 1146 case 255:
yabahiro 24:41c23c9a5058 1147 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1148 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1149 motor[TIRE_FL].pwm = 40;
yabahiro 24:41c23c9a5058 1150 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1151 motor[TIRE_BR].pwm = 40;
yabahiro 24:41c23c9a5058 1152 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1153 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1154 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1155 mtc=true;
yabahiro 24:41c23c9a5058 1156 break;
yabahiro 24:41c23c9a5058 1157 case 253:
yabahiro 24:41c23c9a5058 1158 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1159 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1160 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1161 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1162 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1163 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1164 motor[TIRE_BL].pwm = 20;
yabahiro 24:41c23c9a5058 1165 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1166 mtc=true;
yabahiro 24:41c23c9a5058 1167 break;
yabahiro 24:41c23c9a5058 1168 case 254:
yabahiro 24:41c23c9a5058 1169 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1170 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1171 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1172 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1173 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1174 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1175 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1176 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1177 mtc=true;
yabahiro 24:41c23c9a5058 1178 break;
yabahiro 24:41c23c9a5058 1179 case 1:
yabahiro 24:41c23c9a5058 1180 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1181 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1182 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1183 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1184 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1185 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1186 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1187 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1188 mtc=true;
yabahiro 24:41c23c9a5058 1189 break;
yabahiro 24:41c23c9a5058 1190 case 3:
yabahiro 24:41c23c9a5058 1191 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1192 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1193 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1194 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1195 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1196 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1197 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1198 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1199 mtc=true;
yabahiro 24:41c23c9a5058 1200 break;
yabahiro 24:41c23c9a5058 1201 case 2:
yabahiro 24:41c23c9a5058 1202 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1203 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1204 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1205 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1206 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1207 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1208 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1209 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1210 mtc=true;
yabahiro 24:41c23c9a5058 1211 break;
yabahiro 24:41c23c9a5058 1212 }
yabahiro 24:41c23c9a5058 1213 if(g[1]!=98&&mtc==true) {
yabahiro 24:41c23c9a5058 1214 switch(g[0]) {
yabahiro 23:694ecfa97839 1215 case 0:
yabahiro 24:41c23c9a5058 1216 mtc=false;
yabahiro 23:694ecfa97839 1217 break;
yabahiro 23:694ecfa97839 1218 case 255:
yabahiro 27:545148741493 1219 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1220 mtc=false;
yabahiro 23:694ecfa97839 1221 break;
yabahiro 23:694ecfa97839 1222 case 253:
yabahiro 27:545148741493 1223 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1224 mtc=false;
yabahiro 23:694ecfa97839 1225 break;
yabahiro 23:694ecfa97839 1226 case 254:
yabahiro 27:545148741493 1227 motor[TIRE_FL].pwm += 20;
yabahiro 27:545148741493 1228 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1229 mtc=false;
yabahiro 23:694ecfa97839 1230 break;
yabahiro 23:694ecfa97839 1231 case 1:
yabahiro 27:545148741493 1232 motor[TIRE_FR].pwm += 5;
yabahiro 24:41c23c9a5058 1233 mtc=false;
yabahiro 23:694ecfa97839 1234 break;
yabahiro 23:694ecfa97839 1235 case 3:
yabahiro 27:545148741493 1236 motor[TIRE_FR].pwm += 10;
yabahiro 24:41c23c9a5058 1237 mtc=false;
yabahiro 23:694ecfa97839 1238 break;
yabahiro 23:694ecfa97839 1239 case 2:
yabahiro 27:545148741493 1240 motor[TIRE_FR].pwm += 20;
yabahiro 27:545148741493 1241 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1242 mtc=false;
yabahiro 23:694ecfa97839 1243 break;
yabahiro 23:694ecfa97839 1244 }
yabahiro 23:694ecfa97839 1245 }
yabahiro 23:694ecfa97839 1246 } else if(mode==51) {
yabahiro 25:38d4964e8aad 1247 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1248 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1249 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1250 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1251 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1252 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1253 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1254 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 1255 if(count>5000&&g[3]==0) {
yabahiro 27:545148741493 1256 count=0;
yabahiro 24:41c23c9a5058 1257 mode=52;
yabahiro 24:41c23c9a5058 1258 }
yabahiro 24:41c23c9a5058 1259 } else if(mode==52) {
yabahiro 24:41c23c9a5058 1260 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1261 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1262 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1263 motor[TIRE_FL].dir = FOR;
yabahiro 24:41c23c9a5058 1264 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1265 motor[TIRE_BR].dir = BACK;
yabahiro 24:41c23c9a5058 1266 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1267 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 1268 if(count>5000&&g[3]==0) {
yabahiro 27:545148741493 1269 count=0;
yabahiro 27:545148741493 1270 cross=0;
yabahiro 24:41c23c9a5058 1271 mode=60;
yabahiro 24:41c23c9a5058 1272 }
yabahiro 24:41c23c9a5058 1273 } else if(mode==60) {
yabahiro 24:41c23c9a5058 1274 switch(g[3]) {
yabahiro 24:41c23c9a5058 1275 case 98:
yabahiro 24:41c23c9a5058 1276 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1277 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1278 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1279 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1280 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1281 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1282 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1283 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 1284 if(count>50000) {
yabahiro 27:545148741493 1285 cross++;
yabahiro 27:545148741493 1286 count=0;
yabahiro 24:41c23c9a5058 1287 }
yabahiro 24:41c23c9a5058 1288 mtc2=true;
yabahiro 27:545148741493 1289 if(cross==2) {
yabahiro 24:41c23c9a5058 1290 mode=61;
yabahiro 27:545148741493 1291 count=0;
yabahiro 27:545148741493 1292 cross=0;
yabahiro 24:41c23c9a5058 1293 }
yabahiro 24:41c23c9a5058 1294 break;
yabahiro 24:41c23c9a5058 1295 case 0:
yabahiro 24:41c23c9a5058 1296 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1297 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1298 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1299 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1300 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1301 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1302 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1303 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1304 mtc2=true;
yabahiro 24:41c23c9a5058 1305 break;
yabahiro 24:41c23c9a5058 1306 case 255:
yabahiro 24:41c23c9a5058 1307 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1308 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1309 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1310 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1311 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1312 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1313 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1314 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1315 mtc2=true;
yabahiro 24:41c23c9a5058 1316 break;
yabahiro 24:41c23c9a5058 1317 case 253:
yabahiro 24:41c23c9a5058 1318 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1319 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1320 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1321 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1322 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1323 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1324 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1325 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1326 mtc2=true;
yabahiro 24:41c23c9a5058 1327 break;
yabahiro 24:41c23c9a5058 1328 case 254:
yabahiro 24:41c23c9a5058 1329 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1330 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1331 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1332 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1333 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1334 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1335 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1336 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1337 mtc2=true;
yabahiro 24:41c23c9a5058 1338 break;
yabahiro 24:41c23c9a5058 1339 case 1:
yabahiro 24:41c23c9a5058 1340 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1341 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1342 motor[TIRE_FL].pwm = 40;
yabahiro 24:41c23c9a5058 1343 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1344 motor[TIRE_BR].pwm = 40;
yabahiro 24:41c23c9a5058 1345 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1346 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1347 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1348 mtc2=true;
yabahiro 24:41c23c9a5058 1349 break;
yabahiro 24:41c23c9a5058 1350 case 3:
yabahiro 24:41c23c9a5058 1351 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1352 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1353 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1354 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1355 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1356 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1357 motor[TIRE_BL].pwm = 20;
yabahiro 24:41c23c9a5058 1358 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1359 mtc2=true;
yabahiro 24:41c23c9a5058 1360 break;
yabahiro 24:41c23c9a5058 1361 case 2:
yabahiro 24:41c23c9a5058 1362 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1363 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1364 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1365 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1366 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1367 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1368 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1369 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1370 mtc2=true;
yabahiro 24:41c23c9a5058 1371 break;
yabahiro 24:41c23c9a5058 1372 }
yabahiro 24:41c23c9a5058 1373 if(g[2]!=98&&mtc2==true) {
yabahiro 24:41c23c9a5058 1374 switch(g[2]) {
yabahiro 24:41c23c9a5058 1375 case 0:
yabahiro 24:41c23c9a5058 1376 mtc2=false;
yabahiro 24:41c23c9a5058 1377 break;
yabahiro 24:41c23c9a5058 1378 case 255:
yabahiro 24:41c23c9a5058 1379 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1380 mtc2=false;
yabahiro 24:41c23c9a5058 1381 break;
yabahiro 24:41c23c9a5058 1382 case 253:
yabahiro 24:41c23c9a5058 1383 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1384 mtc2=false;
yabahiro 24:41c23c9a5058 1385 break;
yabahiro 24:41c23c9a5058 1386 case 254:
yabahiro 24:41c23c9a5058 1387 motor[TIRE_FL].pwm += 20;
yabahiro 24:41c23c9a5058 1388 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1389 mtc2=false;
yabahiro 24:41c23c9a5058 1390 break;
yabahiro 24:41c23c9a5058 1391 case 1:
yabahiro 24:41c23c9a5058 1392 motor[TIRE_BL].pwm += 5;
yabahiro 24:41c23c9a5058 1393 mtc2=false;
yabahiro 24:41c23c9a5058 1394 break;
yabahiro 24:41c23c9a5058 1395 case 3:
yabahiro 24:41c23c9a5058 1396 motor[TIRE_BL].pwm += 10;
yabahiro 24:41c23c9a5058 1397 mtc2=false;
yabahiro 24:41c23c9a5058 1398 break;
yabahiro 24:41c23c9a5058 1399 case 2:
yabahiro 24:41c23c9a5058 1400 motor[TIRE_BL].pwm += 20;
yabahiro 24:41c23c9a5058 1401 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1402 mtc2=false;
yabahiro 24:41c23c9a5058 1403 break;
yabahiro 24:41c23c9a5058 1404 }
yabahiro 24:41c23c9a5058 1405 }
yabahiro 24:41c23c9a5058 1406 } else if(mode==61) {
yabahiro 25:38d4964e8aad 1407 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1408 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1409 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1410 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1411 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1412 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1413 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1414 motor[TIRE_BL].dir = FREE;
yabahiro 27:545148741493 1415 if(count>5000&&g[1]==0) {
yabahiro 27:545148741493 1416 count=0;
yabahiro 24:41c23c9a5058 1417 mode=62;
yabahiro 24:41c23c9a5058 1418 }
yabahiro 24:41c23c9a5058 1419 } else if(mode==62) {
yabahiro 24:41c23c9a5058 1420 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1421 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1422 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1423 motor[TIRE_FL].dir = FOR;
yabahiro 24:41c23c9a5058 1424 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1425 motor[TIRE_BR].dir = BACK;
yabahiro 24:41c23c9a5058 1426 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1427 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1428 if(count>5000&&g[1]==0) {
yabahiro 27:545148741493 1429 count=0;
yabahiro 24:41c23c9a5058 1430 mode=70;
yabahiro 24:41c23c9a5058 1431 }
yabahiro 24:41c23c9a5058 1432 } else if(mode==70) {
yabahiro 24:41c23c9a5058 1433 switch(g[1]) {
yabahiro 24:41c23c9a5058 1434 case 98:
yabahiro 24:41c23c9a5058 1435 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1436 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1437 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1438 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1439 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1440 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1441 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1442 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1443 if(count>100000) {
yabahiro 27:545148741493 1444 cross++;
yabahiro 27:545148741493 1445 count=0;
yabahiro 24:41c23c9a5058 1446 }
yabahiro 24:41c23c9a5058 1447 mtc=true;
yabahiro 27:545148741493 1448 if(cross==1) {
yabahiro 24:41c23c9a5058 1449 mode=71;
yabahiro 27:545148741493 1450 count=0;
yabahiro 24:41c23c9a5058 1451 }
yabahiro 24:41c23c9a5058 1452 break;
yabahiro 24:41c23c9a5058 1453 case 0:
yabahiro 24:41c23c9a5058 1454 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1455 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1456 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1457 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1458 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1459 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1460 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1461 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1462 mtc=true;
yabahiro 24:41c23c9a5058 1463 break;
yabahiro 24:41c23c9a5058 1464 case 255:
yabahiro 24:41c23c9a5058 1465 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1466 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1467 motor[TIRE_FL].pwm = 40;
yabahiro 24:41c23c9a5058 1468 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1469 motor[TIRE_BR].pwm = 40;
yabahiro 24:41c23c9a5058 1470 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1471 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1472 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1473 mtc=true;
yabahiro 24:41c23c9a5058 1474 break;
yabahiro 24:41c23c9a5058 1475 case 253:
yabahiro 24:41c23c9a5058 1476 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1477 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1478 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1479 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1480 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1481 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1482 motor[TIRE_BL].pwm = 20;
yabahiro 24:41c23c9a5058 1483 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1484 mtc=true;
yabahiro 24:41c23c9a5058 1485 break;
yabahiro 24:41c23c9a5058 1486 case 254:
yabahiro 24:41c23c9a5058 1487 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1488 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1489 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1490 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1491 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1492 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1493 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1494 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1495 mtc=true;
yabahiro 24:41c23c9a5058 1496 break;
yabahiro 24:41c23c9a5058 1497 case 1:
yabahiro 24:41c23c9a5058 1498 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1499 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1500 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1501 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1502 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1503 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1504 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1505 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1506 mtc=true;
yabahiro 24:41c23c9a5058 1507 break;
yabahiro 24:41c23c9a5058 1508 case 3:
yabahiro 24:41c23c9a5058 1509 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1510 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1511 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1512 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1513 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1514 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1515 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1516 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1517 mtc=true;
yabahiro 24:41c23c9a5058 1518 break;
yabahiro 24:41c23c9a5058 1519 case 2:
yabahiro 24:41c23c9a5058 1520 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1521 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1522 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1523 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1524 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1525 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1526 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1527 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1528 mtc=true;
yabahiro 24:41c23c9a5058 1529 break;
yabahiro 24:41c23c9a5058 1530 }
yabahiro 24:41c23c9a5058 1531 if(g[1]!=98&&mtc==true) {
yabahiro 24:41c23c9a5058 1532 switch(g[0]) {
yabahiro 24:41c23c9a5058 1533 case 0:
yabahiro 24:41c23c9a5058 1534 mtc=false;
yabahiro 24:41c23c9a5058 1535 break;
yabahiro 24:41c23c9a5058 1536 case 255:
yabahiro 27:545148741493 1537 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1538 mtc=false;
yabahiro 24:41c23c9a5058 1539 break;
yabahiro 24:41c23c9a5058 1540 case 253:
yabahiro 27:545148741493 1541 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1542 mtc=false;
yabahiro 24:41c23c9a5058 1543 break;
yabahiro 24:41c23c9a5058 1544 case 254:
yabahiro 27:545148741493 1545 motor[TIRE_FL].pwm += 20;
yabahiro 27:545148741493 1546 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1547 mtc=false;
yabahiro 24:41c23c9a5058 1548 break;
yabahiro 24:41c23c9a5058 1549 case 1:
yabahiro 27:545148741493 1550 motor[TIRE_FR].pwm += 5;
yabahiro 24:41c23c9a5058 1551 mtc=false;
yabahiro 24:41c23c9a5058 1552 break;
yabahiro 24:41c23c9a5058 1553 case 3:
yabahiro 27:545148741493 1554 motor[TIRE_FR].pwm += 10;
yabahiro 24:41c23c9a5058 1555 mtc=false;
yabahiro 24:41c23c9a5058 1556 break;
yabahiro 24:41c23c9a5058 1557 case 2:
yabahiro 27:545148741493 1558 motor[TIRE_FR].pwm += 20;
yabahiro 27:545148741493 1559 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1560 mtc=false;
yabahiro 24:41c23c9a5058 1561 break;
yabahiro 24:41c23c9a5058 1562 }
yabahiro 24:41c23c9a5058 1563 }
yabahiro 24:41c23c9a5058 1564 } else if(mode==71) {
yabahiro 24:41c23c9a5058 1565 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1566 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1567 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1568 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1569 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1570 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1571 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1572 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1573 if(count>5000&&g[3]==0) {
yabahiro 27:545148741493 1574 count=0;
kishibekairohan 26:be5121da0f77 1575 mode=72;
kishibekairohan 26:be5121da0f77 1576 }
yabahiro 27:545148741493 1577 } else if(mode==72) {
kishibekairohan 26:be5121da0f77 1578 motor[TIRE_FR].pwm = 15;
kishibekairohan 26:be5121da0f77 1579 motor[TIRE_FR].dir = BACK;
kishibekairohan 26:be5121da0f77 1580 motor[TIRE_FL].pwm = 15;
kishibekairohan 26:be5121da0f77 1581 motor[TIRE_FL].dir = FOR;
kishibekairohan 26:be5121da0f77 1582 motor[TIRE_BR].pwm = 15;
kishibekairohan 26:be5121da0f77 1583 motor[TIRE_BR].dir = BACK;
kishibekairohan 26:be5121da0f77 1584 motor[TIRE_BL].pwm = 15;
kishibekairohan 26:be5121da0f77 1585 motor[TIRE_BL].dir = FOR;
yabahiro 27:545148741493 1586 count++;
yabahiro 27:545148741493 1587 if(count>50000&&g[3]==0) {
yabahiro 27:545148741493 1588 count=0;
yabahiro 27:545148741493 1589 cross=0;
kishibekairohan 26:be5121da0f77 1590 mode=80;
kishibekairohan 26:be5121da0f77 1591 }
yabahiro 27:545148741493 1592 } else if(mode==80) {
yabahiro 27:545148741493 1593 if(count>=400000) {
yabahiro 27:545148741493 1594 mode=100;
yabahiro 27:545148741493 1595 }
yabahiro 27:545148741493 1596 motor[TIRE_FR].pwm = 40;
yabahiro 27:545148741493 1597 motor[TIRE_FR].dir = FOR;
yabahiro 27:545148741493 1598 motor[TIRE_FL].pwm = 40;
yabahiro 27:545148741493 1599 motor[TIRE_FL].dir = BACK;
yabahiro 27:545148741493 1600 motor[TIRE_BR].pwm = 40;
yabahiro 27:545148741493 1601 motor[TIRE_BR].dir = FOR;
yabahiro 27:545148741493 1602 motor[TIRE_BL].pwm = 40;
yabahiro 27:545148741493 1603 motor[TIRE_BL].dir = BACK;
yabahiro 27:545148741493 1604 } else if(mode==100) {
kishibekairohan 26:be5121da0f77 1605 motor[TIRE_FR].pwm = 0;
kishibekairohan 26:be5121da0f77 1606 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1607 motor[TIRE_FL].pwm = 0;
kishibekairohan 26:be5121da0f77 1608 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1609 motor[TIRE_BR].pwm = 0;
kishibekairohan 26:be5121da0f77 1610 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1611 motor[TIRE_BL].pwm = 0;
kishibekairohan 26:be5121da0f77 1612 motor[TIRE_BL].dir = BRAKE;
yabahiro 27:545148741493 1613 } else {
yabahiro 21:1f1e9c585da8 1614 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 1615 motor[TIRE_FR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1616 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 1617 motor[TIRE_FL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1618 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 1619 motor[TIRE_BR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1620 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 1621 motor[TIRE_BL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1622 }
yabahiro 21:1f1e9c585da8 1623
kishibekairohan 13:b6e02d6261d7 1624 }
yabahiro 23:694ecfa97839 1625
kishibekairohan 13:b6e02d6261d7 1626 #endif
kishibekairohan 13:b6e02d6261d7 1627
kishibekairohan 13:b6e02d6261d7 1628 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 1629 static void Process3()
kishibekairohan 13:b6e02d6261d7 1630 {
yabahiro 23:694ecfa97839 1631
kishibekairohan 13:b6e02d6261d7 1632 }
kishibekairohan 13:b6e02d6261d7 1633 #endif
kishibekairohan 13:b6e02d6261d7 1634
kishibekairohan 13:b6e02d6261d7 1635 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 1636 static void Process4()
yabahiro 21:1f1e9c585da8 1637 {
kishibekairohan 15:dfcec98f5aa9 1638
kishibekairohan 13:b6e02d6261d7 1639 }
kishibekairohan 13:b6e02d6261d7 1640 #endif
kishibekairohan 13:b6e02d6261d7 1641
kishibekairohan 13:b6e02d6261d7 1642 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 1643 static void Process5()
yabahiro 21:1f1e9c585da8 1644 {
yabahiro 21:1f1e9c585da8 1645
kishibekairohan 13:b6e02d6261d7 1646 }
kishibekairohan 13:b6e02d6261d7 1647 #endif
kishibekairohan 13:b6e02d6261d7 1648
kishibekairohan 13:b6e02d6261d7 1649 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 1650 static void Process6()
kishibekairohan 13:b6e02d6261d7 1651 {
yabahiro 21:1f1e9c585da8 1652
kishibekairohan 13:b6e02d6261d7 1653 }
kishibekairohan 13:b6e02d6261d7 1654 #endif
kishibekairohan 13:b6e02d6261d7 1655
kishibekairohan 13:b6e02d6261d7 1656 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 1657 static void Process7()
kishibekairohan 13:b6e02d6261d7 1658 {
yabahiro 21:1f1e9c585da8 1659
kishibekairohan 12:c09b3e08a316 1660 }
kishibekairohan 12:c09b3e08a316 1661 #endif
kishibekairohan 12:c09b3e08a316 1662
yabahiro 21:1f1e9c585da8 1663 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 1664 static void Process8()
kishibekairohan 12:c09b3e08a316 1665 {
yabahiro 21:1f1e9c585da8 1666
kishibekairohan 12:c09b3e08a316 1667 }
kishibekairohan 12:c09b3e08a316 1668 #endif
kishibekairohan 12:c09b3e08a316 1669
kishibekairohan 13:b6e02d6261d7 1670 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 1671 static void Process9()
kishibekairohan 12:c09b3e08a316 1672 {
yabahiro 21:1f1e9c585da8 1673
t_yamamoto 0:669ef71cba68 1674 }
t_yamamoto 0:669ef71cba68 1675 #endif
t_yamamoto 0:669ef71cba68 1676 #endif
t_yamamoto 0:669ef71cba68 1677 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1678
t_yamamoto 0:669ef71cba68 1679 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1680 {
t_yamamoto 0:669ef71cba68 1681
yabahiro 21:1f1e9c585da8 1682 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 1683 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 1684 #endif
t_yamamoto 0:669ef71cba68 1685
yabahiro 21:1f1e9c585da8 1686 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 1687 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 21:1f1e9c585da8 1688 motor[i].dir = FREE;
yabahiro 21:1f1e9c585da8 1689 motor[i].pwm = 0;
yabahiro 21:1f1e9c585da8 1690 }
yabahiro 21:1f1e9c585da8 1691 #endif
t_yamamoto 0:669ef71cba68 1692 }
t_yamamoto 0:669ef71cba68 1693
yabahiro 21:1f1e9c585da8 1694 /*
yabahiro 21:1f1e9c585da8 1695 void BuzzerTimer_func()
yabahiro 21:1f1e9c585da8 1696 {
kishibekairohan 16:3f2c2d89372b 1697 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 1698 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 1699 }
yabahiro 21:1f1e9c585da8 1700 */
yabahiro 21:1f1e9c585da8 1701 void TapeLedEms_func()
yabahiro 21:1f1e9c585da8 1702 {
kishibekairohan 16:3f2c2d89372b 1703 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1704 }
kishibekairohan 16:3f2c2d89372b 1705
kishibekairohan 16:3f2c2d89372b 1706 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 1707
kishibekairohan 16:3f2c2d89372b 1708
t_yamamoto 0:669ef71cba68 1709 #pragma endregion