大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_old

aa

Dependencies:   mbed

System/Process/Process.cpp@35:33c6cdba5630, 2019-10-29 (annotated)

Committer:
kishibekairohan
Date:
Tue Oct 29 14:54:17 2019 +0000
Revision:
35:33c6cdba5630
Parent:
34:2fce61e3ebc4
Child:
36:418ee5567687
ai;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
Ryosei 27:dd9f27fce7d1 3 #include <stdlib.h>
7ka884 4:ba9df71868df 4 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 5 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 6 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
M_souta 30:8f092276b2ba 8 #include "../../Switch/Switch.h"
Ryosei 27:dd9f27fce7d1 9
t_yamamoto 0:669ef71cba68 10 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 12 #include "../Using.h"
t_yamamoto 0:669ef71cba68 13
t_yamamoto 0:669ef71cba68 14 using namespace SWITCH;
7ka884 4:ba9df71868df 15 using namespace PID_SPACE;
Ryosei 27:dd9f27fce7d1 16
M_souta 21:e3b58d675c1c 17 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 18
t_yamamoto 0:669ef71cba68 19 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 20 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 21 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static bool lock;
t_yamamoto 0:669ef71cba68 24 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 25 static int current;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 30 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 31 #endif
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 36
kishibekairohan 2:c015739085d3 37
M_souta 30:8f092276b2ba 38 //Serial pc(USBTX, USBRX);
kishibekairohan 16:3f2c2d89372b 39
Ryosei 27:dd9f27fce7d1 40 const int omni[15][15] = {
Ryosei 27:dd9f27fce7d1 41 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 42 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 43 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 44 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei 27:dd9f27fce7d1 45 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei 27:dd9f27fce7d1 46 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei 27:dd9f27fce7d1 47 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei 27:dd9f27fce7d1 48 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei 27:dd9f27fce7d1 49 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei 27:dd9f27fce7d1 50 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei 27:dd9f27fce7d1 51 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei 27:dd9f27fce7d1 52 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei 27:dd9f27fce7d1 53 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei 27:dd9f27fce7d1 54 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei 27:dd9f27fce7d1 55 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 56 };
M_souta 21:e3b58d675c1c 57
M_souta 21:e3b58d675c1c 58 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 59
M_souta 21:e3b58d675c1c 60 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 61 uint8_t SetPWM(int);
M_souta 18:c694bae76e51 62
yabahiro 33:dd598bb23ad1 63 uint8_t Setpwm(int pwmVal)
yabahiro 33:dd598bb23ad1 64 {
yabahiro 33:dd598bb23ad1 65 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
yabahiro 33:dd598bb23ad1 66 else return abs(pwmVal);
yabahiro 33:dd598bb23ad1 67 }
yabahiro 33:dd598bb23ad1 68
kishibekairohan 35:33c6cdba5630 69 DigitalIn EMC(PC_13); //非常停止確認用
yabahiro 33:dd598bb23ad1 70
yabahiro 33:dd598bb23ad1 71 bool start_flag = true;
yabahiro 33:dd598bb23ad1 72 bool UP_flag = false;
yabahiro 33:dd598bb23ad1 73 bool SW_flag = false;
yabahiro 33:dd598bb23ad1 74 bool Air_flag = false;
yabahiro 33:dd598bb23ad1 75 bool zyouge;
yabahiro 33:dd598bb23ad1 76 bool zyougedo;
yabahiro 33:dd598bb23ad1 77 bool dz1=true;
yabahiro 33:dd598bb23ad1 78 bool dz1i=false;
yabahiro 33:dd598bb23ad1 79 bool dz2=true;
yabahiro 33:dd598bb23ad1 80 bool dz2i=false;
yabahiro 33:dd598bb23ad1 81 bool dz3=true;
yabahiro 33:dd598bb23ad1 82 bool dz3i=false;
yabahiro 33:dd598bb23ad1 83 bool dz4=true;
yabahiro 33:dd598bb23ad1 84 bool dz4i=false;
yabahiro 33:dd598bb23ad1 85 bool zone;
yabahiro 33:dd598bb23ad1 86 bool through=false;
yabahiro 33:dd598bb23ad1 87 bool counts=false;
yabahiro 33:dd598bb23ad1 88 bool mtc=false;
yabahiro 33:dd598bb23ad1 89 bool mtc2 = false;
yabahiro 33:dd598bb23ad1 90 bool emcs=false;
yabahiro 33:dd598bb23ad1 91 bool kiri;
yabahiro 33:dd598bb23ad1 92
yabahiro 33:dd598bb23ad1 93 int g[8];
yabahiro 33:dd598bb23ad1 94 int Twsh;
yabahiro 33:dd598bb23ad1 95 int mode =0;
yabahiro 33:dd598bb23ad1 96 int cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 97 int cros=0;
yabahiro 33:dd598bb23ad1 98 int countss=100000;//wait代替え
yabahiro 33:dd598bb23ad1 99
yabahiro 33:dd598bb23ad1 100 ///*********PWM調整用*********///
yabahiro 33:dd598bb23ad1 101 int fast =60;
yabahiro 33:dd598bb23ad1 102 int normal = 40;
yabahiro 33:dd598bb23ad1 103 int slow = 20;
yabahiro 33:dd598bb23ad1 104
t_yamamoto 0:669ef71cba68 105 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 106
t_yamamoto 0:669ef71cba68 107 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 108 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 109 static void Process0(void);
yabahiro 33:dd598bb23ad1 110 //手動モード
t_yamamoto 0:669ef71cba68 111 #endif
t_yamamoto 0:669ef71cba68 112 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 113 static void Process1(void);
t_yamamoto 0:669ef71cba68 114 #endif
t_yamamoto 0:669ef71cba68 115 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 116 static void Process2(void);
t_yamamoto 0:669ef71cba68 117 #endif
t_yamamoto 0:669ef71cba68 118 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 119 static void Process3(void);
t_yamamoto 0:669ef71cba68 120 #endif
t_yamamoto 0:669ef71cba68 121 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 122 static void Process4(void);
t_yamamoto 0:669ef71cba68 123 #endif
t_yamamoto 0:669ef71cba68 124 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 125 static void Process5(void);
t_yamamoto 0:669ef71cba68 126 #endif
t_yamamoto 0:669ef71cba68 127 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 128 static void Process6(void);
t_yamamoto 0:669ef71cba68 129 #endif
t_yamamoto 0:669ef71cba68 130 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 131 static void Process7(void);
t_yamamoto 0:669ef71cba68 132 #endif
t_yamamoto 0:669ef71cba68 133 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 134 static void Process8(void);
t_yamamoto 0:669ef71cba68 135 #endif
t_yamamoto 0:669ef71cba68 136 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 137 static void Process9(void);
t_yamamoto 0:669ef71cba68 138 #endif
t_yamamoto 0:669ef71cba68 139 #endif
t_yamamoto 0:669ef71cba68 140
t_yamamoto 0:669ef71cba68 141 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 142 {
Ryosei 27:dd9f27fce7d1 143 #pragma region USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 144 /*Replace here with the initialization code of your variables.*/
Ryosei 27:dd9f27fce7d1 145
Ryosei 27:dd9f27fce7d1 146 #pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 147
Ryosei 27:dd9f27fce7d1 148 lock = true;
Ryosei 27:dd9f27fce7d1 149 processChangeComp = true;
Ryosei 27:dd9f27fce7d1 150 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 151
Ryosei 27:dd9f27fce7d1 152 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 153 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 154 Process[0] = Process0;
Ryosei 27:dd9f27fce7d1 155 #endif
Ryosei 27:dd9f27fce7d1 156 #if USE_PROCESS_NUM>1
Ryosei 27:dd9f27fce7d1 157 Process[1] = Process1;
Ryosei 27:dd9f27fce7d1 158 #endif
Ryosei 27:dd9f27fce7d1 159 #if USE_PROCESS_NUM>2
Ryosei 27:dd9f27fce7d1 160 Process[2] = Process2;
Ryosei 27:dd9f27fce7d1 161 #endif
Ryosei 27:dd9f27fce7d1 162 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 163 Process[3] = Process3;
Ryosei 27:dd9f27fce7d1 164 #endif
Ryosei 27:dd9f27fce7d1 165 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 166 Process[4] = Process4;
Ryosei 27:dd9f27fce7d1 167 #endif
Ryosei 27:dd9f27fce7d1 168 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 169 Process[5] = Process5;
Ryosei 27:dd9f27fce7d1 170 #endif
Ryosei 27:dd9f27fce7d1 171 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 172 Process[6] = Process6;
Ryosei 27:dd9f27fce7d1 173 #endif
Ryosei 27:dd9f27fce7d1 174 #if USE_PROCESS_NUM>7
Ryosei 27:dd9f27fce7d1 175 Process[7] = Process7;
Ryosei 27:dd9f27fce7d1 176 #endif
Ryosei 27:dd9f27fce7d1 177 #if USE_PROCESS_NUM>8
Ryosei 27:dd9f27fce7d1 178 Process[8] = Process8;
Ryosei 27:dd9f27fce7d1 179 #endif
Ryosei 27:dd9f27fce7d1 180 #if USE_PROCESS_NUM>9
Ryosei 27:dd9f27fce7d1 181 Process[9] = Process9;
Ryosei 27:dd9f27fce7d1 182 #endif
Ryosei 27:dd9f27fce7d1 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 {
Ryosei 27:dd9f27fce7d1 188 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 189 if(controller->Button.HOME) lock = false;
Ryosei 27:dd9f27fce7d1 190
Ryosei 27:dd9f27fce7d1 191 if(controller->Button.START && processChangeComp) {
Ryosei 27:dd9f27fce7d1 192 current++;
Ryosei 27:dd9f27fce7d1 193 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei 27:dd9f27fce7d1 194 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 195 } else if(controller->Button.SELECT && processChangeComp) {
Ryosei 27:dd9f27fce7d1 196 current--;
Ryosei 27:dd9f27fce7d1 197 if (current < 0) current = 0;
Ryosei 27:dd9f27fce7d1 198 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 199 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei 27:dd9f27fce7d1 200 #endif
Ryosei 27:dd9f27fce7d1 201
Ryosei 27:dd9f27fce7d1 202 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 203 ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei 27:dd9f27fce7d1 204 #endif
Ryosei 27:dd9f27fce7d1 205
Ryosei 27:dd9f27fce7d1 206 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 207 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei 27:dd9f27fce7d1 208 #endif
Ryosei 27:dd9f27fce7d1 209
Ryosei 27:dd9f27fce7d1 210 #ifdef USE_RS485
Ryosei 27:dd9f27fce7d1 211 ACTUATORHUB::ActuatorHub::Update();
Ryosei 27:dd9f27fce7d1 212 //LINEHUB::LineHub::Update();
Ryosei 27:dd9f27fce7d1 213 #endif
Ryosei 27:dd9f27fce7d1 214
Ryosei 27:dd9f27fce7d1 215 }
Ryosei 27:dd9f27fce7d1 216
Ryosei 27:dd9f27fce7d1 217 void SystemProcess()
Ryosei 27:dd9f27fce7d1 218 {
Ryosei 27:dd9f27fce7d1 219 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 220
Ryosei 27:dd9f27fce7d1 221 while(1) {
yabahiro 33:dd598bb23ad1 222 if(EMC==0) {
yabahiro 33:dd598bb23ad1 223 fast=40;
yabahiro 33:dd598bb23ad1 224 normal=30;
yabahiro 33:dd598bb23ad1 225 slow=20;
yabahiro 33:dd598bb23ad1 226 cross=0;//十字数える用
yabahiro 33:dd598bb23ad1 227 cros=0;
yabahiro 33:dd598bb23ad1 228 countss=0;//wait代替え
yabahiro 33:dd598bb23ad1 229 UP_flag = false;
yabahiro 33:dd598bb23ad1 230 SW_flag = false;
yabahiro 33:dd598bb23ad1 231 Air_flag = false;
yabahiro 33:dd598bb23ad1 232 zyouge=false;
yabahiro 33:dd598bb23ad1 233 zyougedo=false;
yabahiro 33:dd598bb23ad1 234 dz1=true;
yabahiro 33:dd598bb23ad1 235 dz1i=false;
yabahiro 33:dd598bb23ad1 236 dz2=true;
yabahiro 33:dd598bb23ad1 237 dz2i=false;
yabahiro 33:dd598bb23ad1 238 dz3=true;
yabahiro 33:dd598bb23ad1 239 dz3i=false;
yabahiro 33:dd598bb23ad1 240 dz4=true;
yabahiro 33:dd598bb23ad1 241 dz4i=false;
yabahiro 33:dd598bb23ad1 242 through=false;
yabahiro 33:dd598bb23ad1 243 counts=false;
yabahiro 33:dd598bb23ad1 244 mtc=false;
yabahiro 33:dd598bb23ad1 245 mtc2 = false;
yabahiro 33:dd598bb23ad1 246 current = 0;
yabahiro 33:dd598bb23ad1 247 SystemProcessInitialize();
yabahiro 33:dd598bb23ad1 248 if(emcs==false) {
yabahiro 33:dd598bb23ad1 249 AllActuatorReset();
yabahiro 33:dd598bb23ad1 250 emcs=true;
yabahiro 33:dd598bb23ad1 251 }
yabahiro 33:dd598bb23ad1 252 } else {
yabahiro 33:dd598bb23ad1 253 emcs=false;
yabahiro 33:dd598bb23ad1 254 }
Ryosei 27:dd9f27fce7d1 255
yabahiro 33:dd598bb23ad1 256 if(mode==0) {
yabahiro 33:dd598bb23ad1 257 if(ZoneSW) {
kishibekairohan 35:33c6cdba5630 258 LED::LED::TapeledMode(1);
yabahiro 33:dd598bb23ad1 259 } else {
kishibekairohan 35:33c6cdba5630 260 LED::LED::TapeledMode(2);
yabahiro 33:dd598bb23ad1 261 }
yabahiro 33:dd598bb23ad1 262 }
yabahiro 33:dd598bb23ad1 263 for(int i = 0; i < 8; i++) {
yabahiro 33:dd598bb23ad1 264 g[i] = LineHub::GetPara(i);
yabahiro 33:dd598bb23ad1 265 }
yabahiro 33:dd598bb23ad1 266 if(ThSW) {
yabahiro 33:dd598bb23ad1 267 Twsh=1;
yabahiro 33:dd598bb23ad1 268 } else {
yabahiro 33:dd598bb23ad1 269 Twsh=2;
yabahiro 33:dd598bb23ad1 270 }
yabahiro 33:dd598bb23ad1 271
yabahiro 33:dd598bb23ad1 272 if(StertSW && start_flag == true) {
yabahiro 33:dd598bb23ad1 273 start_flag = false;
yabahiro 33:dd598bb23ad1 274 lock = false;
yabahiro 33:dd598bb23ad1 275 if(ZoneSW) {
yabahiro 33:dd598bb23ad1 276 countss=0;
yabahiro 33:dd598bb23ad1 277 mode=1;
kishibekairohan 34:2fce61e3ebc4 278 current = 3;
yabahiro 33:dd598bb23ad1 279
yabahiro 33:dd598bb23ad1 280 } else {
kishibekairohan 34:2fce61e3ebc4 281 countss = 0;
yabahiro 33:dd598bb23ad1 282 mode=1;
kishibekairohan 34:2fce61e3ebc4 283 current = 2;
yabahiro 33:dd598bb23ad1 284 }
yabahiro 33:dd598bb23ad1 285 }
yabahiro 33:dd598bb23ad1 286 if(DWSW||DOSW) {
yabahiro 33:dd598bb23ad1 287 if(DWSW) {
yabahiro 33:dd598bb23ad1 288 zyouge=true;
yabahiro 33:dd598bb23ad1 289 }
yabahiro 33:dd598bb23ad1 290 if(DOSW) {
yabahiro 33:dd598bb23ad1 291 if(DOLS) {
yabahiro 33:dd598bb23ad1 292 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 293 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 294 } else {
yabahiro 33:dd598bb23ad1 295 motor[MOTOR_0].pwm = 50;
yabahiro 33:dd598bb23ad1 296 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 297 }
yabahiro 33:dd598bb23ad1 298 }
yabahiro 33:dd598bb23ad1 299 } else if(zyouge==false) {
yabahiro 33:dd598bb23ad1 300 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 301 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 302 }
yabahiro 33:dd598bb23ad1 303 if(zyouge) {
yabahiro 33:dd598bb23ad1 304 if(Twsh==1) {
yabahiro 33:dd598bb23ad1 305 motor[MOTOR_0].pwm = 215;
yabahiro 33:dd598bb23ad1 306 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 307 if(UPLS) {
yabahiro 33:dd598bb23ad1 308 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 309 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 310 zyouge=false;
yabahiro 33:dd598bb23ad1 311 }
yabahiro 33:dd598bb23ad1 312 }
yabahiro 33:dd598bb23ad1 313 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 314 motor[MOTOR_0].pwm = 245;
yabahiro 33:dd598bb23ad1 315 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 316 if(UPLS2) {
yabahiro 33:dd598bb23ad1 317 motor[MOTOR_0].pwm = 100;
yabahiro 33:dd598bb23ad1 318 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 319 zyouge=false;
yabahiro 33:dd598bb23ad1 320 }
yabahiro 33:dd598bb23ad1 321 }
yabahiro 33:dd598bb23ad1 322 }
yabahiro 33:dd598bb23ad1 323 if(AIRSW) {
yabahiro 33:dd598bb23ad1 324 if(SW_flag==false) {
yabahiro 33:dd598bb23ad1 325 if(Air_flag==false) {
yabahiro 33:dd598bb23ad1 326 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 327 Air_flag=true;
yabahiro 33:dd598bb23ad1 328 SW_flag=true;
yabahiro 33:dd598bb23ad1 329 } else {
yabahiro 33:dd598bb23ad1 330 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 331 Air_flag=false;
yabahiro 33:dd598bb23ad1 332 SW_flag=true;
yabahiro 33:dd598bb23ad1 333 }
yabahiro 33:dd598bb23ad1 334 }
yabahiro 33:dd598bb23ad1 335 } else {
yabahiro 33:dd598bb23ad1 336 if(SW_flag==true) {
yabahiro 33:dd598bb23ad1 337 countss++;
yabahiro 33:dd598bb23ad1 338 if(countss>=2000) {
yabahiro 33:dd598bb23ad1 339 SW_flag=false;
yabahiro 33:dd598bb23ad1 340 countss=0;
yabahiro 33:dd598bb23ad1 341 }
yabahiro 33:dd598bb23ad1 342 }
yabahiro 33:dd598bb23ad1 343 }
yabahiro 33:dd598bb23ad1 344
yabahiro 33:dd598bb23ad1 345
yabahiro 33:dd598bb23ad1 346 #ifdef USE_MU
Ryosei 27:dd9f27fce7d1 347 controller = CONTROLLER::Controller::GetData();
yabahiro 33:dd598bb23ad1 348 #endif
yabahiro 33:dd598bb23ad1 349
Ryosei 27:dd9f27fce7d1 350
yabahiro 33:dd598bb23ad1 351 #ifdef USE_ERRORCHECK
kishibekairohan 34:2fce61e3ebc4 352 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag){
Ryosei 27:dd9f27fce7d1 353 CONTROLLER::Controller::DataReset();
Ryosei 27:dd9f27fce7d1 354 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 355 lock = true;
Ryosei 27:dd9f27fce7d1 356 } else
yabahiro 33:dd598bb23ad1 357
yabahiro 33:dd598bb23ad1 358 #endif
Ryosei 27:dd9f27fce7d1 359 {
Ryosei 27:dd9f27fce7d1 360
yabahiro 33:dd598bb23ad1 361 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 362 if(!lock) {
Ryosei 27:dd9f27fce7d1 363 Process[current]();
Ryosei 27:dd9f27fce7d1 364 } else
yabahiro 33:dd598bb23ad1 365 #endif
Ryosei 27:dd9f27fce7d1 366 {
Ryosei 27:dd9f27fce7d1 367 //ロック時の処理
Ryosei 27:dd9f27fce7d1 368 }
Ryosei 27:dd9f27fce7d1 369 }
yabahiro 33:dd598bb23ad1 370
Ryosei 27:dd9f27fce7d1 371 SystemProcessUpdate();
Ryosei 27:dd9f27fce7d1 372 }
t_yamamoto 0:669ef71cba68 373 }
t_yamamoto 0:669ef71cba68 374
kishibekairohan 2:c015739085d3 375
kishibekairohan 2:c015739085d3 376
kishibekairohan 2:c015739085d3 377
t_yamamoto 0:669ef71cba68 378 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 379 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 380 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 381 static void Process0()
Ryosei 27:dd9f27fce7d1 382 {
yabahiro 33:dd598bb23ad1 383
yabahiro 33:dd598bb23ad1 384 //AllActuatorReset();
t_yamamoto 0:669ef71cba68 385 }
t_yamamoto 0:669ef71cba68 386 #endif
t_yamamoto 0:669ef71cba68 387
t_yamamoto 0:669ef71cba68 388 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 389 static void Process1()
t_yamamoto 0:669ef71cba68 390 {
kishibekairohan 35:33c6cdba5630 391 LED::LED::TapeledMode(6);
yabahiro 33:dd598bb23ad1 392 start_flag = true;
yabahiro 33:dd598bb23ad1 393 mtc=false;
yabahiro 33:dd598bb23ad1 394 mtc2 = false;
yabahiro 33:dd598bb23ad1 395 mode=1;
yabahiro 33:dd598bb23ad1 396 cross=0;
yabahiro 33:dd598bb23ad1 397 Twsh=1;
yabahiro 33:dd598bb23ad1 398 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 399 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 400 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 33:dd598bb23ad1 401 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 402
yabahiro 33:dd598bb23ad1 403 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 404 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 405 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 406 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 33:dd598bb23ad1 407
yabahiro 33:dd598bb23ad1 408 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 409
yabahiro 33:dd598bb23ad1 410 if(controller->Button.UP) {
yabahiro 33:dd598bb23ad1 411 motor[MOTOR_0].pwm =200;
yabahiro 33:dd598bb23ad1 412 motor[MOTOR_0].dir = FOR;
yabahiro 33:dd598bb23ad1 413 if(UPLS) {
yabahiro 33:dd598bb23ad1 414 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 415 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 416 }
yabahiro 33:dd598bb23ad1 417 }
yabahiro 33:dd598bb23ad1 418 if(controller->Button.DOWN) {
yabahiro 33:dd598bb23ad1 419 motor[MOTOR_0].pwm = 50;
yabahiro 33:dd598bb23ad1 420 motor[MOTOR_0].dir = BACK;
yabahiro 33:dd598bb23ad1 421 }
yabahiro 33:dd598bb23ad1 422 } else {
yabahiro 33:dd598bb23ad1 423 motor[MOTOR_0].pwm = 0;
yabahiro 33:dd598bb23ad1 424 motor[MOTOR_0].dir = BRAKE;
yabahiro 33:dd598bb23ad1 425 }
yabahiro 33:dd598bb23ad1 426
yabahiro 33:dd598bb23ad1 427 if(controller->Button.Y) {
yabahiro 33:dd598bb23ad1 428 if(dz2==true) {
yabahiro 33:dd598bb23ad1 429 if(dz2i==false) {
yabahiro 33:dd598bb23ad1 430 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 431 dz2i=true;
yabahiro 33:dd598bb23ad1 432 } else {
yabahiro 33:dd598bb23ad1 433 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 434 dz2i=false;
yabahiro 33:dd598bb23ad1 435 }
yabahiro 33:dd598bb23ad1 436 dz2=false;
yabahiro 33:dd598bb23ad1 437 }
yabahiro 33:dd598bb23ad1 438 } else {
yabahiro 33:dd598bb23ad1 439 dz2=true;
yabahiro 33:dd598bb23ad1 440 }
yabahiro 33:dd598bb23ad1 441
yabahiro 33:dd598bb23ad1 442 if(controller->Button.A) {
yabahiro 33:dd598bb23ad1 443 if(dz3==true) {
yabahiro 33:dd598bb23ad1 444 if(dz3i==false) {
yabahiro 33:dd598bb23ad1 445
yabahiro 33:dd598bb23ad1 446 dz3i=true;
yabahiro 33:dd598bb23ad1 447 } else {
yabahiro 33:dd598bb23ad1 448 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 449 dz3i=false;
yabahiro 33:dd598bb23ad1 450 }
yabahiro 33:dd598bb23ad1 451 dz3=false;
yabahiro 33:dd598bb23ad1 452 }
yabahiro 33:dd598bb23ad1 453 } else {
yabahiro 33:dd598bb23ad1 454 dz3=true;
yabahiro 33:dd598bb23ad1 455 }
yabahiro 33:dd598bb23ad1 456
yabahiro 33:dd598bb23ad1 457 if(controller->Button.B) {
yabahiro 33:dd598bb23ad1 458 if(dz4==true) {
yabahiro 33:dd598bb23ad1 459 if(dz4i==false) {
kishibekairohan 35:33c6cdba5630 460 solenoid.solenoid1 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 461 dz4i=true;
yabahiro 33:dd598bb23ad1 462 } else {
kishibekairohan 35:33c6cdba5630 463 solenoid.solenoid1 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 464 dz4i=false;
yabahiro 33:dd598bb23ad1 465 }
yabahiro 33:dd598bb23ad1 466 dz4=false;
yabahiro 33:dd598bb23ad1 467 }
yabahiro 33:dd598bb23ad1 468 } else {
yabahiro 33:dd598bb23ad1 469 dz4=true;
yabahiro 33:dd598bb23ad1 470 }
t_yamamoto 0:669ef71cba68 471 }
t_yamamoto 0:669ef71cba68 472 #endif
t_yamamoto 0:669ef71cba68 473
t_yamamoto 0:669ef71cba68 474 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 475 static void Process2()
Ryosei 27:dd9f27fce7d1 476 {
yabahiro 33:dd598bb23ad1 477 //Oh So 青ゾーン
yabahiro 33:dd598bb23ad1 478 countss++;
kishibekairohan 35:33c6cdba5630 479 LED::LED::TapeledMode(4);
yabahiro 33:dd598bb23ad1 480 if(mode==1) {//スタートゾーンから白線検知
yabahiro 33:dd598bb23ad1 481 fast=40;
yabahiro 33:dd598bb23ad1 482 normal=30;
yabahiro 33:dd598bb23ad1 483 slow=20;
yabahiro 33:dd598bb23ad1 484 motor[TIRE_FR].pwm = 27;
yabahiro 33:dd598bb23ad1 485 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 486 motor[TIRE_FL].pwm = 33;
yabahiro 33:dd598bb23ad1 487 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 488 motor[TIRE_BR].pwm = 33;
yabahiro 33:dd598bb23ad1 489 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 490 motor[TIRE_BL].pwm = 27;
yabahiro 33:dd598bb23ad1 491 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 492 //スタート後どれくらい無検知でうごくか↓
yabahiro 33:dd598bb23ad1 493 if(g[2]==0&&countss>=100000) {
yabahiro 33:dd598bb23ad1 494 countss=100000;
yabahiro 33:dd598bb23ad1 495 cross=0;
yabahiro 33:dd598bb23ad1 496 mode=3;
yabahiro 33:dd598bb23ad1 497 }
yabahiro 33:dd598bb23ad1 498 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 33:dd598bb23ad1 499 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 500 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 501 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 502 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 503 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 504 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 505 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 506 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 507 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 508 mode=10;
yabahiro 33:dd598bb23ad1 509 countss=100000;
yabahiro 33:dd598bb23ad1 510 }
yabahiro 33:dd598bb23ad1 511 } else if(mode==10) { //縦ライントレース
M_souta 26:4c0ce2f05688 512
yabahiro 33:dd598bb23ad1 513 if(countss==false&&g[2]==0) {
yabahiro 33:dd598bb23ad1 514 cros++;
yabahiro 33:dd598bb23ad1 515 countss=true;
yabahiro 33:dd598bb23ad1 516 }
yabahiro 33:dd598bb23ad1 517 if(countss==true&&g[2]==99) {
yabahiro 33:dd598bb23ad1 518 countss=false;
yabahiro 33:dd598bb23ad1 519 }
yabahiro 33:dd598bb23ad1 520
yabahiro 33:dd598bb23ad1 521 if(cros==Twsh) {
yabahiro 33:dd598bb23ad1 522 mode=11;
yabahiro 33:dd598bb23ad1 523 countss=0;
yabahiro 33:dd598bb23ad1 524 cros=0;
yabahiro 33:dd598bb23ad1 525 }//ゆっくりモードに入れなかった時の保険
yabahiro 33:dd598bb23ad1 526 switch(g[1]) {
yabahiro 33:dd598bb23ad1 527 case 98:
yabahiro 33:dd598bb23ad1 528 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 529 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 530 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 531 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 532 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 533 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 534 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 535 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 536 mtc=true;
yabahiro 33:dd598bb23ad1 537 if(countss>100000) {
yabahiro 33:dd598bb23ad1 538 cross++;
yabahiro 33:dd598bb23ad1 539 countss=0;
yabahiro 33:dd598bb23ad1 540 }
yabahiro 33:dd598bb23ad1 541 if(cross==Twsh) {
yabahiro 33:dd598bb23ad1 542 mode=11;
yabahiro 33:dd598bb23ad1 543 countss=0;
yabahiro 33:dd598bb23ad1 544 cross=0;
yabahiro 33:dd598bb23ad1 545 }
yabahiro 33:dd598bb23ad1 546 break;
yabahiro 33:dd598bb23ad1 547 case 0:
yabahiro 33:dd598bb23ad1 548 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 549 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 550 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 551 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 552 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 553 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 554 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 555 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 556 mtc=true;
yabahiro 33:dd598bb23ad1 557 break;
yabahiro 33:dd598bb23ad1 558 case 255:
yabahiro 33:dd598bb23ad1 559 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 560 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 561 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 562 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 563 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 564 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 565 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 566 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 567 mtc=true;
yabahiro 33:dd598bb23ad1 568 break;
yabahiro 33:dd598bb23ad1 569 case 253:
yabahiro 33:dd598bb23ad1 570 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 571 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 572 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 573 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 574 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 575 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 576 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 577 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 578 mtc=true;
yabahiro 33:dd598bb23ad1 579 break;
yabahiro 33:dd598bb23ad1 580 case 254:
yabahiro 33:dd598bb23ad1 581 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 582 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 583 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 584 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 585 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 586 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 587 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 588 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 589 mtc=true;
yabahiro 33:dd598bb23ad1 590 break;
yabahiro 33:dd598bb23ad1 591 case 1:
yabahiro 33:dd598bb23ad1 592 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 593 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 594 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 595 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 596 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 597 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 598 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 599 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 600 mtc=true;
yabahiro 33:dd598bb23ad1 601 break;
yabahiro 33:dd598bb23ad1 602 case 3:
yabahiro 33:dd598bb23ad1 603 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 604 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 605 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 606 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 607 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 608 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 609 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 610 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 611 mtc=true;
yabahiro 33:dd598bb23ad1 612 break;
yabahiro 33:dd598bb23ad1 613 case 2:
yabahiro 33:dd598bb23ad1 614 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 615 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 616 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 617 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 618 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 619 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 620 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 621 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 622 mtc=true;
yabahiro 33:dd598bb23ad1 623 break;
yabahiro 33:dd598bb23ad1 624 }
yabahiro 33:dd598bb23ad1 625 if(g[0]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 626 switch(g[0]) {
yabahiro 33:dd598bb23ad1 627 case 0:
yabahiro 33:dd598bb23ad1 628 mtc=false;
yabahiro 33:dd598bb23ad1 629 break;
yabahiro 33:dd598bb23ad1 630 case 255:
yabahiro 33:dd598bb23ad1 631 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 632 mtc=false;
yabahiro 33:dd598bb23ad1 633 break;
yabahiro 33:dd598bb23ad1 634 case 253:
yabahiro 33:dd598bb23ad1 635 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 636 mtc=false;
yabahiro 33:dd598bb23ad1 637 break;
yabahiro 33:dd598bb23ad1 638 case 254:
yabahiro 33:dd598bb23ad1 639 motor[TIRE_FL].pwm += 20;
yabahiro 33:dd598bb23ad1 640 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 641 mtc=false;
yabahiro 33:dd598bb23ad1 642 break;
yabahiro 33:dd598bb23ad1 643 case 1:
yabahiro 33:dd598bb23ad1 644 motor[TIRE_FR].pwm += 5;
yabahiro 33:dd598bb23ad1 645 mtc=false;
yabahiro 33:dd598bb23ad1 646 break;
yabahiro 33:dd598bb23ad1 647 case 3:
yabahiro 33:dd598bb23ad1 648 motor[TIRE_FR].pwm += 10;
yabahiro 33:dd598bb23ad1 649 mtc=false;
yabahiro 33:dd598bb23ad1 650 break;
yabahiro 33:dd598bb23ad1 651 case 2:
yabahiro 33:dd598bb23ad1 652 motor[TIRE_FR].pwm += 20;
yabahiro 33:dd598bb23ad1 653 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 654 mtc=false;
yabahiro 33:dd598bb23ad1 655 break;
yabahiro 33:dd598bb23ad1 656 }
yabahiro 33:dd598bb23ad1 657 }
yabahiro 33:dd598bb23ad1 658 } else if(mode==11) {
yabahiro 33:dd598bb23ad1 659 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 660 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 661 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 662 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 663 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 664 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 665 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 666 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 667 if(g[2]==0) {
yabahiro 33:dd598bb23ad1 668 countss=0;
yabahiro 33:dd598bb23ad1 669 cross=0;
yabahiro 33:dd598bb23ad1 670 mode=20;
yabahiro 33:dd598bb23ad1 671 }
yabahiro 33:dd598bb23ad1 672 } else if(mode==20) {
yabahiro 33:dd598bb23ad1 673
yabahiro 33:dd598bb23ad1 674 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 675 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 676 }
yabahiro 33:dd598bb23ad1 677
yabahiro 33:dd598bb23ad1 678 if(counts==false&&g[1]==0) {
yabahiro 33:dd598bb23ad1 679 cros++;
yabahiro 33:dd598bb23ad1 680 counts=true;
yabahiro 33:dd598bb23ad1 681 }
yabahiro 33:dd598bb23ad1 682 if(counts==true&&g[1]==99) {
yabahiro 33:dd598bb23ad1 683 counts=false;
yabahiro 33:dd598bb23ad1 684 }
yabahiro 33:dd598bb23ad1 685
yabahiro 33:dd598bb23ad1 686 if(cros==2) {
yabahiro 33:dd598bb23ad1 687 //mode=21;
yabahiro 33:dd598bb23ad1 688 //countss=0;
yabahiro 33:dd598bb23ad1 689 //cros=0;
yabahiro 33:dd598bb23ad1 690 }//ゆっくりモードに入れなかった時の保険
yabahiro 33:dd598bb23ad1 691 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 692 fast=35;
yabahiro 33:dd598bb23ad1 693 normal=20;
yabahiro 33:dd598bb23ad1 694 slow=15;
yabahiro 33:dd598bb23ad1 695 }
yabahiro 33:dd598bb23ad1 696 switch(g[2]) {
yabahiro 33:dd598bb23ad1 697 case 98:
yabahiro 33:dd598bb23ad1 698 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 699 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 700 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 701 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 702 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 703 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 704 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 705 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 706 if(countss>50000) {
yabahiro 33:dd598bb23ad1 707 cross++;
yabahiro 33:dd598bb23ad1 708 countss=0;
yabahiro 33:dd598bb23ad1 709 }
yabahiro 33:dd598bb23ad1 710 mtc2=true;
yabahiro 33:dd598bb23ad1 711 if(cross==2) {
yabahiro 33:dd598bb23ad1 712 mode=22;
yabahiro 33:dd598bb23ad1 713 cross=0;
yabahiro 33:dd598bb23ad1 714 countss=0;
yabahiro 33:dd598bb23ad1 715 fast=40;
yabahiro 33:dd598bb23ad1 716 normal=30;
yabahiro 33:dd598bb23ad1 717 slow=20;
yabahiro 33:dd598bb23ad1 718 }
yabahiro 33:dd598bb23ad1 719 break;
yabahiro 33:dd598bb23ad1 720 case 0:
yabahiro 33:dd598bb23ad1 721 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 722 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 723 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 724 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 725 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 726 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 727 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 728 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 729 mtc2=true;
yabahiro 33:dd598bb23ad1 730 break;
yabahiro 33:dd598bb23ad1 731 case 255:
yabahiro 33:dd598bb23ad1 732 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 733 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 734 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 735 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 736 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 737 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 738 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 739 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 740 mtc2=true;
yabahiro 33:dd598bb23ad1 741 break;
yabahiro 33:dd598bb23ad1 742 case 253:
yabahiro 33:dd598bb23ad1 743 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 744 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 745 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 746 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 747 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 748 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 749 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 750 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 751 mtc2=true;
yabahiro 33:dd598bb23ad1 752 break;
yabahiro 33:dd598bb23ad1 753 case 254:
yabahiro 33:dd598bb23ad1 754 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 755 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 756 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 757 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 758 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 759 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 760 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 761 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 762 mtc2=true;
yabahiro 33:dd598bb23ad1 763 break;
yabahiro 33:dd598bb23ad1 764 case 1:
yabahiro 33:dd598bb23ad1 765 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 766 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 767 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 768 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 769 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 770 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 771 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 772 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 773 mtc2=true;
yabahiro 33:dd598bb23ad1 774 break;
yabahiro 33:dd598bb23ad1 775 case 3:
yabahiro 33:dd598bb23ad1 776 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 777 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 778 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 779 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 780 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 781 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 782 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 783 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 784 mtc2=true;
yabahiro 33:dd598bb23ad1 785 break;
yabahiro 33:dd598bb23ad1 786 case 2:
yabahiro 33:dd598bb23ad1 787 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 788 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 789 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 790 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 791 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 792 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 793 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 794 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 795 mtc2=true;
yabahiro 33:dd598bb23ad1 796 break;
yabahiro 33:dd598bb23ad1 797 }
yabahiro 33:dd598bb23ad1 798 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 799 switch(g[3]) {
yabahiro 33:dd598bb23ad1 800 case 0:
yabahiro 33:dd598bb23ad1 801 mtc2=false;
yabahiro 33:dd598bb23ad1 802 break;
yabahiro 33:dd598bb23ad1 803 case 255:
yabahiro 33:dd598bb23ad1 804 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 805 mtc2=false;
yabahiro 33:dd598bb23ad1 806 break;
yabahiro 33:dd598bb23ad1 807 case 253:
yabahiro 33:dd598bb23ad1 808 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 809 mtc2=false;
yabahiro 33:dd598bb23ad1 810 break;
yabahiro 33:dd598bb23ad1 811 case 254:
yabahiro 33:dd598bb23ad1 812 motor[TIRE_BL].pwm += 20;
yabahiro 33:dd598bb23ad1 813 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 814 mtc2=false;
yabahiro 33:dd598bb23ad1 815 break;
yabahiro 33:dd598bb23ad1 816 case 1:
yabahiro 33:dd598bb23ad1 817 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 818 mtc2=false;
yabahiro 33:dd598bb23ad1 819 break;
yabahiro 33:dd598bb23ad1 820 case 3:
yabahiro 33:dd598bb23ad1 821 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 822 mtc2=false;
yabahiro 33:dd598bb23ad1 823 break;
yabahiro 33:dd598bb23ad1 824 case 2:
yabahiro 33:dd598bb23ad1 825 motor[TIRE_FL].pwm += 20;
yabahiro 33:dd598bb23ad1 826 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 827 mtc2=false;
yabahiro 33:dd598bb23ad1 828 break;
yabahiro 33:dd598bb23ad1 829 }
yabahiro 33:dd598bb23ad1 830 }
yabahiro 33:dd598bb23ad1 831
yabahiro 33:dd598bb23ad1 832 } else if(mode==21) {
yabahiro 33:dd598bb23ad1 833 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 834 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 835 motor[TIRE_FL].pwm = 30;
yabahiro 33:dd598bb23ad1 836 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 837 motor[TIRE_BR].pwm = 30;
yabahiro 33:dd598bb23ad1 838 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 839 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 840 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 841 if(countss<=100000) {
yabahiro 33:dd598bb23ad1 842 mode=22;
yabahiro 33:dd598bb23ad1 843 }
yabahiro 33:dd598bb23ad1 844 }
yabahiro 33:dd598bb23ad1 845 kiri=true;
yabahiro 33:dd598bb23ad1 846 if(kiri) {
yabahiro 33:dd598bb23ad1 847 if(mode==22) {
yabahiro 33:dd598bb23ad1 848 motor[TIRE_FR].pwm = 100;
yabahiro 33:dd598bb23ad1 849 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 850 motor[TIRE_FL].pwm = 100;
yabahiro 33:dd598bb23ad1 851 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 852 motor[TIRE_BR].pwm = 100;
yabahiro 33:dd598bb23ad1 853 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 854 motor[TIRE_BL].pwm = 100;
yabahiro 33:dd598bb23ad1 855 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 856 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 857 mode=23;
yabahiro 33:dd598bb23ad1 858 }
yabahiro 33:dd598bb23ad1 859 } else if(mode==23) {
yabahiro 33:dd598bb23ad1 860 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 861 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 862 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 863 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 864 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 865 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 866 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 867 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 868 if(g[2]==98) {
yabahiro 33:dd598bb23ad1 869 mode=24;
yabahiro 33:dd598bb23ad1 870 countss=0;
yabahiro 33:dd598bb23ad1 871 }
yabahiro 33:dd598bb23ad1 872 } else if(mode==24) {
yabahiro 33:dd598bb23ad1 873 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 874 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 875 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 876 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 877 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 878 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 879 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 880 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 881 if(g[2]!=98) {
yabahiro 33:dd598bb23ad1 882 countss=0;
yabahiro 33:dd598bb23ad1 883 mode=25;
yabahiro 33:dd598bb23ad1 884 }
yabahiro 33:dd598bb23ad1 885 } else if(mode==25) {
yabahiro 33:dd598bb23ad1 886 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 887 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 888 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 889 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 890 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 891 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 892 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 893 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 894 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 895 mode=26;
yabahiro 33:dd598bb23ad1 896 countss=0;
yabahiro 33:dd598bb23ad1 897 }
yabahiro 33:dd598bb23ad1 898 } else if(mode==26) {
yabahiro 33:dd598bb23ad1 899 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 900 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 901 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 902 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 903 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 904 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 905 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 906 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 907 if(countss>=5000) {
yabahiro 33:dd598bb23ad1 908 mode=27;
yabahiro 33:dd598bb23ad1 909 countss=0;
yabahiro 33:dd598bb23ad1 910 }
yabahiro 33:dd598bb23ad1 911 } else if(mode==27) {
yabahiro 33:dd598bb23ad1 912 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 913 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 914 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 915 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 916 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 917 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 918 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 919 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 920 //どれくらい回転するかって値
yabahiro 33:dd598bb23ad1 921 if(countss>=4000) {
yabahiro 33:dd598bb23ad1 922 countss=0;
yabahiro 33:dd598bb23ad1 923 mode=28;
yabahiro 33:dd598bb23ad1 924 }
yabahiro 33:dd598bb23ad1 925 } else if(mode==28) {
yabahiro 33:dd598bb23ad1 926 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 927 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 928 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 929 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 930 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 931 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 932 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 933 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 934 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 935 mode=30;
yabahiro 33:dd598bb23ad1 936 countss=0;
yabahiro 33:dd598bb23ad1 937 }
yabahiro 33:dd598bb23ad1 938 }
yabahiro 33:dd598bb23ad1 939 } else {
yabahiro 33:dd598bb23ad1 940 if(mode==22) {
yabahiro 33:dd598bb23ad1 941 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 942 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 943 motor[TIRE_FL].pwm = 25;
yabahiro 33:dd598bb23ad1 944 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 945 motor[TIRE_BR].pwm = 25;
yabahiro 33:dd598bb23ad1 946 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 947 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 948 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 949 if(g[0]==0&&countss>=10000) {
yabahiro 33:dd598bb23ad1 950 mode=30;
yabahiro 33:dd598bb23ad1 951 countss=0;
yabahiro 33:dd598bb23ad1 952 cros=0;
yabahiro 33:dd598bb23ad1 953 }
yabahiro 33:dd598bb23ad1 954 }
yabahiro 33:dd598bb23ad1 955 }
yabahiro 33:dd598bb23ad1 956 if(mode==30) {
yabahiro 33:dd598bb23ad1 957 motor[TIRE_FR].pwm = 60;
yabahiro 33:dd598bb23ad1 958 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 959 motor[TIRE_FL].pwm = 60;
yabahiro 33:dd598bb23ad1 960 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 961 motor[TIRE_BR].pwm = 60;
yabahiro 33:dd598bb23ad1 962 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 963 motor[TIRE_BL].pwm = 57;
yabahiro 33:dd598bb23ad1 964 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 965 if(g[1]==98) {
yabahiro 33:dd598bb23ad1 966 mode=31;
yabahiro 33:dd598bb23ad1 967 countss=0;
yabahiro 33:dd598bb23ad1 968 }
yabahiro 33:dd598bb23ad1 969 } else if(mode==31) {
yabahiro 33:dd598bb23ad1 970 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 971 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 972 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 973 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 974 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 975 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 976 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 977 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 978 if(countss>=5000) {
yabahiro 33:dd598bb23ad1 979 mode=32;
yabahiro 33:dd598bb23ad1 980 countss=0;
yabahiro 33:dd598bb23ad1 981 }
yabahiro 33:dd598bb23ad1 982 } else if(mode==32) {
yabahiro 33:dd598bb23ad1 983 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 984 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 985 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 986 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 987 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 988 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 989 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 990 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 991 if(countss>=30000) {
yabahiro 33:dd598bb23ad1 992 mode=33;
yabahiro 33:dd598bb23ad1 993 countss=0;
yabahiro 33:dd598bb23ad1 994 }
yabahiro 33:dd598bb23ad1 995 } else if(mode==33) {
yabahiro 33:dd598bb23ad1 996 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 997 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 998 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 999 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1000 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1001 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1002 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 1003 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1004 if(g[3]==0&&countss>=20000) {
yabahiro 33:dd598bb23ad1 1005 countss=0;
yabahiro 33:dd598bb23ad1 1006 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 1007 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 1008 }
yabahiro 33:dd598bb23ad1 1009 mode=40;
yabahiro 33:dd598bb23ad1 1010 }
yabahiro 33:dd598bb23ad1 1011 } else if(mode==40) {
yabahiro 33:dd598bb23ad1 1012 if(LIB) {
yabahiro 33:dd598bb23ad1 1013 mode=42;
yabahiro 33:dd598bb23ad1 1014 }
yabahiro 33:dd598bb23ad1 1015 switch(g[3]) {
yabahiro 33:dd598bb23ad1 1016 case 98:
yabahiro 33:dd598bb23ad1 1017 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1018 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1019 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1020 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1021 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1022 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1023 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1024 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1025 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 1026 cross++;
yabahiro 33:dd598bb23ad1 1027 countss=0;
yabahiro 33:dd598bb23ad1 1028 }
yabahiro 33:dd598bb23ad1 1029 mtc2=true;
yabahiro 33:dd598bb23ad1 1030 if(cross==2) {
yabahiro 33:dd598bb23ad1 1031 mode=41;
yabahiro 33:dd598bb23ad1 1032 countss=0;
yabahiro 33:dd598bb23ad1 1033 cross=0;
yabahiro 33:dd598bb23ad1 1034 }
yabahiro 33:dd598bb23ad1 1035 break;
yabahiro 33:dd598bb23ad1 1036 case 0:
yabahiro 33:dd598bb23ad1 1037 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1038 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1039 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1040 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1041 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1042 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1043 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1044 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1045 mtc2=true;
yabahiro 33:dd598bb23ad1 1046 break;
yabahiro 33:dd598bb23ad1 1047 case 255:
yabahiro 33:dd598bb23ad1 1048 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1049 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1050 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1051 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1052 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1053 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1054 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1055 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1056 mtc2=true;
yabahiro 33:dd598bb23ad1 1057 break;
yabahiro 33:dd598bb23ad1 1058 case 253:
yabahiro 33:dd598bb23ad1 1059 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1060 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1061 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1062 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1063 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1064 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1065 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1066 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1067 mtc2=true;
yabahiro 33:dd598bb23ad1 1068 break;
yabahiro 33:dd598bb23ad1 1069 case 254:
yabahiro 33:dd598bb23ad1 1070 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1071 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1072 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1073 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1074 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1075 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1076 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1077 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1078 mtc2=true;
yabahiro 33:dd598bb23ad1 1079 break;
yabahiro 33:dd598bb23ad1 1080 case 1:
yabahiro 33:dd598bb23ad1 1081 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1082 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1083 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1084 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1085 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1086 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1087 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1088 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1089 mtc2=true;
yabahiro 33:dd598bb23ad1 1090 break;
yabahiro 33:dd598bb23ad1 1091 case 3:
yabahiro 33:dd598bb23ad1 1092 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1093 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1094 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1095 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1096 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1097 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1098 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1099 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1100 mtc2=true;
yabahiro 33:dd598bb23ad1 1101 break;
yabahiro 33:dd598bb23ad1 1102 case 2:
yabahiro 33:dd598bb23ad1 1103 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1104 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1105 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1106 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1107 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1108 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1109 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1110 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1111 mtc2=true;
yabahiro 33:dd598bb23ad1 1112 break;
yabahiro 33:dd598bb23ad1 1113 }
yabahiro 33:dd598bb23ad1 1114 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 1115 switch(g[2]) {
yabahiro 33:dd598bb23ad1 1116 case 0:
yabahiro 33:dd598bb23ad1 1117 mtc2=false;
yabahiro 33:dd598bb23ad1 1118 break;
yabahiro 33:dd598bb23ad1 1119 case 255:
yabahiro 33:dd598bb23ad1 1120 motor[TIRE_FR].pwm += 5;
yabahiro 33:dd598bb23ad1 1121 mtc2=false;
yabahiro 33:dd598bb23ad1 1122 break;
yabahiro 33:dd598bb23ad1 1123 case 253:
yabahiro 33:dd598bb23ad1 1124 motor[TIRE_FR].pwm += 10;
yabahiro 33:dd598bb23ad1 1125 mtc2=false;
yabahiro 33:dd598bb23ad1 1126 break;
yabahiro 33:dd598bb23ad1 1127 case 254:
yabahiro 33:dd598bb23ad1 1128 motor[TIRE_FR].pwm += 20;
yabahiro 33:dd598bb23ad1 1129 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1130 mtc2=false;
yabahiro 33:dd598bb23ad1 1131 break;
yabahiro 33:dd598bb23ad1 1132 case 1:
yabahiro 33:dd598bb23ad1 1133 motor[TIRE_BR].pwm += 5;
yabahiro 33:dd598bb23ad1 1134 mtc2=false;
yabahiro 33:dd598bb23ad1 1135 break;
yabahiro 33:dd598bb23ad1 1136 case 3:
yabahiro 33:dd598bb23ad1 1137 motor[TIRE_BR].pwm += 10;
yabahiro 33:dd598bb23ad1 1138 mtc2=false;
yabahiro 33:dd598bb23ad1 1139 break;
yabahiro 33:dd598bb23ad1 1140 case 2:
yabahiro 33:dd598bb23ad1 1141 motor[TIRE_BR].pwm += 20;
yabahiro 33:dd598bb23ad1 1142 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1143 mtc2=false;
yabahiro 33:dd598bb23ad1 1144 break;
yabahiro 33:dd598bb23ad1 1145 }
yabahiro 33:dd598bb23ad1 1146 }
yabahiro 33:dd598bb23ad1 1147 } else if(mode==41) {
yabahiro 33:dd598bb23ad1 1148 if(LIF||LIB) {
yabahiro 33:dd598bb23ad1 1149 mode=42;
yabahiro 33:dd598bb23ad1 1150 }
yabahiro 33:dd598bb23ad1 1151 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 1152 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1153 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1154 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1155 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1156 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1157 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 1158 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1159 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 1160 countss=0;
yabahiro 33:dd598bb23ad1 1161 mode=70;
yabahiro 33:dd598bb23ad1 1162 normal=60;
yabahiro 33:dd598bb23ad1 1163 fast=70;
yabahiro 33:dd598bb23ad1 1164 slow=40;
yabahiro 33:dd598bb23ad1 1165 }
yabahiro 33:dd598bb23ad1 1166 } else if(mode==42) {
yabahiro 33:dd598bb23ad1 1167 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1168 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1169 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1170 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1171 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 1172 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1173 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1174 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1175 if(g[0]==0) {
yabahiro 33:dd598bb23ad1 1176 countss=0;
yabahiro 33:dd598bb23ad1 1177 slow=40;
yabahiro 33:dd598bb23ad1 1178 }
yabahiro 33:dd598bb23ad1 1179
yabahiro 33:dd598bb23ad1 1180 } else if(mode==70)
yabahiro 33:dd598bb23ad1 1181 switch(g[0]) {
yabahiro 33:dd598bb23ad1 1182 case 99:
yabahiro 33:dd598bb23ad1 1183 if(countss>=100000) {
yabahiro 33:dd598bb23ad1 1184 cross++;
yabahiro 33:dd598bb23ad1 1185 mode=100;
yabahiro 33:dd598bb23ad1 1186 countss=0;
yabahiro 33:dd598bb23ad1 1187 }
yabahiro 33:dd598bb23ad1 1188 if(cross==2) {
yabahiro 33:dd598bb23ad1 1189 cross=0;
yabahiro 33:dd598bb23ad1 1190 mode=100;
yabahiro 33:dd598bb23ad1 1191 }
yabahiro 33:dd598bb23ad1 1192 break;
yabahiro 33:dd598bb23ad1 1193 case 98:
yabahiro 33:dd598bb23ad1 1194 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1195 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1196 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1197 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1198 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1199 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1200 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1201 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1202 break;
yabahiro 33:dd598bb23ad1 1203 case 0:
yabahiro 33:dd598bb23ad1 1204 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1205 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1206 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1207 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1208 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1209 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1210 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1211 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1212 mtc=true;
yabahiro 33:dd598bb23ad1 1213 break;
yabahiro 33:dd598bb23ad1 1214 case 255:
yabahiro 33:dd598bb23ad1 1215 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1216 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1217 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1218 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1219 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1220 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1221 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1222 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1223 mtc=true;
yabahiro 33:dd598bb23ad1 1224 break;
yabahiro 33:dd598bb23ad1 1225 case 253:
yabahiro 33:dd598bb23ad1 1226 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1227 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1228 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1229 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1230 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1231 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1232 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1233 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1234 mtc=true;
yabahiro 33:dd598bb23ad1 1235 break;
yabahiro 33:dd598bb23ad1 1236 case 254:
yabahiro 33:dd598bb23ad1 1237 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1238 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1239 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1240 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1241 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1242 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1243 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1244 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1245 mtc=true;
yabahiro 33:dd598bb23ad1 1246 break;
yabahiro 33:dd598bb23ad1 1247 case 1:
yabahiro 33:dd598bb23ad1 1248 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1249 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1250 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1251 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1252 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1253 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1254 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1255 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1256 mtc=true;
yabahiro 33:dd598bb23ad1 1257 break;
yabahiro 33:dd598bb23ad1 1258 case 3:
yabahiro 33:dd598bb23ad1 1259 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1260 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1261 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1262 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1263 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1264 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1265 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1266 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1267 mtc=true;
yabahiro 33:dd598bb23ad1 1268 break;
yabahiro 33:dd598bb23ad1 1269 case 2:
yabahiro 33:dd598bb23ad1 1270 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1271 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1272 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1273 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1274 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1275 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1276 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1277 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1278 mtc=true;
yabahiro 33:dd598bb23ad1 1279 break;
yabahiro 33:dd598bb23ad1 1280 }
yabahiro 33:dd598bb23ad1 1281 if(g[1]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 1282 switch(g[1]) {
yabahiro 33:dd598bb23ad1 1283 case 0:
yabahiro 33:dd598bb23ad1 1284 mtc=false;
yabahiro 33:dd598bb23ad1 1285 break;
yabahiro 33:dd598bb23ad1 1286 case 255:
yabahiro 33:dd598bb23ad1 1287 motor[TIRE_BR].pwm += 5;
yabahiro 33:dd598bb23ad1 1288 mtc=false;
yabahiro 33:dd598bb23ad1 1289 break;
yabahiro 33:dd598bb23ad1 1290 case 253:
yabahiro 33:dd598bb23ad1 1291 motor[TIRE_BR].pwm += 10;
yabahiro 33:dd598bb23ad1 1292 mtc=false;
yabahiro 33:dd598bb23ad1 1293 break;
yabahiro 33:dd598bb23ad1 1294 case 254:
yabahiro 33:dd598bb23ad1 1295 motor[TIRE_BR].pwm += 20;
yabahiro 33:dd598bb23ad1 1296 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1297 mtc=false;
yabahiro 33:dd598bb23ad1 1298 break;
yabahiro 33:dd598bb23ad1 1299 case 1:
yabahiro 33:dd598bb23ad1 1300 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 1301 mtc=false;
yabahiro 33:dd598bb23ad1 1302 break;
yabahiro 33:dd598bb23ad1 1303 case 3:
yabahiro 33:dd598bb23ad1 1304 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 1305 mtc=false;
yabahiro 33:dd598bb23ad1 1306 break;
yabahiro 33:dd598bb23ad1 1307 case 2:
yabahiro 33:dd598bb23ad1 1308 motor[TIRE_BL].pwm += 20;
yabahiro 33:dd598bb23ad1 1309 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1310 mtc=false;
yabahiro 33:dd598bb23ad1 1311 break;
yabahiro 33:dd598bb23ad1 1312 }
yabahiro 33:dd598bb23ad1 1313 } else if(mode==100) {
yabahiro 33:dd598bb23ad1 1314 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 1315 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1316 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1317 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1318 motor[TIRE_BR].pwm = 23;
yabahiro 33:dd598bb23ad1 1319 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1320 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 1321 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1322 }
kishibekairohan 13:b6e02d6261d7 1323 }
kishibekairohan 13:b6e02d6261d7 1324 #endif
kishibekairohan 13:b6e02d6261d7 1325
kishibekairohan 13:b6e02d6261d7 1326 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 1327 static void Process3()
kishibekairohan 13:b6e02d6261d7 1328 {
yabahiro 33:dd598bb23ad1 1329 //Clearly 赤ゾーン
kishibekairohan 35:33c6cdba5630 1330 LED::LED::TapeledMode(3);
yabahiro 33:dd598bb23ad1 1331 countss++;
yabahiro 33:dd598bb23ad1 1332 if(mode==1) {//スタートゾーンから白線検知
yabahiro 33:dd598bb23ad1 1333 fast=40;
yabahiro 33:dd598bb23ad1 1334 normal=30;
yabahiro 33:dd598bb23ad1 1335 slow=20;
yabahiro 33:dd598bb23ad1 1336 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1337 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1338 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1339 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1340 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1341 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1342 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1343 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1344 if(g[2]==0&&countss>=100000) {
yabahiro 33:dd598bb23ad1 1345 countss=100000;
yabahiro 33:dd598bb23ad1 1346 cross=0;
yabahiro 33:dd598bb23ad1 1347 mode=3;
yabahiro 33:dd598bb23ad1 1348 }
yabahiro 33:dd598bb23ad1 1349 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 33:dd598bb23ad1 1350 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1351 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1352 motor[TIRE_FL].pwm = 25;
yabahiro 33:dd598bb23ad1 1353 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1354 motor[TIRE_BR].pwm = 25;
yabahiro 33:dd598bb23ad1 1355 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1356 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1357 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1358 if(g[0]==0) {
yabahiro 33:dd598bb23ad1 1359 mode=10;
yabahiro 33:dd598bb23ad1 1360 countss=100000;
yabahiro 33:dd598bb23ad1 1361 }
yabahiro 33:dd598bb23ad1 1362 } else if(mode==10) { //縦ライントレース
yabahiro 33:dd598bb23ad1 1363 if(counts==false&&g[2]==0) {
yabahiro 33:dd598bb23ad1 1364 cros++;
yabahiro 33:dd598bb23ad1 1365 counts=true;
yabahiro 33:dd598bb23ad1 1366 }
yabahiro 33:dd598bb23ad1 1367 if(counts==true&&g[2]==99) {
yabahiro 33:dd598bb23ad1 1368 counts=false;
yabahiro 33:dd598bb23ad1 1369 }
yabahiro 33:dd598bb23ad1 1370
yabahiro 33:dd598bb23ad1 1371 if(cros==Twsh) {
yabahiro 33:dd598bb23ad1 1372 mode=11;
yabahiro 33:dd598bb23ad1 1373 countss=0;
yabahiro 33:dd598bb23ad1 1374 cros=0;
yabahiro 33:dd598bb23ad1 1375 }//ゆっくりモードに入れなかった時の保険
yabahiro 33:dd598bb23ad1 1376 switch(g[0]) {
yabahiro 33:dd598bb23ad1 1377 case 98:
yabahiro 33:dd598bb23ad1 1378 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1379 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1380 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1381 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1382 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1383 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1384 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1385 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1386 mtc=true;
yabahiro 33:dd598bb23ad1 1387 if(countss>80000) {
yabahiro 33:dd598bb23ad1 1388 cross++;
yabahiro 33:dd598bb23ad1 1389 countss=0;
yabahiro 33:dd598bb23ad1 1390 }
yabahiro 33:dd598bb23ad1 1391 if(cross==Twsh) {
yabahiro 33:dd598bb23ad1 1392 mode=11;
yabahiro 33:dd598bb23ad1 1393 countss=0;
yabahiro 33:dd598bb23ad1 1394 cross=0;
yabahiro 33:dd598bb23ad1 1395 }
yabahiro 33:dd598bb23ad1 1396 break;
yabahiro 33:dd598bb23ad1 1397 case 0:
yabahiro 33:dd598bb23ad1 1398 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1399 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1400 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1401 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1402 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1403 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1404 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1405 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1406 mtc=true;
yabahiro 33:dd598bb23ad1 1407 break;
yabahiro 33:dd598bb23ad1 1408 case 255:
yabahiro 33:dd598bb23ad1 1409 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1410 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1411 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1412 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1413 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1414 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1415 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1416 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1417 mtc=true;
yabahiro 33:dd598bb23ad1 1418 break;
yabahiro 33:dd598bb23ad1 1419 case 253:
yabahiro 33:dd598bb23ad1 1420 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1421 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1422 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1423 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1424 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1425 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1426 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1427 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1428 mtc=true;
yabahiro 33:dd598bb23ad1 1429 break;
yabahiro 33:dd598bb23ad1 1430 case 254:
yabahiro 33:dd598bb23ad1 1431 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1432 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1433 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1434 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1435 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1436 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1437 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1438 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1439 mtc=true;
yabahiro 33:dd598bb23ad1 1440 break;
yabahiro 33:dd598bb23ad1 1441 case 1:
yabahiro 33:dd598bb23ad1 1442 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1443 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1444 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1445 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1446 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1447 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1448 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1449 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1450 mtc=true;
yabahiro 33:dd598bb23ad1 1451 break;
yabahiro 33:dd598bb23ad1 1452 case 3:
yabahiro 33:dd598bb23ad1 1453 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1454 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1455 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1456 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1457 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1458 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1459 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1460 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1461 mtc=true;
yabahiro 33:dd598bb23ad1 1462 break;
yabahiro 33:dd598bb23ad1 1463 case 2:
yabahiro 33:dd598bb23ad1 1464 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1465 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1466 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1467 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1468 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1469 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1470 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1471 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1472 mtc=true;
yabahiro 33:dd598bb23ad1 1473 break;
yabahiro 33:dd598bb23ad1 1474 }
yabahiro 33:dd598bb23ad1 1475 if(g[0]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 1476 switch(g[1]) {
yabahiro 33:dd598bb23ad1 1477 case 0:
yabahiro 33:dd598bb23ad1 1478 mtc=false;
yabahiro 33:dd598bb23ad1 1479 break;
yabahiro 33:dd598bb23ad1 1480 case 255:
yabahiro 33:dd598bb23ad1 1481 motor[TIRE_BR].pwm += 5;
yabahiro 33:dd598bb23ad1 1482 mtc=false;
yabahiro 33:dd598bb23ad1 1483 break;
yabahiro 33:dd598bb23ad1 1484 case 253:
yabahiro 33:dd598bb23ad1 1485 motor[TIRE_BR].pwm += 10;
yabahiro 33:dd598bb23ad1 1486 mtc=false;
yabahiro 33:dd598bb23ad1 1487 break;
yabahiro 33:dd598bb23ad1 1488 case 254:
yabahiro 33:dd598bb23ad1 1489 motor[TIRE_BR].pwm += 20;
yabahiro 33:dd598bb23ad1 1490 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1491 mtc=false;
yabahiro 33:dd598bb23ad1 1492 break;
yabahiro 33:dd598bb23ad1 1493 case 1:
yabahiro 33:dd598bb23ad1 1494 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 1495 mtc=false;
yabahiro 33:dd598bb23ad1 1496 break;
yabahiro 33:dd598bb23ad1 1497 case 3:
yabahiro 33:dd598bb23ad1 1498 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 1499 mtc=false;
yabahiro 33:dd598bb23ad1 1500 break;
yabahiro 33:dd598bb23ad1 1501 case 2:
yabahiro 33:dd598bb23ad1 1502 motor[TIRE_BL].pwm += 20;
yabahiro 33:dd598bb23ad1 1503 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1504 mtc=false;
yabahiro 33:dd598bb23ad1 1505 break;
yabahiro 33:dd598bb23ad1 1506 }
yabahiro 33:dd598bb23ad1 1507 }
yabahiro 33:dd598bb23ad1 1508 } else if(mode==11) {
yabahiro 33:dd598bb23ad1 1509 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1510 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1511 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1512 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1513 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1514 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1515 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1516 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1517 if(g[2]==0) {
yabahiro 33:dd598bb23ad1 1518 countss=0;
yabahiro 33:dd598bb23ad1 1519 cross=0;
yabahiro 33:dd598bb23ad1 1520 mode=20;
yabahiro 33:dd598bb23ad1 1521 }
yabahiro 33:dd598bb23ad1 1522 } else if(mode==20) {
yabahiro 33:dd598bb23ad1 1523
yabahiro 33:dd598bb23ad1 1524 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 1525 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 33:dd598bb23ad1 1526 }
yabahiro 33:dd598bb23ad1 1527 if(counts==false&&g[0]==0) {
yabahiro 33:dd598bb23ad1 1528 cros++;
yabahiro 33:dd598bb23ad1 1529 counts=true;
yabahiro 33:dd598bb23ad1 1530 }
yabahiro 33:dd598bb23ad1 1531 if(counts==true&&g[0]==99) {
yabahiro 33:dd598bb23ad1 1532 counts=false;
yabahiro 33:dd598bb23ad1 1533 }
yabahiro 33:dd598bb23ad1 1534
yabahiro 33:dd598bb23ad1 1535 if(cros==2) {
yabahiro 33:dd598bb23ad1 1536
yabahiro 33:dd598bb23ad1 1537 // mode=21;
yabahiro 33:dd598bb23ad1 1538 // countss=0;
yabahiro 33:dd598bb23ad1 1539 // cros=0;
yabahiro 33:dd598bb23ad1 1540 }//ゆっくりモードに入れなかった時の保険
yabahiro 33:dd598bb23ad1 1541 if(g[0]==0) {
yabahiro 33:dd598bb23ad1 1542 fast=35;
yabahiro 33:dd598bb23ad1 1543 normal=20;
yabahiro 33:dd598bb23ad1 1544 slow=15;
yabahiro 33:dd598bb23ad1 1545 }
yabahiro 33:dd598bb23ad1 1546 switch(g[2]) {
yabahiro 33:dd598bb23ad1 1547 case 98:
yabahiro 33:dd598bb23ad1 1548 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1549 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1550 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1551 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1552 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1553 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1554 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1555 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1556 if(countss>30000) {
yabahiro 33:dd598bb23ad1 1557 cross++;
yabahiro 33:dd598bb23ad1 1558 countss=0;
yabahiro 33:dd598bb23ad1 1559 }
yabahiro 33:dd598bb23ad1 1560 mtc2=true;
yabahiro 33:dd598bb23ad1 1561 if(cross==2) {
yabahiro 33:dd598bb23ad1 1562 mode=22;
yabahiro 33:dd598bb23ad1 1563 countss=0;
yabahiro 33:dd598bb23ad1 1564 cross=0;
yabahiro 33:dd598bb23ad1 1565 fast=40;
yabahiro 33:dd598bb23ad1 1566 normal=30;
yabahiro 33:dd598bb23ad1 1567 slow=20;
yabahiro 33:dd598bb23ad1 1568 }
yabahiro 33:dd598bb23ad1 1569 break;
yabahiro 33:dd598bb23ad1 1570 case 0:
yabahiro 33:dd598bb23ad1 1571 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1572 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1573 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1574 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1575 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1576 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1577 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1578 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1579 mtc2=true;
yabahiro 33:dd598bb23ad1 1580 break;
yabahiro 33:dd598bb23ad1 1581 case 255:
yabahiro 33:dd598bb23ad1 1582 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1583 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1584 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1585 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1586 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1587 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1588 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1589 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1590 mtc2=true;
yabahiro 33:dd598bb23ad1 1591 break;
yabahiro 33:dd598bb23ad1 1592 case 253:
yabahiro 33:dd598bb23ad1 1593 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1594 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1595 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1596 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1597 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1598 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1599 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1600 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1601 mtc2=true;
yabahiro 33:dd598bb23ad1 1602 break;
yabahiro 33:dd598bb23ad1 1603 case 254:
yabahiro 33:dd598bb23ad1 1604 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1605 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1606 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1607 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1608 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1609 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1610 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1611 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1612 mtc2=true;
yabahiro 33:dd598bb23ad1 1613 break;
yabahiro 33:dd598bb23ad1 1614 case 1:
yabahiro 33:dd598bb23ad1 1615 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1616 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1617 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1618 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1619 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1620 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1621 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1622 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1623 mtc2=true;
yabahiro 33:dd598bb23ad1 1624 break;
yabahiro 33:dd598bb23ad1 1625 case 3:
yabahiro 33:dd598bb23ad1 1626 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1627 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1628 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1629 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1630 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1631 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1632 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1633 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1634 mtc2=true;
yabahiro 33:dd598bb23ad1 1635 break;
yabahiro 33:dd598bb23ad1 1636 case 2:
yabahiro 33:dd598bb23ad1 1637 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1638 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1639 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1640 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1641 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1642 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1643 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1644 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1645 mtc2=true;
yabahiro 33:dd598bb23ad1 1646 break;
yabahiro 33:dd598bb23ad1 1647 }
yabahiro 33:dd598bb23ad1 1648 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 1649 switch(g[3]) {
yabahiro 33:dd598bb23ad1 1650 case 0:
yabahiro 33:dd598bb23ad1 1651 mtc2=false;
yabahiro 33:dd598bb23ad1 1652 break;
yabahiro 33:dd598bb23ad1 1653 case 255:
yabahiro 33:dd598bb23ad1 1654 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 1655 mtc2=false;
yabahiro 33:dd598bb23ad1 1656 break;
yabahiro 33:dd598bb23ad1 1657 case 253:
yabahiro 33:dd598bb23ad1 1658 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 1659 mtc2=false;
yabahiro 33:dd598bb23ad1 1660 break;
yabahiro 33:dd598bb23ad1 1661 case 254:
yabahiro 33:dd598bb23ad1 1662 motor[TIRE_BL].pwm += 20;
yabahiro 33:dd598bb23ad1 1663 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1664 mtc2=false;
yabahiro 33:dd598bb23ad1 1665 break;
yabahiro 33:dd598bb23ad1 1666 case 1:
yabahiro 33:dd598bb23ad1 1667 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 1668 mtc2=false;
yabahiro 33:dd598bb23ad1 1669 break;
yabahiro 33:dd598bb23ad1 1670 case 3:
yabahiro 33:dd598bb23ad1 1671 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 1672 mtc2=false;
yabahiro 33:dd598bb23ad1 1673 break;
yabahiro 33:dd598bb23ad1 1674 case 2:
yabahiro 33:dd598bb23ad1 1675 motor[TIRE_FL].pwm += 20;
yabahiro 33:dd598bb23ad1 1676 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1677 mtc2=false;
yabahiro 33:dd598bb23ad1 1678 break;
yabahiro 33:dd598bb23ad1 1679 }
yabahiro 33:dd598bb23ad1 1680 }
yabahiro 33:dd598bb23ad1 1681 } else if(mode==21) {
yabahiro 33:dd598bb23ad1 1682 motor[TIRE_FR].pwm = 30;
yabahiro 33:dd598bb23ad1 1683 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1684 motor[TIRE_FL].pwm = 30;
yabahiro 33:dd598bb23ad1 1685 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1686 motor[TIRE_BR].pwm = 30;
yabahiro 33:dd598bb23ad1 1687 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1688 motor[TIRE_BL].pwm = 30;
yabahiro 33:dd598bb23ad1 1689 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1690 if(g[2]==98&&countss>=10000) {
yabahiro 33:dd598bb23ad1 1691 mode=22;
yabahiro 33:dd598bb23ad1 1692 countss=0;
yabahiro 33:dd598bb23ad1 1693 }
yabahiro 33:dd598bb23ad1 1694 }
yabahiro 33:dd598bb23ad1 1695 kiri=true;
yabahiro 33:dd598bb23ad1 1696 if(kiri) {
yabahiro 33:dd598bb23ad1 1697 if(mode==22) {
yabahiro 33:dd598bb23ad1 1698 motor[TIRE_FR].pwm = 100;
yabahiro 33:dd598bb23ad1 1699 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1700 motor[TIRE_FL].pwm = 100;
yabahiro 33:dd598bb23ad1 1701 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1702 motor[TIRE_BR].pwm = 100;
yabahiro 33:dd598bb23ad1 1703 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1704 motor[TIRE_BL].pwm = 100;
yabahiro 33:dd598bb23ad1 1705 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1706 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 1707 mode=23;
yabahiro 33:dd598bb23ad1 1708 }
yabahiro 33:dd598bb23ad1 1709 } else if(mode==23) {
yabahiro 33:dd598bb23ad1 1710 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 1711 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1712 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1713 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1714 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 1715 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1716 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 1717 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1718 if(g[2]==98) {
yabahiro 33:dd598bb23ad1 1719 mode=24;
yabahiro 33:dd598bb23ad1 1720 countss=0;
yabahiro 33:dd598bb23ad1 1721 }
yabahiro 33:dd598bb23ad1 1722 } else if(mode==24) {
yabahiro 33:dd598bb23ad1 1723 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 1724 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1725 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1726 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1727 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 1728 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1729 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 1730 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1731 if(g[2]!=98) {
yabahiro 33:dd598bb23ad1 1732 countss=0;
yabahiro 33:dd598bb23ad1 1733 mode=25;
yabahiro 33:dd598bb23ad1 1734 }
yabahiro 33:dd598bb23ad1 1735 } else if(mode==25) {
yabahiro 33:dd598bb23ad1 1736 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 1737 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1738 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 1739 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1740 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 1741 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1742 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 1743 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1744 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 1745 mode=26;
yabahiro 33:dd598bb23ad1 1746 countss=0;
yabahiro 33:dd598bb23ad1 1747 }
yabahiro 33:dd598bb23ad1 1748 } else if(mode==26) {
yabahiro 33:dd598bb23ad1 1749 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 1750 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1751 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1752 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1753 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 1754 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1755 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 1756 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 1757 if(countss>=5000) {
yabahiro 33:dd598bb23ad1 1758 mode=27;
yabahiro 33:dd598bb23ad1 1759 countss=0;
yabahiro 33:dd598bb23ad1 1760 }
yabahiro 33:dd598bb23ad1 1761 } else if(mode==27) {
yabahiro 33:dd598bb23ad1 1762 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 1763 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 1764 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 1765 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1766 motor[TIRE_BR].pwm = 20;
yabahiro 33:dd598bb23ad1 1767 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1768 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 1769 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1770 //どれくらい回転するかって値
yabahiro 33:dd598bb23ad1 1771 if(countss>=100) {
yabahiro 33:dd598bb23ad1 1772 countss=0;
yabahiro 33:dd598bb23ad1 1773 mode=30;
yabahiro 33:dd598bb23ad1 1774 }
yabahiro 33:dd598bb23ad1 1775 } else if(mode==24) {
yabahiro 33:dd598bb23ad1 1776 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 1777 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1778 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 1779 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1780 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 1781 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1782 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 1783 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1784 if(countss>=10000) {
yabahiro 33:dd598bb23ad1 1785 mode=30;
yabahiro 33:dd598bb23ad1 1786 countss=0;
yabahiro 33:dd598bb23ad1 1787 }
yabahiro 33:dd598bb23ad1 1788 }
yabahiro 33:dd598bb23ad1 1789 } else {
yabahiro 33:dd598bb23ad1 1790 if(mode==22) {
yabahiro 33:dd598bb23ad1 1791 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1792 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1793 motor[TIRE_FL].pwm = 25;
yabahiro 33:dd598bb23ad1 1794 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1795 motor[TIRE_BR].pwm = 25;
yabahiro 33:dd598bb23ad1 1796 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1797 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1798 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1799 if(g[0]==0&&countss>=10000) {
yabahiro 33:dd598bb23ad1 1800 mode=30;
yabahiro 33:dd598bb23ad1 1801 countss=0;
yabahiro 33:dd598bb23ad1 1802 cros=0;
yabahiro 33:dd598bb23ad1 1803 }
yabahiro 33:dd598bb23ad1 1804 }
yabahiro 33:dd598bb23ad1 1805 }
yabahiro 33:dd598bb23ad1 1806 if(mode==30) {
yabahiro 33:dd598bb23ad1 1807 motor[TIRE_FR].pwm = 59;
yabahiro 33:dd598bb23ad1 1808 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1809 motor[TIRE_FL].pwm = 62;
yabahiro 33:dd598bb23ad1 1810 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1811 motor[TIRE_BR].pwm = 59;
yabahiro 33:dd598bb23ad1 1812 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1813 motor[TIRE_BL].pwm = 62;
yabahiro 33:dd598bb23ad1 1814 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1815 if(g[0]==98) {
yabahiro 33:dd598bb23ad1 1816 mode=31;
yabahiro 33:dd598bb23ad1 1817 countss=0;
yabahiro 33:dd598bb23ad1 1818 }
yabahiro 33:dd598bb23ad1 1819 } else if(mode==31) {
yabahiro 33:dd598bb23ad1 1820 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1821 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1822 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1823 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1824 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1825 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1826 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1827 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1828 if(countss>=5000) {
yabahiro 33:dd598bb23ad1 1829 countss=0;
yabahiro 33:dd598bb23ad1 1830 mode=32;
yabahiro 33:dd598bb23ad1 1831 }
yabahiro 33:dd598bb23ad1 1832 } else if(mode==32) {
yabahiro 33:dd598bb23ad1 1833 motor[TIRE_FR].pwm = 10;
yabahiro 33:dd598bb23ad1 1834 motor[TIRE_FR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1835 motor[TIRE_FL].pwm = 10;
yabahiro 33:dd598bb23ad1 1836 motor[TIRE_FL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1837 motor[TIRE_BR].pwm = 10;
yabahiro 33:dd598bb23ad1 1838 motor[TIRE_BR].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1839 motor[TIRE_BL].pwm = 10;
yabahiro 33:dd598bb23ad1 1840 motor[TIRE_BL].dir = BRAKE;
yabahiro 33:dd598bb23ad1 1841 if(countss>=40000) {
yabahiro 33:dd598bb23ad1 1842 mode=33;
yabahiro 33:dd598bb23ad1 1843 countss=0;
yabahiro 33:dd598bb23ad1 1844 }
yabahiro 33:dd598bb23ad1 1845 } else if(mode==33) {
yabahiro 33:dd598bb23ad1 1846 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1847 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1848 motor[TIRE_FL].pwm = 30;
yabahiro 33:dd598bb23ad1 1849 motor[TIRE_FL].dir = FOR;
yabahiro 33:dd598bb23ad1 1850 motor[TIRE_BR].pwm = 30;
yabahiro 33:dd598bb23ad1 1851 motor[TIRE_BR].dir = BACK;
yabahiro 33:dd598bb23ad1 1852 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1853 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1854 if(g[3]==0&&countss>=20000) {
yabahiro 33:dd598bb23ad1 1855 countss=0;
yabahiro 33:dd598bb23ad1 1856 if(Twsh==2) {
yabahiro 33:dd598bb23ad1 1857 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 1858 }
yabahiro 33:dd598bb23ad1 1859 mode=40;
yabahiro 33:dd598bb23ad1 1860 }
yabahiro 33:dd598bb23ad1 1861 } else if(mode==40) {
yabahiro 33:dd598bb23ad1 1862 if(LIB) {
yabahiro 33:dd598bb23ad1 1863 mode=42;
yabahiro 33:dd598bb23ad1 1864 }
yabahiro 33:dd598bb23ad1 1865 if(counts==false&&g[0]==0) {
yabahiro 33:dd598bb23ad1 1866 cros++;
yabahiro 33:dd598bb23ad1 1867 counts=true;
yabahiro 33:dd598bb23ad1 1868 }
yabahiro 33:dd598bb23ad1 1869 if(counts==true&&g[0]==99) {
yabahiro 33:dd598bb23ad1 1870 counts=false;
yabahiro 33:dd598bb23ad1 1871 }
yabahiro 33:dd598bb23ad1 1872
yabahiro 33:dd598bb23ad1 1873 if(cros==1) {
yabahiro 33:dd598bb23ad1 1874 // mode=41;
yabahiro 33:dd598bb23ad1 1875 //countss=0;
yabahiro 33:dd598bb23ad1 1876 //cros=0;
yabahiro 33:dd598bb23ad1 1877 }//ゆっくりモードに入れなかった時の保険
yabahiro 33:dd598bb23ad1 1878
yabahiro 33:dd598bb23ad1 1879 switch(g[3]) {
yabahiro 33:dd598bb23ad1 1880 case 98:
yabahiro 33:dd598bb23ad1 1881 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1882 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1883 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1884 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1885 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1886 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1887 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1888 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1889 if(countss>20000) {
yabahiro 33:dd598bb23ad1 1890 cross++;
yabahiro 33:dd598bb23ad1 1891 countss=0;
yabahiro 33:dd598bb23ad1 1892 }
yabahiro 33:dd598bb23ad1 1893 mtc2=true;
yabahiro 33:dd598bb23ad1 1894 if(cross==2) {
yabahiro 33:dd598bb23ad1 1895 mode=41;
yabahiro 33:dd598bb23ad1 1896 countss=0;
yabahiro 33:dd598bb23ad1 1897 cross=0;
yabahiro 33:dd598bb23ad1 1898 }
yabahiro 33:dd598bb23ad1 1899 break;
yabahiro 33:dd598bb23ad1 1900 case 0:
yabahiro 33:dd598bb23ad1 1901 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1902 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1903 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1904 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1905 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1906 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1907 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1908 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1909 mtc2=true;
yabahiro 33:dd598bb23ad1 1910 break;
yabahiro 33:dd598bb23ad1 1911 case 255:
yabahiro 33:dd598bb23ad1 1912 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1913 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1914 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 1915 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1916 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 1917 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1918 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1919 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1920 mtc2=true;
yabahiro 33:dd598bb23ad1 1921 break;
yabahiro 33:dd598bb23ad1 1922 case 253:
yabahiro 33:dd598bb23ad1 1923 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1924 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1925 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1926 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1927 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1928 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1929 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1930 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1931 mtc2=true;
yabahiro 33:dd598bb23ad1 1932 break;
yabahiro 33:dd598bb23ad1 1933 case 254:
yabahiro 33:dd598bb23ad1 1934 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 1935 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 1936 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 1937 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1938 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 1939 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1940 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1941 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 1942 mtc2=true;
yabahiro 33:dd598bb23ad1 1943 break;
yabahiro 33:dd598bb23ad1 1944 case 1:
yabahiro 33:dd598bb23ad1 1945 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 1946 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1947 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 1948 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 1949 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 1950 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 1951 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 1952 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1953 mtc2=true;
yabahiro 33:dd598bb23ad1 1954 break;
yabahiro 33:dd598bb23ad1 1955 case 3:
yabahiro 33:dd598bb23ad1 1956 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 1957 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1958 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1959 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1960 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1961 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1962 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 1963 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1964 mtc2=true;
yabahiro 33:dd598bb23ad1 1965 break;
yabahiro 33:dd598bb23ad1 1966 case 2:
yabahiro 33:dd598bb23ad1 1967 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 1968 motor[TIRE_FR].dir = BACK;
yabahiro 33:dd598bb23ad1 1969 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 1970 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 1971 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 1972 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 1973 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 1974 motor[TIRE_BL].dir = FOR;
yabahiro 33:dd598bb23ad1 1975 mtc2=true;
yabahiro 33:dd598bb23ad1 1976 break;
yabahiro 33:dd598bb23ad1 1977 }
yabahiro 33:dd598bb23ad1 1978 if(g[2]!=98&&mtc2==true) {
yabahiro 33:dd598bb23ad1 1979 switch(g[2]) {
yabahiro 33:dd598bb23ad1 1980 case 0:
yabahiro 33:dd598bb23ad1 1981 mtc2=false;
yabahiro 33:dd598bb23ad1 1982 break;
yabahiro 33:dd598bb23ad1 1983 case 255:
yabahiro 33:dd598bb23ad1 1984 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 1985 mtc2=false;
yabahiro 33:dd598bb23ad1 1986 break;
yabahiro 33:dd598bb23ad1 1987 case 253:
yabahiro 33:dd598bb23ad1 1988 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 1989 mtc2=false;
yabahiro 33:dd598bb23ad1 1990 break;
yabahiro 33:dd598bb23ad1 1991 case 254:
yabahiro 33:dd598bb23ad1 1992 motor[TIRE_FL].pwm += 20;
yabahiro 33:dd598bb23ad1 1993 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 1994 mtc2=false;
yabahiro 33:dd598bb23ad1 1995 break;
yabahiro 33:dd598bb23ad1 1996 case 1:
yabahiro 33:dd598bb23ad1 1997 motor[TIRE_BL].pwm += 5;
yabahiro 33:dd598bb23ad1 1998 mtc2=false;
yabahiro 33:dd598bb23ad1 1999 break;
yabahiro 33:dd598bb23ad1 2000 case 3:
yabahiro 33:dd598bb23ad1 2001 motor[TIRE_BL].pwm += 10;
yabahiro 33:dd598bb23ad1 2002 mtc2=false;
yabahiro 33:dd598bb23ad1 2003 break;
yabahiro 33:dd598bb23ad1 2004 case 2:
yabahiro 33:dd598bb23ad1 2005 motor[TIRE_BL].pwm += 20;
yabahiro 33:dd598bb23ad1 2006 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2007 mtc2=false;
yabahiro 33:dd598bb23ad1 2008 break;
yabahiro 33:dd598bb23ad1 2009 }
yabahiro 33:dd598bb23ad1 2010 }
yabahiro 33:dd598bb23ad1 2011 } else if(mode==41) {
yabahiro 33:dd598bb23ad1 2012 if(LIF||LIB) {
yabahiro 33:dd598bb23ad1 2013 mode=42;
yabahiro 33:dd598bb23ad1 2014 }
yabahiro 33:dd598bb23ad1 2015 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2016 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 2017 motor[TIRE_FL].pwm = 30;
yabahiro 33:dd598bb23ad1 2018 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2019 motor[TIRE_BR].pwm = 30;
yabahiro 33:dd598bb23ad1 2020 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2021 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2022 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2023 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 2024 countss=0;
yabahiro 33:dd598bb23ad1 2025 mode=70;
yabahiro 33:dd598bb23ad1 2026 normal=60;
yabahiro 33:dd598bb23ad1 2027 fast=70;
yabahiro 33:dd598bb23ad1 2028 slow=40;
yabahiro 33:dd598bb23ad1 2029 }
yabahiro 33:dd598bb23ad1 2030 } else if(mode==42) {
yabahiro 33:dd598bb23ad1 2031 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 2032 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2033 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2034 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2035 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2036 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 2037 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 2038 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2039 if(g[1]==0) {
yabahiro 33:dd598bb23ad1 2040 countss=0;
yabahiro 33:dd598bb23ad1 2041 mode=70;
yabahiro 33:dd598bb23ad1 2042 }
yabahiro 33:dd598bb23ad1 2043 } else if(mode==70) {
yabahiro 33:dd598bb23ad1 2044 switch(g[1]) {
yabahiro 33:dd598bb23ad1 2045 case 99:
yabahiro 33:dd598bb23ad1 2046 if(countss>=100000) {
yabahiro 33:dd598bb23ad1 2047 mode=100;
yabahiro 33:dd598bb23ad1 2048 }
yabahiro 33:dd598bb23ad1 2049 break;
yabahiro 33:dd598bb23ad1 2050 case 98:
yabahiro 33:dd598bb23ad1 2051 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 2052 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2053 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 2054 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2055 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 2056 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2057 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 2058 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2059 break;
yabahiro 33:dd598bb23ad1 2060 case 0:
yabahiro 33:dd598bb23ad1 2061 motor[TIRE_FR].pwm = 80;
yabahiro 33:dd598bb23ad1 2062 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2063 motor[TIRE_FL].pwm = 80;
yabahiro 33:dd598bb23ad1 2064 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2065 motor[TIRE_BR].pwm = 80;
yabahiro 33:dd598bb23ad1 2066 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2067 motor[TIRE_BL].pwm = 80;
yabahiro 33:dd598bb23ad1 2068 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2069 mtc=true;
yabahiro 33:dd598bb23ad1 2070 break;
yabahiro 33:dd598bb23ad1 2071 case 255:
yabahiro 33:dd598bb23ad1 2072 motor[TIRE_FR].pwm = fast;
yabahiro 33:dd598bb23ad1 2073 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2074 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 2075 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2076 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 2077 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2078 motor[TIRE_BL].pwm = fast;
yabahiro 33:dd598bb23ad1 2079 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2080 mtc=true;
yabahiro 33:dd598bb23ad1 2081 break;
yabahiro 33:dd598bb23ad1 2082 case 253:
yabahiro 33:dd598bb23ad1 2083 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 2084 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2085 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2086 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2087 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2088 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 2089 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 2090 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2091 mtc=true;
yabahiro 33:dd598bb23ad1 2092 break;
yabahiro 33:dd598bb23ad1 2093 case 254:
yabahiro 33:dd598bb23ad1 2094 motor[TIRE_FR].pwm = normal;
yabahiro 33:dd598bb23ad1 2095 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2096 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2097 motor[TIRE_FL].dir = FREE;
yabahiro 33:dd598bb23ad1 2098 motor[TIRE_BR].pwm = 0;
yabahiro 33:dd598bb23ad1 2099 motor[TIRE_BR].dir = FREE;
yabahiro 33:dd598bb23ad1 2100 motor[TIRE_BL].pwm = normal;
yabahiro 33:dd598bb23ad1 2101 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2102 mtc=true;
yabahiro 33:dd598bb23ad1 2103 break;
yabahiro 33:dd598bb23ad1 2104 case 1:
yabahiro 33:dd598bb23ad1 2105 motor[TIRE_FR].pwm = slow;
yabahiro 33:dd598bb23ad1 2106 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2107 motor[TIRE_FL].pwm = fast;
yabahiro 33:dd598bb23ad1 2108 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2109 motor[TIRE_BR].pwm = fast;
yabahiro 33:dd598bb23ad1 2110 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2111 motor[TIRE_BL].pwm = slow;
yabahiro 33:dd598bb23ad1 2112 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2113 mtc=true;
yabahiro 33:dd598bb23ad1 2114 break;
yabahiro 33:dd598bb23ad1 2115 case 3:
yabahiro 33:dd598bb23ad1 2116 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2117 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 2118 motor[TIRE_FL].pwm = slow;
yabahiro 33:dd598bb23ad1 2119 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2120 motor[TIRE_BR].pwm = slow;
yabahiro 33:dd598bb23ad1 2121 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2122 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2123 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2124 mtc=true;
yabahiro 33:dd598bb23ad1 2125 break;
yabahiro 33:dd598bb23ad1 2126 case 2:
yabahiro 33:dd598bb23ad1 2127 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2128 motor[TIRE_FR].dir = FREE;
yabahiro 33:dd598bb23ad1 2129 motor[TIRE_FL].pwm = normal;
yabahiro 33:dd598bb23ad1 2130 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2131 motor[TIRE_BR].pwm = normal;
yabahiro 33:dd598bb23ad1 2132 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2133 motor[TIRE_BL].pwm = 0;
yabahiro 33:dd598bb23ad1 2134 motor[TIRE_BL].dir = FREE;
yabahiro 33:dd598bb23ad1 2135 mtc=true;
yabahiro 33:dd598bb23ad1 2136 break;
yabahiro 33:dd598bb23ad1 2137 }
yabahiro 33:dd598bb23ad1 2138 if(g[1]!=98&&mtc==true) {
yabahiro 33:dd598bb23ad1 2139 switch(g[0]) {
yabahiro 33:dd598bb23ad1 2140 case 0:
yabahiro 33:dd598bb23ad1 2141 mtc=false;
yabahiro 33:dd598bb23ad1 2142 break;
yabahiro 33:dd598bb23ad1 2143 case 255:
yabahiro 33:dd598bb23ad1 2144 motor[TIRE_FL].pwm += 5;
yabahiro 33:dd598bb23ad1 2145 mtc=false;
yabahiro 33:dd598bb23ad1 2146 break;
yabahiro 33:dd598bb23ad1 2147 case 253:
yabahiro 33:dd598bb23ad1 2148 motor[TIRE_FL].pwm += 10;
yabahiro 33:dd598bb23ad1 2149 mtc=false;
yabahiro 33:dd598bb23ad1 2150 break;
yabahiro 33:dd598bb23ad1 2151 case 254:
yabahiro 33:dd598bb23ad1 2152 motor[TIRE_FL].pwm += 20;
yabahiro 33:dd598bb23ad1 2153 motor[TIRE_FR].pwm = 0;
yabahiro 33:dd598bb23ad1 2154 mtc=false;
yabahiro 33:dd598bb23ad1 2155 break;
yabahiro 33:dd598bb23ad1 2156 case 1:
yabahiro 33:dd598bb23ad1 2157 motor[TIRE_FR].pwm += 5;
yabahiro 33:dd598bb23ad1 2158 mtc=false;
yabahiro 33:dd598bb23ad1 2159 break;
yabahiro 33:dd598bb23ad1 2160 case 3:
yabahiro 33:dd598bb23ad1 2161 motor[TIRE_FR].pwm += 10;
yabahiro 33:dd598bb23ad1 2162 mtc=false;
yabahiro 33:dd598bb23ad1 2163 break;
yabahiro 33:dd598bb23ad1 2164 case 2:
yabahiro 33:dd598bb23ad1 2165 motor[TIRE_FR].pwm += 20;
yabahiro 33:dd598bb23ad1 2166 motor[TIRE_FL].pwm = 0;
yabahiro 33:dd598bb23ad1 2167 mtc=false;
yabahiro 33:dd598bb23ad1 2168 break;
yabahiro 33:dd598bb23ad1 2169 }
yabahiro 33:dd598bb23ad1 2170 }
yabahiro 33:dd598bb23ad1 2171 } else if(mode==100) {
yabahiro 33:dd598bb23ad1 2172 motor[TIRE_FR].pwm = 20;
yabahiro 33:dd598bb23ad1 2173 motor[TIRE_FR].dir = FOR;
yabahiro 33:dd598bb23ad1 2174 motor[TIRE_FL].pwm = 20;
yabahiro 33:dd598bb23ad1 2175 motor[TIRE_FL].dir = BACK;
yabahiro 33:dd598bb23ad1 2176 motor[TIRE_BR].pwm = 23;
yabahiro 33:dd598bb23ad1 2177 motor[TIRE_BR].dir = FOR;
yabahiro 33:dd598bb23ad1 2178 motor[TIRE_BL].pwm = 20;
yabahiro 33:dd598bb23ad1 2179 motor[TIRE_BL].dir = BACK;
yabahiro 33:dd598bb23ad1 2180 } else {
yabahiro 33:dd598bb23ad1 2181 }
yabahiro 33:dd598bb23ad1 2182
kishibekairohan 13:b6e02d6261d7 2183 }
kishibekairohan 13:b6e02d6261d7 2184 #endif
kishibekairohan 13:b6e02d6261d7 2185
kishibekairohan 13:b6e02d6261d7 2186 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 2187 static void Process4()
M_souta 22:7d93f79a3686 2188 {
yabahiro 33:dd598bb23ad1 2189
kishibekairohan 13:b6e02d6261d7 2190 }
kishibekairohan 13:b6e02d6261d7 2191 #endif
kishibekairohan 13:b6e02d6261d7 2192
kishibekairohan 13:b6e02d6261d7 2193 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 2194 static void Process5()
Ryosei 27:dd9f27fce7d1 2195 {
Ryosei 27:dd9f27fce7d1 2196
kishibekairohan 13:b6e02d6261d7 2197 }
kishibekairohan 13:b6e02d6261d7 2198 #endif
kishibekairohan 13:b6e02d6261d7 2199
kishibekairohan 13:b6e02d6261d7 2200 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 2201 static void Process6()
kishibekairohan 13:b6e02d6261d7 2202 {
Ryosei 27:dd9f27fce7d1 2203
kishibekairohan 13:b6e02d6261d7 2204 }
kishibekairohan 13:b6e02d6261d7 2205 #endif
kishibekairohan 13:b6e02d6261d7 2206
kishibekairohan 13:b6e02d6261d7 2207 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 2208 static void Process7()
kishibekairohan 13:b6e02d6261d7 2209 {
yabahiro 33:dd598bb23ad1 2210
kishibekairohan 12:c09b3e08a316 2211 }
kishibekairohan 12:c09b3e08a316 2212 #endif
kishibekairohan 12:c09b3e08a316 2213
Ryosei 27:dd9f27fce7d1 2214 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 2215 static void Process8()
kishibekairohan 12:c09b3e08a316 2216 {
yabahiro 33:dd598bb23ad1 2217
kishibekairohan 12:c09b3e08a316 2218 }
kishibekairohan 12:c09b3e08a316 2219 #endif
kishibekairohan 12:c09b3e08a316 2220
kishibekairohan 13:b6e02d6261d7 2221 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 2222 static void Process9()
kishibekairohan 12:c09b3e08a316 2223 {
Ryosei 27:dd9f27fce7d1 2224
t_yamamoto 0:669ef71cba68 2225 }
t_yamamoto 0:669ef71cba68 2226 #endif
t_yamamoto 0:669ef71cba68 2227 #endif
t_yamamoto 0:669ef71cba68 2228 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 2229
t_yamamoto 0:669ef71cba68 2230 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 2231 {
yabahiro 33:dd598bb23ad1 2232 #ifdef USE_SOLENOID
yabahiro 33:dd598bb23ad1 2233 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 33:dd598bb23ad1 2234 #endif
yabahiro 33:dd598bb23ad1 2235
yabahiro 33:dd598bb23ad1 2236 #ifdef USE_MOTOR
yabahiro 33:dd598bb23ad1 2237 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 33:dd598bb23ad1 2238 motor[i].dir = FREE;
yabahiro 33:dd598bb23ad1 2239 motor[i].pwm = 0;
yabahiro 33:dd598bb23ad1 2240 }
yabahiro 33:dd598bb23ad1 2241 #endif
t_yamamoto 0:669ef71cba68 2242 }
t_yamamoto 0:669ef71cba68 2243
kishibekairohan 16:3f2c2d89372b 2244 #pragma region USER-DEFINED-FUNCTIONS
M_souta 29:44d5454ce8fa 2245
M_souta 29:44d5454ce8fa 2246 uint8_t SetStatus(int pwmVal)
Ryosei 27:dd9f27fce7d1 2247 {
M_souta 29:44d5454ce8fa 2248 if (pwmVal < 0) return BACK;
M_souta 29:44d5454ce8fa 2249 else if (pwmVal > 0) return FOR;
M_souta 29:44d5454ce8fa 2250 else if (pwmVal == 0) return BRAKE;
M_souta 29:44d5454ce8fa 2251 else return BRAKE;
M_souta 22:7d93f79a3686 2252 }
kishibekairohan 16:3f2c2d89372b 2253
M_souta 29:44d5454ce8fa 2254 uint8_t SetPWM(int pwmVal)
Ryosei 27:dd9f27fce7d1 2255 {
M_souta 29:44d5454ce8fa 2256 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 29:44d5454ce8fa 2257 else return abs(pwmVal);
M_souta 22:7d93f79a3686 2258 }
kishibekairohan 16:3f2c2d89372b 2259
t_yamamoto 0:669ef71cba68 2260 #pragma endregion