大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@33:dd598bb23ad1, 2019-10-28 (annotated)

Committer:
yabahiro
Date:
Mon Oct 28 21:11:17 2019 +0000
Revision:
33:dd598bb23ad1
Parent:
32:985ec7913f2a
Child:
34:2fce61e3ebc4
aa;

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