大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@23:694ecfa97839, 2019-09-15 (annotated)

Committer:
yabahiro
Date:
Sun Sep 15 11:53:56 2019 +0000
Revision:
23:694ecfa97839
Parent:
22:c47f29caded6
Child:
24:41c23c9a5058
ewf

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
kishibekairohan 2:c015739085d3 268
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 21:1f1e9c585da8 275
t_yamamoto 0:669ef71cba68 276 }
t_yamamoto 0:669ef71cba68 277 #endif
t_yamamoto 0:669ef71cba68 278
t_yamamoto 0:669ef71cba68 279 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 280
yabahiro 21:1f1e9c585da8 281
yabahiro 21:1f1e9c585da8 282 bool dz1=true;
yabahiro 21:1f1e9c585da8 283 bool dz1i=false;
yabahiro 21:1f1e9c585da8 284 bool dz2=true;
yabahiro 21:1f1e9c585da8 285 bool dz2i=false;
yabahiro 21:1f1e9c585da8 286 bool dz3=true;
yabahiro 21:1f1e9c585da8 287 bool dz3i=false;
yabahiro 21:1f1e9c585da8 288 bool dz4=true;
yabahiro 21:1f1e9c585da8 289 bool dz4i=false;
yabahiro 21:1f1e9c585da8 290
yabahiro 23:694ecfa97839 291 int mode=10;
yabahiro 21:1f1e9c585da8 292 int mode1=0;
yabahiro 21:1f1e9c585da8 293 int mode1g0=100000;
yabahiro 21:1f1e9c585da8 294
kishibekairohan 22:c47f29caded6 295
kishibekairohan 16:3f2c2d89372b 296 static void Process1()
t_yamamoto 0:669ef71cba68 297 {
yabahiro 21:1f1e9c585da8 298 mode1g0=100000;
yabahiro 23:694ecfa97839 299 mode=10;
yabahiro 21:1f1e9c585da8 300 mode1=0;
yabahiro 21:1f1e9c585da8 301 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 302 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 303 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 304 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 305
yabahiro 21:1f1e9c585da8 306 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 307 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 308 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 309 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 310
yabahiro 21:1f1e9c585da8 311 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 312
yabahiro 21:1f1e9c585da8 313 if(controller->Button.UP) {
yabahiro 21:1f1e9c585da8 314 motor[MOTOR_0].pwm = 110;
yabahiro 21:1f1e9c585da8 315 motor[MOTOR_0].dir = FOR;
yabahiro 21:1f1e9c585da8 316 }
yabahiro 21:1f1e9c585da8 317 if(controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 318 motor[MOTOR_0].pwm = 50;
yabahiro 21:1f1e9c585da8 319 motor[MOTOR_0].dir = BACK;
yabahiro 21:1f1e9c585da8 320 }
yabahiro 21:1f1e9c585da8 321 } else {
yabahiro 21:1f1e9c585da8 322 motor[MOTOR_0].pwm = 0;
yabahiro 21:1f1e9c585da8 323 motor[MOTOR_0].dir = BRAKE;
yabahiro 21:1f1e9c585da8 324 }
yabahiro 21:1f1e9c585da8 325 if(controller->Button.X) {
yabahiro 21:1f1e9c585da8 326 if(dz1==true) {
yabahiro 21:1f1e9c585da8 327 if(dz1i==false) {
yabahiro 21:1f1e9c585da8 328 solenoid.solenoid1 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 329 dz1i=true;
yabahiro 21:1f1e9c585da8 330 } else {
yabahiro 21:1f1e9c585da8 331 solenoid.solenoid1 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 332 dz1i=false;
yabahiro 21:1f1e9c585da8 333 }
yabahiro 21:1f1e9c585da8 334 dz1=false;
yabahiro 21:1f1e9c585da8 335 }
yabahiro 21:1f1e9c585da8 336 } else {
yabahiro 21:1f1e9c585da8 337 dz1=true;
yabahiro 21:1f1e9c585da8 338 }
yabahiro 21:1f1e9c585da8 339
yabahiro 21:1f1e9c585da8 340 if(controller->Button.Y) {
yabahiro 21:1f1e9c585da8 341 if(dz2==true) {
yabahiro 21:1f1e9c585da8 342 if(dz2i==false) {
yabahiro 21:1f1e9c585da8 343 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 344 dz2i=true;
yabahiro 21:1f1e9c585da8 345 } else {
yabahiro 21:1f1e9c585da8 346 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 347 dz2i=false;
yabahiro 21:1f1e9c585da8 348 }
yabahiro 21:1f1e9c585da8 349 dz2=false;
yabahiro 21:1f1e9c585da8 350 }
yabahiro 21:1f1e9c585da8 351 } else {
yabahiro 21:1f1e9c585da8 352 dz2=true;
yabahiro 21:1f1e9c585da8 353 }
yabahiro 21:1f1e9c585da8 354
yabahiro 21:1f1e9c585da8 355 if(controller->Button.A) {
yabahiro 21:1f1e9c585da8 356 if(dz3==true) {
yabahiro 21:1f1e9c585da8 357 if(dz3i==false) {
yabahiro 21:1f1e9c585da8 358 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 359 dz3i=true;
yabahiro 21:1f1e9c585da8 360 } else {
yabahiro 21:1f1e9c585da8 361 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 362 dz3i=false;
yabahiro 21:1f1e9c585da8 363 }
yabahiro 21:1f1e9c585da8 364 dz3=false;
yabahiro 21:1f1e9c585da8 365 }
yabahiro 21:1f1e9c585da8 366 } else {
yabahiro 21:1f1e9c585da8 367 dz3=true;
yabahiro 21:1f1e9c585da8 368 }
yabahiro 21:1f1e9c585da8 369
yabahiro 21:1f1e9c585da8 370 if(controller->Button.B) {
yabahiro 21:1f1e9c585da8 371 if(dz4==true) {
yabahiro 21:1f1e9c585da8 372 if(dz4i==false) {
yabahiro 21:1f1e9c585da8 373 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 374 dz4i=true;
yabahiro 21:1f1e9c585da8 375 } else {
yabahiro 21:1f1e9c585da8 376 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 377 dz4i=false;
yabahiro 21:1f1e9c585da8 378 }
yabahiro 21:1f1e9c585da8 379 dz4=false;
yabahiro 21:1f1e9c585da8 380 }
yabahiro 21:1f1e9c585da8 381 } else {
yabahiro 21:1f1e9c585da8 382 dz4=true;
yabahiro 21:1f1e9c585da8 383 }
yabahiro 21:1f1e9c585da8 384 /*
yabahiro 21:1f1e9c585da8 385 if(controller->Button.RIGHT){
yabahiro 21:1f1e9c585da8 386 motor[MOTOR_1].dir = FOR;
yabahiro 21:1f1e9c585da8 387 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 388 if (ARM_1){
yabahiro 21:1f1e9c585da8 389 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 390 }
yabahiro 21:1f1e9c585da8 391 }
yabahiro 21:1f1e9c585da8 392 else if(controller->Button.LEFT){
yabahiro 21:1f1e9c585da8 393 motor[MOTOR_1].dir = BACK;
yabahiro 21:1f1e9c585da8 394 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 395 if (ARM_0){
yabahiro 21:1f1e9c585da8 396 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 397 }
yabahiro 21:1f1e9c585da8 398 }
yabahiro 21:1f1e9c585da8 399 */
t_yamamoto 0:669ef71cba68 400 }
t_yamamoto 0:669ef71cba68 401 #endif
t_yamamoto 0:669ef71cba68 402
t_yamamoto 0:669ef71cba68 403 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 404 bool mtc=false;
kishibekairohan 22:c47f29caded6 405 //追加
kishibekairohan 22:c47f29caded6 406 bool mtc2 = false;
yabahiro 21:1f1e9c585da8 407
kishibekairohan 16:3f2c2d89372b 408 static void Process2()
yabahiro 21:1f1e9c585da8 409 {
yabahiro 21:1f1e9c585da8 410 // 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 411 mode1g0++;
yabahiro 23:694ecfa97839 412 if(g[0]!=99&&mode==10) {
yabahiro 21:1f1e9c585da8 413 switch(g[0]) {
yabahiro 21:1f1e9c585da8 414 case 98:
yabahiro 21:1f1e9c585da8 415 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 416 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 417 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 418 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 419 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 420 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 421 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 422 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 423 if(mode1g0>100000) {
yabahiro 21:1f1e9c585da8 424 mode1++;
yabahiro 21:1f1e9c585da8 425 mode1g0=0;
yabahiro 21:1f1e9c585da8 426 }
yabahiro 21:1f1e9c585da8 427 mtc=true;
yabahiro 21:1f1e9c585da8 428 if(mode1==2) {
yabahiro 23:694ecfa97839 429 mode=11;
yabahiro 23:694ecfa97839 430 mode1g0=0;
yabahiro 21:1f1e9c585da8 431 }
yabahiro 21:1f1e9c585da8 432 break;
yabahiro 21:1f1e9c585da8 433 case 0:
yabahiro 21:1f1e9c585da8 434 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 435 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 436 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 437 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 438 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 439 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 440 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 441 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 442 mtc=true;
yabahiro 21:1f1e9c585da8 443 break;
yabahiro 21:1f1e9c585da8 444 case 255:
yabahiro 21:1f1e9c585da8 445 motor[TIRE_FR].pwm = 60;
yabahiro 21:1f1e9c585da8 446 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 447 motor[TIRE_FL].pwm = 40;
yabahiro 21:1f1e9c585da8 448 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 449 motor[TIRE_BR].pwm = 40;
yabahiro 21:1f1e9c585da8 450 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 451 motor[TIRE_BL].pwm = 60;
yabahiro 21:1f1e9c585da8 452 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 453 mtc=true;
yabahiro 21:1f1e9c585da8 454 break;
yabahiro 21:1f1e9c585da8 455 case 253:
yabahiro 21:1f1e9c585da8 456 motor[TIRE_FR].pwm = 20;
yabahiro 21:1f1e9c585da8 457 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 458 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 459 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 460 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 461 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 462 motor[TIRE_BL].pwm = 20;
yabahiro 21:1f1e9c585da8 463 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 464 mtc=true;
yabahiro 21:1f1e9c585da8 465 break;
yabahiro 21:1f1e9c585da8 466 case 254:
yabahiro 21:1f1e9c585da8 467 motor[TIRE_FR].pwm = 30;
yabahiro 21:1f1e9c585da8 468 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 469 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 470 motor[TIRE_FL].dir = FREE;
yabahiro 21:1f1e9c585da8 471 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 472 motor[TIRE_BR].dir = FREE;
yabahiro 21:1f1e9c585da8 473 motor[TIRE_BL].pwm = 30;
yabahiro 21:1f1e9c585da8 474 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 475 mtc=true;
yabahiro 21:1f1e9c585da8 476 break;
yabahiro 21:1f1e9c585da8 477 case 1:
yabahiro 21:1f1e9c585da8 478 motor[TIRE_FR].pwm = 40;
yabahiro 21:1f1e9c585da8 479 motor[TIRE_FR].dir = BACK;
yabahiro 21:1f1e9c585da8 480 motor[TIRE_FL].pwm = 60;
yabahiro 21:1f1e9c585da8 481 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 482 motor[TIRE_BR].pwm = 60;
yabahiro 21:1f1e9c585da8 483 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 484 motor[TIRE_BL].pwm = 40;
yabahiro 21:1f1e9c585da8 485 motor[TIRE_BL].dir = FOR;
yabahiro 21:1f1e9c585da8 486 mtc=true;
yabahiro 21:1f1e9c585da8 487 break;
yabahiro 21:1f1e9c585da8 488 case 3:
yabahiro 21:1f1e9c585da8 489 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 490 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 491 motor[TIRE_FL].pwm = 20;
yabahiro 21:1f1e9c585da8 492 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 493 motor[TIRE_BR].pwm = 20;
yabahiro 21:1f1e9c585da8 494 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 495 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 496 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 497 mtc=true;
yabahiro 21:1f1e9c585da8 498 break;
yabahiro 21:1f1e9c585da8 499 case 2:
yabahiro 21:1f1e9c585da8 500 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 501 motor[TIRE_FR].dir = FREE;
yabahiro 21:1f1e9c585da8 502 motor[TIRE_FL].pwm = 30;
yabahiro 21:1f1e9c585da8 503 motor[TIRE_FL].dir = FOR;
yabahiro 21:1f1e9c585da8 504 motor[TIRE_BR].pwm = 30;
yabahiro 21:1f1e9c585da8 505 motor[TIRE_BR].dir = BACK;
yabahiro 21:1f1e9c585da8 506 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 507 motor[TIRE_BL].dir = FREE;
yabahiro 21:1f1e9c585da8 508 mtc=true;
yabahiro 21:1f1e9c585da8 509 break;
yabahiro 21:1f1e9c585da8 510 }
yabahiro 21:1f1e9c585da8 511 if(g[0]!=98&&mtc==true) {
yabahiro 21:1f1e9c585da8 512 switch(g[1]) {
yabahiro 21:1f1e9c585da8 513 case 0:
yabahiro 21:1f1e9c585da8 514 mtc=false;
yabahiro 21:1f1e9c585da8 515 break;
yabahiro 21:1f1e9c585da8 516 case 255:
yabahiro 21:1f1e9c585da8 517 motor[TIRE_BR].pwm += 5;
yabahiro 21:1f1e9c585da8 518 mtc=false;
yabahiro 21:1f1e9c585da8 519 break;
yabahiro 21:1f1e9c585da8 520 case 253:
yabahiro 21:1f1e9c585da8 521 motor[TIRE_BR].pwm += 10;
yabahiro 21:1f1e9c585da8 522 mtc=false;
yabahiro 21:1f1e9c585da8 523 break;
yabahiro 21:1f1e9c585da8 524 case 254:
yabahiro 21:1f1e9c585da8 525 motor[TIRE_BR].pwm += 20;
yabahiro 21:1f1e9c585da8 526 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 527 mtc=false;
yabahiro 21:1f1e9c585da8 528 break;
yabahiro 21:1f1e9c585da8 529 case 1:
yabahiro 21:1f1e9c585da8 530 motor[TIRE_BL].pwm += 5;
yabahiro 21:1f1e9c585da8 531 mtc=false;
yabahiro 21:1f1e9c585da8 532 break;
yabahiro 21:1f1e9c585da8 533 case 3:
yabahiro 21:1f1e9c585da8 534 motor[TIRE_BL].pwm += 10;
yabahiro 21:1f1e9c585da8 535 mtc=false;
yabahiro 21:1f1e9c585da8 536 break;
yabahiro 21:1f1e9c585da8 537 case 2:
yabahiro 21:1f1e9c585da8 538 motor[TIRE_BL].pwm += 20;
yabahiro 21:1f1e9c585da8 539 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 540 mtc=false;
yabahiro 21:1f1e9c585da8 541 break;
yabahiro 21:1f1e9c585da8 542 }
yabahiro 21:1f1e9c585da8 543 }
yabahiro 23:694ecfa97839 544 } else if(mode==11) {
yabahiro 23:694ecfa97839 545 motor[TIRE_FR].pwm = 15;
yabahiro 21:1f1e9c585da8 546 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 547 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 548 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 549 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 550 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 551 motor[TIRE_BL].pwm = 15;
yabahiro 21:1f1e9c585da8 552 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 553 if(mode1g0>5000&&g[2]==0) {
yabahiro 23:694ecfa97839 554 mode1g0=0;
yabahiro 23:694ecfa97839 555 mode=12;
yabahiro 21:1f1e9c585da8 556 }
yabahiro 23:694ecfa97839 557 } else if(mode==12) {
yabahiro 21:1f1e9c585da8 558 motor[TIRE_FR].pwm = 15;
yabahiro 21:1f1e9c585da8 559 motor[TIRE_FR].dir = FOR;
yabahiro 21:1f1e9c585da8 560 motor[TIRE_FL].pwm = 15;
yabahiro 21:1f1e9c585da8 561 motor[TIRE_FL].dir = BACK;
yabahiro 21:1f1e9c585da8 562 motor[TIRE_BR].pwm = 15;
yabahiro 21:1f1e9c585da8 563 motor[TIRE_BR].dir = FOR;
yabahiro 21:1f1e9c585da8 564 motor[TIRE_BL].pwm = 15;
yabahiro 21:1f1e9c585da8 565 motor[TIRE_BL].dir = BACK;
yabahiro 21:1f1e9c585da8 566 mode1g0++;
yabahiro 21:1f1e9c585da8 567 if(mode1g0>10000&&g[2]==0) {
yabahiro 23:694ecfa97839 568 mode1g0=0;
yabahiro 23:694ecfa97839 569 mode1=0;
yabahiro 23:694ecfa97839 570 mode=20;
yabahiro 21:1f1e9c585da8 571 }
yabahiro 23:694ecfa97839 572 } else if(mode==20) {
yabahiro 23:694ecfa97839 573 switch(g[2]) {
kishibekairohan 22:c47f29caded6 574 case 98:
kishibekairohan 22:c47f29caded6 575 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 576 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 577 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 578 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 579 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 580 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 581 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 582 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 583 if(mode1g0>100000) {
kishibekairohan 22:c47f29caded6 584 mode1++;
kishibekairohan 22:c47f29caded6 585 mode1g0=0;
kishibekairohan 22:c47f29caded6 586 }
kishibekairohan 22:c47f29caded6 587 mtc2=true;
yabahiro 23:694ecfa97839 588 if(mode1==3) {
yabahiro 23:694ecfa97839 589 mode=21;
yabahiro 23:694ecfa97839 590 mode1g0=0;
yabahiro 23:694ecfa97839 591 mode1=0;
kishibekairohan 22:c47f29caded6 592 }
kishibekairohan 22:c47f29caded6 593 break;
kishibekairohan 22:c47f29caded6 594 case 0:
kishibekairohan 22:c47f29caded6 595 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 596 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 597 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 598 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 599 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 600 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 601 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 602 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 603 mtc2=true;
kishibekairohan 22:c47f29caded6 604 break;
kishibekairohan 22:c47f29caded6 605 case 255:
kishibekairohan 22:c47f29caded6 606 motor[TIRE_FR].pwm = 40;
kishibekairohan 22:c47f29caded6 607 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 608 motor[TIRE_FL].pwm = 60;
kishibekairohan 22:c47f29caded6 609 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 610 motor[TIRE_BR].pwm = 60;
kishibekairohan 22:c47f29caded6 611 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 612 motor[TIRE_BL].pwm = 40;
kishibekairohan 22:c47f29caded6 613 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 614 mtc2=true;
kishibekairohan 22:c47f29caded6 615 break;
kishibekairohan 22:c47f29caded6 616 case 253:
kishibekairohan 22:c47f29caded6 617 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 618 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 619 motor[TIRE_FL].pwm = 20;
kishibekairohan 22:c47f29caded6 620 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 621 motor[TIRE_BR].pwm = 20;
kishibekairohan 22:c47f29caded6 622 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 623 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 624 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 625 mtc2=true;
kishibekairohan 22:c47f29caded6 626 break;
kishibekairohan 22:c47f29caded6 627 case 254:
kishibekairohan 22:c47f29caded6 628 motor[TIRE_FR].pwm = 0;
kishibekairohan 22:c47f29caded6 629 motor[TIRE_FR].dir = FREE;
kishibekairohan 22:c47f29caded6 630 motor[TIRE_FL].pwm = 30;
kishibekairohan 22:c47f29caded6 631 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 632 motor[TIRE_BR].pwm = 30;
kishibekairohan 22:c47f29caded6 633 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 634 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 635 motor[TIRE_BL].dir = FREE;
kishibekairohan 22:c47f29caded6 636 mtc2=true;
kishibekairohan 22:c47f29caded6 637 break;
kishibekairohan 22:c47f29caded6 638 case 1:
kishibekairohan 22:c47f29caded6 639 motor[TIRE_FR].pwm = 60;
kishibekairohan 22:c47f29caded6 640 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 641 motor[TIRE_FL].pwm = 40;
kishibekairohan 22:c47f29caded6 642 motor[TIRE_FL].dir = FOR;
kishibekairohan 22:c47f29caded6 643 motor[TIRE_BR].pwm = 40;
kishibekairohan 22:c47f29caded6 644 motor[TIRE_BR].dir = BACK;
kishibekairohan 22:c47f29caded6 645 motor[TIRE_BL].pwm = 60;
kishibekairohan 22:c47f29caded6 646 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 647 mtc2=true;
kishibekairohan 22:c47f29caded6 648 break;
kishibekairohan 22:c47f29caded6 649 case 3:
kishibekairohan 22:c47f29caded6 650 motor[TIRE_FR].pwm = 20;
kishibekairohan 22:c47f29caded6 651 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 652 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 653 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 654 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 655 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 656 motor[TIRE_BL].pwm = 20;
kishibekairohan 22:c47f29caded6 657 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 658 mtc2=true;
kishibekairohan 22:c47f29caded6 659 break;
kishibekairohan 22:c47f29caded6 660 case 2:
kishibekairohan 22:c47f29caded6 661 motor[TIRE_FR].pwm = 30;
kishibekairohan 22:c47f29caded6 662 motor[TIRE_FR].dir = FOR;
kishibekairohan 22:c47f29caded6 663 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 664 motor[TIRE_FL].dir = FREE;
kishibekairohan 22:c47f29caded6 665 motor[TIRE_BR].pwm = 0;
kishibekairohan 22:c47f29caded6 666 motor[TIRE_BR].dir = FREE;
kishibekairohan 22:c47f29caded6 667 motor[TIRE_BL].pwm = 30;
kishibekairohan 22:c47f29caded6 668 motor[TIRE_BL].dir = BACK;
kishibekairohan 22:c47f29caded6 669 mtc2=true;
kishibekairohan 22:c47f29caded6 670 break;
kishibekairohan 22:c47f29caded6 671 }
kishibekairohan 22:c47f29caded6 672 if(g[2]!=98&&mtc2==true) {
kishibekairohan 22:c47f29caded6 673 switch(g[3]) {
kishibekairohan 22:c47f29caded6 674 case 0:
kishibekairohan 22:c47f29caded6 675 mtc2=false;
kishibekairohan 22:c47f29caded6 676 break;
kishibekairohan 22:c47f29caded6 677 case 255:
kishibekairohan 22:c47f29caded6 678 motor[TIRE_BL].pwm += 5;
kishibekairohan 22:c47f29caded6 679 mtc2=false;
kishibekairohan 22:c47f29caded6 680 break;
kishibekairohan 22:c47f29caded6 681 case 253:
kishibekairohan 22:c47f29caded6 682 motor[TIRE_BL].pwm += 10;
kishibekairohan 22:c47f29caded6 683 mtc2=false;
kishibekairohan 22:c47f29caded6 684 break;
kishibekairohan 22:c47f29caded6 685 case 254:
kishibekairohan 22:c47f29caded6 686 motor[TIRE_BL].pwm += 20;
kishibekairohan 22:c47f29caded6 687 motor[TIRE_FL].pwm = 0;
kishibekairohan 22:c47f29caded6 688 mtc2=false;
kishibekairohan 22:c47f29caded6 689 break;
kishibekairohan 22:c47f29caded6 690 case 1:
kishibekairohan 22:c47f29caded6 691 motor[TIRE_FL].pwm += 5;
kishibekairohan 22:c47f29caded6 692 mtc2=false;
kishibekairohan 22:c47f29caded6 693 break;
kishibekairohan 22:c47f29caded6 694 case 3:
kishibekairohan 22:c47f29caded6 695 motor[TIRE_FL].pwm += 10;
kishibekairohan 22:c47f29caded6 696 mtc2=false;
kishibekairohan 22:c47f29caded6 697 break;
kishibekairohan 22:c47f29caded6 698 case 2:
kishibekairohan 22:c47f29caded6 699 motor[TIRE_FL].pwm += 20;
kishibekairohan 22:c47f29caded6 700 motor[TIRE_BL].pwm = 0;
kishibekairohan 22:c47f29caded6 701 mtc2=false;
kishibekairohan 22:c47f29caded6 702 break;
kishibekairohan 22:c47f29caded6 703 }
kishibekairohan 22:c47f29caded6 704 }
yabahiro 23:694ecfa97839 705 } else if(mode==21) {
yabahiro 23:694ecfa97839 706 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 707 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 708 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 709 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 710 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 711 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 712 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 713 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 714 if(mode1g0>5000&&g[0]==0) {
yabahiro 23:694ecfa97839 715 mode1g0=0;
yabahiro 23:694ecfa97839 716 mode=22;
yabahiro 23:694ecfa97839 717 }
yabahiro 23:694ecfa97839 718 } else if(mode==22) {
yabahiro 23:694ecfa97839 719 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 720 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 721 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 722 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 723 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 724 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 725 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 726 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 727 if(mode1g0>10000&&g[0]==0) {
yabahiro 23:694ecfa97839 728 mode1g0=0;
yabahiro 23:694ecfa97839 729 mode1=0;
yabahiro 23:694ecfa97839 730 mode=30;
yabahiro 23:694ecfa97839 731 }
yabahiro 23:694ecfa97839 732 } else if(mode==30) {
yabahiro 23:694ecfa97839 733 switch(g[0]) {
yabahiro 23:694ecfa97839 734 case 98:
yabahiro 23:694ecfa97839 735 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 736 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 737 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 738 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 739 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 740 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 741 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 742 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 743 if(mode1g0>100000) {
yabahiro 23:694ecfa97839 744 mode1++;
yabahiro 23:694ecfa97839 745 mode1g0=0;
yabahiro 23:694ecfa97839 746 }
yabahiro 23:694ecfa97839 747 mtc=true;
yabahiro 23:694ecfa97839 748 if(mode1==1) {
yabahiro 23:694ecfa97839 749 mode=31;
yabahiro 23:694ecfa97839 750 mode1g0=0;
yabahiro 23:694ecfa97839 751 }
yabahiro 23:694ecfa97839 752 break;
yabahiro 23:694ecfa97839 753 case 0:
yabahiro 23:694ecfa97839 754 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 755 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 756 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 757 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 758 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 759 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 760 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 761 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 762 mtc=true;
yabahiro 23:694ecfa97839 763 break;
yabahiro 23:694ecfa97839 764 case 255:
yabahiro 23:694ecfa97839 765 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 766 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 767 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 768 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 769 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 770 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 771 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 772 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 773 mtc=true;
yabahiro 23:694ecfa97839 774 break;
yabahiro 23:694ecfa97839 775 case 253:
yabahiro 23:694ecfa97839 776 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 777 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 778 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 779 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 780 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 781 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 782 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 783 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 784 mtc=true;
yabahiro 23:694ecfa97839 785 break;
yabahiro 23:694ecfa97839 786 case 254:
yabahiro 23:694ecfa97839 787 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 788 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 789 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 790 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 791 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 792 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 793 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 794 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 795 mtc=true;
yabahiro 23:694ecfa97839 796 break;
yabahiro 23:694ecfa97839 797 case 1:
yabahiro 23:694ecfa97839 798 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 799 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 800 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 801 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 802 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 803 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 804 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 805 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 806 mtc=true;
yabahiro 23:694ecfa97839 807 break;
yabahiro 23:694ecfa97839 808 case 3:
yabahiro 23:694ecfa97839 809 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 810 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 811 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 812 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 813 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 814 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 815 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 816 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 817 mtc=true;
yabahiro 23:694ecfa97839 818 break;
yabahiro 23:694ecfa97839 819 case 2:
yabahiro 23:694ecfa97839 820 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 821 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 822 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 823 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 824 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 825 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 826 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 827 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 828 mtc=true;
yabahiro 23:694ecfa97839 829 break;
yabahiro 23:694ecfa97839 830 }
yabahiro 23:694ecfa97839 831 if(g[0]!=98&&mtc==true) {
yabahiro 23:694ecfa97839 832 switch(g[1]) {
yabahiro 23:694ecfa97839 833 case 0:
yabahiro 23:694ecfa97839 834 mtc=false;
yabahiro 23:694ecfa97839 835 break;
yabahiro 23:694ecfa97839 836 case 255:
yabahiro 23:694ecfa97839 837 motor[TIRE_BR].pwm += 5;
yabahiro 23:694ecfa97839 838 mtc=false;
yabahiro 23:694ecfa97839 839 break;
yabahiro 23:694ecfa97839 840 case 253:
yabahiro 23:694ecfa97839 841 motor[TIRE_BR].pwm += 10;
yabahiro 23:694ecfa97839 842 mtc=false;
yabahiro 23:694ecfa97839 843 break;
yabahiro 23:694ecfa97839 844 case 254:
yabahiro 23:694ecfa97839 845 motor[TIRE_BR].pwm += 20;
yabahiro 23:694ecfa97839 846 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 847 mtc=false;
yabahiro 23:694ecfa97839 848 break;
yabahiro 23:694ecfa97839 849 case 1:
yabahiro 23:694ecfa97839 850 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 851 mtc=false;
yabahiro 23:694ecfa97839 852 break;
yabahiro 23:694ecfa97839 853 case 3:
yabahiro 23:694ecfa97839 854 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 855 mtc=false;
yabahiro 23:694ecfa97839 856 break;
yabahiro 23:694ecfa97839 857 case 2:
yabahiro 23:694ecfa97839 858 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 859 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 860 mtc=false;
yabahiro 23:694ecfa97839 861 break;
yabahiro 23:694ecfa97839 862 }
yabahiro 23:694ecfa97839 863 }
yabahiro 23:694ecfa97839 864 } else if(mode==31) {
yabahiro 23:694ecfa97839 865 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 866 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 867 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 868 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 869 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 870 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 871 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 872 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 873 mode1g0++;
yabahiro 23:694ecfa97839 874 if(mode1g0>5000&&g[3]==0) {
yabahiro 23:694ecfa97839 875 mode1g0=0;
yabahiro 23:694ecfa97839 876 mode=32;
yabahiro 23:694ecfa97839 877 }
yabahiro 23:694ecfa97839 878 } else if(mode==32) {
yabahiro 23:694ecfa97839 879 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 880 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 881 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 882 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 883 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 884 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 885 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 886 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 887 mode1g0++;
yabahiro 23:694ecfa97839 888 if(mode1g0>5000&&g[3]==0) {
yabahiro 23:694ecfa97839 889 mode1g0=0;
yabahiro 23:694ecfa97839 890 mode1=0;
yabahiro 23:694ecfa97839 891 mode=40;
yabahiro 23:694ecfa97839 892 }
yabahiro 23:694ecfa97839 893 } else if(mode==40) {
yabahiro 23:694ecfa97839 894 switch(g[3]) {
yabahiro 23:694ecfa97839 895 case 98:
yabahiro 23:694ecfa97839 896 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 897 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 898 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 899 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 900 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 901 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 902 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 903 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 904 if(mode1g0>100000) {
yabahiro 23:694ecfa97839 905 mode1++;
yabahiro 23:694ecfa97839 906 mode1g0=0;
yabahiro 23:694ecfa97839 907 }
yabahiro 23:694ecfa97839 908 mtc2=true;
yabahiro 23:694ecfa97839 909 if(mode1==1) {
yabahiro 23:694ecfa97839 910 mode=41;
yabahiro 23:694ecfa97839 911 mode1g0=0;
yabahiro 23:694ecfa97839 912 mode1=0;
yabahiro 23:694ecfa97839 913 }
yabahiro 23:694ecfa97839 914 break;
yabahiro 23:694ecfa97839 915 case 0:
yabahiro 23:694ecfa97839 916 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 917 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 918 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 919 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 920 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 921 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 922 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 923 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 924 mtc2=true;
yabahiro 23:694ecfa97839 925 break;
yabahiro 23:694ecfa97839 926 case 255:
yabahiro 23:694ecfa97839 927 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 928 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 929 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 930 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 931 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 932 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 933 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 934 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 935 mtc2=true;
yabahiro 23:694ecfa97839 936 break;
yabahiro 23:694ecfa97839 937 case 253:
yabahiro 23:694ecfa97839 938 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 939 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 940 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 941 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 942 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 943 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 944 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 945 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 946 mtc2=true;
yabahiro 23:694ecfa97839 947 break;
yabahiro 23:694ecfa97839 948 case 254:
yabahiro 23:694ecfa97839 949 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 950 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 951 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 952 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 953 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 954 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 955 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 956 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 957 mtc2=true;
yabahiro 23:694ecfa97839 958 break;
yabahiro 23:694ecfa97839 959 case 1:
yabahiro 23:694ecfa97839 960 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 961 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 962 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 963 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 964 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 965 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 966 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 967 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 968 mtc2=true;
yabahiro 23:694ecfa97839 969 break;
yabahiro 23:694ecfa97839 970 case 3:
yabahiro 23:694ecfa97839 971 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 972 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 973 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 974 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 975 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 976 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 977 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 978 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 979 mtc2=true;
yabahiro 23:694ecfa97839 980 break;
yabahiro 23:694ecfa97839 981 case 2:
yabahiro 23:694ecfa97839 982 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 983 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 984 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 985 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 986 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 987 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 988 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 989 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 990 mtc2=true;
yabahiro 23:694ecfa97839 991 break;
yabahiro 23:694ecfa97839 992 }
yabahiro 23:694ecfa97839 993 if(g[2]!=98&&mtc2==true) {
yabahiro 23:694ecfa97839 994 switch(g[2]) {
yabahiro 23:694ecfa97839 995 case 0:
yabahiro 23:694ecfa97839 996 mtc2=false;
yabahiro 23:694ecfa97839 997 break;
yabahiro 23:694ecfa97839 998 case 255:
yabahiro 23:694ecfa97839 999 motor[TIRE_FL].pwm += 5;
yabahiro 23:694ecfa97839 1000 mtc2=false;
yabahiro 23:694ecfa97839 1001 break;
yabahiro 23:694ecfa97839 1002 case 253:
yabahiro 23:694ecfa97839 1003 motor[TIRE_FL].pwm += 10;
yabahiro 23:694ecfa97839 1004 mtc2=false;
yabahiro 23:694ecfa97839 1005 break;
yabahiro 23:694ecfa97839 1006 case 254:
yabahiro 23:694ecfa97839 1007 motor[TIRE_FL].pwm += 20;
yabahiro 23:694ecfa97839 1008 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1009 mtc2=false;
yabahiro 23:694ecfa97839 1010 break;
yabahiro 23:694ecfa97839 1011 case 1:
yabahiro 23:694ecfa97839 1012 motor[TIRE_BL].pwm += 5;
yabahiro 23:694ecfa97839 1013 mtc2=false;
yabahiro 23:694ecfa97839 1014 break;
yabahiro 23:694ecfa97839 1015 case 3:
yabahiro 23:694ecfa97839 1016 motor[TIRE_BL].pwm += 10;
yabahiro 23:694ecfa97839 1017 mtc2=false;
yabahiro 23:694ecfa97839 1018 break;
yabahiro 23:694ecfa97839 1019 case 2:
yabahiro 23:694ecfa97839 1020 motor[TIRE_BL].pwm += 20;
yabahiro 23:694ecfa97839 1021 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1022 mtc2=false;
yabahiro 23:694ecfa97839 1023 break;
yabahiro 23:694ecfa97839 1024 }
yabahiro 23:694ecfa97839 1025 }
yabahiro 23:694ecfa97839 1026 } else if(mode==41) {
yabahiro 23:694ecfa97839 1027 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1028 motor[TIRE_FR].dir = BACK;
yabahiro 23:694ecfa97839 1029 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1030 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1031 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1032 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1033 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1034 motor[TIRE_BL].dir = FOR;
yabahiro 23:694ecfa97839 1035 if(mode1g0>5000&&g[1]==0) {
yabahiro 23:694ecfa97839 1036 mode1g0=0;
yabahiro 23:694ecfa97839 1037 mode=42;
yabahiro 23:694ecfa97839 1038 }
yabahiro 23:694ecfa97839 1039 } else if(mode==42) {
yabahiro 23:694ecfa97839 1040 motor[TIRE_FR].pwm = 15;
yabahiro 23:694ecfa97839 1041 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1042 motor[TIRE_FL].pwm = 15;
yabahiro 23:694ecfa97839 1043 motor[TIRE_FL].dir = FOR;
yabahiro 23:694ecfa97839 1044 motor[TIRE_BR].pwm = 15;
yabahiro 23:694ecfa97839 1045 motor[TIRE_BR].dir = BACK;
yabahiro 23:694ecfa97839 1046 motor[TIRE_BL].pwm = 15;
yabahiro 23:694ecfa97839 1047 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1048 if(mode1g0>5000&&g[1]==0) {
yabahiro 23:694ecfa97839 1049 mode1g0=0;
yabahiro 23:694ecfa97839 1050 mode=50;
yabahiro 23:694ecfa97839 1051 } else if(mode==50) {
yabahiro 23:694ecfa97839 1052 switch(g[1]) {
yabahiro 23:694ecfa97839 1053 case 98:
yabahiro 23:694ecfa97839 1054 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 1055 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1056 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1057 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1058 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1059 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1060 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 1061 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1062 if(mode1g0>100000) {
yabahiro 23:694ecfa97839 1063 mode1++;
yabahiro 23:694ecfa97839 1064 mode1g0=0;
yabahiro 23:694ecfa97839 1065 }
yabahiro 23:694ecfa97839 1066 mtc=true;
yabahiro 23:694ecfa97839 1067 if(mode1==1) {
yabahiro 23:694ecfa97839 1068 mode=51;
yabahiro 23:694ecfa97839 1069 mode1g0=0;
yabahiro 23:694ecfa97839 1070 }
yabahiro 23:694ecfa97839 1071 break;
yabahiro 23:694ecfa97839 1072 case 0:
yabahiro 23:694ecfa97839 1073 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 1074 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1075 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 1076 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1077 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 1078 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1079 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 1080 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1081 mtc=true;
yabahiro 23:694ecfa97839 1082 break;
yabahiro 23:694ecfa97839 1083 case 255:
yabahiro 23:694ecfa97839 1084 motor[TIRE_FR].pwm = 60;
yabahiro 23:694ecfa97839 1085 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1086 motor[TIRE_FL].pwm = 40;
yabahiro 23:694ecfa97839 1087 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1088 motor[TIRE_BR].pwm = 40;
yabahiro 23:694ecfa97839 1089 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1090 motor[TIRE_BL].pwm = 60;
yabahiro 23:694ecfa97839 1091 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1092 mtc=true;
yabahiro 23:694ecfa97839 1093 break;
yabahiro 23:694ecfa97839 1094 case 253:
yabahiro 23:694ecfa97839 1095 motor[TIRE_FR].pwm = 20;
yabahiro 23:694ecfa97839 1096 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1097 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1098 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1099 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1100 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1101 motor[TIRE_BL].pwm = 20;
yabahiro 23:694ecfa97839 1102 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1103 mtc=true;
yabahiro 23:694ecfa97839 1104 break;
yabahiro 23:694ecfa97839 1105 case 254:
yabahiro 23:694ecfa97839 1106 motor[TIRE_FR].pwm = 30;
yabahiro 23:694ecfa97839 1107 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1108 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1109 motor[TIRE_FL].dir = FREE;
yabahiro 23:694ecfa97839 1110 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1111 motor[TIRE_BR].dir = FREE;
yabahiro 23:694ecfa97839 1112 motor[TIRE_BL].pwm = 30;
yabahiro 23:694ecfa97839 1113 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1114 mtc=true;
yabahiro 23:694ecfa97839 1115 break;
yabahiro 23:694ecfa97839 1116 case 1:
yabahiro 23:694ecfa97839 1117 motor[TIRE_FR].pwm = 40;
yabahiro 23:694ecfa97839 1118 motor[TIRE_FR].dir = FOR;
yabahiro 23:694ecfa97839 1119 motor[TIRE_FL].pwm = 60;
yabahiro 23:694ecfa97839 1120 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1121 motor[TIRE_BR].pwm = 60;
yabahiro 23:694ecfa97839 1122 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1123 motor[TIRE_BL].pwm = 40;
yabahiro 23:694ecfa97839 1124 motor[TIRE_BL].dir = BACK;
yabahiro 23:694ecfa97839 1125 mtc=true;
yabahiro 23:694ecfa97839 1126 break;
yabahiro 23:694ecfa97839 1127 case 3:
yabahiro 23:694ecfa97839 1128 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1129 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1130 motor[TIRE_FL].pwm = 20;
yabahiro 23:694ecfa97839 1131 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1132 motor[TIRE_BR].pwm = 20;
yabahiro 23:694ecfa97839 1133 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1134 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1135 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1136 mtc=true;
yabahiro 23:694ecfa97839 1137 break;
yabahiro 23:694ecfa97839 1138 case 2:
yabahiro 23:694ecfa97839 1139 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1140 motor[TIRE_FR].dir = FREE;
yabahiro 23:694ecfa97839 1141 motor[TIRE_FL].pwm = 30;
yabahiro 23:694ecfa97839 1142 motor[TIRE_FL].dir = BACK;
yabahiro 23:694ecfa97839 1143 motor[TIRE_BR].pwm = 30;
yabahiro 23:694ecfa97839 1144 motor[TIRE_BR].dir = FOR;
yabahiro 23:694ecfa97839 1145 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1146 motor[TIRE_BL].dir = FREE;
yabahiro 23:694ecfa97839 1147 mtc=true;
yabahiro 23:694ecfa97839 1148 break;
yabahiro 23:694ecfa97839 1149 }
yabahiro 23:694ecfa97839 1150 if(g[1]!=98&&mtc==true) {
yabahiro 23:694ecfa97839 1151 switch(g[0]) {
yabahiro 23:694ecfa97839 1152 case 0:
yabahiro 23:694ecfa97839 1153 mtc=false;
yabahiro 23:694ecfa97839 1154 break;
yabahiro 23:694ecfa97839 1155 case 255:
yabahiro 23:694ecfa97839 1156 motor[TIRE_FR].pwm += 5;
yabahiro 23:694ecfa97839 1157 mtc=false;
yabahiro 23:694ecfa97839 1158 break;
yabahiro 23:694ecfa97839 1159 case 253:
yabahiro 23:694ecfa97839 1160 motor[TIRE_FR].pwm += 10;
yabahiro 23:694ecfa97839 1161 mtc=false;
yabahiro 23:694ecfa97839 1162 break;
yabahiro 23:694ecfa97839 1163 case 254:
yabahiro 23:694ecfa97839 1164 motor[TIRE_FR].pwm += 20;
yabahiro 23:694ecfa97839 1165 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1166 mtc=false;
yabahiro 23:694ecfa97839 1167 break;
yabahiro 23:694ecfa97839 1168 case 1:
yabahiro 23:694ecfa97839 1169 motor[TIRE_FL].pwm += 5;
yabahiro 23:694ecfa97839 1170 mtc=false;
yabahiro 23:694ecfa97839 1171 break;
yabahiro 23:694ecfa97839 1172 case 3:
yabahiro 23:694ecfa97839 1173 motor[TIRE_FL].pwm += 10;
yabahiro 23:694ecfa97839 1174 mtc=false;
yabahiro 23:694ecfa97839 1175 break;
yabahiro 23:694ecfa97839 1176 case 2:
yabahiro 23:694ecfa97839 1177 motor[TIRE_FL].pwm += 20;
yabahiro 23:694ecfa97839 1178 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1179 mtc=false;
yabahiro 23:694ecfa97839 1180 break;
yabahiro 23:694ecfa97839 1181 }
yabahiro 23:694ecfa97839 1182 }
yabahiro 23:694ecfa97839 1183 }
yabahiro 23:694ecfa97839 1184 } else if(mode==51) {
yabahiro 23:694ecfa97839 1185 motor[TIRE_FR].pwm = 0;
yabahiro 23:694ecfa97839 1186 motor[TIRE_FR].dir = BRAKE;
yabahiro 23:694ecfa97839 1187 motor[TIRE_FL].pwm = 0;
yabahiro 23:694ecfa97839 1188 motor[TIRE_FL].dir = BRAKE;
yabahiro 23:694ecfa97839 1189 motor[TIRE_BR].pwm = 0;
yabahiro 23:694ecfa97839 1190 motor[TIRE_BR].dir = BRAKE;
yabahiro 23:694ecfa97839 1191 motor[TIRE_BL].pwm = 0;
yabahiro 23:694ecfa97839 1192 motor[TIRE_BL].dir = BRAKE;
yabahiro 23:694ecfa97839 1193 } else if(mode==52) {
yabahiro 23:694ecfa97839 1194 } else {
yabahiro 21:1f1e9c585da8 1195 motor[TIRE_FR].pwm = 0;
yabahiro 21:1f1e9c585da8 1196 motor[TIRE_FR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1197 motor[TIRE_FL].pwm = 0;
yabahiro 21:1f1e9c585da8 1198 motor[TIRE_FL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1199 motor[TIRE_BR].pwm = 0;
yabahiro 21:1f1e9c585da8 1200 motor[TIRE_BR].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1201 motor[TIRE_BL].pwm = 0;
yabahiro 21:1f1e9c585da8 1202 motor[TIRE_BL].dir = BRAKE;
yabahiro 21:1f1e9c585da8 1203 }
yabahiro 21:1f1e9c585da8 1204
kishibekairohan 13:b6e02d6261d7 1205 }
yabahiro 23:694ecfa97839 1206
kishibekairohan 13:b6e02d6261d7 1207 #endif
kishibekairohan 13:b6e02d6261d7 1208
kishibekairohan 13:b6e02d6261d7 1209 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 1210 static void Process3()
kishibekairohan 13:b6e02d6261d7 1211 {
yabahiro 23:694ecfa97839 1212
kishibekairohan 13:b6e02d6261d7 1213 }
kishibekairohan 13:b6e02d6261d7 1214 #endif
kishibekairohan 13:b6e02d6261d7 1215
kishibekairohan 13:b6e02d6261d7 1216 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 1217 static void Process4()
yabahiro 21:1f1e9c585da8 1218 {
kishibekairohan 15:dfcec98f5aa9 1219
kishibekairohan 13:b6e02d6261d7 1220 }
kishibekairohan 13:b6e02d6261d7 1221 #endif
kishibekairohan 13:b6e02d6261d7 1222
kishibekairohan 13:b6e02d6261d7 1223 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 1224 static void Process5()
yabahiro 21:1f1e9c585da8 1225 {
yabahiro 21:1f1e9c585da8 1226
kishibekairohan 13:b6e02d6261d7 1227 }
kishibekairohan 13:b6e02d6261d7 1228 #endif
kishibekairohan 13:b6e02d6261d7 1229
kishibekairohan 13:b6e02d6261d7 1230 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 1231 static void Process6()
kishibekairohan 13:b6e02d6261d7 1232 {
yabahiro 21:1f1e9c585da8 1233
kishibekairohan 13:b6e02d6261d7 1234 }
kishibekairohan 13:b6e02d6261d7 1235 #endif
kishibekairohan 13:b6e02d6261d7 1236
kishibekairohan 13:b6e02d6261d7 1237 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 1238 static void Process7()
kishibekairohan 13:b6e02d6261d7 1239 {
yabahiro 21:1f1e9c585da8 1240
kishibekairohan 12:c09b3e08a316 1241 }
kishibekairohan 12:c09b3e08a316 1242 #endif
kishibekairohan 12:c09b3e08a316 1243
yabahiro 21:1f1e9c585da8 1244 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 1245 static void Process8()
kishibekairohan 12:c09b3e08a316 1246 {
yabahiro 21:1f1e9c585da8 1247
kishibekairohan 12:c09b3e08a316 1248 }
kishibekairohan 12:c09b3e08a316 1249 #endif
kishibekairohan 12:c09b3e08a316 1250
kishibekairohan 13:b6e02d6261d7 1251 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 1252 static void Process9()
kishibekairohan 12:c09b3e08a316 1253 {
yabahiro 21:1f1e9c585da8 1254
t_yamamoto 0:669ef71cba68 1255 }
t_yamamoto 0:669ef71cba68 1256 #endif
t_yamamoto 0:669ef71cba68 1257 #endif
t_yamamoto 0:669ef71cba68 1258 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1259
t_yamamoto 0:669ef71cba68 1260 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1261 {
t_yamamoto 0:669ef71cba68 1262
yabahiro 21:1f1e9c585da8 1263 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 1264 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 1265 #endif
t_yamamoto 0:669ef71cba68 1266
yabahiro 21:1f1e9c585da8 1267 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 1268 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 21:1f1e9c585da8 1269 motor[i].dir = FREE;
yabahiro 21:1f1e9c585da8 1270 motor[i].pwm = 0;
yabahiro 21:1f1e9c585da8 1271 }
yabahiro 21:1f1e9c585da8 1272 #endif
t_yamamoto 0:669ef71cba68 1273 }
t_yamamoto 0:669ef71cba68 1274
yabahiro 21:1f1e9c585da8 1275 /*
yabahiro 21:1f1e9c585da8 1276 void BuzzerTimer_func()
yabahiro 21:1f1e9c585da8 1277 {
kishibekairohan 16:3f2c2d89372b 1278 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 1279 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 1280 }
yabahiro 21:1f1e9c585da8 1281 */
yabahiro 21:1f1e9c585da8 1282 void TapeLedEms_func()
yabahiro 21:1f1e9c585da8 1283 {
kishibekairohan 16:3f2c2d89372b 1284 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1285 }
kishibekairohan 16:3f2c2d89372b 1286
kishibekairohan 16:3f2c2d89372b 1287 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 1288
kishibekairohan 16:3f2c2d89372b 1289
t_yamamoto 0:669ef71cba68 1290 #pragma endregion