大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_1123

aa

Dependencies:   mbed

System/Process/Process.cpp@37:e77816810627, 2019-10-30 (annotated)

Committer:
kishibekairohan
Date:
Wed Oct 30 07:32:56 2019 +0000
Revision:
37:e77816810627
Parent:
36:418ee5567687
Child:
38:40e7fdf3c694
h

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