大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@34:2fce61e3ebc4, 2019-10-29 (annotated)

Committer:
kishibekairohan
Date:
Tue Oct 29 08:35:05 2019 +0000
Revision:
34:2fce61e3ebc4
Parent:
33:dd598bb23ad1
Child:
35:33c6cdba5630
kakakakkaka

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