大季 矢花 / Mbed 2 deprecated 2019_Master_9_2

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@26:be5121da0f77, 2019-09-17 (annotated)

Committer:
kishibekairohan
Date:
Tue Sep 17 12:02:53 2019 +0000
Revision:
26:be5121da0f77
Parent:
25:38d4964e8aad
Child:
27:545148741493
;;;;

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 21:1f1e9c585da8 273 static void Process0()
yabahiro 21:1f1e9c585da8 274 {
yabahiro 25:38d4964e8aad 275 if(StertSW) {
yabahiro 25:38d4964e8aad 276 if(ZoneSW) {
yabahiro 25:38d4964e8aad 277 current = 1;
yabahiro 25:38d4964e8aad 278 Twsh=2;
kishibekairohan 26:be5121da0f77 279 }else{
yabahiro 25:38d4964e8aad 280 current = 1;
yabahiro 25:38d4964e8aad 281 Twsh=3;
yabahiro 25:38d4964e8aad 282 }
yabahiro 25:38d4964e8aad 283 }
t_yamamoto 0:669ef71cba68 284 }
t_yamamoto 0:669ef71cba68 285 #endif
t_yamamoto 0:669ef71cba68 286
t_yamamoto 0:669ef71cba68 287 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 288
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
kishibekairohan 26:be5121da0f77 299 //int mode=10;
kishibekairohan 26:be5121da0f77 300 int mode =10;
yabahiro 21:1f1e9c585da8 301 int mode1=0;
yabahiro 21:1f1e9c585da8 302 int mode1g0=100000;
yabahiro 21:1f1e9c585da8 303
kishibekairohan 22:c47f29caded6 304
kishibekairohan 16:3f2c2d89372b 305 static void Process1()
t_yamamoto 0:669ef71cba68 306 {
yabahiro 21:1f1e9c585da8 307 mode1g0=100000;
yabahiro 25:38d4964e8aad 308 mode=1;
yabahiro 21:1f1e9c585da8 309 mode1=0;
yabahiro 21:1f1e9c585da8 310 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 311 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 312 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 313 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 314
yabahiro 21:1f1e9c585da8 315 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 316 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 317 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 318 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 319
yabahiro 21:1f1e9c585da8 320 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 321
yabahiro 21:1f1e9c585da8 322 if(controller->Button.UP) {
yabahiro 21:1f1e9c585da8 323 motor[MOTOR_0].pwm = 110;
yabahiro 21:1f1e9c585da8 324 motor[MOTOR_0].dir = FOR;
yabahiro 21:1f1e9c585da8 325 }
yabahiro 21:1f1e9c585da8 326 if(controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 327 motor[MOTOR_0].pwm = 50;
yabahiro 21:1f1e9c585da8 328 motor[MOTOR_0].dir = BACK;
yabahiro 21:1f1e9c585da8 329 }
yabahiro 21:1f1e9c585da8 330 } else {
yabahiro 21:1f1e9c585da8 331 motor[MOTOR_0].pwm = 0;
yabahiro 21:1f1e9c585da8 332 motor[MOTOR_0].dir = BRAKE;
yabahiro 21:1f1e9c585da8 333 }
yabahiro 21:1f1e9c585da8 334 if(controller->Button.X) {
yabahiro 21:1f1e9c585da8 335 if(dz1==true) {
yabahiro 21:1f1e9c585da8 336 if(dz1i==false) {
yabahiro 21:1f1e9c585da8 337 solenoid.solenoid1 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 338 dz1i=true;
yabahiro 21:1f1e9c585da8 339 } else {
yabahiro 21:1f1e9c585da8 340 solenoid.solenoid1 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 341 dz1i=false;
yabahiro 21:1f1e9c585da8 342 }
yabahiro 21:1f1e9c585da8 343 dz1=false;
yabahiro 21:1f1e9c585da8 344 }
yabahiro 21:1f1e9c585da8 345 } else {
yabahiro 21:1f1e9c585da8 346 dz1=true;
yabahiro 21:1f1e9c585da8 347 }
yabahiro 21:1f1e9c585da8 348
yabahiro 21:1f1e9c585da8 349 if(controller->Button.Y) {
yabahiro 21:1f1e9c585da8 350 if(dz2==true) {
yabahiro 21:1f1e9c585da8 351 if(dz2i==false) {
yabahiro 21:1f1e9c585da8 352 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 353 dz2i=true;
yabahiro 21:1f1e9c585da8 354 } else {
yabahiro 21:1f1e9c585da8 355 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 356 dz2i=false;
yabahiro 21:1f1e9c585da8 357 }
yabahiro 21:1f1e9c585da8 358 dz2=false;
yabahiro 21:1f1e9c585da8 359 }
yabahiro 21:1f1e9c585da8 360 } else {
yabahiro 21:1f1e9c585da8 361 dz2=true;
yabahiro 21:1f1e9c585da8 362 }
yabahiro 21:1f1e9c585da8 363
yabahiro 21:1f1e9c585da8 364 if(controller->Button.A) {
yabahiro 21:1f1e9c585da8 365 if(dz3==true) {
yabahiro 21:1f1e9c585da8 366 if(dz3i==false) {
yabahiro 21:1f1e9c585da8 367 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 368 dz3i=true;
yabahiro 21:1f1e9c585da8 369 } else {
yabahiro 21:1f1e9c585da8 370 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 371 dz3i=false;
yabahiro 21:1f1e9c585da8 372 }
yabahiro 21:1f1e9c585da8 373 dz3=false;
yabahiro 21:1f1e9c585da8 374 }
yabahiro 21:1f1e9c585da8 375 } else {
yabahiro 21:1f1e9c585da8 376 dz3=true;
yabahiro 21:1f1e9c585da8 377 }
yabahiro 21:1f1e9c585da8 378
yabahiro 21:1f1e9c585da8 379 if(controller->Button.B) {
yabahiro 21:1f1e9c585da8 380 if(dz4==true) {
yabahiro 21:1f1e9c585da8 381 if(dz4i==false) {
yabahiro 21:1f1e9c585da8 382 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 383 dz4i=true;
yabahiro 21:1f1e9c585da8 384 } else {
yabahiro 21:1f1e9c585da8 385 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 386 dz4i=false;
yabahiro 21:1f1e9c585da8 387 }
yabahiro 21:1f1e9c585da8 388 dz4=false;
yabahiro 21:1f1e9c585da8 389 }
yabahiro 21:1f1e9c585da8 390 } else {
yabahiro 21:1f1e9c585da8 391 dz4=true;
yabahiro 21:1f1e9c585da8 392 }
yabahiro 21:1f1e9c585da8 393 /*
yabahiro 21:1f1e9c585da8 394 if(controller->Button.RIGHT){
yabahiro 21:1f1e9c585da8 395 motor[MOTOR_1].dir = FOR;
yabahiro 21:1f1e9c585da8 396 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 397 if (ARM_1){
yabahiro 21:1f1e9c585da8 398 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 399 }
yabahiro 21:1f1e9c585da8 400 }
yabahiro 21:1f1e9c585da8 401 else if(controller->Button.LEFT){
yabahiro 21:1f1e9c585da8 402 motor[MOTOR_1].dir = BACK;
yabahiro 21:1f1e9c585da8 403 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 404 if (ARM_0){
yabahiro 21:1f1e9c585da8 405 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 406 }
yabahiro 21:1f1e9c585da8 407 }
yabahiro 21:1f1e9c585da8 408 */
t_yamamoto 0:669ef71cba68 409 }
t_yamamoto 0:669ef71cba68 410 #endif
t_yamamoto 0:669ef71cba68 411
t_yamamoto 0:669ef71cba68 412 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 413 bool mtc=false;
kishibekairohan 22:c47f29caded6 414 //追加
kishibekairohan 22:c47f29caded6 415 bool mtc2 = false;
yabahiro 21:1f1e9c585da8 416
kishibekairohan 16:3f2c2d89372b 417 static void Process2()
yabahiro 21:1f1e9c585da8 418 {
kishibekairohan 26:be5121da0f77 419 //追加
kishibekairohan 26:be5121da0f77 420 //Twsh = 2;
yabahiro 21:1f1e9c585da8 421 // 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 21:1f1e9c585da8 422 mode1g0++;
kishibekairohan 26:be5121da0f77 423
yabahiro 25:38d4964e8aad 424 if(mode==1) {
kishibekairohan 26:be5121da0f77 425 motor[TIRE_FR].pwm = 30;
yabahiro 25:38d4964e8aad 426 motor[TIRE_FR].dir = BACK;
kishibekairohan 26:be5121da0f77 427 motor[TIRE_FL].pwm = 30;
yabahiro 25:38d4964e8aad 428 motor[TIRE_FL].dir = FOR;
kishibekairohan 26:be5121da0f77 429 motor[TIRE_BR].pwm = 30;
yabahiro 25:38d4964e8aad 430 motor[TIRE_BR].dir = BACK;
kishibekairohan 26:be5121da0f77 431 motor[TIRE_BL].pwm = 30;
yabahiro 25:38d4964e8aad 432 motor[TIRE_BL].dir = FOR;
kishibekairohan 26:be5121da0f77 433 if(mode1g0>100000&&g[2]==0) {//変更5000→100000
yabahiro 25:38d4964e8aad 434 mode1g0=0;
yabahiro 25:38d4964e8aad 435 mode=2;
yabahiro 25:38d4964e8aad 436 }
kishibekairohan 26:be5121da0f77 437 }else if(mode==2) {
yabahiro 25:38d4964e8aad 438 switch(g[2]) {
yabahiro 25:38d4964e8aad 439 case 98:
yabahiro 25:38d4964e8aad 440 motor[TIRE_FR].pwm = 30;
yabahiro 25:38d4964e8aad 441 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 442 motor[TIRE_FL].pwm = 30;
yabahiro 25:38d4964e8aad 443 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 444 motor[TIRE_BR].pwm = 30;
yabahiro 25:38d4964e8aad 445 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 446 motor[TIRE_BL].pwm = 30;
yabahiro 25:38d4964e8aad 447 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 448 if(mode1g0>100000) {
yabahiro 25:38d4964e8aad 449 mode1++;
yabahiro 25:38d4964e8aad 450 mode1g0=0;
yabahiro 25:38d4964e8aad 451 }
yabahiro 25:38d4964e8aad 452 mtc2=true;
yabahiro 25:38d4964e8aad 453 if(mode1==1) {
yabahiro 25:38d4964e8aad 454 mode=3;
yabahiro 25:38d4964e8aad 455 mode1g0=0;
yabahiro 25:38d4964e8aad 456 mode1=0;
yabahiro 25:38d4964e8aad 457 }
yabahiro 25:38d4964e8aad 458 break;
yabahiro 25:38d4964e8aad 459 case 0:
yabahiro 25:38d4964e8aad 460 motor[TIRE_FR].pwm = 60;
yabahiro 25:38d4964e8aad 461 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 462 motor[TIRE_FL].pwm = 60;
yabahiro 25:38d4964e8aad 463 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 464 motor[TIRE_BR].pwm = 60;
yabahiro 25:38d4964e8aad 465 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 466 motor[TIRE_BL].pwm = 60;
yabahiro 25:38d4964e8aad 467 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 468 mtc2=true;
yabahiro 25:38d4964e8aad 469 break;
yabahiro 25:38d4964e8aad 470 case 255:
yabahiro 25:38d4964e8aad 471 motor[TIRE_FR].pwm = 40;
yabahiro 25:38d4964e8aad 472 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 473 motor[TIRE_FL].pwm = 60;
yabahiro 25:38d4964e8aad 474 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 475 motor[TIRE_BR].pwm = 60;
yabahiro 25:38d4964e8aad 476 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 477 motor[TIRE_BL].pwm = 40;
yabahiro 25:38d4964e8aad 478 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 479 mtc2=true;
yabahiro 25:38d4964e8aad 480 break;
yabahiro 25:38d4964e8aad 481 case 253:
yabahiro 25:38d4964e8aad 482 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 483 motor[TIRE_FR].dir = FREE;
yabahiro 25:38d4964e8aad 484 motor[TIRE_FL].pwm = 20;
yabahiro 25:38d4964e8aad 485 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 486 motor[TIRE_BR].pwm = 20;
yabahiro 25:38d4964e8aad 487 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 488 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 489 motor[TIRE_BL].dir = FREE;
yabahiro 25:38d4964e8aad 490 mtc2=true;
yabahiro 25:38d4964e8aad 491 break;
yabahiro 25:38d4964e8aad 492 case 254:
yabahiro 25:38d4964e8aad 493 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 494 motor[TIRE_FR].dir = FREE;
yabahiro 25:38d4964e8aad 495 motor[TIRE_FL].pwm = 30;
yabahiro 25:38d4964e8aad 496 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 497 motor[TIRE_BR].pwm = 30;
yabahiro 25:38d4964e8aad 498 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 499 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 500 motor[TIRE_BL].dir = FREE;
yabahiro 25:38d4964e8aad 501 mtc2=true;
yabahiro 25:38d4964e8aad 502 break;
yabahiro 25:38d4964e8aad 503 case 1:
yabahiro 25:38d4964e8aad 504 motor[TIRE_FR].pwm = 60;
yabahiro 25:38d4964e8aad 505 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 506 motor[TIRE_FL].pwm = 40;
yabahiro 25:38d4964e8aad 507 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 508 motor[TIRE_BR].pwm = 40;
yabahiro 25:38d4964e8aad 509 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 510 motor[TIRE_BL].pwm = 60;
yabahiro 25:38d4964e8aad 511 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 512 mtc2=true;
yabahiro 25:38d4964e8aad 513 break;
yabahiro 25:38d4964e8aad 514 case 3:
yabahiro 25:38d4964e8aad 515 motor[TIRE_FR].pwm = 20;
yabahiro 25:38d4964e8aad 516 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 517 motor[TIRE_FL].pwm = 0;
yabahiro 25:38d4964e8aad 518 motor[TIRE_FL].dir = FREE;
yabahiro 25:38d4964e8aad 519 motor[TIRE_BR].pwm = 0;
yabahiro 25:38d4964e8aad 520 motor[TIRE_BR].dir = FREE;
yabahiro 25:38d4964e8aad 521 motor[TIRE_BL].pwm = 20;
yabahiro 25:38d4964e8aad 522 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 523 mtc2=true;
yabahiro 25:38d4964e8aad 524 break;
yabahiro 25:38d4964e8aad 525 case 2:
yabahiro 25:38d4964e8aad 526 motor[TIRE_FR].pwm = 30;
yabahiro 25:38d4964e8aad 527 motor[TIRE_FR].dir = FOR;
yabahiro 25:38d4964e8aad 528 motor[TIRE_FL].pwm = 0;
yabahiro 25:38d4964e8aad 529 motor[TIRE_FL].dir = FREE;
yabahiro 25:38d4964e8aad 530 motor[TIRE_BR].pwm = 0;
yabahiro 25:38d4964e8aad 531 motor[TIRE_BR].dir = FREE;
yabahiro 25:38d4964e8aad 532 motor[TIRE_BL].pwm = 30;
yabahiro 25:38d4964e8aad 533 motor[TIRE_BL].dir = BACK;
yabahiro 25:38d4964e8aad 534 mtc2=true;
yabahiro 25:38d4964e8aad 535 break;
yabahiro 25:38d4964e8aad 536 }
yabahiro 25:38d4964e8aad 537 if(g[2]!=98&&mtc2==true) {
yabahiro 25:38d4964e8aad 538 switch(g[3]) {
yabahiro 25:38d4964e8aad 539 case 0:
yabahiro 25:38d4964e8aad 540 mtc2=false;
yabahiro 25:38d4964e8aad 541 break;
yabahiro 25:38d4964e8aad 542 case 255:
yabahiro 25:38d4964e8aad 543 motor[TIRE_BL].pwm += 5;
yabahiro 25:38d4964e8aad 544 mtc2=false;
yabahiro 25:38d4964e8aad 545 break;
yabahiro 25:38d4964e8aad 546 case 253:
yabahiro 25:38d4964e8aad 547 motor[TIRE_BL].pwm += 10;
yabahiro 25:38d4964e8aad 548 mtc2=false;
yabahiro 25:38d4964e8aad 549 break;
yabahiro 25:38d4964e8aad 550 case 254:
yabahiro 25:38d4964e8aad 551 motor[TIRE_BL].pwm += 20;
yabahiro 25:38d4964e8aad 552 motor[TIRE_FL].pwm = 0;
yabahiro 25:38d4964e8aad 553 mtc2=false;
yabahiro 25:38d4964e8aad 554 break;
yabahiro 25:38d4964e8aad 555 case 1:
yabahiro 25:38d4964e8aad 556 motor[TIRE_FL].pwm += 5;
yabahiro 25:38d4964e8aad 557 mtc2=false;
yabahiro 25:38d4964e8aad 558 break;
yabahiro 25:38d4964e8aad 559 case 3:
yabahiro 25:38d4964e8aad 560 motor[TIRE_FL].pwm += 10;
yabahiro 25:38d4964e8aad 561 mtc2=false;
yabahiro 25:38d4964e8aad 562 break;
yabahiro 25:38d4964e8aad 563 case 2:
yabahiro 25:38d4964e8aad 564 motor[TIRE_FL].pwm += 20;
yabahiro 25:38d4964e8aad 565 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 566 mtc2=false;
yabahiro 25:38d4964e8aad 567 break;
yabahiro 25:38d4964e8aad 568 }
yabahiro 25:38d4964e8aad 569 }
yabahiro 25:38d4964e8aad 570 } else if(mode==3) {
yabahiro 25:38d4964e8aad 571 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 572 motor[TIRE_FR].dir = FREE;
yabahiro 25:38d4964e8aad 573 motor[TIRE_FL].pwm = 15;
yabahiro 25:38d4964e8aad 574 motor[TIRE_FL].dir = FOR;
yabahiro 25:38d4964e8aad 575 motor[TIRE_BR].pwm = 15;
yabahiro 25:38d4964e8aad 576 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 577 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 578 motor[TIRE_BL].dir = FREE;
yabahiro 25:38d4964e8aad 579 if(mode1g0>5000&&g[0]==0) {
yabahiro 25:38d4964e8aad 580 mode1g0=0;
yabahiro 25:38d4964e8aad 581 mode=4;
yabahiro 25:38d4964e8aad 582 }
yabahiro 25:38d4964e8aad 583 } else if(mode==4) {
yabahiro 25:38d4964e8aad 584 motor[TIRE_FR].pwm = 15;
yabahiro 25:38d4964e8aad 585 motor[TIRE_FR].dir = BACK;
yabahiro 25:38d4964e8aad 586 motor[TIRE_FL].pwm = 15;
yabahiro 25:38d4964e8aad 587 motor[TIRE_FL].dir = BACK;
yabahiro 25:38d4964e8aad 588 motor[TIRE_BR].pwm = 15;
yabahiro 25:38d4964e8aad 589 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 590 motor[TIRE_BL].pwm = 15;
yabahiro 25:38d4964e8aad 591 motor[TIRE_BL].dir = FOR;
yabahiro 25:38d4964e8aad 592 if(mode1g0>10000&&g[0]==0) {
yabahiro 25:38d4964e8aad 593 mode1g0=0;
yabahiro 25:38d4964e8aad 594 mode1=0;
yabahiro 25:38d4964e8aad 595 mode=10;
yabahiro 25:38d4964e8aad 596 }
yabahiro 25:38d4964e8aad 597 } else if(g[0]!=99&&mode==10) {
yabahiro 21:1f1e9c585da8 598 switch(g[0]) {
yabahiro 21:1f1e9c585da8 599 case 98:
yabahiro 21:1f1e9c585da8 600 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 601 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 602 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 603 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 604 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 605 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 606 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 607 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 608 if(mode1g0>100000) {
yabahiro 21:1f1e9c585da8 609 mode1++;
yabahiro 21:1f1e9c585da8 610 mode1g0=0;
yabahiro 21:1f1e9c585da8 611 }
yabahiro 21:1f1e9c585da8 612 mtc=true;
yabahiro 25:38d4964e8aad 613 if(mode1==Twsh) {
yabahiro 23:694ecfa97839 614 mode=11;
yabahiro 23:694ecfa97839 615 mode1g0=0;
yabahiro 21:1f1e9c585da8 616 }
yabahiro 21:1f1e9c585da8 617 break;
yabahiro 21:1f1e9c585da8 618 case 0:
yabahiro 21:1f1e9c585da8 619 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 620 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 621 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 622 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 623 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 624 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 625 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 626 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 627 mtc=true;
yabahiro 21:1f1e9c585da8 628 break;
yabahiro 21:1f1e9c585da8 629 case 255:
yabahiro 21:1f1e9c585da8 630 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 631 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 632 motor[TIRE_FL].pwm = 40;
yabahiro 21:1f1e9c585da8 633 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 634 motor[TIRE_BR].pwm = 40;
yabahiro 21:1f1e9c585da8 635 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 636 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 637 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 638 mtc=true;
yabahiro 21:1f1e9c585da8 639 break;
yabahiro 21:1f1e9c585da8 640 case 253:
yabahiro 21:1f1e9c585da8 641 motor[TIRE_FR].pwm = 20;
yabahiro 21:1f1e9c585da8 642 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 643 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 644 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 645 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 646 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 647 motor[TIRE_BL].pwm = 20;
yabahiro 21:1f1e9c585da8 648 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 649 mtc=true;
yabahiro 21:1f1e9c585da8 650 break;
yabahiro 21:1f1e9c585da8 651 case 254:
yabahiro 21:1f1e9c585da8 652 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 653 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 654 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 655 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 656 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 657 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 658 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 659 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 660 mtc=true;
yabahiro 21:1f1e9c585da8 661 break;
yabahiro 21:1f1e9c585da8 662 case 1:
yabahiro 21:1f1e9c585da8 663 motor[TIRE_FR].pwm = 40;
yabahiro 21:1f1e9c585da8 664 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 665 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 666 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 667 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 668 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 669 motor[TIRE_BL].pwm = 40;
yabahiro 21:1f1e9c585da8 670 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 671 mtc=true;
yabahiro 21:1f1e9c585da8 672 break;
yabahiro 21:1f1e9c585da8 673 case 3:
yabahiro 21:1f1e9c585da8 674 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 675 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 676 motor[TIRE_FL].pwm = 20;
yabahiro 21:1f1e9c585da8 677 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 678 motor[TIRE_BR].pwm = 20;
yabahiro 21:1f1e9c585da8 679 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 680 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 681 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 682 mtc=true;
yabahiro 21:1f1e9c585da8 683 break;
yabahiro 21:1f1e9c585da8 684 case 2:
yabahiro 21:1f1e9c585da8 685 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 686 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 687 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 688 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 689 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 690 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 691 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 692 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 693 mtc=true;
yabahiro 21:1f1e9c585da8 694 break;
yabahiro 21:1f1e9c585da8 695 }
yabahiro 21:1f1e9c585da8 696 if(g[0]!=98&&mtc==true) {
yabahiro 21:1f1e9c585da8 697 switch(g[1]) {
yabahiro 21:1f1e9c585da8 698 case 0:
yabahiro 21:1f1e9c585da8 699 mtc=false;
yabahiro 21:1f1e9c585da8 700 break;
yabahiro 21:1f1e9c585da8 701 case 255:
yabahiro 21:1f1e9c585da8 702 motor[TIRE_BR].pwm += 5;
yabahiro 21:1f1e9c585da8 703 mtc=false;
yabahiro 21:1f1e9c585da8 704 break;
yabahiro 21:1f1e9c585da8 705 case 253:
yabahiro 21:1f1e9c585da8 706 motor[TIRE_BR].pwm += 10;
yabahiro 21:1f1e9c585da8 707 mtc=false;
yabahiro 21:1f1e9c585da8 708 break;
yabahiro 21:1f1e9c585da8 709 case 254:
yabahiro 21:1f1e9c585da8 710 motor[TIRE_BR].pwm += 20;
yabahiro 21:1f1e9c585da8 711 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 712 mtc=false;
yabahiro 21:1f1e9c585da8 713 break;
yabahiro 21:1f1e9c585da8 714 case 1:
yabahiro 21:1f1e9c585da8 715 motor[TIRE_BL].pwm += 5;
yabahiro 21:1f1e9c585da8 716 mtc=false;
yabahiro 21:1f1e9c585da8 717 break;
yabahiro 21:1f1e9c585da8 718 case 3:
yabahiro 21:1f1e9c585da8 719 motor[TIRE_BL].pwm += 10;
yabahiro 21:1f1e9c585da8 720 mtc=false;
yabahiro 21:1f1e9c585da8 721 break;
yabahiro 21:1f1e9c585da8 722 case 2:
yabahiro 21:1f1e9c585da8 723 motor[TIRE_BL].pwm += 20;
yabahiro 21:1f1e9c585da8 724 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 725 mtc=false;
yabahiro 21:1f1e9c585da8 726 break;
yabahiro 21:1f1e9c585da8 727 }
yabahiro 21:1f1e9c585da8 728 }
yabahiro 23:694ecfa97839 729 } else if(mode==11) {
yabahiro 25:38d4964e8aad 730 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 731 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 732 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 733 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 734 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 735 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 736 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 737 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 738 if(mode1g0>5000&&g[2]==0) {
yabahiro 23:694ecfa97839 739 mode1g0=0;
yabahiro 23:694ecfa97839 740 mode=12;
yabahiro 21:1f1e9c585da8 741 }
yabahiro 23:694ecfa97839 742 } else if(mode==12) {
yabahiro 21:1f1e9c585da8 743 motor[TIRE_FR].pwm = 15;
yabahiro 21:1f1e9c585da8 744 motor[TIRE_FR].dir = FOR;
yabahiro 21:1f1e9c585da8 745 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 746 motor[TIRE_FL].dir = BACK;
yabahiro 21:1f1e9c585da8 747 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 748 motor[TIRE_BR].dir = FOR;
yabahiro 21:1f1e9c585da8 749 motor[TIRE_BL].pwm = 15;
yabahiro 21:1f1e9c585da8 750 motor[TIRE_BL].dir = BACK;
yabahiro 21:1f1e9c585da8 751 mode1g0++;
yabahiro 21:1f1e9c585da8 752 if(mode1g0>10000&&g[2]==0) {
yabahiro 23:694ecfa97839 753 mode1g0=0;
yabahiro 23:694ecfa97839 754 mode1=0;
yabahiro 23:694ecfa97839 755 mode=20;
yabahiro 21:1f1e9c585da8 756 }
yabahiro 23:694ecfa97839 757 } else if(mode==20) {
yabahiro 23:694ecfa97839 758 switch(g[2]) {
kishibekairohan 22:c47f29caded6 759 case 98:
kishibekairohan 22:c47f29caded6 760 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 761 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 762 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 763 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 764 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 765 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 766 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 767 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 768 if(mode1g0>100000) {
kishibekairohan 22:c47f29caded6 769 mode1++;
kishibekairohan 22:c47f29caded6 770 mode1g0=0;
kishibekairohan 22:c47f29caded6 771 }
kishibekairohan 22:c47f29caded6 772 mtc2=true;
yabahiro 23:694ecfa97839 773 if(mode1==3) {
yabahiro 23:694ecfa97839 774 mode=21;
yabahiro 23:694ecfa97839 775 mode1g0=0;
yabahiro 23:694ecfa97839 776 mode1=0;
kishibekairohan 22:c47f29caded6 777 }
kishibekairohan 22:c47f29caded6 778 break;
kishibekairohan 22:c47f29caded6 779 case 0:
kishibekairohan 22:c47f29caded6 780 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 781 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 782 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 783 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 784 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 785 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 786 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 787 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 788 mtc2=true;
kishibekairohan 22:c47f29caded6 789 break;
kishibekairohan 22:c47f29caded6 790 case 255:
kishibekairohan 22:c47f29caded6 791 motor[TIRE_FR].pwm = 40;
kishibekairohan 22:c47f29caded6 792 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 793 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 794 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 795 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 796 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 797 motor[TIRE_BL].pwm = 40;
kishibekairohan 22:c47f29caded6 798 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 799 mtc2=true;
kishibekairohan 22:c47f29caded6 800 break;
kishibekairohan 22:c47f29caded6 801 case 253:
kishibekairohan 22:c47f29caded6 802 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 803 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 804 motor[TIRE_FL].pwm = 20;
kishibekairohan 22:c47f29caded6 805 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 806 motor[TIRE_BR].pwm = 20;
kishibekairohan 22:c47f29caded6 807 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 808 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 809 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 810 mtc2=true;
kishibekairohan 22:c47f29caded6 811 break;
kishibekairohan 22:c47f29caded6 812 case 254:
kishibekairohan 22:c47f29caded6 813 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 814 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 815 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 816 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 817 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 818 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 819 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 820 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 821 mtc2=true;
kishibekairohan 22:c47f29caded6 822 break;
kishibekairohan 22:c47f29caded6 823 case 1:
kishibekairohan 22:c47f29caded6 824 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 825 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 826 motor[TIRE_FL].pwm = 40;
kishibekairohan 22:c47f29caded6 827 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 828 motor[TIRE_BR].pwm = 40;
kishibekairohan 22:c47f29caded6 829 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 830 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 831 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 832 mtc2=true;
kishibekairohan 22:c47f29caded6 833 break;
kishibekairohan 22:c47f29caded6 834 case 3:
kishibekairohan 22:c47f29caded6 835 motor[TIRE_FR].pwm = 20;
kishibekairohan 22:c47f29caded6 836 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 837 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 838 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 839 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 840 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 841 motor[TIRE_BL].pwm = 20;
kishibekairohan 22:c47f29caded6 842 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 843 mtc2=true;
kishibekairohan 22:c47f29caded6 844 break;
kishibekairohan 22:c47f29caded6 845 case 2:
kishibekairohan 22:c47f29caded6 846 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 847 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 848 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 849 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 850 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 851 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 852 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 853 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 854 mtc2=true;
kishibekairohan 22:c47f29caded6 855 break;
kishibekairohan 22:c47f29caded6 856 }
kishibekairohan 22:c47f29caded6 857 if(g[2]!=98&&mtc2==true) {
kishibekairohan 22:c47f29caded6 858 switch(g[3]) {
kishibekairohan 22:c47f29caded6 859 case 0:
kishibekairohan 22:c47f29caded6 860 mtc2=false;
kishibekairohan 22:c47f29caded6 861 break;
kishibekairohan 22:c47f29caded6 862 case 255:
kishibekairohan 22:c47f29caded6 863 motor[TIRE_BL].pwm += 5;
kishibekairohan 22:c47f29caded6 864 mtc2=false;
kishibekairohan 22:c47f29caded6 865 break;
kishibekairohan 22:c47f29caded6 866 case 253:
kishibekairohan 22:c47f29caded6 867 motor[TIRE_BL].pwm += 10;
kishibekairohan 22:c47f29caded6 868 mtc2=false;
kishibekairohan 22:c47f29caded6 869 break;
kishibekairohan 22:c47f29caded6 870 case 254:
kishibekairohan 22:c47f29caded6 871 motor[TIRE_BL].pwm += 20;
kishibekairohan 22:c47f29caded6 872 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 873 mtc2=false;
kishibekairohan 22:c47f29caded6 874 break;
kishibekairohan 22:c47f29caded6 875 case 1:
kishibekairohan 22:c47f29caded6 876 motor[TIRE_FL].pwm += 5;
kishibekairohan 22:c47f29caded6 877 mtc2=false;
kishibekairohan 22:c47f29caded6 878 break;
kishibekairohan 22:c47f29caded6 879 case 3:
kishibekairohan 22:c47f29caded6 880 motor[TIRE_FL].pwm += 10;
kishibekairohan 22:c47f29caded6 881 mtc2=false;
kishibekairohan 22:c47f29caded6 882 break;
kishibekairohan 22:c47f29caded6 883 case 2:
kishibekairohan 22:c47f29caded6 884 motor[TIRE_FL].pwm += 20;
kishibekairohan 22:c47f29caded6 885 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 886 mtc2=false;
kishibekairohan 22:c47f29caded6 887 break;
kishibekairohan 22:c47f29caded6 888 }
kishibekairohan 22:c47f29caded6 889 }
yabahiro 23:694ecfa97839 890 } else if(mode==21) {
yabahiro 25:38d4964e8aad 891 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 892 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 893 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 894 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 895 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 896 motor[TIRE_BR].dir = BACK;
yabahiro 25:38d4964e8aad 897 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 898 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 899 if(mode1g0>5000&&g[0]==0) {
yabahiro 23:694ecfa97839 900 mode1g0=0;
yabahiro 23:694ecfa97839 901 mode=22;
yabahiro 23:694ecfa97839 902 }
yabahiro 23:694ecfa97839 903 } else if(mode==22) {
yabahiro 23:694ecfa97839 904 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 905 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 906 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 907 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 908 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 909 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 910 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 911 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 912 if(mode1g0>10000&&g[0]==0) {
yabahiro 23:694ecfa97839 913 mode1g0=0;
yabahiro 23:694ecfa97839 914 mode1=0;
yabahiro 23:694ecfa97839 915 mode=30;
yabahiro 23:694ecfa97839 916 }
yabahiro 23:694ecfa97839 917 } else if(mode==30) {
yabahiro 23:694ecfa97839 918 switch(g[0]) {
yabahiro 23:694ecfa97839 919 case 98:
yabahiro 23:694ecfa97839 920 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 921 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 922 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 923 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 924 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 925 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 926 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 927 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 928 if(mode1g0>100000) {
yabahiro 23:694ecfa97839 929 mode1++;
yabahiro 23:694ecfa97839 930 mode1g0=0;
yabahiro 23:694ecfa97839 931 }
yabahiro 23:694ecfa97839 932 mtc=true;
yabahiro 23:694ecfa97839 933 if(mode1==1) {
yabahiro 23:694ecfa97839 934 mode=31;
yabahiro 23:694ecfa97839 935 mode1g0=0;
yabahiro 23:694ecfa97839 936 }
yabahiro 23:694ecfa97839 937 break;
yabahiro 23:694ecfa97839 938 case 0:
yabahiro 23:694ecfa97839 939 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 940 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 941 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 942 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 943 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 944 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 945 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 946 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 947 mtc=true;
yabahiro 23:694ecfa97839 948 break;
yabahiro 23:694ecfa97839 949 case 255:
yabahiro 23:694ecfa97839 950 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 951 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 952 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 953 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 954 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 955 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 956 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 957 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 958 mtc=true;
yabahiro 23:694ecfa97839 959 break;
yabahiro 23:694ecfa97839 960 case 253:
yabahiro 23:694ecfa97839 961 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 962 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 963 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 964 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 965 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 966 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 967 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 968 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 969 mtc=true;
yabahiro 23:694ecfa97839 970 break;
yabahiro 23:694ecfa97839 971 case 254:
yabahiro 23:694ecfa97839 972 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 973 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 974 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 975 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 976 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 977 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 978 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 979 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 980 mtc=true;
yabahiro 23:694ecfa97839 981 break;
yabahiro 23:694ecfa97839 982 case 1:
yabahiro 23:694ecfa97839 983 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 984 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 985 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 986 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 987 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 988 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 989 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 990 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 991 mtc=true;
yabahiro 23:694ecfa97839 992 break;
yabahiro 23:694ecfa97839 993 case 3:
yabahiro 23:694ecfa97839 994 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 995 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 996 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 997 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 998 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 999 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 1000 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1001 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1002 mtc=true;
yabahiro 23:694ecfa97839 1003 break;
yabahiro 23:694ecfa97839 1004 case 2:
yabahiro 23:694ecfa97839 1005 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1006 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1007 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1008 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 1009 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1010 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 1011 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1012 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1013 mtc=true;
yabahiro 23:694ecfa97839 1014 break;
yabahiro 23:694ecfa97839 1015 }
yabahiro 23:694ecfa97839 1016 if(g[0]!=98&&mtc==true) {
yabahiro 23:694ecfa97839 1017 switch(g[1]) {
yabahiro 23:694ecfa97839 1018 case 0:
yabahiro 23:694ecfa97839 1019 mtc=false;
yabahiro 23:694ecfa97839 1020 break;
yabahiro 23:694ecfa97839 1021 case 255:
yabahiro 23:694ecfa97839 1022 motor[TIRE_BR].pwm += 5;
yabahiro 23:694ecfa97839 1023 mtc=false;
yabahiro 23:694ecfa97839 1024 break;
yabahiro 23:694ecfa97839 1025 case 253:
yabahiro 23:694ecfa97839 1026 motor[TIRE_BR].pwm += 10;
yabahiro 23:694ecfa97839 1027 mtc=false;
yabahiro 23:694ecfa97839 1028 break;
yabahiro 23:694ecfa97839 1029 case 254:
yabahiro 23:694ecfa97839 1030 motor[TIRE_BR].pwm += 20;
yabahiro 23:694ecfa97839 1031 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1032 mtc=false;
yabahiro 23:694ecfa97839 1033 break;
yabahiro 23:694ecfa97839 1034 case 1:
yabahiro 23:694ecfa97839 1035 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 1036 mtc=false;
yabahiro 23:694ecfa97839 1037 break;
yabahiro 23:694ecfa97839 1038 case 3:
yabahiro 23:694ecfa97839 1039 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 1040 mtc=false;
yabahiro 23:694ecfa97839 1041 break;
yabahiro 23:694ecfa97839 1042 case 2:
yabahiro 23:694ecfa97839 1043 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 1044 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1045 mtc=false;
yabahiro 23:694ecfa97839 1046 break;
yabahiro 23:694ecfa97839 1047 }
yabahiro 23:694ecfa97839 1048 }
yabahiro 23:694ecfa97839 1049 } else if(mode==31) {
yabahiro 23:694ecfa97839 1050 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1051 motor[TIRE_FR].dir = BACK;
yabahiro 25:38d4964e8aad 1052 motor[TIRE_FL].pwm = 0;
yabahiro 25:38d4964e8aad 1053 motor[TIRE_FL].dir = FREE;
yabahiro 25:38d4964e8aad 1054 motor[TIRE_BR].pwm = 0;
yabahiro 25:38d4964e8aad 1055 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1056 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1057 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1058 mode1g0++;
yabahiro 23:694ecfa97839 1059 if(mode1g0>5000&&g[3]==0) {
yabahiro 23:694ecfa97839 1060 mode1g0=0;
yabahiro 23:694ecfa97839 1061 mode=32;
yabahiro 23:694ecfa97839 1062 }
yabahiro 23:694ecfa97839 1063 } else if(mode==32) {
yabahiro 23:694ecfa97839 1064 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1065 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1066 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1067 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1068 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1069 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1070 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1071 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1072 mode1g0++;
yabahiro 23:694ecfa97839 1073 if(mode1g0>5000&&g[3]==0) {
yabahiro 23:694ecfa97839 1074 mode1g0=0;
yabahiro 23:694ecfa97839 1075 mode1=0;
yabahiro 23:694ecfa97839 1076 mode=40;
yabahiro 23:694ecfa97839 1077 }
yabahiro 23:694ecfa97839 1078 } else if(mode==40) {
yabahiro 23:694ecfa97839 1079 switch(g[3]) {
yabahiro 23:694ecfa97839 1080 case 98:
yabahiro 23:694ecfa97839 1081 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 1082 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1083 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1084 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1085 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1086 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1087 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 1088 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1089 if(mode1g0>100000) {
yabahiro 23:694ecfa97839 1090 mode1++;
yabahiro 23:694ecfa97839 1091 mode1g0=0;
yabahiro 23:694ecfa97839 1092 }
yabahiro 23:694ecfa97839 1093 mtc2=true;
yabahiro 23:694ecfa97839 1094 if(mode1==1) {
yabahiro 23:694ecfa97839 1095 mode=41;
yabahiro 23:694ecfa97839 1096 mode1g0=0;
yabahiro 23:694ecfa97839 1097 mode1=0;
yabahiro 23:694ecfa97839 1098 }
yabahiro 23:694ecfa97839 1099 break;
yabahiro 23:694ecfa97839 1100 case 0:
yabahiro 23:694ecfa97839 1101 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 1102 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1103 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 1104 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1105 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 1106 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1107 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 1108 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1109 mtc2=true;
yabahiro 23:694ecfa97839 1110 break;
yabahiro 23:694ecfa97839 1111 case 255:
yabahiro 23:694ecfa97839 1112 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 1113 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1114 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 1115 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1116 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 1117 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1118 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 1119 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1120 mtc2=true;
yabahiro 23:694ecfa97839 1121 break;
yabahiro 23:694ecfa97839 1122 case 253:
yabahiro 23:694ecfa97839 1123 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1124 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1125 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 1126 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1127 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 1128 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1129 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1130 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1131 mtc2=true;
yabahiro 23:694ecfa97839 1132 break;
yabahiro 23:694ecfa97839 1133 case 254:
yabahiro 23:694ecfa97839 1134 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1135 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1136 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1137 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1138 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1139 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1140 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1141 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1142 mtc2=true;
yabahiro 23:694ecfa97839 1143 break;
yabahiro 23:694ecfa97839 1144 case 1:
yabahiro 23:694ecfa97839 1145 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 1146 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1147 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 1148 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1149 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 1150 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1151 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 1152 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1153 mtc2=true;
yabahiro 23:694ecfa97839 1154 break;
yabahiro 23:694ecfa97839 1155 case 3:
yabahiro 23:694ecfa97839 1156 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 1157 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1158 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1159 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1160 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1161 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1162 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 1163 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1164 mtc2=true;
yabahiro 23:694ecfa97839 1165 break;
yabahiro 23:694ecfa97839 1166 case 2:
yabahiro 23:694ecfa97839 1167 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 1168 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1169 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1170 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1171 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1172 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1173 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 1174 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1175 mtc2=true;
yabahiro 23:694ecfa97839 1176 break;
yabahiro 23:694ecfa97839 1177 }
yabahiro 23:694ecfa97839 1178 if(g[2]!=98&&mtc2==true) {
yabahiro 23:694ecfa97839 1179 switch(g[2]) {
yabahiro 23:694ecfa97839 1180 case 0:
yabahiro 23:694ecfa97839 1181 mtc2=false;
yabahiro 23:694ecfa97839 1182 break;
yabahiro 23:694ecfa97839 1183 case 255:
yabahiro 23:694ecfa97839 1184 motor[TIRE_FL].pwm += 5;
yabahiro 23:694ecfa97839 1185 mtc2=false;
yabahiro 23:694ecfa97839 1186 break;
yabahiro 23:694ecfa97839 1187 case 253:
yabahiro 23:694ecfa97839 1188 motor[TIRE_FL].pwm += 10;
yabahiro 23:694ecfa97839 1189 mtc2=false;
yabahiro 23:694ecfa97839 1190 break;
yabahiro 23:694ecfa97839 1191 case 254:
yabahiro 23:694ecfa97839 1192 motor[TIRE_FL].pwm += 20;
yabahiro 23:694ecfa97839 1193 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1194 mtc2=false;
yabahiro 23:694ecfa97839 1195 break;
yabahiro 23:694ecfa97839 1196 case 1:
yabahiro 23:694ecfa97839 1197 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 1198 mtc2=false;
yabahiro 23:694ecfa97839 1199 break;
yabahiro 23:694ecfa97839 1200 case 3:
yabahiro 23:694ecfa97839 1201 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 1202 mtc2=false;
yabahiro 23:694ecfa97839 1203 break;
yabahiro 23:694ecfa97839 1204 case 2:
yabahiro 23:694ecfa97839 1205 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 1206 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1207 mtc2=false;
yabahiro 23:694ecfa97839 1208 break;
yabahiro 23:694ecfa97839 1209 }
yabahiro 23:694ecfa97839 1210 }
yabahiro 23:694ecfa97839 1211 } else if(mode==41) {
yabahiro 25:38d4964e8aad 1212 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1213 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1214 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1215 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1216 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1217 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1218 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1219 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1220 if(mode1g0>5000&&g[1]==0) {
yabahiro 23:694ecfa97839 1221 mode1g0=0;
yabahiro 23:694ecfa97839 1222 mode=42;
yabahiro 23:694ecfa97839 1223 }
yabahiro 23:694ecfa97839 1224 } else if(mode==42) {
yabahiro 23:694ecfa97839 1225 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1226 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1227 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1228 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 1229 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1230 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 1231 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1232 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1233 if(mode1g0>5000&&g[1]==0) {
yabahiro 23:694ecfa97839 1234 mode1g0=0;
yabahiro 23:694ecfa97839 1235 mode=50;
yabahiro 24:41c23c9a5058 1236 }
yabahiro 24:41c23c9a5058 1237 } else if(mode==50) {
yabahiro 24:41c23c9a5058 1238 switch(g[1]) {
yabahiro 24:41c23c9a5058 1239 case 98:
yabahiro 24:41c23c9a5058 1240 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1241 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1242 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1243 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1244 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1245 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1246 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1247 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1248 if(mode1g0>100000) {
yabahiro 24:41c23c9a5058 1249 mode1++;
yabahiro 24:41c23c9a5058 1250 mode1g0=0;
yabahiro 24:41c23c9a5058 1251 }
yabahiro 24:41c23c9a5058 1252 mtc=true;
yabahiro 24:41c23c9a5058 1253 if(mode1==1) {
yabahiro 24:41c23c9a5058 1254 mode=51;
yabahiro 24:41c23c9a5058 1255 mode1g0=0;
yabahiro 24:41c23c9a5058 1256 }
yabahiro 24:41c23c9a5058 1257 break;
yabahiro 24:41c23c9a5058 1258 case 0:
yabahiro 24:41c23c9a5058 1259 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1260 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1261 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1262 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1263 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1264 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1265 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1266 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1267 mtc=true;
yabahiro 24:41c23c9a5058 1268 break;
yabahiro 24:41c23c9a5058 1269 case 255:
yabahiro 24:41c23c9a5058 1270 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1271 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1272 motor[TIRE_FL].pwm = 40;
yabahiro 24:41c23c9a5058 1273 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1274 motor[TIRE_BR].pwm = 40;
yabahiro 24:41c23c9a5058 1275 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1276 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1277 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1278 mtc=true;
yabahiro 24:41c23c9a5058 1279 break;
yabahiro 24:41c23c9a5058 1280 case 253:
yabahiro 24:41c23c9a5058 1281 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1282 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1283 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1284 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1285 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1286 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1287 motor[TIRE_BL].pwm = 20;
yabahiro 24:41c23c9a5058 1288 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1289 mtc=true;
yabahiro 24:41c23c9a5058 1290 break;
yabahiro 24:41c23c9a5058 1291 case 254:
yabahiro 24:41c23c9a5058 1292 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1293 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1294 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1295 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1296 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1297 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1298 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1299 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1300 mtc=true;
yabahiro 24:41c23c9a5058 1301 break;
yabahiro 24:41c23c9a5058 1302 case 1:
yabahiro 24:41c23c9a5058 1303 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1304 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1305 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1306 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1307 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1308 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1309 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1310 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1311 mtc=true;
yabahiro 24:41c23c9a5058 1312 break;
yabahiro 24:41c23c9a5058 1313 case 3:
yabahiro 24:41c23c9a5058 1314 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1315 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1316 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1317 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1318 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1319 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1320 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1321 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1322 mtc=true;
yabahiro 24:41c23c9a5058 1323 break;
yabahiro 24:41c23c9a5058 1324 case 2:
yabahiro 24:41c23c9a5058 1325 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1326 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1327 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1328 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1329 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1330 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1331 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1332 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1333 mtc=true;
yabahiro 24:41c23c9a5058 1334 break;
yabahiro 24:41c23c9a5058 1335 }
yabahiro 24:41c23c9a5058 1336 if(g[1]!=98&&mtc==true) {
yabahiro 24:41c23c9a5058 1337 switch(g[0]) {
yabahiro 23:694ecfa97839 1338 case 0:
yabahiro 24:41c23c9a5058 1339 mtc=false;
yabahiro 23:694ecfa97839 1340 break;
yabahiro 23:694ecfa97839 1341 case 255:
yabahiro 24:41c23c9a5058 1342 motor[TIRE_FR].pwm += 5;
yabahiro 24:41c23c9a5058 1343 mtc=false;
yabahiro 23:694ecfa97839 1344 break;
yabahiro 23:694ecfa97839 1345 case 253:
yabahiro 24:41c23c9a5058 1346 motor[TIRE_FR].pwm += 10;
yabahiro 24:41c23c9a5058 1347 mtc=false;
yabahiro 23:694ecfa97839 1348 break;
yabahiro 23:694ecfa97839 1349 case 254:
yabahiro 24:41c23c9a5058 1350 motor[TIRE_FR].pwm += 20;
yabahiro 23:694ecfa97839 1351 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1352 mtc=false;
yabahiro 23:694ecfa97839 1353 break;
yabahiro 23:694ecfa97839 1354 case 1:
yabahiro 24:41c23c9a5058 1355 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1356 mtc=false;
yabahiro 23:694ecfa97839 1357 break;
yabahiro 23:694ecfa97839 1358 case 3:
yabahiro 24:41c23c9a5058 1359 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1360 mtc=false;
yabahiro 23:694ecfa97839 1361 break;
yabahiro 23:694ecfa97839 1362 case 2:
yabahiro 24:41c23c9a5058 1363 motor[TIRE_FL].pwm += 20;
yabahiro 23:694ecfa97839 1364 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1365 mtc=false;
yabahiro 23:694ecfa97839 1366 break;
yabahiro 23:694ecfa97839 1367 }
yabahiro 23:694ecfa97839 1368 }
yabahiro 23:694ecfa97839 1369 } else if(mode==51) {
yabahiro 25:38d4964e8aad 1370 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1371 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1372 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1373 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1374 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1375 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1376 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1377 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1378 mode1g0++;
yabahiro 24:41c23c9a5058 1379 if(mode1g0>5000&&g[3]==0) {
yabahiro 24:41c23c9a5058 1380 mode1g0=0;
yabahiro 24:41c23c9a5058 1381 mode=52;
yabahiro 24:41c23c9a5058 1382 }
yabahiro 24:41c23c9a5058 1383 } else if(mode==52) {
yabahiro 24:41c23c9a5058 1384 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1385 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1386 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1387 motor[TIRE_FL].dir = FOR;
yabahiro 24:41c23c9a5058 1388 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1389 motor[TIRE_BR].dir = BACK;
yabahiro 24:41c23c9a5058 1390 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1391 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1392 mode1g0++;
yabahiro 24:41c23c9a5058 1393 if(mode1g0>5000&&g[3]==0) {
yabahiro 24:41c23c9a5058 1394 mode1g0=0;
yabahiro 24:41c23c9a5058 1395 mode1=0;
yabahiro 24:41c23c9a5058 1396 mode=60;
yabahiro 24:41c23c9a5058 1397 }
yabahiro 24:41c23c9a5058 1398 } else if(mode==60) {
yabahiro 24:41c23c9a5058 1399 switch(g[3]) {
yabahiro 24:41c23c9a5058 1400 case 98:
yabahiro 24:41c23c9a5058 1401 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1402 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1403 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1404 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1405 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1406 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1407 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1408 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1409 if(mode1g0>100000) {
yabahiro 24:41c23c9a5058 1410 mode1++;
yabahiro 24:41c23c9a5058 1411 mode1g0=0;
yabahiro 24:41c23c9a5058 1412 }
yabahiro 24:41c23c9a5058 1413 mtc2=true;
yabahiro 24:41c23c9a5058 1414 if(mode1==1) {
yabahiro 24:41c23c9a5058 1415 mode=61;
yabahiro 24:41c23c9a5058 1416 mode1g0=0;
yabahiro 24:41c23c9a5058 1417 mode1=0;
yabahiro 24:41c23c9a5058 1418 }
yabahiro 24:41c23c9a5058 1419 break;
yabahiro 24:41c23c9a5058 1420 case 0:
yabahiro 24:41c23c9a5058 1421 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1422 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1423 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1424 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1425 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1426 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1427 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1428 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1429 mtc2=true;
yabahiro 24:41c23c9a5058 1430 break;
yabahiro 24:41c23c9a5058 1431 case 255:
yabahiro 24:41c23c9a5058 1432 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1433 motor[TIRE_FR].dir = BACK;
yabahiro 24:41c23c9a5058 1434 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1435 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1436 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1437 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1438 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1439 motor[TIRE_BL].dir = FOR;
yabahiro 24:41c23c9a5058 1440 mtc2=true;
yabahiro 24:41c23c9a5058 1441 break;
yabahiro 24:41c23c9a5058 1442 case 253:
yabahiro 24:41c23c9a5058 1443 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1444 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1445 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1446 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1447 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1448 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1449 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1450 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1451 mtc2=true;
yabahiro 24:41c23c9a5058 1452 break;
yabahiro 24:41c23c9a5058 1453 case 254:
yabahiro 24:41c23c9a5058 1454 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1455 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1456 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1457 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1458 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1459 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1460 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1461 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1462 mtc2=true;
yabahiro 24:41c23c9a5058 1463 break;
yabahiro 24:41c23c9a5058 1464 case 1:
yabahiro 24:41c23c9a5058 1465 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1466 motor[TIRE_FR].dir = BACK;
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 = FOR;
yabahiro 24:41c23c9a5058 1473 mtc2=true;
yabahiro 24:41c23c9a5058 1474 break;
yabahiro 24:41c23c9a5058 1475 case 3:
yabahiro 24:41c23c9a5058 1476 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1477 motor[TIRE_FR].dir = BACK;
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 = FOR;
yabahiro 24:41c23c9a5058 1484 mtc2=true;
yabahiro 24:41c23c9a5058 1485 break;
yabahiro 24:41c23c9a5058 1486 case 2:
yabahiro 24:41c23c9a5058 1487 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1488 motor[TIRE_FR].dir = BACK;
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 = FOR;
yabahiro 24:41c23c9a5058 1495 mtc2=true;
yabahiro 24:41c23c9a5058 1496 break;
yabahiro 24:41c23c9a5058 1497 }
yabahiro 24:41c23c9a5058 1498 if(g[2]!=98&&mtc2==true) {
yabahiro 24:41c23c9a5058 1499 switch(g[2]) {
yabahiro 24:41c23c9a5058 1500 case 0:
yabahiro 24:41c23c9a5058 1501 mtc2=false;
yabahiro 24:41c23c9a5058 1502 break;
yabahiro 24:41c23c9a5058 1503 case 255:
yabahiro 24:41c23c9a5058 1504 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1505 mtc2=false;
yabahiro 24:41c23c9a5058 1506 break;
yabahiro 24:41c23c9a5058 1507 case 253:
yabahiro 24:41c23c9a5058 1508 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1509 mtc2=false;
yabahiro 24:41c23c9a5058 1510 break;
yabahiro 24:41c23c9a5058 1511 case 254:
yabahiro 24:41c23c9a5058 1512 motor[TIRE_FL].pwm += 20;
yabahiro 24:41c23c9a5058 1513 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1514 mtc2=false;
yabahiro 24:41c23c9a5058 1515 break;
yabahiro 24:41c23c9a5058 1516 case 1:
yabahiro 24:41c23c9a5058 1517 motor[TIRE_BL].pwm += 5;
yabahiro 24:41c23c9a5058 1518 mtc2=false;
yabahiro 24:41c23c9a5058 1519 break;
yabahiro 24:41c23c9a5058 1520 case 3:
yabahiro 24:41c23c9a5058 1521 motor[TIRE_BL].pwm += 10;
yabahiro 24:41c23c9a5058 1522 mtc2=false;
yabahiro 24:41c23c9a5058 1523 break;
yabahiro 24:41c23c9a5058 1524 case 2:
yabahiro 24:41c23c9a5058 1525 motor[TIRE_BL].pwm += 20;
yabahiro 24:41c23c9a5058 1526 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1527 mtc2=false;
yabahiro 24:41c23c9a5058 1528 break;
yabahiro 24:41c23c9a5058 1529 }
yabahiro 24:41c23c9a5058 1530 }
yabahiro 24:41c23c9a5058 1531 } else if(mode==61) {
yabahiro 25:38d4964e8aad 1532 motor[TIRE_FR].pwm = 0;
yabahiro 25:38d4964e8aad 1533 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1534 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1535 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1536 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1537 motor[TIRE_BR].dir = FOR;
yabahiro 25:38d4964e8aad 1538 motor[TIRE_BL].pwm = 0;
yabahiro 25:38d4964e8aad 1539 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1540 if(mode1g0>5000&&g[1]==0) {
yabahiro 24:41c23c9a5058 1541 mode1g0=0;
yabahiro 24:41c23c9a5058 1542 mode=62;
yabahiro 24:41c23c9a5058 1543 }
yabahiro 24:41c23c9a5058 1544 } else if(mode==62) {
yabahiro 24:41c23c9a5058 1545 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1546 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1547 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1548 motor[TIRE_FL].dir = FOR;
yabahiro 24:41c23c9a5058 1549 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1550 motor[TIRE_BR].dir = BACK;
yabahiro 24:41c23c9a5058 1551 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1552 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1553 if(mode1g0>5000&&g[1]==0) {
yabahiro 24:41c23c9a5058 1554 mode1g0=0;
yabahiro 24:41c23c9a5058 1555 mode=70;
yabahiro 24:41c23c9a5058 1556 }
yabahiro 24:41c23c9a5058 1557 } else if(mode==70) {
yabahiro 24:41c23c9a5058 1558 switch(g[1]) {
yabahiro 24:41c23c9a5058 1559 case 98:
yabahiro 24:41c23c9a5058 1560 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1561 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1562 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1563 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1564 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1565 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1566 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1567 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1568 if(mode1g0>100000) {
yabahiro 24:41c23c9a5058 1569 mode1++;
yabahiro 24:41c23c9a5058 1570 mode1g0=0;
yabahiro 24:41c23c9a5058 1571 }
yabahiro 24:41c23c9a5058 1572 mtc=true;
yabahiro 24:41c23c9a5058 1573 if(mode1==1) {
yabahiro 24:41c23c9a5058 1574 mode=71;
yabahiro 24:41c23c9a5058 1575 mode1g0=0;
yabahiro 24:41c23c9a5058 1576 }
yabahiro 24:41c23c9a5058 1577 break;
yabahiro 24:41c23c9a5058 1578 case 0:
yabahiro 24:41c23c9a5058 1579 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1580 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1581 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1582 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1583 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1584 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1585 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1586 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1587 mtc=true;
yabahiro 24:41c23c9a5058 1588 break;
yabahiro 24:41c23c9a5058 1589 case 255:
yabahiro 24:41c23c9a5058 1590 motor[TIRE_FR].pwm = 60;
yabahiro 24:41c23c9a5058 1591 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1592 motor[TIRE_FL].pwm = 40;
yabahiro 24:41c23c9a5058 1593 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1594 motor[TIRE_BR].pwm = 40;
yabahiro 24:41c23c9a5058 1595 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1596 motor[TIRE_BL].pwm = 60;
yabahiro 24:41c23c9a5058 1597 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1598 mtc=true;
yabahiro 24:41c23c9a5058 1599 break;
yabahiro 24:41c23c9a5058 1600 case 253:
yabahiro 24:41c23c9a5058 1601 motor[TIRE_FR].pwm = 20;
yabahiro 24:41c23c9a5058 1602 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1603 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1604 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1605 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1606 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1607 motor[TIRE_BL].pwm = 20;
yabahiro 24:41c23c9a5058 1608 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1609 mtc=true;
yabahiro 24:41c23c9a5058 1610 break;
yabahiro 24:41c23c9a5058 1611 case 254:
yabahiro 24:41c23c9a5058 1612 motor[TIRE_FR].pwm = 30;
yabahiro 24:41c23c9a5058 1613 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1614 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1615 motor[TIRE_FL].dir = FREE;
yabahiro 24:41c23c9a5058 1616 motor[TIRE_BR].pwm = 0;
yabahiro 24:41c23c9a5058 1617 motor[TIRE_BR].dir = FREE;
yabahiro 24:41c23c9a5058 1618 motor[TIRE_BL].pwm = 30;
yabahiro 24:41c23c9a5058 1619 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1620 mtc=true;
yabahiro 24:41c23c9a5058 1621 break;
yabahiro 24:41c23c9a5058 1622 case 1:
yabahiro 24:41c23c9a5058 1623 motor[TIRE_FR].pwm = 40;
yabahiro 24:41c23c9a5058 1624 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1625 motor[TIRE_FL].pwm = 60;
yabahiro 24:41c23c9a5058 1626 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1627 motor[TIRE_BR].pwm = 60;
yabahiro 24:41c23c9a5058 1628 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1629 motor[TIRE_BL].pwm = 40;
yabahiro 24:41c23c9a5058 1630 motor[TIRE_BL].dir = BACK;
yabahiro 24:41c23c9a5058 1631 mtc=true;
yabahiro 24:41c23c9a5058 1632 break;
yabahiro 24:41c23c9a5058 1633 case 3:
yabahiro 24:41c23c9a5058 1634 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1635 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1636 motor[TIRE_FL].pwm = 20;
yabahiro 24:41c23c9a5058 1637 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1638 motor[TIRE_BR].pwm = 20;
yabahiro 24:41c23c9a5058 1639 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1640 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1641 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1642 mtc=true;
yabahiro 24:41c23c9a5058 1643 break;
yabahiro 24:41c23c9a5058 1644 case 2:
yabahiro 24:41c23c9a5058 1645 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1646 motor[TIRE_FR].dir = FREE;
yabahiro 24:41c23c9a5058 1647 motor[TIRE_FL].pwm = 30;
yabahiro 24:41c23c9a5058 1648 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1649 motor[TIRE_BR].pwm = 30;
yabahiro 24:41c23c9a5058 1650 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1651 motor[TIRE_BL].pwm = 0;
yabahiro 24:41c23c9a5058 1652 motor[TIRE_BL].dir = FREE;
yabahiro 24:41c23c9a5058 1653 mtc=true;
yabahiro 24:41c23c9a5058 1654 break;
yabahiro 24:41c23c9a5058 1655 }
yabahiro 24:41c23c9a5058 1656 if(g[1]!=98&&mtc==true) {
yabahiro 24:41c23c9a5058 1657 switch(g[0]) {
yabahiro 24:41c23c9a5058 1658 case 0:
yabahiro 24:41c23c9a5058 1659 mtc=false;
yabahiro 24:41c23c9a5058 1660 break;
yabahiro 24:41c23c9a5058 1661 case 255:
yabahiro 24:41c23c9a5058 1662 motor[TIRE_FR].pwm += 5;
yabahiro 24:41c23c9a5058 1663 mtc=false;
yabahiro 24:41c23c9a5058 1664 break;
yabahiro 24:41c23c9a5058 1665 case 253:
yabahiro 24:41c23c9a5058 1666 motor[TIRE_FR].pwm += 10;
yabahiro 24:41c23c9a5058 1667 mtc=false;
yabahiro 24:41c23c9a5058 1668 break;
yabahiro 24:41c23c9a5058 1669 case 254:
yabahiro 24:41c23c9a5058 1670 motor[TIRE_FR].pwm += 20;
yabahiro 24:41c23c9a5058 1671 motor[TIRE_FL].pwm = 0;
yabahiro 24:41c23c9a5058 1672 mtc=false;
yabahiro 24:41c23c9a5058 1673 break;
yabahiro 24:41c23c9a5058 1674 case 1:
yabahiro 24:41c23c9a5058 1675 motor[TIRE_FL].pwm += 5;
yabahiro 24:41c23c9a5058 1676 mtc=false;
yabahiro 24:41c23c9a5058 1677 break;
yabahiro 24:41c23c9a5058 1678 case 3:
yabahiro 24:41c23c9a5058 1679 motor[TIRE_FL].pwm += 10;
yabahiro 24:41c23c9a5058 1680 mtc=false;
yabahiro 24:41c23c9a5058 1681 break;
yabahiro 24:41c23c9a5058 1682 case 2:
yabahiro 24:41c23c9a5058 1683 motor[TIRE_FL].pwm += 20;
yabahiro 24:41c23c9a5058 1684 motor[TIRE_FR].pwm = 0;
yabahiro 24:41c23c9a5058 1685 mtc=false;
yabahiro 24:41c23c9a5058 1686 break;
yabahiro 24:41c23c9a5058 1687 }
yabahiro 24:41c23c9a5058 1688 }
yabahiro 24:41c23c9a5058 1689 } else if(mode==71) {
yabahiro 24:41c23c9a5058 1690 motor[TIRE_FR].pwm = 15;
yabahiro 24:41c23c9a5058 1691 motor[TIRE_FR].dir = FOR;
yabahiro 24:41c23c9a5058 1692 motor[TIRE_FL].pwm = 15;
yabahiro 24:41c23c9a5058 1693 motor[TIRE_FL].dir = BACK;
yabahiro 24:41c23c9a5058 1694 motor[TIRE_BR].pwm = 15;
yabahiro 24:41c23c9a5058 1695 motor[TIRE_BR].dir = FOR;
yabahiro 24:41c23c9a5058 1696 motor[TIRE_BL].pwm = 15;
yabahiro 24:41c23c9a5058 1697 motor[TIRE_BL].dir = BACK;
kishibekairohan 26:be5121da0f77 1698 mode1g0++;
kishibekairohan 26:be5121da0f77 1699 if(mode1g0>5000&&g[3]==0) {
kishibekairohan 26:be5121da0f77 1700 mode1g0=0;
kishibekairohan 26:be5121da0f77 1701 mode=72;
kishibekairohan 26:be5121da0f77 1702 }
kishibekairohan 26:be5121da0f77 1703 }else if(mode==72) {
kishibekairohan 26:be5121da0f77 1704 motor[TIRE_FR].pwm = 15;
kishibekairohan 26:be5121da0f77 1705 motor[TIRE_FR].dir = BACK;
kishibekairohan 26:be5121da0f77 1706 motor[TIRE_FL].pwm = 15;
kishibekairohan 26:be5121da0f77 1707 motor[TIRE_FL].dir = FOR;
kishibekairohan 26:be5121da0f77 1708 motor[TIRE_BR].pwm = 15;
kishibekairohan 26:be5121da0f77 1709 motor[TIRE_BR].dir = BACK;
kishibekairohan 26:be5121da0f77 1710 motor[TIRE_BL].pwm = 15;
kishibekairohan 26:be5121da0f77 1711 motor[TIRE_BL].dir = FOR;
kishibekairohan 26:be5121da0f77 1712 mode1g0++;
kishibekairohan 26:be5121da0f77 1713 if(mode1g0>100000&&g[3]==0) {
kishibekairohan 26:be5121da0f77 1714 mode1g0=0;
kishibekairohan 26:be5121da0f77 1715 mode1=0;
kishibekairohan 26:be5121da0f77 1716 mode=80;
kishibekairohan 26:be5121da0f77 1717 }
kishibekairohan 26:be5121da0f77 1718 }else if(mode==80) {
kishibekairohan 26:be5121da0f77 1719 motor[TIRE_FR].pwm = 0;
kishibekairohan 26:be5121da0f77 1720 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1721 motor[TIRE_FL].pwm = 0;
kishibekairohan 26:be5121da0f77 1722 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1723 motor[TIRE_BR].pwm = 0;
kishibekairohan 26:be5121da0f77 1724 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1725 motor[TIRE_BL].pwm = 0;
kishibekairohan 26:be5121da0f77 1726 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 26:be5121da0f77 1727 }else {
yabahiro 21:1f1e9c585da8 1728 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 1729 motor[TIRE_FR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1730 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 1731 motor[TIRE_FL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1732 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 1733 motor[TIRE_BR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1734 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 1735 motor[TIRE_BL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1736 }
yabahiro 21:1f1e9c585da8 1737
kishibekairohan 13:b6e02d6261d7 1738 }
yabahiro 23:694ecfa97839 1739
kishibekairohan 13:b6e02d6261d7 1740 #endif
kishibekairohan 13:b6e02d6261d7 1741
kishibekairohan 13:b6e02d6261d7 1742 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 1743 static void Process3()
kishibekairohan 13:b6e02d6261d7 1744 {
yabahiro 23:694ecfa97839 1745
kishibekairohan 13:b6e02d6261d7 1746 }
kishibekairohan 13:b6e02d6261d7 1747 #endif
kishibekairohan 13:b6e02d6261d7 1748
kishibekairohan 13:b6e02d6261d7 1749 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 1750 static void Process4()
yabahiro 21:1f1e9c585da8 1751 {
kishibekairohan 15:dfcec98f5aa9 1752
kishibekairohan 13:b6e02d6261d7 1753 }
kishibekairohan 13:b6e02d6261d7 1754 #endif
kishibekairohan 13:b6e02d6261d7 1755
kishibekairohan 13:b6e02d6261d7 1756 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 1757 static void Process5()
yabahiro 21:1f1e9c585da8 1758 {
yabahiro 21:1f1e9c585da8 1759
kishibekairohan 13:b6e02d6261d7 1760 }
kishibekairohan 13:b6e02d6261d7 1761 #endif
kishibekairohan 13:b6e02d6261d7 1762
kishibekairohan 13:b6e02d6261d7 1763 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 1764 static void Process6()
kishibekairohan 13:b6e02d6261d7 1765 {
yabahiro 21:1f1e9c585da8 1766
kishibekairohan 13:b6e02d6261d7 1767 }
kishibekairohan 13:b6e02d6261d7 1768 #endif
kishibekairohan 13:b6e02d6261d7 1769
kishibekairohan 13:b6e02d6261d7 1770 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 1771 static void Process7()
kishibekairohan 13:b6e02d6261d7 1772 {
yabahiro 21:1f1e9c585da8 1773
kishibekairohan 12:c09b3e08a316 1774 }
kishibekairohan 12:c09b3e08a316 1775 #endif
kishibekairohan 12:c09b3e08a316 1776
yabahiro 21:1f1e9c585da8 1777 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 1778 static void Process8()
kishibekairohan 12:c09b3e08a316 1779 {
yabahiro 21:1f1e9c585da8 1780
kishibekairohan 12:c09b3e08a316 1781 }
kishibekairohan 12:c09b3e08a316 1782 #endif
kishibekairohan 12:c09b3e08a316 1783
kishibekairohan 13:b6e02d6261d7 1784 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 1785 static void Process9()
kishibekairohan 12:c09b3e08a316 1786 {
yabahiro 21:1f1e9c585da8 1787
t_yamamoto 0:669ef71cba68 1788 }
t_yamamoto 0:669ef71cba68 1789 #endif
t_yamamoto 0:669ef71cba68 1790 #endif
t_yamamoto 0:669ef71cba68 1791 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1792
t_yamamoto 0:669ef71cba68 1793 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1794 {
t_yamamoto 0:669ef71cba68 1795
yabahiro 21:1f1e9c585da8 1796 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 1797 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 1798 #endif
t_yamamoto 0:669ef71cba68 1799
yabahiro 21:1f1e9c585da8 1800 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 1801 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 21:1f1e9c585da8 1802 motor[i].dir = FREE;
yabahiro 21:1f1e9c585da8 1803 motor[i].pwm = 0;
yabahiro 21:1f1e9c585da8 1804 }
yabahiro 21:1f1e9c585da8 1805 #endif
t_yamamoto 0:669ef71cba68 1806 }
t_yamamoto 0:669ef71cba68 1807
yabahiro 21:1f1e9c585da8 1808 /*
yabahiro 21:1f1e9c585da8 1809 void BuzzerTimer_func()
yabahiro 21:1f1e9c585da8 1810 {
kishibekairohan 16:3f2c2d89372b 1811 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 1812 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 1813 }
yabahiro 21:1f1e9c585da8 1814 */
yabahiro 21:1f1e9c585da8 1815 void TapeLedEms_func()
yabahiro 21:1f1e9c585da8 1816 {
kishibekairohan 16:3f2c2d89372b 1817 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1818 }
kishibekairohan 16:3f2c2d89372b 1819
kishibekairohan 16:3f2c2d89372b 1820 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 1821
kishibekairohan 16:3f2c2d89372b 1822
t_yamamoto 0:669ef71cba68 1823 #pragma endregion