大季 矢花 / Mbed 2 deprecated 2019_Master_NEWMODE

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@42:4f9689c0a349, 2019-09-30 (annotated)

Committer:
yabahiro
Date:
Mon Sep 30 11:33:22 2019 +0000
Revision:
42:4f9689c0a349
Parent:
41:e1dd6d97f17d
Child:
43:677f7446612e
qq;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
yabahiro 21:1f1e9c585da8 8 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 11 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
kishibekairohan 16:3f2c2d89372b 13 #include "../../Input/Encoder/Encoder.h"
t_yamamoto 0:669ef71cba68 14 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 15 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 16 #include "../Using.h"
t_yamamoto 0:669ef71cba68 17
t_yamamoto 0:669ef71cba68 18 using namespace SWITCH;
7ka884 4:ba9df71868df 19 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 20 using namespace ENCODER;
yabahiro 21:1f1e9c585da8 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
t_yamamoto 0:669ef71cba68 23 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 24 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 26
t_yamamoto 0:669ef71cba68 27 static bool lock;
t_yamamoto 0:669ef71cba68 28 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 29 static int current;
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 34 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 35 #endif
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 40
kishibekairohan 2:c015739085d3 41 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 42
kishibekairohan 16:3f2c2d89372b 43 //**************Encoder***************
kishibekairohan 16:3f2c2d89372b 44 const int PerRev = 256;
kishibekairohan 16:3f2c2d89372b 45 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 46 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 47 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 48 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
kishibekairohan 16:3f2c2d89372b 49 //**************Encoder***************
kishibekairohan 7:e88c5d47a3be 50
kishibekairohan 16:3f2c2d89372b 51 //**************Buzzer****************
kishibekairohan 11:028a150943b5 52 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 53 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 54 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 55 bool EMGflag = false;
yabahiro 21:1f1e9c585da8 56 //PWMOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 57 //**************Buzzer****************
kishibekairohan 11:028a150943b5 58
kishibekairohan 11:028a150943b5 59 //************TapeLed*****************
kishibekairohan 11:028a150943b5 60 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 61 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 62 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 63 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 64 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 65 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 66
M_souta 18:c694bae76e51 67 float tireProRPM[4];
M_souta 18:c694bae76e51 68 float tireTarRPM[4];
yabahiro 21:1f1e9c585da8 69 float tirepwm[4];
yabahiro 21:1f1e9c585da8 70
yabahiro 21:1f1e9c585da8 71 const int omni[15][15] = {
yabahiro 21:1f1e9c585da8 72 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 73 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 74 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
yabahiro 21:1f1e9c585da8 75 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
yabahiro 21:1f1e9c585da8 76 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
yabahiro 21:1f1e9c585da8 77 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
yabahiro 21:1f1e9c585da8 78 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
yabahiro 21:1f1e9c585da8 79 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
yabahiro 21:1f1e9c585da8 80 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
yabahiro 21:1f1e9c585da8 81 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
yabahiro 21:1f1e9c585da8 82 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
yabahiro 21:1f1e9c585da8 83 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
yabahiro 21:1f1e9c585da8 84 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
yabahiro 21:1f1e9c585da8 85 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
yabahiro 21:1f1e9c585da8 86 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
yabahiro 21:1f1e9c585da8 87 };
yabahiro 21:1f1e9c585da8 88
yabahiro 21:1f1e9c585da8 89 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
yabahiro 21:1f1e9c585da8 90 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
yabahiro 21:1f1e9c585da8 91
yabahiro 21:1f1e9c585da8 92 uint8_t SetStatus(int);
yabahiro 21:1f1e9c585da8 93 uint8_t SetStatus(int pwmVal)
yabahiro 21:1f1e9c585da8 94 {
yabahiro 21:1f1e9c585da8 95 if (pwmVal < 0) return BACK;
yabahiro 21:1f1e9c585da8 96 else if (pwmVal > 0) return FOR;
yabahiro 21:1f1e9c585da8 97 else if (pwmVal == 0) return BRAKE;
yabahiro 21:1f1e9c585da8 98 else return BRAKE;
yabahiro 21:1f1e9c585da8 99 }
yabahiro 21:1f1e9c585da8 100 uint8_t Setpwm(int);
yabahiro 21:1f1e9c585da8 101 uint8_t Setpwm(int pwmVal)
yabahiro 21:1f1e9c585da8 102 {
yabahiro 21:1f1e9c585da8 103 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
yabahiro 21:1f1e9c585da8 104 else return abs(pwmVal);
yabahiro 21:1f1e9c585da8 105 }
M_souta 18:c694bae76e51 106
yabahiro 42:4f9689c0a349 107 int selectnum[16][4]= {
yabahiro 42:4f9689c0a349 108 //1,2,4,8
yabahiro 42:4f9689c0a349 109 {0,0,0,0},
yabahiro 42:4f9689c0a349 110 {1,0,0,0},
yabahiro 42:4f9689c0a349 111 {0,1,0,0},
yabahiro 42:4f9689c0a349 112 {1,1,0,0},
yabahiro 42:4f9689c0a349 113 {0,0,1,0},
yabahiro 42:4f9689c0a349 114 {1,0,1,0},
yabahiro 42:4f9689c0a349 115 {0,1,1,0},
yabahiro 42:4f9689c0a349 116 {1,1,1,0},
yabahiro 42:4f9689c0a349 117 {0,0,0,1},
yabahiro 42:4f9689c0a349 118 {1,0,0,1},
yabahiro 42:4f9689c0a349 119 {0,1,0,1},
yabahiro 42:4f9689c0a349 120 {1,1,0,1},
yabahiro 42:4f9689c0a349 121 {0,0,1,1},
yabahiro 42:4f9689c0a349 122 {1,0,1,1},
yabahiro 42:4f9689c0a349 123 {0,1,1,1},
yabahiro 42:4f9689c0a349 124 {1,1,1,1}
yabahiro 42:4f9689c0a349 125 };
yabahiro 42:4f9689c0a349 126
yabahiro 42:4f9689c0a349 127 void LedMode(int num)
yabahiro 35:94f026ab4d30 128 {
yabahiro 42:4f9689c0a349 129 POTENTIOMETER::dio[0]= selectnum[num][0];
yabahiro 42:4f9689c0a349 130 POTENTIOMETER::dio[1]= selectnum[num][1];;
yabahiro 42:4f9689c0a349 131 POTENTIOMETER::dio[2]= selectnum[num][2];;
yabahiro 42:4f9689c0a349 132 POTENTIOMETER::dio[3]= selectnum[num][3];;
yabahiro 35:94f026ab4d30 133
yabahiro 35:94f026ab4d30 134 }
yabahiro 31:45f97698857b 135 int Twsh;
yabahiro 31:45f97698857b 136 bool UP_flag = false;
yabahiro 35:94f026ab4d30 137 bool SW_flag = false;
yabahiro 31:45f97698857b 138 bool Air_flag = false;
yabahiro 35:94f026ab4d30 139 bool zyouge;
yabahiro 39:f89166d7411c 140 bool zyougedo;
yabahiro 31:45f97698857b 141 bool dz1=true;
yabahiro 31:45f97698857b 142 bool dz1i=false;
yabahiro 31:45f97698857b 143 bool dz2=true;
yabahiro 31:45f97698857b 144 bool dz2i=false;
yabahiro 31:45f97698857b 145 bool dz3=true;
yabahiro 31:45f97698857b 146 bool dz3i=false;
yabahiro 31:45f97698857b 147 bool dz4=true;
yabahiro 31:45f97698857b 148 bool dz4i=false;
yabahiro 31:45f97698857b 149 bool zone;
yabahiro 31:45f97698857b 150 bool through=false;
yabahiro 35:94f026ab4d30 151 bool counts=false;
yabahiro 36:788274604650 152 bool mtc=false;
yabahiro 36:788274604650 153 bool mtc2 = false;
yabahiro 31:45f97698857b 154
yabahiro 41:e1dd6d97f17d 155 bool start_flag = true;
yabahiro 41:e1dd6d97f17d 156
yabahiro 42:4f9689c0a349 157 int mode =0;
yabahiro 31:45f97698857b 158 int cross=0;//十字数える用
yabahiro 35:94f026ab4d30 159 int cros=0;
yabahiro 31:45f97698857b 160 int count=100000;//wait代替え
kishibekairohan 33:656aa14904a5 161
kishibekairohan 33:656aa14904a5 162 ///*********PWM調整用*********///
yabahiro 40:7f93984e8ba1 163 int fast =60;
yabahiro 40:7f93984e8ba1 164 int normal = 40;
yabahiro 40:7f93984e8ba1 165 int slow = 20;
yabahiro 31:45f97698857b 166
t_yamamoto 0:669ef71cba68 167 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 168
t_yamamoto 0:669ef71cba68 169 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 170 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 171 static void Process0(void);
t_yamamoto 0:669ef71cba68 172 #endif
t_yamamoto 0:669ef71cba68 173 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 174 static void Process1(void);
t_yamamoto 0:669ef71cba68 175 #endif
t_yamamoto 0:669ef71cba68 176 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 177 static void Process2(void);
t_yamamoto 0:669ef71cba68 178 #endif
t_yamamoto 0:669ef71cba68 179 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 180 static void Process3(void);
t_yamamoto 0:669ef71cba68 181 #endif
t_yamamoto 0:669ef71cba68 182 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 183 static void Process4(void);
t_yamamoto 0:669ef71cba68 184 #endif
t_yamamoto 0:669ef71cba68 185 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 186 static void Process5(void);
t_yamamoto 0:669ef71cba68 187 #endif
t_yamamoto 0:669ef71cba68 188 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 189 static void Process6(void);
t_yamamoto 0:669ef71cba68 190 #endif
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 192 static void Process7(void);
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 195 static void Process8(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 198 static void Process9(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #endif
t_yamamoto 0:669ef71cba68 201
t_yamamoto 0:669ef71cba68 202 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 203 {
yabahiro 21:1f1e9c585da8 204 #pragma region USER-DEFINED_VARIABLE_INIT
yabahiro 21:1f1e9c585da8 205 /*Replace here with the initialization code of your variables.*/
yabahiro 21:1f1e9c585da8 206 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 207
yabahiro 21:1f1e9c585da8 208 lock = true;
yabahiro 21:1f1e9c585da8 209 processChangeComp = true;
yabahiro 21:1f1e9c585da8 210 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 211
yabahiro 21:1f1e9c585da8 212 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 213 #if USE_PROCESS_NUM>0
yabahiro 21:1f1e9c585da8 214 Process[0] = Process0;
yabahiro 21:1f1e9c585da8 215 #endif
yabahiro 21:1f1e9c585da8 216 #if USE_PROCESS_NUM>1
yabahiro 21:1f1e9c585da8 217 Process[1] = Process1;
yabahiro 21:1f1e9c585da8 218 #endif
yabahiro 21:1f1e9c585da8 219 #if USE_PROCESS_NUM>2
yabahiro 21:1f1e9c585da8 220 Process[2] = Process2;
yabahiro 21:1f1e9c585da8 221 #endif
yabahiro 21:1f1e9c585da8 222 #if USE_PROCESS_NUM>3
yabahiro 21:1f1e9c585da8 223 Process[3] = Process3;
yabahiro 21:1f1e9c585da8 224 #endif
yabahiro 21:1f1e9c585da8 225 #if USE_PROCESS_NUM>4
yabahiro 21:1f1e9c585da8 226 Process[4] = Process4;
yabahiro 21:1f1e9c585da8 227 #endif
yabahiro 21:1f1e9c585da8 228 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 229 Process[5] = Process5;
yabahiro 21:1f1e9c585da8 230 #endif
yabahiro 21:1f1e9c585da8 231 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 232 Process[6] = Process6;
yabahiro 21:1f1e9c585da8 233 #endif
yabahiro 21:1f1e9c585da8 234 #if USE_PROCESS_NUM>7
yabahiro 21:1f1e9c585da8 235 Process[7] = Process7;
yabahiro 21:1f1e9c585da8 236 #endif
yabahiro 21:1f1e9c585da8 237 #if USE_PROCESS_NUM>8
yabahiro 21:1f1e9c585da8 238 Process[8] = Process8;
yabahiro 21:1f1e9c585da8 239 #endif
yabahiro 21:1f1e9c585da8 240 #if USE_PROCESS_NUM>9
yabahiro 21:1f1e9c585da8 241 Process[9] = Process9;
yabahiro 21:1f1e9c585da8 242 #endif
yabahiro 21:1f1e9c585da8 243 #endif
t_yamamoto 0:669ef71cba68 244 }
t_yamamoto 0:669ef71cba68 245
t_yamamoto 0:669ef71cba68 246 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 247 {
yabahiro 21:1f1e9c585da8 248 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 249 if(controller->Button.HOME) lock = false;
yabahiro 21:1f1e9c585da8 250
yabahiro 21:1f1e9c585da8 251 if(controller->Button.START && processChangeComp) {
yabahiro 21:1f1e9c585da8 252 current++;
yabahiro 21:1f1e9c585da8 253 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
yabahiro 21:1f1e9c585da8 254 processChangeComp = false;
yabahiro 21:1f1e9c585da8 255 } else if(controller->Button.SELECT && processChangeComp) {
yabahiro 21:1f1e9c585da8 256 current--;
yabahiro 21:1f1e9c585da8 257 if (current < 0) current = 0;
yabahiro 21:1f1e9c585da8 258 processChangeComp = false;
yabahiro 21:1f1e9c585da8 259 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
yabahiro 21:1f1e9c585da8 260 #endif
yabahiro 21:1f1e9c585da8 261
yabahiro 21:1f1e9c585da8 262 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 263 ACTUATORHUB::MOTOR::Motor::Update(motor);
yabahiro 21:1f1e9c585da8 264 #endif
yabahiro 21:1f1e9c585da8 265
yabahiro 21:1f1e9c585da8 266 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 267 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
yabahiro 21:1f1e9c585da8 268 #endif
yabahiro 21:1f1e9c585da8 269
yabahiro 21:1f1e9c585da8 270 #ifdef USE_RS485
yabahiro 21:1f1e9c585da8 271 ACTUATORHUB::ActuatorHub::Update();
yabahiro 21:1f1e9c585da8 272 #endif
yabahiro 21:1f1e9c585da8 273
yabahiro 21:1f1e9c585da8 274 }
yabahiro 21:1f1e9c585da8 275
yabahiro 21:1f1e9c585da8 276 int g[8];
yabahiro 21:1f1e9c585da8 277
yabahiro 21:1f1e9c585da8 278 void SystemProcess()
yabahiro 21:1f1e9c585da8 279 {
yabahiro 21:1f1e9c585da8 280 SystemProcessInitialize();
yabahiro 21:1f1e9c585da8 281
yabahiro 21:1f1e9c585da8 282 while(1) {
yabahiro 42:4f9689c0a349 283 if(mode==0) {
yabahiro 42:4f9689c0a349 284 if(ZoneSW) {
yabahiro 42:4f9689c0a349 285 LedMode(1);
yabahiro 42:4f9689c0a349 286 } else {
yabahiro 42:4f9689c0a349 287 LedMode(2);
yabahiro 42:4f9689c0a349 288 }
yabahiro 42:4f9689c0a349 289 }
yabahiro 21:1f1e9c585da8 290 for(int i = 0; i < 8; i++) {
yabahiro 21:1f1e9c585da8 291 g[i] = LineHub::GetPara(i);
yabahiro 21:1f1e9c585da8 292 }
yabahiro 41:e1dd6d97f17d 293 if(ThSW) {
yabahiro 41:e1dd6d97f17d 294 Twsh=1;
yabahiro 41:e1dd6d97f17d 295 } else {
yabahiro 41:e1dd6d97f17d 296 Twsh=2;
yabahiro 41:e1dd6d97f17d 297 }
yabahiro 42:4f9689c0a349 298
yabahiro 41:e1dd6d97f17d 299 if(StertSW && start_flag) {
yabahiro 42:4f9689c0a349 300 start_flag = false;
yabahiro 42:4f9689c0a349 301 lock = false;
yabahiro 42:4f9689c0a349 302 if(ZoneSW) {
yabahiro 42:4f9689c0a349 303 mode=1;
yabahiro 42:4f9689c0a349 304 current = 2;
yabahiro 42:4f9689c0a349 305 } else {
yabahiro 42:4f9689c0a349 306 mode=1;
yabahiro 42:4f9689c0a349 307 current = 5;
yabahiro 41:e1dd6d97f17d 308 }
yabahiro 42:4f9689c0a349 309 }
yabahiro 41:e1dd6d97f17d 310 if(DWSW||DOSW) {
yabahiro 41:e1dd6d97f17d 311 if(DWSW) {
yabahiro 41:e1dd6d97f17d 312 zyouge=true;
yabahiro 41:e1dd6d97f17d 313 }
yabahiro 41:e1dd6d97f17d 314 if(DOSW) {
yabahiro 41:e1dd6d97f17d 315 if(DOLS) {
yabahiro 41:e1dd6d97f17d 316 motor[MOTOR_0].pwm = 100;
yabahiro 41:e1dd6d97f17d 317 motor[MOTOR_0].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 318 } else {
yabahiro 41:e1dd6d97f17d 319 motor[MOTOR_0].pwm = 50;
yabahiro 41:e1dd6d97f17d 320 motor[MOTOR_0].dir = BACK;
yabahiro 41:e1dd6d97f17d 321 }
yabahiro 41:e1dd6d97f17d 322 }
yabahiro 42:4f9689c0a349 323 } else if(zyouge==false) {
yabahiro 41:e1dd6d97f17d 324 motor[MOTOR_0].pwm = 100;
yabahiro 41:e1dd6d97f17d 325 motor[MOTOR_0].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 326 }
yabahiro 41:e1dd6d97f17d 327 if(zyouge) {
yabahiro 41:e1dd6d97f17d 328 if(Twsh==1) {
yabahiro 41:e1dd6d97f17d 329 motor[MOTOR_0].pwm = 220;
yabahiro 41:e1dd6d97f17d 330 motor[MOTOR_0].dir = FOR;
yabahiro 41:e1dd6d97f17d 331 if(UPLS) {
yabahiro 41:e1dd6d97f17d 332 motor[MOTOR_0].pwm = 100;
yabahiro 41:e1dd6d97f17d 333 motor[MOTOR_0].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 334 zyouge=false;
yabahiro 41:e1dd6d97f17d 335 }
yabahiro 41:e1dd6d97f17d 336 }
yabahiro 41:e1dd6d97f17d 337 if(Twsh==2) {
yabahiro 41:e1dd6d97f17d 338 motor[MOTOR_0].pwm = 230;
yabahiro 41:e1dd6d97f17d 339 motor[MOTOR_0].dir = FOR;
yabahiro 41:e1dd6d97f17d 340 if(UPLS2) {
yabahiro 41:e1dd6d97f17d 341 motor[MOTOR_0].pwm = 100;
yabahiro 41:e1dd6d97f17d 342 motor[MOTOR_0].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 343 zyouge=false;
yabahiro 41:e1dd6d97f17d 344 }
yabahiro 41:e1dd6d97f17d 345 }
yabahiro 41:e1dd6d97f17d 346 }
yabahiro 41:e1dd6d97f17d 347 if(AIRSW) {
yabahiro 41:e1dd6d97f17d 348 if(SW_flag==false) {
yabahiro 41:e1dd6d97f17d 349 if(Air_flag==false) {
yabahiro 41:e1dd6d97f17d 350 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 41:e1dd6d97f17d 351 Air_flag=true;
yabahiro 41:e1dd6d97f17d 352 SW_flag=true;
yabahiro 41:e1dd6d97f17d 353 } else {
yabahiro 41:e1dd6d97f17d 354 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 41:e1dd6d97f17d 355 Air_flag=false;
yabahiro 41:e1dd6d97f17d 356 SW_flag=true;
yabahiro 41:e1dd6d97f17d 357 }
yabahiro 41:e1dd6d97f17d 358 }
yabahiro 41:e1dd6d97f17d 359 } else {
yabahiro 42:4f9689c0a349 360 if( SW_flag==true) {
yabahiro 42:4f9689c0a349 361 count++;
yabahiro 42:4f9689c0a349 362 if(count>=5000) {
yabahiro 42:4f9689c0a349 363 SW_flag=false;
yabahiro 42:4f9689c0a349 364 count=0;
yabahiro 42:4f9689c0a349 365 }
yabahiro 42:4f9689c0a349 366 }
yabahiro 41:e1dd6d97f17d 367 }
yabahiro 35:94f026ab4d30 368
yabahiro 36:788274604650 369 //printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
yabahiro 35:94f026ab4d30 370
yabahiro 21:1f1e9c585da8 371 #ifdef USE_MU
yabahiro 21:1f1e9c585da8 372 controller = CONTROLLER::Controller::GetData();
yabahiro 21:1f1e9c585da8 373 #endif
yabahiro 21:1f1e9c585da8 374
yabahiro 21:1f1e9c585da8 375 #ifdef USE_ERRORCHECK
yabahiro 41:e1dd6d97f17d 376 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { //& start_flag
yabahiro 21:1f1e9c585da8 377 CONTROLLER::Controller::DataReset();
yabahiro 21:1f1e9c585da8 378 AllActuatorReset();
yabahiro 21:1f1e9c585da8 379 lock = true;
yabahiro 21:1f1e9c585da8 380 } else
yabahiro 21:1f1e9c585da8 381 #endif
yabahiro 21:1f1e9c585da8 382 {
yabahiro 21:1f1e9c585da8 383
yabahiro 21:1f1e9c585da8 384 #ifdef USE_SUBPROCESS
yabahiro 21:1f1e9c585da8 385 if(!lock) {
yabahiro 21:1f1e9c585da8 386 Process[current]();
yabahiro 21:1f1e9c585da8 387 } else
yabahiro 21:1f1e9c585da8 388 #endif
yabahiro 21:1f1e9c585da8 389 {
yabahiro 21:1f1e9c585da8 390 //ロック時の処理
yabahiro 21:1f1e9c585da8 391 }
yabahiro 21:1f1e9c585da8 392 }
yabahiro 21:1f1e9c585da8 393 /*
yabahiro 21:1f1e9c585da8 394 //Emergency!
yabahiro 21:1f1e9c585da8 395 if(!EMG_0 && !EMG_1 && !EMGflag) {
yabahiro 21:1f1e9c585da8 396 buzzer = 0;
yabahiro 21:1f1e9c585da8 397 BuzzerTimer.attach(BuzzerTimer_func, 1);
yabahiro 21:1f1e9c585da8 398 EMGflag = true;
yabahiro 21:1f1e9c585da8 399 LED_DEBUG0 = 1;
yabahiro 21:1f1e9c585da8 400 }
yabahiro 21:1f1e9c585da8 401 if(EMG_0 && EMG_1 && EMGflag) {
yabahiro 21:1f1e9c585da8 402 buzzer = 1;
yabahiro 21:1f1e9c585da8 403 BuzzerTimer.detach();
yabahiro 21:1f1e9c585da8 404 EMGflag = false;
yabahiro 21:1f1e9c585da8 405 }
yabahiro 21:1f1e9c585da8 406 */
yabahiro 21:1f1e9c585da8 407 SystemProcessUpdate();
yabahiro 21:1f1e9c585da8 408 }
t_yamamoto 0:669ef71cba68 409 }
t_yamamoto 0:669ef71cba68 410
t_yamamoto 0:669ef71cba68 411 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 412 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 413 #if USE_PROCESS_NUM>0
yabahiro 21:1f1e9c585da8 414 static void Process0()
yabahiro 21:1f1e9c585da8 415 {
yabahiro 42:4f9689c0a349 416 /*
yabahiro 36:788274604650 417 mode=1;
yabahiro 36:788274604650 418 LedMode(1);
yabahiro 35:94f026ab4d30 419 if(ThSW) {
yabahiro 35:94f026ab4d30 420 Twsh=1;
yabahiro 35:94f026ab4d30 421 } else {
yabahiro 35:94f026ab4d30 422 Twsh=2;
yabahiro 31:45f97698857b 423 }
yabahiro 36:788274604650 424
yabahiro 25:38d4964e8aad 425 if(StertSW) {
yabahiro 41:e1dd6d97f17d 426 //start_flag == false;
yabahiro 41:e1dd6d97f17d 427 //lock = false;
yabahiro 36:788274604650 428 if(ZoneSW) {
yabahiro 31:45f97698857b 429 mode=1;
yabahiro 31:45f97698857b 430 current = 2;
yabahiro 31:45f97698857b 431 } else {
yabahiro 31:45f97698857b 432 mode=1;
yabahiro 40:7f93984e8ba1 433 current = 5;
yabahiro 31:45f97698857b 434 }
yabahiro 25:38d4964e8aad 435 }
yabahiro 35:94f026ab4d30 436 if(DWSW) {
yabahiro 35:94f026ab4d30 437 zyouge=true;
yabahiro 35:94f026ab4d30 438 }
yabahiro 39:f89166d7411c 439 if(DOSW) {
yabahiro 39:f89166d7411c 440 zyougedo=true;
yabahiro 39:f89166d7411c 441 }
yabahiro 35:94f026ab4d30 442 if(AIRSW) {
yabahiro 35:94f026ab4d30 443 if(SW_flag==false) {
yabahiro 35:94f026ab4d30 444 if(Air_flag==false) {
yabahiro 35:94f026ab4d30 445 solenoid.solenoid3 = SOLENOID_ON;
yabahiro 35:94f026ab4d30 446 Air_flag=true;
yabahiro 35:94f026ab4d30 447 SW_flag=true;
yabahiro 35:94f026ab4d30 448 } else {
yabahiro 35:94f026ab4d30 449 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 35:94f026ab4d30 450 Air_flag=false;
yabahiro 35:94f026ab4d30 451 SW_flag=true;
yabahiro 35:94f026ab4d30 452 }
yabahiro 35:94f026ab4d30 453 }
yabahiro 35:94f026ab4d30 454 } else {
yabahiro 35:94f026ab4d30 455 SW_flag=false;
yabahiro 31:45f97698857b 456 }
yabahiro 31:45f97698857b 457
yabahiro 35:94f026ab4d30 458 if(zyouge) {
yabahiro 35:94f026ab4d30 459 if(Twsh==1) {
yabahiro 35:94f026ab4d30 460 motor[MOTOR_0].pwm = 220;
yabahiro 35:94f026ab4d30 461 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 462 if(UPLS) {
yabahiro 35:94f026ab4d30 463 motor[MOTOR_0].pwm = 100;
yabahiro 35:94f026ab4d30 464 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 465 zyouge=false;
yabahiro 35:94f026ab4d30 466 }
yabahiro 35:94f026ab4d30 467 }
yabahiro 35:94f026ab4d30 468 if(Twsh==2) {
yabahiro 37:7cfa39c41245 469 motor[MOTOR_0].pwm = 230;
yabahiro 35:94f026ab4d30 470 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 471 if(UPLS2) {
yabahiro 35:94f026ab4d30 472 motor[MOTOR_0].pwm = 100;
yabahiro 35:94f026ab4d30 473 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 474 zyouge=false;
yabahiro 35:94f026ab4d30 475 }
yabahiro 31:45f97698857b 476 }
yabahiro 39:f89166d7411c 477 }
yabahiro 39:f89166d7411c 478 if(zyougedo) {
yabahiro 39:f89166d7411c 479 motor[MOTOR_0].pwm = 50;
yabahiro 39:f89166d7411c 480 motor[MOTOR_0].dir = BACK;
yabahiro 39:f89166d7411c 481 if(DOLS) {
yabahiro 39:f89166d7411c 482 motor[MOTOR_0].pwm = 100;
yabahiro 39:f89166d7411c 483 motor[MOTOR_0].dir = BRAKE;
yabahiro 39:f89166d7411c 484 zyougedo=false;
yabahiro 39:f89166d7411c 485 }
yabahiro 31:45f97698857b 486 }
yabahiro 42:4f9689c0a349 487 */
yabahiro 35:94f026ab4d30 488 }
yabahiro 31:45f97698857b 489
t_yamamoto 0:669ef71cba68 490 #endif
t_yamamoto 0:669ef71cba68 491
t_yamamoto 0:669ef71cba68 492 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 493 static void Process1()
t_yamamoto 0:669ef71cba68 494 {
yabahiro 42:4f9689c0a349 495 LedMode(6);
yabahiro 41:e1dd6d97f17d 496 start_flag = true;
yabahiro 36:788274604650 497 mtc=false;
yabahiro 36:788274604650 498 mtc2 = false;
yabahiro 31:45f97698857b 499 mode=1;
yabahiro 27:545148741493 500 cross=0;
yabahiro 35:94f026ab4d30 501 Twsh=1;
yabahiro 21:1f1e9c585da8 502 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 503 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 504 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 505 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
yabahiro 21:1f1e9c585da8 506
yabahiro 21:1f1e9c585da8 507 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 508 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 509 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 510 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
yabahiro 21:1f1e9c585da8 511
yabahiro 21:1f1e9c585da8 512 if(controller->Button.UP||controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 513
yabahiro 21:1f1e9c585da8 514 if(controller->Button.UP) {
yabahiro 35:94f026ab4d30 515 motor[MOTOR_0].pwm =200;
yabahiro 21:1f1e9c585da8 516 motor[MOTOR_0].dir = FOR;
yabahiro 35:94f026ab4d30 517 if(UPLS) {
yabahiro 35:94f026ab4d30 518 motor[MOTOR_0].pwm = 0;
yabahiro 35:94f026ab4d30 519 motor[MOTOR_0].dir = BRAKE;
yabahiro 35:94f026ab4d30 520 }
yabahiro 21:1f1e9c585da8 521 }
yabahiro 21:1f1e9c585da8 522 if(controller->Button.DOWN) {
yabahiro 21:1f1e9c585da8 523 motor[MOTOR_0].pwm = 50;
yabahiro 21:1f1e9c585da8 524 motor[MOTOR_0].dir = BACK;
yabahiro 21:1f1e9c585da8 525 }
yabahiro 21:1f1e9c585da8 526 } else {
yabahiro 21:1f1e9c585da8 527 motor[MOTOR_0].pwm = 0;
yabahiro 21:1f1e9c585da8 528 motor[MOTOR_0].dir = BRAKE;
yabahiro 21:1f1e9c585da8 529 }
yabahiro 21:1f1e9c585da8 530
yabahiro 21:1f1e9c585da8 531 if(controller->Button.Y) {
yabahiro 21:1f1e9c585da8 532 if(dz2==true) {
yabahiro 21:1f1e9c585da8 533 if(dz2i==false) {
yabahiro 21:1f1e9c585da8 534 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 535 dz2i=true;
yabahiro 21:1f1e9c585da8 536 } else {
yabahiro 21:1f1e9c585da8 537 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 538 dz2i=false;
yabahiro 21:1f1e9c585da8 539 }
yabahiro 21:1f1e9c585da8 540 dz2=false;
yabahiro 21:1f1e9c585da8 541 }
yabahiro 21:1f1e9c585da8 542 } else {
yabahiro 21:1f1e9c585da8 543 dz2=true;
yabahiro 21:1f1e9c585da8 544 }
yabahiro 21:1f1e9c585da8 545
yabahiro 21:1f1e9c585da8 546 if(controller->Button.A) {
yabahiro 21:1f1e9c585da8 547 if(dz3==true) {
yabahiro 21:1f1e9c585da8 548 if(dz3i==false) {
yabahiro 30:9450dc1d53cc 549
yabahiro 21:1f1e9c585da8 550 dz3i=true;
yabahiro 21:1f1e9c585da8 551 } else {
yabahiro 21:1f1e9c585da8 552 solenoid.solenoid3 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 553 dz3i=false;
yabahiro 21:1f1e9c585da8 554 }
yabahiro 21:1f1e9c585da8 555 dz3=false;
yabahiro 21:1f1e9c585da8 556 }
yabahiro 21:1f1e9c585da8 557 } else {
yabahiro 21:1f1e9c585da8 558 dz3=true;
yabahiro 21:1f1e9c585da8 559 }
yabahiro 21:1f1e9c585da8 560
yabahiro 21:1f1e9c585da8 561 if(controller->Button.B) {
yabahiro 21:1f1e9c585da8 562 if(dz4==true) {
yabahiro 21:1f1e9c585da8 563 if(dz4i==false) {
yabahiro 21:1f1e9c585da8 564 solenoid.solenoid4 = SOLENOID_ON;
yabahiro 21:1f1e9c585da8 565 dz4i=true;
yabahiro 21:1f1e9c585da8 566 } else {
yabahiro 21:1f1e9c585da8 567 solenoid.solenoid4 = SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 568 dz4i=false;
yabahiro 21:1f1e9c585da8 569 }
yabahiro 21:1f1e9c585da8 570 dz4=false;
yabahiro 21:1f1e9c585da8 571 }
yabahiro 21:1f1e9c585da8 572 } else {
yabahiro 21:1f1e9c585da8 573 dz4=true;
yabahiro 21:1f1e9c585da8 574 }
yabahiro 21:1f1e9c585da8 575 /*
yabahiro 21:1f1e9c585da8 576 if(controller->Button.RIGHT){
yabahiro 21:1f1e9c585da8 577 motor[MOTOR_1].dir = FOR;
yabahiro 21:1f1e9c585da8 578 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 579 if (ARM_1){
yabahiro 21:1f1e9c585da8 580 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 581 }
yabahiro 21:1f1e9c585da8 582 }
yabahiro 21:1f1e9c585da8 583 else if(controller->Button.LEFT){
yabahiro 21:1f1e9c585da8 584 motor[MOTOR_1].dir = BACK;
yabahiro 21:1f1e9c585da8 585 motor[MOTOR_1].pwm = 60;
yabahiro 21:1f1e9c585da8 586 if (ARM_0){
yabahiro 21:1f1e9c585da8 587 motor[MOTOR_1].dir = BRAKE;
yabahiro 21:1f1e9c585da8 588 }
yabahiro 21:1f1e9c585da8 589 }
yabahiro 21:1f1e9c585da8 590 */
t_yamamoto 0:669ef71cba68 591 }
t_yamamoto 0:669ef71cba68 592 #endif
t_yamamoto 0:669ef71cba68 593
t_yamamoto 0:669ef71cba68 594 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 595 static void Process2()
yabahiro 21:1f1e9c585da8 596 {
yabahiro 42:4f9689c0a349 597 // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
yabahiro 36:788274604650 598 LedMode(3);
yabahiro 31:45f97698857b 599 count++;
yabahiro 31:45f97698857b 600 if(mode==1) {//スタートゾーンから白線検知
yabahiro 42:4f9689c0a349 601 fast=40;
yabahiro 42:4f9689c0a349 602 normal=30;
yabahiro 42:4f9689c0a349 603 slow=20;
kishibekairohan 33:656aa14904a5 604 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 605 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 606 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 607 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 608 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 609 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 610 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 611 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 612 if(g[2]==0) {
yabahiro 31:45f97698857b 613 count=100000;
yabahiro 31:45f97698857b 614 cross=0;
yabahiro 31:45f97698857b 615 mode=3;
yabahiro 31:45f97698857b 616 }
yabahiro 31:45f97698857b 617 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 31:45f97698857b 618 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 619 motor[TIRE_FR].dir = FREE;
yabahiro 42:4f9689c0a349 620 motor[TIRE_FL].pwm = 25;
yabahiro 31:45f97698857b 621 motor[TIRE_FL].dir = FOR;
yabahiro 42:4f9689c0a349 622 motor[TIRE_BR].pwm = 25;
yabahiro 31:45f97698857b 623 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 624 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 625 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 626 if(g[0]==0) {
yabahiro 35:94f026ab4d30 627 mode=10;
yabahiro 35:94f026ab4d30 628 count=100000;
yabahiro 31:45f97698857b 629 }
yabahiro 35:94f026ab4d30 630 } else if(mode==10) { //縦ライントレース
yabahiro 35:94f026ab4d30 631
yabahiro 35:94f026ab4d30 632 if(counts==false&&g[2]==0) {
yabahiro 35:94f026ab4d30 633 cros++;
yabahiro 35:94f026ab4d30 634 counts=true;
yabahiro 31:45f97698857b 635 }
yabahiro 35:94f026ab4d30 636 if(counts==true&&g[2]==99) {
yabahiro 35:94f026ab4d30 637 counts=false;
yabahiro 35:94f026ab4d30 638 }
yabahiro 35:94f026ab4d30 639
yabahiro 35:94f026ab4d30 640 if(cros==Twsh) {
yabahiro 35:94f026ab4d30 641 mode=11;
yabahiro 31:45f97698857b 642 count=0;
yabahiro 35:94f026ab4d30 643 cros=0;
yabahiro 35:94f026ab4d30 644 }//ゆっくりモードに入れなかった時の保険
yabahiro 31:45f97698857b 645 switch(g[0]) {
yabahiro 31:45f97698857b 646 case 98:
kishibekairohan 33:656aa14904a5 647 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 648 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 649 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 650 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 651 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 652 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 653 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 654 motor[TIRE_BL].dir = FOR;
yabahiro 35:94f026ab4d30 655 mtc=true;
yabahiro 35:94f026ab4d30 656 if(count>80000) {
yabahiro 31:45f97698857b 657 cross++;
yabahiro 31:45f97698857b 658 count=0;
yabahiro 31:45f97698857b 659 }
yabahiro 31:45f97698857b 660 if(cross==Twsh) {
yabahiro 31:45f97698857b 661 mode=11;
yabahiro 31:45f97698857b 662 count=0;
yabahiro 35:94f026ab4d30 663 cross=0;
yabahiro 31:45f97698857b 664 }
yabahiro 31:45f97698857b 665 break;
yabahiro 31:45f97698857b 666 case 0:
kishibekairohan 33:656aa14904a5 667 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 668 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 669 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 670 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 671 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 672 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 673 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 674 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 675 mtc=true;
yabahiro 31:45f97698857b 676 break;
yabahiro 31:45f97698857b 677 case 255:
kishibekairohan 33:656aa14904a5 678 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 679 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 680 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 681 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 682 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 683 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 684 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 685 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 686 mtc=true;
yabahiro 31:45f97698857b 687 break;
yabahiro 31:45f97698857b 688 case 253:
kishibekairohan 33:656aa14904a5 689 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 690 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 691 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 692 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 693 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 694 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 695 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 696 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 697 mtc=true;
yabahiro 31:45f97698857b 698 break;
yabahiro 31:45f97698857b 699 case 254:
kishibekairohan 33:656aa14904a5 700 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 701 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 702 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 703 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 704 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 705 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 706 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 707 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 708 mtc=true;
yabahiro 31:45f97698857b 709 break;
yabahiro 31:45f97698857b 710 case 1:
kishibekairohan 33:656aa14904a5 711 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 712 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 713 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 714 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 715 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 716 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 717 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 718 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 719 mtc=true;
yabahiro 31:45f97698857b 720 break;
yabahiro 31:45f97698857b 721 case 3:
yabahiro 31:45f97698857b 722 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 723 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 724 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 725 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 726 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 727 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 728 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 729 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 730 mtc=true;
yabahiro 31:45f97698857b 731 break;
yabahiro 31:45f97698857b 732 case 2:
yabahiro 31:45f97698857b 733 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 734 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 735 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 736 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 737 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 738 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 739 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 740 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 741 mtc=true;
yabahiro 31:45f97698857b 742 break;
yabahiro 31:45f97698857b 743 }
yabahiro 31:45f97698857b 744 if(g[0]!=98&&mtc==true) {
yabahiro 31:45f97698857b 745 switch(g[1]) {
yabahiro 31:45f97698857b 746 case 0:
yabahiro 31:45f97698857b 747 mtc=false;
yabahiro 31:45f97698857b 748 break;
yabahiro 31:45f97698857b 749 case 255:
yabahiro 31:45f97698857b 750 motor[TIRE_BR].pwm += 5;
yabahiro 31:45f97698857b 751 mtc=false;
yabahiro 31:45f97698857b 752 break;
yabahiro 31:45f97698857b 753 case 253:
yabahiro 31:45f97698857b 754 motor[TIRE_BR].pwm += 10;
yabahiro 31:45f97698857b 755 mtc=false;
yabahiro 31:45f97698857b 756 break;
yabahiro 31:45f97698857b 757 case 254:
yabahiro 31:45f97698857b 758 motor[TIRE_BR].pwm += 20;
yabahiro 31:45f97698857b 759 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 760 mtc=false;
yabahiro 31:45f97698857b 761 break;
yabahiro 31:45f97698857b 762 case 1:
yabahiro 31:45f97698857b 763 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 764 mtc=false;
yabahiro 31:45f97698857b 765 break;
yabahiro 31:45f97698857b 766 case 3:
yabahiro 31:45f97698857b 767 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 768 mtc=false;
yabahiro 31:45f97698857b 769 break;
yabahiro 31:45f97698857b 770 case 2:
yabahiro 31:45f97698857b 771 motor[TIRE_BL].pwm += 20;
yabahiro 31:45f97698857b 772 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 773 mtc=false;
yabahiro 31:45f97698857b 774 break;
yabahiro 21:1f1e9c585da8 775 }
yabahiro 31:45f97698857b 776 }
yabahiro 31:45f97698857b 777 } else if(mode==11) {
yabahiro 31:45f97698857b 778 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 779 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 780 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 781 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 782 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 783 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 784 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 785 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 786 if(g[2]==0) {
yabahiro 31:45f97698857b 787 count=0;
yabahiro 31:45f97698857b 788 cross=0;
yabahiro 31:45f97698857b 789 mode=20;
yabahiro 31:45f97698857b 790 }
yabahiro 31:45f97698857b 791 } else if(mode==20) {
yabahiro 35:94f026ab4d30 792
yabahiro 38:391a0c8e4c16 793 if(Twsh==2) {
yabahiro 38:391a0c8e4c16 794 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 38:391a0c8e4c16 795 }
yabahiro 35:94f026ab4d30 796 if(counts==false&&g[0]==0) {
yabahiro 35:94f026ab4d30 797 cros++;
yabahiro 35:94f026ab4d30 798 counts=true;
yabahiro 35:94f026ab4d30 799 }
yabahiro 35:94f026ab4d30 800 if(counts==true&&g[0]==99) {
yabahiro 35:94f026ab4d30 801 counts=false;
yabahiro 35:94f026ab4d30 802 }
yabahiro 35:94f026ab4d30 803
yabahiro 35:94f026ab4d30 804 if(cros==2) {
yabahiro 37:7cfa39c41245 805
yabahiro 37:7cfa39c41245 806 // mode=21;
yabahiro 37:7cfa39c41245 807 // count=0;
yabahiro 37:7cfa39c41245 808 // cros=0;
yabahiro 35:94f026ab4d30 809 }//ゆっくりモードに入れなかった時の保険
yabahiro 35:94f026ab4d30 810
yabahiro 31:45f97698857b 811 switch(g[2]) {
yabahiro 31:45f97698857b 812 case 98:
kishibekairohan 33:656aa14904a5 813 motor[TIRE_FR].pwm = normal;
kishibekairohan 22:c47f29caded6 814 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 815 motor[TIRE_FL].pwm = normal;
kishibekairohan 22:c47f29caded6 816 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 817 motor[TIRE_BR].pwm = normal;
kishibekairohan 22:c47f29caded6 818 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 819 motor[TIRE_BL].pwm = normal;
kishibekairohan 22:c47f29caded6 820 motor[TIRE_BL].dir = BACK;
yabahiro 42:4f9689c0a349 821 if(count>50000) {
yabahiro 31:45f97698857b 822 cross++;
yabahiro 31:45f97698857b 823 count=0;
yabahiro 31:45f97698857b 824 }
yabahiro 31:45f97698857b 825 mtc2=true;
yabahiro 42:4f9689c0a349 826 if(cross==1) {
yabahiro 31:45f97698857b 827 mode=21;
yabahiro 31:45f97698857b 828 count=0;
yabahiro 31:45f97698857b 829 cross=0;
yabahiro 31:45f97698857b 830 }
yabahiro 31:45f97698857b 831 break;
yabahiro 31:45f97698857b 832 case 0:
kishibekairohan 33:656aa14904a5 833 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 834 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 835 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 836 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 837 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 838 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 839 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 840 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 841 mtc2=true;
yabahiro 31:45f97698857b 842 break;
yabahiro 31:45f97698857b 843 case 255:
kishibekairohan 33:656aa14904a5 844 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 845 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 846 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 847 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 848 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 849 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 850 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 851 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 852 mtc2=true;
yabahiro 31:45f97698857b 853 break;
yabahiro 31:45f97698857b 854 case 253:
yabahiro 31:45f97698857b 855 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 856 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 857 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 858 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 859 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 860 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 861 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 862 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 863 mtc2=true;
yabahiro 31:45f97698857b 864 break;
yabahiro 31:45f97698857b 865 case 254:
yabahiro 31:45f97698857b 866 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 867 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 868 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 869 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 870 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 871 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 872 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 873 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 874 mtc2=true;
yabahiro 31:45f97698857b 875 break;
yabahiro 31:45f97698857b 876 case 1:
kishibekairohan 33:656aa14904a5 877 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 878 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 879 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 880 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 881 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 882 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 883 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 884 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 885 mtc2=true;
yabahiro 31:45f97698857b 886 break;
yabahiro 31:45f97698857b 887 case 3:
kishibekairohan 33:656aa14904a5 888 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 889 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 890 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 891 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 892 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 893 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 894 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 895 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 896 mtc2=true;
yabahiro 31:45f97698857b 897 break;
yabahiro 31:45f97698857b 898 case 2:
kishibekairohan 33:656aa14904a5 899 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 900 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 901 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 902 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 903 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 904 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 905 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 906 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 907 mtc2=true;
yabahiro 31:45f97698857b 908 break;
yabahiro 31:45f97698857b 909 }
yabahiro 31:45f97698857b 910 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 911 switch(g[3]) {
yabahiro 29:c78a46f51209 912 case 0:
yabahiro 31:45f97698857b 913 mtc2=false;
yabahiro 29:c78a46f51209 914 break;
yabahiro 29:c78a46f51209 915 case 255:
yabahiro 31:45f97698857b 916 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 917 mtc2=false;
yabahiro 29:c78a46f51209 918 break;
yabahiro 29:c78a46f51209 919 case 253:
yabahiro 31:45f97698857b 920 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 921 mtc2=false;
yabahiro 29:c78a46f51209 922 break;
yabahiro 29:c78a46f51209 923 case 254:
yabahiro 31:45f97698857b 924 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 925 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 926 mtc2=false;
yabahiro 29:c78a46f51209 927 break;
yabahiro 29:c78a46f51209 928 case 1:
yabahiro 31:45f97698857b 929 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 930 mtc2=false;
yabahiro 29:c78a46f51209 931 break;
yabahiro 29:c78a46f51209 932 case 3:
yabahiro 31:45f97698857b 933 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 934 mtc2=false;
yabahiro 29:c78a46f51209 935 break;
yabahiro 29:c78a46f51209 936 case 2:
yabahiro 31:45f97698857b 937 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 938 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 939 mtc2=false;
yabahiro 29:c78a46f51209 940 break;
yabahiro 29:c78a46f51209 941 }
yabahiro 31:45f97698857b 942 }
yabahiro 31:45f97698857b 943 } else if(mode==21) {
yabahiro 42:4f9689c0a349 944 motor[TIRE_FR].pwm = 26;
yabahiro 42:4f9689c0a349 945 motor[TIRE_FR].dir = FOR;
yabahiro 42:4f9689c0a349 946 motor[TIRE_FL].pwm = 30;
yabahiro 42:4f9689c0a349 947 motor[TIRE_FL].dir = FOR;
yabahiro 42:4f9689c0a349 948 motor[TIRE_BR].pwm = 26;
yabahiro 42:4f9689c0a349 949 motor[TIRE_BR].dir = BACK;
yabahiro 42:4f9689c0a349 950 motor[TIRE_BL].pwm = 30;
yabahiro 42:4f9689c0a349 951 motor[TIRE_BL].dir = BACK;
yabahiro 42:4f9689c0a349 952 if(g[2]==98&&count>=10000) {
yabahiro 42:4f9689c0a349 953 mode=22;
yabahiro 42:4f9689c0a349 954 count=0;
yabahiro 42:4f9689c0a349 955 }
yabahiro 42:4f9689c0a349 956 } else if(mode==22) {
yabahiro 42:4f9689c0a349 957 motor[TIRE_FR].pwm = 20;
yabahiro 42:4f9689c0a349 958 motor[TIRE_FR].dir = BACK;
yabahiro 42:4f9689c0a349 959 motor[TIRE_FL].pwm = 20;
yabahiro 42:4f9689c0a349 960 motor[TIRE_FL].dir = BACK;
yabahiro 42:4f9689c0a349 961 motor[TIRE_BR].pwm = 20;
yabahiro 42:4f9689c0a349 962 motor[TIRE_BR].dir = FOR;
yabahiro 42:4f9689c0a349 963 motor[TIRE_BL].pwm = 20;
yabahiro 42:4f9689c0a349 964 motor[TIRE_BL].dir = FOR;
yabahiro 42:4f9689c0a349 965 if(g[2]!=98) {
yabahiro 42:4f9689c0a349 966 count=0;
yabahiro 42:4f9689c0a349 967 mode=23;
yabahiro 42:4f9689c0a349 968 }
yabahiro 42:4f9689c0a349 969 } else if(mode==23) {
yabahiro 42:4f9689c0a349 970 motor[TIRE_FR].pwm = 10;
yabahiro 42:4f9689c0a349 971 motor[TIRE_FR].dir = BRAKE;
yabahiro 42:4f9689c0a349 972 motor[TIRE_FL].pwm = 10;
yabahiro 42:4f9689c0a349 973 motor[TIRE_FL].dir = BRAKE;
yabahiro 42:4f9689c0a349 974 motor[TIRE_BR].pwm = 10;
yabahiro 42:4f9689c0a349 975 motor[TIRE_BR].dir = BRAKE;
yabahiro 42:4f9689c0a349 976 motor[TIRE_BL].pwm = 10;
yabahiro 42:4f9689c0a349 977 motor[TIRE_BL].dir = BRAKE;
yabahiro 42:4f9689c0a349 978 if(count<=100000) {
yabahiro 42:4f9689c0a349 979 mode=30;
yabahiro 42:4f9689c0a349 980 }
yabahiro 42:4f9689c0a349 981 } else if(mode==24) {
yabahiro 31:45f97698857b 982 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 983 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 984 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 985 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 986 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 987 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 988 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 989 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 990 if(g[0]==0) {
yabahiro 31:45f97698857b 991 mode=30;
yabahiro 35:94f026ab4d30 992 cros=0;
yabahiro 31:45f97698857b 993 }
yabahiro 31:45f97698857b 994 } else if(mode==30) {
yabahiro 42:4f9689c0a349 995 motor[TIRE_FR].pwm = 63;
yabahiro 38:391a0c8e4c16 996 motor[TIRE_FR].dir = BACK;
yabahiro 38:391a0c8e4c16 997 motor[TIRE_FL].pwm = 60;
yabahiro 38:391a0c8e4c16 998 motor[TIRE_FL].dir = FOR;
yabahiro 38:391a0c8e4c16 999 motor[TIRE_BR].pwm = 60;
yabahiro 38:391a0c8e4c16 1000 motor[TIRE_BR].dir = BACK;
yabahiro 42:4f9689c0a349 1001 motor[TIRE_BL].pwm = 60;
yabahiro 38:391a0c8e4c16 1002 motor[TIRE_BL].dir = FOR;
yabahiro 38:391a0c8e4c16 1003 if(g[0]==98) {
yabahiro 35:94f026ab4d30 1004 mode=31;
yabahiro 38:391a0c8e4c16 1005 count=0;
yabahiro 38:391a0c8e4c16 1006 }
yabahiro 38:391a0c8e4c16 1007 } else if(mode==31) {
yabahiro 42:4f9689c0a349 1008 motor[TIRE_FR].pwm = 0;
yabahiro 42:4f9689c0a349 1009 motor[TIRE_FR].dir = FREE;
yabahiro 42:4f9689c0a349 1010 motor[TIRE_FL].pwm = 0;
yabahiro 42:4f9689c0a349 1011 motor[TIRE_FL].dir = FREE;
yabahiro 42:4f9689c0a349 1012 motor[TIRE_BR].pwm = 0;
yabahiro 42:4f9689c0a349 1013 motor[TIRE_BR].dir = FREE;
yabahiro 42:4f9689c0a349 1014 motor[TIRE_BL].pwm = 0;
yabahiro 42:4f9689c0a349 1015 motor[TIRE_BL].dir = FREE;
yabahiro 42:4f9689c0a349 1016 if(count>=5000) {
yabahiro 38:391a0c8e4c16 1017 count=0;
yabahiro 38:391a0c8e4c16 1018 mode=32;
yabahiro 38:391a0c8e4c16 1019 }
yabahiro 38:391a0c8e4c16 1020 } else if(mode==32) {
yabahiro 42:4f9689c0a349 1021 motor[TIRE_FR].pwm = 10;
yabahiro 42:4f9689c0a349 1022 motor[TIRE_FR].dir = BRAKE;
yabahiro 42:4f9689c0a349 1023 motor[TIRE_FL].pwm = 10;
yabahiro 42:4f9689c0a349 1024 motor[TIRE_FL].dir = BRAKE;
yabahiro 42:4f9689c0a349 1025 motor[TIRE_BR].pwm = 10;
yabahiro 42:4f9689c0a349 1026 motor[TIRE_BR].dir = BRAKE;
yabahiro 42:4f9689c0a349 1027 motor[TIRE_BL].pwm = 10;
yabahiro 42:4f9689c0a349 1028 motor[TIRE_BL].dir = BRAKE;
yabahiro 42:4f9689c0a349 1029 if(count>=40000) {
yabahiro 42:4f9689c0a349 1030 mode=33;
yabahiro 42:4f9689c0a349 1031 count=0;
yabahiro 42:4f9689c0a349 1032 }
yabahiro 42:4f9689c0a349 1033 } else if(mode==33) {
yabahiro 38:391a0c8e4c16 1034 motor[TIRE_FR].pwm = 30;
yabahiro 42:4f9689c0a349 1035 motor[TIRE_FR].dir = BACK;
yabahiro 42:4f9689c0a349 1036 motor[TIRE_FL].pwm = 0;
yabahiro 38:391a0c8e4c16 1037 motor[TIRE_FL].dir = BACK;
yabahiro 42:4f9689c0a349 1038 motor[TIRE_BR].pwm = 0;
yabahiro 38:391a0c8e4c16 1039 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1040 motor[TIRE_BL].pwm = 30;
yabahiro 42:4f9689c0a349 1041 motor[TIRE_BL].dir = FOR;
yabahiro 38:391a0c8e4c16 1042 if(g[3]==0) {
yabahiro 38:391a0c8e4c16 1043 count=0;
yabahiro 39:f89166d7411c 1044 if(Twsh==2) {
yabahiro 39:f89166d7411c 1045 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 39:f89166d7411c 1046 }
yabahiro 38:391a0c8e4c16 1047 mode=40;
yabahiro 38:391a0c8e4c16 1048 }
yabahiro 31:45f97698857b 1049 } else if(mode==40) {
yabahiro 38:391a0c8e4c16 1050 if(LIB) {
yabahiro 38:391a0c8e4c16 1051 mode=42;
yabahiro 38:391a0c8e4c16 1052 }
yabahiro 35:94f026ab4d30 1053 if(counts==false&&g[0]==0) {
yabahiro 35:94f026ab4d30 1054 cros++;
yabahiro 35:94f026ab4d30 1055 counts=true;
yabahiro 35:94f026ab4d30 1056 }
yabahiro 35:94f026ab4d30 1057 if(counts==true&&g[0]==99) {
yabahiro 35:94f026ab4d30 1058 counts=false;
yabahiro 35:94f026ab4d30 1059 }
yabahiro 35:94f026ab4d30 1060
yabahiro 38:391a0c8e4c16 1061 if(cros==1) {
yabahiro 36:788274604650 1062 // mode=41;
yabahiro 35:94f026ab4d30 1063 //count=0;
yabahiro 35:94f026ab4d30 1064 //cros=0;
yabahiro 35:94f026ab4d30 1065 }//ゆっくりモードに入れなかった時の保険
yabahiro 35:94f026ab4d30 1066
yabahiro 31:45f97698857b 1067 switch(g[3]) {
yabahiro 31:45f97698857b 1068 case 98:
kishibekairohan 33:656aa14904a5 1069 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1070 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1071 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1072 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1073 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1074 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1075 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1076 motor[TIRE_BL].dir = FOR;
yabahiro 35:94f026ab4d30 1077 if(count>20000) {
yabahiro 31:45f97698857b 1078 cross++;
yabahiro 31:45f97698857b 1079 count=0;
yabahiro 31:45f97698857b 1080 }
yabahiro 31:45f97698857b 1081 mtc2=true;
yabahiro 42:4f9689c0a349 1082 if(cross==1) {
yabahiro 31:45f97698857b 1083 mode=41;
yabahiro 31:45f97698857b 1084 count=0;
yabahiro 31:45f97698857b 1085 cross=0;
kishibekairohan 22:c47f29caded6 1086 }
yabahiro 31:45f97698857b 1087 break;
yabahiro 31:45f97698857b 1088 case 0:
kishibekairohan 33:656aa14904a5 1089 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1090 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1091 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1092 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1093 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1094 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1095 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1096 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1097 mtc2=true;
yabahiro 31:45f97698857b 1098 break;
yabahiro 31:45f97698857b 1099 case 255:
kishibekairohan 33:656aa14904a5 1100 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1101 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1102 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1103 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1104 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1105 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1106 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1107 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1108 mtc2=true;
yabahiro 31:45f97698857b 1109 break;
yabahiro 31:45f97698857b 1110 case 253:
yabahiro 31:45f97698857b 1111 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1112 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1113 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1114 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1115 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1116 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1117 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1118 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1119 mtc2=true;
yabahiro 31:45f97698857b 1120 break;
yabahiro 31:45f97698857b 1121 case 254:
yabahiro 31:45f97698857b 1122 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1123 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1124 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1125 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1126 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1127 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1128 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1129 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1130 mtc2=true;
yabahiro 31:45f97698857b 1131 break;
yabahiro 31:45f97698857b 1132 case 1:
kishibekairohan 33:656aa14904a5 1133 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1134 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1135 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1136 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1137 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1138 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1139 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1140 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1141 mtc2=true;
yabahiro 31:45f97698857b 1142 break;
yabahiro 31:45f97698857b 1143 case 3:
kishibekairohan 33:656aa14904a5 1144 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1145 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 1146 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1147 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1148 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1149 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1150 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1151 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1152 mtc2=true;
yabahiro 31:45f97698857b 1153 break;
yabahiro 31:45f97698857b 1154 case 2:
kishibekairohan 33:656aa14904a5 1155 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1156 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 1157 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1158 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1159 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1160 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1161 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1162 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1163 mtc2=true;
yabahiro 31:45f97698857b 1164 break;
yabahiro 31:45f97698857b 1165 }
yabahiro 31:45f97698857b 1166 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 1167 switch(g[2]) {
yabahiro 29:c78a46f51209 1168 case 0:
yabahiro 31:45f97698857b 1169 mtc2=false;
yabahiro 29:c78a46f51209 1170 break;
yabahiro 29:c78a46f51209 1171 case 255:
yabahiro 31:45f97698857b 1172 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1173 mtc2=false;
yabahiro 29:c78a46f51209 1174 break;
yabahiro 29:c78a46f51209 1175 case 253:
yabahiro 31:45f97698857b 1176 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1177 mtc2=false;
yabahiro 29:c78a46f51209 1178 break;
yabahiro 29:c78a46f51209 1179 case 254:
yabahiro 31:45f97698857b 1180 motor[TIRE_FL].pwm += 20;
yabahiro 29:c78a46f51209 1181 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1182 mtc2=false;
yabahiro 29:c78a46f51209 1183 break;
yabahiro 29:c78a46f51209 1184 case 1:
yabahiro 31:45f97698857b 1185 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 1186 mtc2=false;
yabahiro 29:c78a46f51209 1187 break;
yabahiro 29:c78a46f51209 1188 case 3:
yabahiro 31:45f97698857b 1189 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 1190 mtc2=false;
yabahiro 29:c78a46f51209 1191 break;
yabahiro 29:c78a46f51209 1192 case 2:
yabahiro 31:45f97698857b 1193 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 1194 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1195 mtc2=false;
yabahiro 29:c78a46f51209 1196 break;
yabahiro 29:c78a46f51209 1197 }
yabahiro 31:45f97698857b 1198 }
yabahiro 31:45f97698857b 1199 } else if(mode==41) {
yabahiro 42:4f9689c0a349 1200 if(LIF||LIB) {
yabahiro 42:4f9689c0a349 1201 mode=42;
yabahiro 42:4f9689c0a349 1202 }
yabahiro 31:45f97698857b 1203 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1204 motor[TIRE_FR].dir = FREE;
yabahiro 35:94f026ab4d30 1205 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1206 motor[TIRE_FL].dir = BACK;
yabahiro 35:94f026ab4d30 1207 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1208 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1209 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1210 motor[TIRE_BL].dir = FREE;
yabahiro 35:94f026ab4d30 1211 if(g[1]==0) {
yabahiro 31:45f97698857b 1212 count=0;
yabahiro 31:45f97698857b 1213 mode=70;
yabahiro 31:45f97698857b 1214 }
yabahiro 38:391a0c8e4c16 1215 } else if(mode==42) {
yabahiro 38:391a0c8e4c16 1216 motor[TIRE_FR].pwm = 20;
yabahiro 38:391a0c8e4c16 1217 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1218 motor[TIRE_FL].pwm = 0;
yabahiro 38:391a0c8e4c16 1219 motor[TIRE_FL].dir = FREE;
yabahiro 38:391a0c8e4c16 1220 motor[TIRE_BR].pwm = 0;
yabahiro 38:391a0c8e4c16 1221 motor[TIRE_BR].dir = FREE;
yabahiro 38:391a0c8e4c16 1222 motor[TIRE_BL].pwm = 20;
yabahiro 38:391a0c8e4c16 1223 motor[TIRE_BL].dir = BACK;
yabahiro 38:391a0c8e4c16 1224 if(g[1]==0) {
yabahiro 38:391a0c8e4c16 1225 count=0;
yabahiro 38:391a0c8e4c16 1226 mode=70;
yabahiro 38:391a0c8e4c16 1227 }
yabahiro 31:45f97698857b 1228 } else if(mode==70) {
yabahiro 31:45f97698857b 1229 switch(g[1]) {
yabahiro 31:45f97698857b 1230 case 99:
yabahiro 31:45f97698857b 1231 if(count>=100000) {
yabahiro 31:45f97698857b 1232 mode=100;
yabahiro 29:c78a46f51209 1233 }
yabahiro 31:45f97698857b 1234 break;
yabahiro 31:45f97698857b 1235 case 98:
kishibekairohan 33:656aa14904a5 1236 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1237 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1238 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1239 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1240 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1241 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1242 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1243 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1244 break;
yabahiro 31:45f97698857b 1245 case 0:
yabahiro 38:391a0c8e4c16 1246 motor[TIRE_FR].pwm = 80;
yabahiro 31:45f97698857b 1247 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1248 motor[TIRE_FL].pwm = 80;
yabahiro 31:45f97698857b 1249 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1250 motor[TIRE_BR].pwm = 80;
yabahiro 31:45f97698857b 1251 motor[TIRE_BR].dir = FOR;
yabahiro 38:391a0c8e4c16 1252 motor[TIRE_BL].pwm = 80;
yabahiro 31:45f97698857b 1253 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1254 mtc=true;
yabahiro 31:45f97698857b 1255 break;
yabahiro 31:45f97698857b 1256 case 255:
kishibekairohan 33:656aa14904a5 1257 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1258 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1259 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1260 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1261 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1262 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1263 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1264 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1265 mtc=true;
yabahiro 31:45f97698857b 1266 break;
yabahiro 31:45f97698857b 1267 case 253:
kishibekairohan 33:656aa14904a5 1268 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1269 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1270 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1271 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1272 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1273 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1274 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1275 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1276 mtc=true;
yabahiro 31:45f97698857b 1277 break;
yabahiro 31:45f97698857b 1278 case 254:
kishibekairohan 33:656aa14904a5 1279 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1280 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1281 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1282 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1283 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1284 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1285 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1286 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1287 mtc=true;
yabahiro 31:45f97698857b 1288 break;
yabahiro 31:45f97698857b 1289 case 1:
kishibekairohan 33:656aa14904a5 1290 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1291 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1292 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1293 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1294 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1295 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1296 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1297 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1298 mtc=true;
yabahiro 31:45f97698857b 1299 break;
yabahiro 31:45f97698857b 1300 case 3:
yabahiro 31:45f97698857b 1301 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1302 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1303 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1304 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1305 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1306 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1307 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1308 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1309 mtc=true;
yabahiro 31:45f97698857b 1310 break;
yabahiro 31:45f97698857b 1311 case 2:
yabahiro 31:45f97698857b 1312 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1313 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1314 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1315 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1316 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1317 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1318 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1319 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1320 mtc=true;
yabahiro 31:45f97698857b 1321 break;
yabahiro 31:45f97698857b 1322 }
yabahiro 31:45f97698857b 1323 if(g[1]!=98&&mtc==true) {
yabahiro 31:45f97698857b 1324 switch(g[0]) {
yabahiro 29:c78a46f51209 1325 case 0:
yabahiro 31:45f97698857b 1326 mtc=false;
yabahiro 29:c78a46f51209 1327 break;
yabahiro 29:c78a46f51209 1328 case 255:
yabahiro 31:45f97698857b 1329 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1330 mtc=false;
yabahiro 29:c78a46f51209 1331 break;
yabahiro 29:c78a46f51209 1332 case 253:
yabahiro 31:45f97698857b 1333 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1334 mtc=false;
yabahiro 29:c78a46f51209 1335 break;
yabahiro 29:c78a46f51209 1336 case 254:
yabahiro 31:45f97698857b 1337 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 1338 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1339 mtc=false;
yabahiro 29:c78a46f51209 1340 break;
yabahiro 29:c78a46f51209 1341 case 1:
yabahiro 31:45f97698857b 1342 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1343 mtc=false;
yabahiro 29:c78a46f51209 1344 break;
yabahiro 29:c78a46f51209 1345 case 3:
yabahiro 31:45f97698857b 1346 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1347 mtc=false;
yabahiro 29:c78a46f51209 1348 break;
yabahiro 29:c78a46f51209 1349 case 2:
yabahiro 31:45f97698857b 1350 motor[TIRE_FR].pwm += 20;
yabahiro 31:45f97698857b 1351 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1352 mtc=false;
yabahiro 29:c78a46f51209 1353 break;
yabahiro 29:c78a46f51209 1354 }
yabahiro 31:45f97698857b 1355 }
yabahiro 31:45f97698857b 1356 } else if(mode==100) {
yabahiro 35:94f026ab4d30 1357 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1358 motor[TIRE_FR].dir = FOR;
yabahiro 35:94f026ab4d30 1359 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1360 motor[TIRE_FL].dir = BACK;
yabahiro 31:45f97698857b 1361 motor[TIRE_BR].pwm = 23;
yabahiro 31:45f97698857b 1362 motor[TIRE_BR].dir = FOR;
yabahiro 35:94f026ab4d30 1363 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1364 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1365 } else {
yabahiro 31:45f97698857b 1366 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1367 motor[TIRE_FR].dir = BRAKE;
yabahiro 31:45f97698857b 1368 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1369 motor[TIRE_FL].dir = BRAKE;
yabahiro 31:45f97698857b 1370 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1371 motor[TIRE_BR].dir = BRAKE;
yabahiro 31:45f97698857b 1372 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1373 motor[TIRE_BL].dir = BRAKE;
yabahiro 31:45f97698857b 1374 }
yabahiro 31:45f97698857b 1375 }
yabahiro 31:45f97698857b 1376 #endif
yabahiro 31:45f97698857b 1377
yabahiro 31:45f97698857b 1378 #if USE_PROCESS_NUM>3
yabahiro 31:45f97698857b 1379 static void Process3()
yabahiro 31:45f97698857b 1380 {
yabahiro 39:f89166d7411c 1381
yabahiro 36:788274604650 1382 }
yabahiro 36:788274604650 1383 #endif
yabahiro 36:788274604650 1384
yabahiro 36:788274604650 1385 #if USE_PROCESS_NUM>4
yabahiro 36:788274604650 1386 static void Process4()
yabahiro 36:788274604650 1387 {
yabahiro 36:788274604650 1388 LedMode(4);
yabahiro 36:788274604650 1389 count++;
yabahiro 31:45f97698857b 1390 if(mode==1) {//スタートゾーンから白線検知
yabahiro 39:f89166d7411c 1391 motor[TIRE_FR].pwm = 40;
yabahiro 31:45f97698857b 1392 motor[TIRE_FR].dir = FOR;
yabahiro 39:f89166d7411c 1393 motor[TIRE_FL].pwm = 40;
yabahiro 31:45f97698857b 1394 motor[TIRE_FL].dir = BACK;
yabahiro 39:f89166d7411c 1395 motor[TIRE_BR].pwm = 40;
yabahiro 31:45f97698857b 1396 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 1397 motor[TIRE_BL].pwm = 45;
yabahiro 31:45f97698857b 1398 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1399 if(g[2]==0) {
yabahiro 31:45f97698857b 1400 count=100000;
yabahiro 31:45f97698857b 1401 cross=0;
yabahiro 36:788274604650 1402 mode=3;
yabahiro 31:45f97698857b 1403 }
yabahiro 36:788274604650 1404 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 39:f89166d7411c 1405 motor[TIRE_FR].pwm = 40;
kishibekairohan 32:90f018b10ad6 1406 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1407 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1408 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1409 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1410 motor[TIRE_BR].dir = FREE;
yabahiro 39:f89166d7411c 1411 motor[TIRE_BL].pwm = 40;
kishibekairohan 32:90f018b10ad6 1412 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1413 if(g[0]==0) {
yabahiro 36:788274604650 1414 mode=10;
yabahiro 36:788274604650 1415 count=100000;
yabahiro 36:788274604650 1416 }
yabahiro 36:788274604650 1417 } else if(mode==10) { //縦ライントレース
yabahiro 36:788274604650 1418
yabahiro 36:788274604650 1419 if(counts==false&&g[2]==0) {
yabahiro 36:788274604650 1420 cros++;
yabahiro 36:788274604650 1421 counts=true;
yabahiro 36:788274604650 1422 }
yabahiro 36:788274604650 1423 if(counts==true&&g[2]==99) {
yabahiro 36:788274604650 1424 counts=false;
yabahiro 36:788274604650 1425 }
yabahiro 36:788274604650 1426
yabahiro 36:788274604650 1427 if(cros==Twsh) {
yabahiro 36:788274604650 1428 mode=11;
yabahiro 31:45f97698857b 1429 count=0;
yabahiro 36:788274604650 1430 cros=0;
yabahiro 36:788274604650 1431 }//ゆっくりモードに入れなかった時の保険
kishibekairohan 32:90f018b10ad6 1432 switch(g[1]) {
yabahiro 31:45f97698857b 1433 case 98:
kishibekairohan 33:656aa14904a5 1434 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1435 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1436 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1437 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1438 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1439 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1440 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1441 motor[TIRE_BL].dir = BACK;
yabahiro 36:788274604650 1442 mtc=true;
yabahiro 36:788274604650 1443 if(count>80000) {
yabahiro 31:45f97698857b 1444 cross++;
yabahiro 31:45f97698857b 1445 count=0;
yabahiro 31:45f97698857b 1446 }
yabahiro 31:45f97698857b 1447 if(cross==Twsh) {
yabahiro 31:45f97698857b 1448 mode=11;
yabahiro 31:45f97698857b 1449 count=0;
yabahiro 36:788274604650 1450 cross=0;
yabahiro 31:45f97698857b 1451 }
yabahiro 31:45f97698857b 1452 break;
yabahiro 31:45f97698857b 1453 case 0:
kishibekairohan 33:656aa14904a5 1454 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1455 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1456 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1457 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1458 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1459 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1460 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1461 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1462 mtc=true;
yabahiro 31:45f97698857b 1463 break;
yabahiro 31:45f97698857b 1464 case 255:
kishibekairohan 33:656aa14904a5 1465 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1466 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1467 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1468 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1469 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1470 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1471 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1472 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1473 mtc=true;
yabahiro 31:45f97698857b 1474 break;
yabahiro 31:45f97698857b 1475 case 253:
kishibekairohan 33:656aa14904a5 1476 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1477 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1478 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1479 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1480 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1481 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1482 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1483 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1484 mtc=true;
yabahiro 31:45f97698857b 1485 break;
yabahiro 31:45f97698857b 1486 case 254:
kishibekairohan 33:656aa14904a5 1487 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1488 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1489 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1490 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1491 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1492 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1493 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1494 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1495 mtc=true;
yabahiro 31:45f97698857b 1496 break;
yabahiro 31:45f97698857b 1497 case 1:
kishibekairohan 33:656aa14904a5 1498 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1499 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1500 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1501 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1502 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1503 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1504 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1505 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1506 mtc=true;
yabahiro 31:45f97698857b 1507 break;
yabahiro 31:45f97698857b 1508 case 3:
yabahiro 31:45f97698857b 1509 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1510 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1511 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1512 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1513 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1514 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1515 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1516 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1517 mtc=true;
yabahiro 31:45f97698857b 1518 break;
yabahiro 31:45f97698857b 1519 case 2:
yabahiro 31:45f97698857b 1520 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1521 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1522 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1523 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1524 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1525 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1526 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1527 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1528 mtc=true;
yabahiro 31:45f97698857b 1529 break;
yabahiro 31:45f97698857b 1530 }
yabahiro 36:788274604650 1531 if(g[0]!=98&&mtc==true) {
yabahiro 39:f89166d7411c 1532 switch(g[0]) {
yabahiro 31:45f97698857b 1533 case 0:
yabahiro 31:45f97698857b 1534 mtc=false;
yabahiro 31:45f97698857b 1535 break;
yabahiro 31:45f97698857b 1536 case 255:
kishibekairohan 32:90f018b10ad6 1537 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1538 mtc=false;
yabahiro 31:45f97698857b 1539 break;
yabahiro 31:45f97698857b 1540 case 253:
kishibekairohan 32:90f018b10ad6 1541 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1542 mtc=false;
yabahiro 31:45f97698857b 1543 break;
yabahiro 31:45f97698857b 1544 case 254:
kishibekairohan 32:90f018b10ad6 1545 motor[TIRE_FL].pwm += 20;
kishibekairohan 32:90f018b10ad6 1546 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1547 mtc=false;
yabahiro 31:45f97698857b 1548 break;
yabahiro 31:45f97698857b 1549 case 1:
kishibekairohan 32:90f018b10ad6 1550 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1551 mtc=false;
yabahiro 31:45f97698857b 1552 break;
yabahiro 31:45f97698857b 1553 case 3:
kishibekairohan 32:90f018b10ad6 1554 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1555 mtc=false;
yabahiro 31:45f97698857b 1556 break;
yabahiro 31:45f97698857b 1557 case 2:
kishibekairohan 32:90f018b10ad6 1558 motor[TIRE_FR].pwm += 20;
kishibekairohan 32:90f018b10ad6 1559 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1560 mtc=false;
yabahiro 31:45f97698857b 1561 break;
yabahiro 29:c78a46f51209 1562 }
yabahiro 31:45f97698857b 1563 }
yabahiro 31:45f97698857b 1564 } else if(mode==11) {
yabahiro 36:788274604650 1565 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 1566 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1567 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1568 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1569 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1570 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1571 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 1572 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1573 if(g[2]==0) {
yabahiro 31:45f97698857b 1574 count=0;
yabahiro 36:788274604650 1575 cross=0;
yabahiro 31:45f97698857b 1576 mode=20;
yabahiro 39:f89166d7411c 1577 fast=40;
yabahiro 39:f89166d7411c 1578 normal=30;
yabahiro 39:f89166d7411c 1579 slow=20;
yabahiro 31:45f97698857b 1580 }
yabahiro 31:45f97698857b 1581 } else if(mode==20) {
yabahiro 36:788274604650 1582
yabahiro 38:391a0c8e4c16 1583 if(Twsh==2) {
yabahiro 38:391a0c8e4c16 1584 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 38:391a0c8e4c16 1585 }
yabahiro 38:391a0c8e4c16 1586
yabahiro 36:788274604650 1587 if(counts==false&&g[1]==0) {
yabahiro 36:788274604650 1588 cros++;
yabahiro 36:788274604650 1589 counts=true;
yabahiro 36:788274604650 1590 }
yabahiro 36:788274604650 1591 if(counts==true&&g[1]==99) {
yabahiro 36:788274604650 1592 counts=false;
yabahiro 36:788274604650 1593 }
yabahiro 36:788274604650 1594
yabahiro 36:788274604650 1595 if(cros==2) {
yabahiro 38:391a0c8e4c16 1596 //mode=21;
yabahiro 38:391a0c8e4c16 1597 //count=0;
yabahiro 38:391a0c8e4c16 1598 //cros=0;
yabahiro 36:788274604650 1599 }//ゆっくりモードに入れなかった時の保険
yabahiro 36:788274604650 1600
yabahiro 31:45f97698857b 1601 switch(g[2]) {
yabahiro 31:45f97698857b 1602 case 98:
kishibekairohan 33:656aa14904a5 1603 motor[TIRE_FR].pwm = normal;
kishibekairohan 22:c47f29caded6 1604 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1605 motor[TIRE_FL].pwm = normal;
yabahiro 23:694ecfa97839 1606 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1607 motor[TIRE_BR].pwm = normal;
yabahiro 23:694ecfa97839 1608 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1609 motor[TIRE_BL].pwm = normal;
yabahiro 29:c78a46f51209 1610 motor[TIRE_BL].dir = BACK;
yabahiro 36:788274604650 1611 if(count>80000) {
yabahiro 31:45f97698857b 1612 cross++;
yabahiro 31:45f97698857b 1613 count=0;
yabahiro 31:45f97698857b 1614 }
yabahiro 31:45f97698857b 1615 mtc2=true;
yabahiro 36:788274604650 1616 if(cross==2) {
yabahiro 31:45f97698857b 1617 mode=21;
yabahiro 31:45f97698857b 1618 count=0;
yabahiro 31:45f97698857b 1619 cross=0;
yabahiro 31:45f97698857b 1620 }
yabahiro 31:45f97698857b 1621 break;
yabahiro 31:45f97698857b 1622 case 0:
kishibekairohan 33:656aa14904a5 1623 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1624 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1625 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1626 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1627 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1628 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1629 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1630 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1631 mtc2=true;
yabahiro 31:45f97698857b 1632 break;
yabahiro 31:45f97698857b 1633 case 255:
yabahiro 36:788274604650 1634 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1635 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1636 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1637 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1638 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1639 motor[TIRE_BR].dir = BACK;
yabahiro 36:788274604650 1640 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1641 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1642 mtc2=true;
yabahiro 31:45f97698857b 1643 break;
yabahiro 31:45f97698857b 1644 case 253:
yabahiro 31:45f97698857b 1645 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1646 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1647 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1648 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1649 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1650 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 1651 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1652 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1653 mtc2=true;
yabahiro 31:45f97698857b 1654 break;
yabahiro 31:45f97698857b 1655 case 254:
yabahiro 31:45f97698857b 1656 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1657 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1658 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1659 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1660 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1661 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 1662 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1663 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1664 mtc2=true;
yabahiro 31:45f97698857b 1665 break;
yabahiro 31:45f97698857b 1666 case 1:
kishibekairohan 33:656aa14904a5 1667 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1668 motor[TIRE_FR].dir = FOR;
kishibekairohan 33:656aa14904a5 1669 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1670 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 1671 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 1672 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 1673 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1674 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1675 mtc2=true;
yabahiro 31:45f97698857b 1676 break;
yabahiro 31:45f97698857b 1677 case 3:
kishibekairohan 33:656aa14904a5 1678 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1679 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1680 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1681 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1682 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1683 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1684 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1685 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1686 mtc2=true;
yabahiro 31:45f97698857b 1687 break;
yabahiro 31:45f97698857b 1688 case 2:
kishibekairohan 33:656aa14904a5 1689 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1690 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1691 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1692 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1693 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1694 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 1695 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1696 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1697 mtc2=true;
yabahiro 31:45f97698857b 1698 break;
yabahiro 31:45f97698857b 1699 }
yabahiro 31:45f97698857b 1700 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 1701 switch(g[3]) {
yabahiro 23:694ecfa97839 1702 case 0:
yabahiro 31:45f97698857b 1703 mtc2=false;
yabahiro 23:694ecfa97839 1704 break;
yabahiro 23:694ecfa97839 1705 case 255:
yabahiro 31:45f97698857b 1706 motor[TIRE_BL].pwm += 5;
yabahiro 31:45f97698857b 1707 mtc2=false;
yabahiro 23:694ecfa97839 1708 break;
yabahiro 23:694ecfa97839 1709 case 253:
yabahiro 31:45f97698857b 1710 motor[TIRE_BL].pwm += 10;
yabahiro 31:45f97698857b 1711 mtc2=false;
yabahiro 23:694ecfa97839 1712 break;
yabahiro 23:694ecfa97839 1713 case 254:
yabahiro 31:45f97698857b 1714 motor[TIRE_BL].pwm += 20;
yabahiro 29:c78a46f51209 1715 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1716 mtc2=false;
yabahiro 23:694ecfa97839 1717 break;
yabahiro 23:694ecfa97839 1718 case 1:
yabahiro 31:45f97698857b 1719 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1720 mtc2=false;
yabahiro 23:694ecfa97839 1721 break;
yabahiro 23:694ecfa97839 1722 case 3:
yabahiro 31:45f97698857b 1723 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1724 mtc2=false;
yabahiro 23:694ecfa97839 1725 break;
yabahiro 23:694ecfa97839 1726 case 2:
yabahiro 31:45f97698857b 1727 motor[TIRE_FL].pwm += 20;
yabahiro 31:45f97698857b 1728 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1729 mtc2=false;
yabahiro 29:c78a46f51209 1730 break;
yabahiro 29:c78a46f51209 1731 }
yabahiro 31:45f97698857b 1732 }
yabahiro 31:45f97698857b 1733 } else if(mode==21) {
yabahiro 36:788274604650 1734 motor[TIRE_FR].pwm = normal;
kishibekairohan 32:90f018b10ad6 1735 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1736 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 1737 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 1738 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 1739 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1740 motor[TIRE_BL].pwm = normal;
kishibekairohan 32:90f018b10ad6 1741 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1742 if(g[1]==0) {
yabahiro 31:45f97698857b 1743 mode=30;
yabahiro 36:788274604650 1744 cros=0;
yabahiro 36:788274604650 1745 zyouge=true;
yabahiro 39:f89166d7411c 1746 fast=80;
yabahiro 39:f89166d7411c 1747 normal=60;
yabahiro 39:f89166d7411c 1748 slow=40;
yabahiro 31:45f97698857b 1749 }
yabahiro 31:45f97698857b 1750 } else if(mode==30) {
yabahiro 38:391a0c8e4c16 1751 motor[TIRE_FR].pwm = 60;
yabahiro 38:391a0c8e4c16 1752 motor[TIRE_FR].dir = FOR;
yabahiro 38:391a0c8e4c16 1753 motor[TIRE_FL].pwm = 60;
yabahiro 38:391a0c8e4c16 1754 motor[TIRE_FL].dir = BACK;
yabahiro 38:391a0c8e4c16 1755 motor[TIRE_BR].pwm = 60;
yabahiro 38:391a0c8e4c16 1756 motor[TIRE_BR].dir = FOR;
yabahiro 39:f89166d7411c 1757 motor[TIRE_BL].pwm = 50;
yabahiro 38:391a0c8e4c16 1758 motor[TIRE_BL].dir = BACK;
yabahiro 38:391a0c8e4c16 1759 if(g[1]==98) {
yabahiro 38:391a0c8e4c16 1760 mode=31;
yabahiro 38:391a0c8e4c16 1761 }
yabahiro 39:f89166d7411c 1762 if(g[3]==0) {
yabahiro 39:f89166d7411c 1763 mode=32;
yabahiro 36:788274604650 1764 count=0;
yabahiro 36:788274604650 1765 cros=0;
yabahiro 36:788274604650 1766 }//ゆっくりモードに入れなかった時の保険
yabahiro 38:391a0c8e4c16 1767 /*
kishibekairohan 32:90f018b10ad6 1768 switch(g[1]) {
yabahiro 31:45f97698857b 1769 case 98:
yabahiro 36:788274604650 1770 motor[TIRE_FR].pwm = 30;
yabahiro 31:45f97698857b 1771 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1772 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1773 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1774 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1775 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1776 motor[TIRE_BL].pwm = 30;
yabahiro 31:45f97698857b 1777 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1778 if(count>100000) {
yabahiro 31:45f97698857b 1779 cross++;
yabahiro 31:45f97698857b 1780 count=0;
yabahiro 31:45f97698857b 1781 }
yabahiro 31:45f97698857b 1782 mtc=true;
yabahiro 31:45f97698857b 1783 if(cross==1) {
yabahiro 36:788274604650 1784 if(Twsh==2) {
yabahiro 36:788274604650 1785 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 36:788274604650 1786 }
yabahiro 31:45f97698857b 1787 mode=31;
yabahiro 31:45f97698857b 1788 count=0;
yabahiro 29:c78a46f51209 1789 }
yabahiro 31:45f97698857b 1790 break;
yabahiro 31:45f97698857b 1791 case 0:
yabahiro 36:788274604650 1792 motor[TIRE_FR].pwm = 60;
yabahiro 31:45f97698857b 1793 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1794 motor[TIRE_FL].pwm = 60;
yabahiro 31:45f97698857b 1795 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1796 motor[TIRE_BR].pwm = 60;
yabahiro 31:45f97698857b 1797 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1798 motor[TIRE_BL].pwm = 60;
yabahiro 31:45f97698857b 1799 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1800 mtc=true;
yabahiro 31:45f97698857b 1801 break;
yabahiro 31:45f97698857b 1802 case 255:
yabahiro 36:788274604650 1803 motor[TIRE_FR].pwm = 60;
yabahiro 31:45f97698857b 1804 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1805 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1806 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1807 motor[TIRE_BR].pwm = 20;
yabahiro 31:45f97698857b 1808 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1809 motor[TIRE_BL].pwm = 60;
yabahiro 31:45f97698857b 1810 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1811 mtc=true;
yabahiro 31:45f97698857b 1812 break;
yabahiro 31:45f97698857b 1813 case 253:
yabahiro 36:788274604650 1814 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1815 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1816 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1817 motor[TIRE_FL].dir = BACK;
yabahiro 31:45f97698857b 1818 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1819 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1820 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1821 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1822 mtc=true;
yabahiro 31:45f97698857b 1823 break;
yabahiro 31:45f97698857b 1824 case 254:
yabahiro 36:788274604650 1825 motor[TIRE_FR].pwm = 30;
yabahiro 31:45f97698857b 1826 motor[TIRE_FR].dir = FOR;
yabahiro 31:45f97698857b 1827 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1828 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 1829 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 1830 motor[TIRE_BR].dir = FREE;
yabahiro 36:788274604650 1831 motor[TIRE_BL].pwm = 30;
yabahiro 31:45f97698857b 1832 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1833 mtc=true;
yabahiro 31:45f97698857b 1834 break;
yabahiro 31:45f97698857b 1835 case 1:
yabahiro 36:788274604650 1836 motor[TIRE_FR].pwm = 20;
yabahiro 31:45f97698857b 1837 motor[TIRE_FR].dir = FOR;
yabahiro 36:788274604650 1838 motor[TIRE_FL].pwm = 60;
yabahiro 31:45f97698857b 1839 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1840 motor[TIRE_BR].pwm = 60;
yabahiro 31:45f97698857b 1841 motor[TIRE_BR].dir = FOR;
yabahiro 36:788274604650 1842 motor[TIRE_BL].pwm = 20;
yabahiro 31:45f97698857b 1843 motor[TIRE_BL].dir = BACK;
yabahiro 31:45f97698857b 1844 mtc=true;
yabahiro 31:45f97698857b 1845 break;
yabahiro 31:45f97698857b 1846 case 3:
yabahiro 31:45f97698857b 1847 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1848 motor[TIRE_FR].dir = FREE;
yabahiro 36:788274604650 1849 motor[TIRE_FL].pwm = 20;
yabahiro 31:45f97698857b 1850 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1851 motor[TIRE_BR].pwm = 20;
yabahiro 31:45f97698857b 1852 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1853 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1854 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1855 mtc=true;
yabahiro 31:45f97698857b 1856 break;
yabahiro 31:45f97698857b 1857 case 2:
yabahiro 31:45f97698857b 1858 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1859 motor[TIRE_FR].dir = FREE;
yabahiro 36:788274604650 1860 motor[TIRE_FL].pwm = 30;
yabahiro 31:45f97698857b 1861 motor[TIRE_FL].dir = BACK;
yabahiro 36:788274604650 1862 motor[TIRE_BR].pwm = 30;
yabahiro 31:45f97698857b 1863 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 1864 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 1865 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 1866 mtc=true;
yabahiro 31:45f97698857b 1867 break;
yabahiro 31:45f97698857b 1868 }
kishibekairohan 32:90f018b10ad6 1869 if(g[1]!=98&&mtc==true) {
kishibekairohan 32:90f018b10ad6 1870 switch(g[0]) {
yabahiro 29:c78a46f51209 1871 case 0:
yabahiro 31:45f97698857b 1872 mtc=false;
yabahiro 29:c78a46f51209 1873 break;
yabahiro 29:c78a46f51209 1874 case 255:
kishibekairohan 34:cef6c124a52d 1875 motor[TIRE_FL].pwm += 5;
yabahiro 31:45f97698857b 1876 mtc=false;
yabahiro 29:c78a46f51209 1877 break;
yabahiro 29:c78a46f51209 1878 case 253:
kishibekairohan 34:cef6c124a52d 1879 motor[TIRE_FL].pwm += 10;
yabahiro 31:45f97698857b 1880 mtc=false;
yabahiro 29:c78a46f51209 1881 break;
yabahiro 29:c78a46f51209 1882 case 254:
kishibekairohan 34:cef6c124a52d 1883 motor[TIRE_FL].pwm += 20;
kishibekairohan 34:cef6c124a52d 1884 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1885 mtc=false;
yabahiro 29:c78a46f51209 1886 break;
yabahiro 29:c78a46f51209 1887 case 1:
kishibekairohan 34:cef6c124a52d 1888 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 1889 mtc=false;
yabahiro 29:c78a46f51209 1890 break;
yabahiro 29:c78a46f51209 1891 case 3:
kishibekairohan 34:cef6c124a52d 1892 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 1893 mtc=false;
yabahiro 29:c78a46f51209 1894 break;
yabahiro 29:c78a46f51209 1895 case 2:
kishibekairohan 34:cef6c124a52d 1896 motor[TIRE_FR].pwm += 20;
kishibekairohan 34:cef6c124a52d 1897 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 1898 mtc=false;
yabahiro 23:694ecfa97839 1899 break;
yabahiro 23:694ecfa97839 1900 }
yabahiro 31:45f97698857b 1901 }
yabahiro 38:391a0c8e4c16 1902 */
yabahiro 31:45f97698857b 1903 } else if(mode==31) {
yabahiro 39:f89166d7411c 1904 motor[TIRE_FR].pwm = 15;
yabahiro 39:f89166d7411c 1905 motor[TIRE_FR].dir = FOR;
yabahiro 39:f89166d7411c 1906 motor[TIRE_FL].pwm = 15;
kishibekairohan 32:90f018b10ad6 1907 motor[TIRE_FL].dir = BACK;
yabahiro 39:f89166d7411c 1908 motor[TIRE_BR].pwm = 15;
kishibekairohan 32:90f018b10ad6 1909 motor[TIRE_BR].dir = FOR;
yabahiro 39:f89166d7411c 1910 motor[TIRE_BL].pwm = 15;
yabahiro 39:f89166d7411c 1911 motor[TIRE_BL].dir = BACK;
yabahiro 35:94f026ab4d30 1912 if(g[3]==0) {
yabahiro 31:45f97698857b 1913 count=0;
yabahiro 39:f89166d7411c 1914 mode=32;
yabahiro 39:f89166d7411c 1915 }
yabahiro 39:f89166d7411c 1916 } else if(mode==32) {
yabahiro 39:f89166d7411c 1917 motor[TIRE_FR].pwm = 30;
yabahiro 39:f89166d7411c 1918 motor[TIRE_FR].dir = BACK;
yabahiro 39:f89166d7411c 1919 motor[TIRE_FL].pwm = 30;
yabahiro 39:f89166d7411c 1920 motor[TIRE_FL].dir = FOR;
yabahiro 39:f89166d7411c 1921 motor[TIRE_BR].pwm = 30;
yabahiro 39:f89166d7411c 1922 motor[TIRE_BR].dir = BACK;
yabahiro 39:f89166d7411c 1923 motor[TIRE_BL].pwm = 30;
yabahiro 39:f89166d7411c 1924 motor[TIRE_BL].dir = FOR;
yabahiro 39:f89166d7411c 1925 if(g[3]==0) {
yabahiro 39:f89166d7411c 1926 count=0;
yabahiro 39:f89166d7411c 1927 if(Twsh==2) {
yabahiro 39:f89166d7411c 1928 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 39:f89166d7411c 1929 }
yabahiro 31:45f97698857b 1930 mode=40;
yabahiro 31:45f97698857b 1931 }
yabahiro 31:45f97698857b 1932 } else if(mode==40) {
yabahiro 36:788274604650 1933
yabahiro 39:f89166d7411c 1934 if(LIF) {
yabahiro 38:391a0c8e4c16 1935 mode=42;
yabahiro 38:391a0c8e4c16 1936 }
yabahiro 38:391a0c8e4c16 1937
yabahiro 36:788274604650 1938 if(counts==false&&g[0]==0) {
yabahiro 36:788274604650 1939 cros++;
yabahiro 36:788274604650 1940 counts=true;
yabahiro 36:788274604650 1941 }
yabahiro 36:788274604650 1942 if(counts==true&&g[0]==99) {
yabahiro 36:788274604650 1943 counts=false;
yabahiro 36:788274604650 1944 }
yabahiro 36:788274604650 1945
yabahiro 40:7f93984e8ba1 1946 if(cros==1) {
yabahiro 36:788274604650 1947 // mode=41;
yabahiro 36:788274604650 1948 //count=0;
yabahiro 36:788274604650 1949 //cros=0;
yabahiro 36:788274604650 1950 }//ゆっくりモードに入れなかった時の保険
yabahiro 36:788274604650 1951
yabahiro 31:45f97698857b 1952 switch(g[3]) {
yabahiro 31:45f97698857b 1953 case 98:
kishibekairohan 33:656aa14904a5 1954 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 1955 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1956 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 1957 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1958 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 1959 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1960 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 1961 motor[TIRE_BL].dir = FOR;
yabahiro 36:788274604650 1962 if(count>20000) {
yabahiro 31:45f97698857b 1963 cross++;
yabahiro 31:45f97698857b 1964 count=0;
yabahiro 31:45f97698857b 1965 }
yabahiro 31:45f97698857b 1966 mtc2=true;
yabahiro 39:f89166d7411c 1967 if(cross==2) {
yabahiro 31:45f97698857b 1968 mode=41;
yabahiro 31:45f97698857b 1969 count=0;
yabahiro 31:45f97698857b 1970 cross=0;
yabahiro 24:41c23c9a5058 1971 }
yabahiro 31:45f97698857b 1972 break;
yabahiro 31:45f97698857b 1973 case 0:
kishibekairohan 33:656aa14904a5 1974 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 1975 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1976 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1977 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1978 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1979 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1980 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 1981 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1982 mtc2=true;
yabahiro 31:45f97698857b 1983 break;
yabahiro 31:45f97698857b 1984 case 255:
kishibekairohan 33:656aa14904a5 1985 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 1986 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 1987 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 1988 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 1989 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 1990 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 1991 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 1992 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 1993 mtc2=true;
yabahiro 31:45f97698857b 1994 break;
yabahiro 31:45f97698857b 1995 case 253:
yabahiro 31:45f97698857b 1996 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 1997 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 1998 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 1999 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2000 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2001 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 2002 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2003 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2004 mtc2=true;
yabahiro 31:45f97698857b 2005 break;
yabahiro 31:45f97698857b 2006 case 254:
yabahiro 31:45f97698857b 2007 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2008 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2009 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2010 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2011 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2012 motor[TIRE_BR].dir = FOR;
yabahiro 31:45f97698857b 2013 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2014 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2015 mtc2=true;
yabahiro 31:45f97698857b 2016 break;
yabahiro 31:45f97698857b 2017 case 1:
kishibekairohan 33:656aa14904a5 2018 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2019 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2020 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2021 motor[TIRE_FL].dir = BACK;
kishibekairohan 33:656aa14904a5 2022 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2023 motor[TIRE_BR].dir = FOR;
kishibekairohan 33:656aa14904a5 2024 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2025 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2026 mtc2=true;
yabahiro 31:45f97698857b 2027 break;
yabahiro 31:45f97698857b 2028 case 3:
kishibekairohan 33:656aa14904a5 2029 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2030 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2031 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2032 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2033 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2034 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2035 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2036 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2037 mtc2=true;
yabahiro 31:45f97698857b 2038 break;
yabahiro 31:45f97698857b 2039 case 2:
kishibekairohan 33:656aa14904a5 2040 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2041 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2042 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2043 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2044 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2045 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2046 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2047 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2048 mtc2=true;
yabahiro 31:45f97698857b 2049 break;
yabahiro 31:45f97698857b 2050 }
yabahiro 31:45f97698857b 2051 if(g[2]!=98&&mtc2==true) {
yabahiro 31:45f97698857b 2052 switch(g[2]) {
yabahiro 23:694ecfa97839 2053 case 0:
yabahiro 31:45f97698857b 2054 mtc2=false;
yabahiro 23:694ecfa97839 2055 break;
yabahiro 23:694ecfa97839 2056 case 255:
yabahiro 39:f89166d7411c 2057 motor[TIRE_FR].pwm += 5;
yabahiro 31:45f97698857b 2058 mtc2=false;
yabahiro 23:694ecfa97839 2059 break;
yabahiro 23:694ecfa97839 2060 case 253:
yabahiro 39:f89166d7411c 2061 motor[TIRE_FR].pwm += 10;
yabahiro 31:45f97698857b 2062 mtc2=false;
yabahiro 23:694ecfa97839 2063 break;
yabahiro 23:694ecfa97839 2064 case 254:
yabahiro 39:f89166d7411c 2065 motor[TIRE_FR].pwm += 20;
yabahiro 39:f89166d7411c 2066 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2067 mtc2=false;
yabahiro 23:694ecfa97839 2068 break;
yabahiro 23:694ecfa97839 2069 case 1:
yabahiro 39:f89166d7411c 2070 motor[TIRE_BR].pwm += 5;
yabahiro 31:45f97698857b 2071 mtc2=false;
yabahiro 23:694ecfa97839 2072 break;
yabahiro 23:694ecfa97839 2073 case 3:
yabahiro 39:f89166d7411c 2074 motor[TIRE_BR].pwm += 10;
yabahiro 31:45f97698857b 2075 mtc2=false;
yabahiro 23:694ecfa97839 2076 break;
yabahiro 23:694ecfa97839 2077 case 2:
yabahiro 39:f89166d7411c 2078 motor[TIRE_BR].pwm += 20;
yabahiro 39:f89166d7411c 2079 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2080 mtc2=false;
yabahiro 29:c78a46f51209 2081 break;
yabahiro 29:c78a46f51209 2082 }
yabahiro 31:45f97698857b 2083 }
yabahiro 31:45f97698857b 2084 } else if(mode==41) {
yabahiro 39:f89166d7411c 2085 motor[TIRE_FR].pwm = 30;
kishibekairohan 32:90f018b10ad6 2086 motor[TIRE_FR].dir = BACK;
yabahiro 36:788274604650 2087 motor[TIRE_FL].pwm = 0;
kishibekairohan 32:90f018b10ad6 2088 motor[TIRE_FL].dir = FREE;
yabahiro 36:788274604650 2089 motor[TIRE_BR].pwm = 0;
kishibekairohan 32:90f018b10ad6 2090 motor[TIRE_BR].dir = FREE;
yabahiro 39:f89166d7411c 2091 motor[TIRE_BL].pwm = 30;
kishibekairohan 32:90f018b10ad6 2092 motor[TIRE_BL].dir = FOR;
yabahiro 36:788274604650 2093 if(g[1]==0) {
yabahiro 31:45f97698857b 2094 count=0;
yabahiro 31:45f97698857b 2095 mode=70;
yabahiro 39:f89166d7411c 2096 normal=60;
yabahiro 39:f89166d7411c 2097 fast=80;
yabahiro 39:f89166d7411c 2098 slow=40;
yabahiro 31:45f97698857b 2099 }
yabahiro 36:788274604650 2100 } else if(mode==70)
kishibekairohan 32:90f018b10ad6 2101 switch(g[0]) {
yabahiro 31:45f97698857b 2102 case 99:
yabahiro 31:45f97698857b 2103 if(count>=100000) {
yabahiro 39:f89166d7411c 2104 cross++;
yabahiro 39:f89166d7411c 2105 mode=100;
yabahiro 39:f89166d7411c 2106 count=0;
yabahiro 39:f89166d7411c 2107 }
yabahiro 39:f89166d7411c 2108 if(cross==3) {
yabahiro 39:f89166d7411c 2109 cross=0;
yabahiro 31:45f97698857b 2110 mode=100;
yabahiro 28:9cd36721cf83 2111 }
yabahiro 31:45f97698857b 2112 break;
yabahiro 31:45f97698857b 2113 case 98:
kishibekairohan 33:656aa14904a5 2114 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2115 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2116 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2117 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2118 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2119 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2120 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2121 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2122 break;
yabahiro 31:45f97698857b 2123 case 0:
kishibekairohan 33:656aa14904a5 2124 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2125 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2126 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 2127 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2128 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 2129 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2130 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2131 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2132 mtc=true;
yabahiro 31:45f97698857b 2133 break;
yabahiro 31:45f97698857b 2134 case 255:
kishibekairohan 33:656aa14904a5 2135 motor[TIRE_FR].pwm = fast;
yabahiro 31:45f97698857b 2136 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2137 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2138 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2139 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2140 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2141 motor[TIRE_BL].pwm = fast;
yabahiro 31:45f97698857b 2142 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2143 mtc=true;
yabahiro 31:45f97698857b 2144 break;
yabahiro 31:45f97698857b 2145 case 253:
kishibekairohan 33:656aa14904a5 2146 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2147 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2148 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2149 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2150 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2151 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2152 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2153 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2154 mtc=true;
yabahiro 31:45f97698857b 2155 break;
yabahiro 31:45f97698857b 2156 case 254:
kishibekairohan 33:656aa14904a5 2157 motor[TIRE_FR].pwm = normal;
yabahiro 31:45f97698857b 2158 motor[TIRE_FR].dir = BACK;
yabahiro 31:45f97698857b 2159 motor[TIRE_FL].pwm = 0;
yabahiro 31:45f97698857b 2160 motor[TIRE_FL].dir = FREE;
yabahiro 31:45f97698857b 2161 motor[TIRE_BR].pwm = 0;
yabahiro 31:45f97698857b 2162 motor[TIRE_BR].dir = FREE;
kishibekairohan 33:656aa14904a5 2163 motor[TIRE_BL].pwm = normal;
yabahiro 31:45f97698857b 2164 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2165 mtc=true;
yabahiro 31:45f97698857b 2166 break;
yabahiro 31:45f97698857b 2167 case 1:
kishibekairohan 33:656aa14904a5 2168 motor[TIRE_FR].pwm = slow;
yabahiro 31:45f97698857b 2169 motor[TIRE_FR].dir = BACK;
kishibekairohan 33:656aa14904a5 2170 motor[TIRE_FL].pwm = fast;
yabahiro 31:45f97698857b 2171 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2172 motor[TIRE_BR].pwm = fast;
yabahiro 31:45f97698857b 2173 motor[TIRE_BR].dir = BACK;
kishibekairohan 33:656aa14904a5 2174 motor[TIRE_BL].pwm = slow;
yabahiro 31:45f97698857b 2175 motor[TIRE_BL].dir = FOR;
yabahiro 31:45f97698857b 2176 mtc=true;
yabahiro 31:45f97698857b 2177 break;
yabahiro 31:45f97698857b 2178 case 3:
yabahiro 31:45f97698857b 2179 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2180 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2181 motor[TIRE_FL].pwm = slow;
yabahiro 31:45f97698857b 2182 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2183 motor[TIRE_BR].pwm = slow;
yabahiro 31:45f97698857b 2184 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 2185 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2186 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2187 mtc=true;
yabahiro 31:45f97698857b 2188 break;
yabahiro 31:45f97698857b 2189 case 2:
yabahiro 31:45f97698857b 2190 motor[TIRE_FR].pwm = 0;
yabahiro 31:45f97698857b 2191 motor[TIRE_FR].dir = FREE;
kishibekairohan 33:656aa14904a5 2192 motor[TIRE_FL].pwm = normal;
yabahiro 31:45f97698857b 2193 motor[TIRE_FL].dir = FOR;
kishibekairohan 33:656aa14904a5 2194 motor[TIRE_BR].pwm = normal;
yabahiro 31:45f97698857b 2195 motor[TIRE_BR].dir = BACK;
yabahiro 31:45f97698857b 2196 motor[TIRE_BL].pwm = 0;
yabahiro 31:45f97698857b 2197 motor[TIRE_BL].dir = FREE;
yabahiro 31:45f97698857b 2198 mtc=true;
yabahiro 31:45f97698857b 2199 break;
yabahiro 31:45f97698857b 2200 }
yabahiro 36:788274604650 2201 if(g[1]!=98&&mtc==true) {
yabahiro 39:f89166d7411c 2202 switch(g[1]) {
yabahiro 36:788274604650 2203 case 0:
yabahiro 36:788274604650 2204 mtc=false;
yabahiro 36:788274604650 2205 break;
yabahiro 36:788274604650 2206 case 255:
yabahiro 36:788274604650 2207 motor[TIRE_BR].pwm += 5;
yabahiro 36:788274604650 2208 mtc=false;
yabahiro 36:788274604650 2209 break;
yabahiro 36:788274604650 2210 case 253:
yabahiro 36:788274604650 2211 motor[TIRE_BR].pwm += 10;
yabahiro 36:788274604650 2212 mtc=false;
yabahiro 36:788274604650 2213 break;
yabahiro 36:788274604650 2214 case 254:
yabahiro 36:788274604650 2215 motor[TIRE_BR].pwm += 20;
yabahiro 36:788274604650 2216 motor[TIRE_BL].pwm = 0;
yabahiro 36:788274604650 2217 mtc=false;
yabahiro 36:788274604650 2218 break;
yabahiro 36:788274604650 2219 case 1:
yabahiro 36:788274604650 2220 motor[TIRE_BL].pwm += 5;
yabahiro 36:788274604650 2221 mtc=false;
yabahiro 36:788274604650 2222 break;
yabahiro 36:788274604650 2223 case 3:
yabahiro 36:788274604650 2224 motor[TIRE_BL].pwm += 10;
yabahiro 36:788274604650 2225 mtc=false;
yabahiro 36:788274604650 2226 break;
yabahiro 36:788274604650 2227 case 2:
yabahiro 36:788274604650 2228 motor[TIRE_BL].pwm += 20;
yabahiro 36:788274604650 2229 motor[TIRE_BR].pwm = 0;
yabahiro 36:788274604650 2230 mtc=false;
yabahiro 36:788274604650 2231 break;
yabahiro 24:41c23c9a5058 2232 }
yabahiro 31:45f97698857b 2233 } else if(mode==100) {
yabahiro 36:788274604650 2234 motor[TIRE_FR].pwm = 20;
kishibekairohan 32:90f018b10ad6 2235 motor[TIRE_FR].dir = BACK;
yabahiro 36:788274604650 2236 motor[TIRE_FL].pwm = 20;
kishibekairohan 32:90f018b10ad6 2237 motor[TIRE_FL].dir = FOR;
yabahiro 31:45f97698857b 2238 motor[TIRE_BR].pwm = 23;
kishibekairohan 32:90f018b10ad6 2239 motor[TIRE_BR].dir = BACK;
yabahiro 36:788274604650 2240 motor[TIRE_BL].pwm = 20;
kishibekairohan 32:90f018b10ad6 2241 motor[TIRE_BL].dir = FOR;
yabahiro 39:f89166d7411c 2242 if(StertSW) {
yabahiro 39:f89166d7411c 2243 SW_flag = true;
yabahiro 39:f89166d7411c 2244 }
yabahiro 40:7f93984e8ba1 2245 if(SW_flag ==true) {
yabahiro 39:f89166d7411c 2246 motor[TIRE_FR].pwm = 100;
yabahiro 40:7f93984e8ba1 2247 motor[TIRE_FR].dir = BRAKE;
yabahiro 40:7f93984e8ba1 2248 motor[TIRE_FL].pwm = 100;
yabahiro 40:7f93984e8ba1 2249 motor[TIRE_FL].dir = BRAKE;
yabahiro 40:7f93984e8ba1 2250 motor[TIRE_BR].pwm = 100;
yabahiro 40:7f93984e8ba1 2251 motor[TIRE_BR].dir = BRAKE;
yabahiro 40:7f93984e8ba1 2252 motor[TIRE_BL].pwm = 100;
yabahiro 40:7f93984e8ba1 2253 motor[TIRE_BL].dir = BRAKE;
yabahiro 40:7f93984e8ba1 2254 fast=60;
yabahiro 40:7f93984e8ba1 2255 normal=40;
yabahiro 40:7f93984e8ba1 2256 slow=20;
yabahiro 40:7f93984e8ba1 2257 mode =1;
yabahiro 40:7f93984e8ba1 2258 cross=0;//十字数える用
yabahiro 40:7f93984e8ba1 2259 cros=0;
yabahiro 40:7f93984e8ba1 2260 count=100000;//wait代替え
yabahiro 40:7f93984e8ba1 2261 UP_flag = false;
yabahiro 39:f89166d7411c 2262 SW_flag = false;
yabahiro 40:7f93984e8ba1 2263 Air_flag = false;
yabahiro 40:7f93984e8ba1 2264 zyouge=false;
yabahiro 40:7f93984e8ba1 2265 zyougedo=false;
yabahiro 40:7f93984e8ba1 2266 dz1=true;
yabahiro 40:7f93984e8ba1 2267 dz1i=false;
yabahiro 40:7f93984e8ba1 2268 dz2=true;
yabahiro 40:7f93984e8ba1 2269 dz2i=false;
yabahiro 40:7f93984e8ba1 2270 dz3=true;
yabahiro 40:7f93984e8ba1 2271 dz3i=false;
yabahiro 40:7f93984e8ba1 2272 dz4=true;
yabahiro 40:7f93984e8ba1 2273 dz4i=false;
yabahiro 40:7f93984e8ba1 2274 through=false;
yabahiro 40:7f93984e8ba1 2275 counts=false;
yabahiro 40:7f93984e8ba1 2276 mtc=false;
yabahiro 40:7f93984e8ba1 2277 mtc2 = false;
yabahiro 40:7f93984e8ba1 2278 current = 0;
yabahiro 40:7f93984e8ba1 2279 if(StertSW==false) {
yabahiro 40:7f93984e8ba1 2280 SW_flag = false;
yabahiro 40:7f93984e8ba1 2281 }
yabahiro 39:f89166d7411c 2282 }
yabahiro 21:1f1e9c585da8 2283 }
yabahiro 40:7f93984e8ba1 2284
yabahiro 40:7f93984e8ba1 2285
kishibekairohan 13:b6e02d6261d7 2286 }
kishibekairohan 13:b6e02d6261d7 2287 #endif
kishibekairohan 13:b6e02d6261d7 2288
kishibekairohan 13:b6e02d6261d7 2289 #if USE_PROCESS_NUM>5
yabahiro 21:1f1e9c585da8 2290 static void Process5()
yabahiro 21:1f1e9c585da8 2291 {
yabahiro 42:4f9689c0a349 2292 LedMode(4);
yabahiro 40:7f93984e8ba1 2293 count++;
yabahiro 40:7f93984e8ba1 2294 if(mode==1) {//スタートゾーンから白線検知
yabahiro 41:e1dd6d97f17d 2295 fast=50;
yabahiro 40:7f93984e8ba1 2296 normal=30;
yabahiro 40:7f93984e8ba1 2297 slow=20;
yabahiro 41:e1dd6d97f17d 2298 motor[TIRE_FR].pwm = 35;
yabahiro 40:7f93984e8ba1 2299 motor[TIRE_FR].dir = FOR;
yabahiro 41:e1dd6d97f17d 2300 motor[TIRE_FL].pwm = 35;
yabahiro 40:7f93984e8ba1 2301 motor[TIRE_FL].dir = BACK;
yabahiro 41:e1dd6d97f17d 2302 motor[TIRE_BR].pwm = 35;
yabahiro 40:7f93984e8ba1 2303 motor[TIRE_BR].dir = FOR;
yabahiro 41:e1dd6d97f17d 2304 motor[TIRE_BL].pwm = 32;
yabahiro 40:7f93984e8ba1 2305 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2306 if(g[2]==0) {
yabahiro 40:7f93984e8ba1 2307 count=100000;
yabahiro 40:7f93984e8ba1 2308 cross=0;
yabahiro 40:7f93984e8ba1 2309 mode=3;
yabahiro 40:7f93984e8ba1 2310 }
yabahiro 40:7f93984e8ba1 2311 } else if(mode==3) { //横ライントレースから縦ライントレースへ
yabahiro 41:e1dd6d97f17d 2312 motor[TIRE_FR].pwm = 27;
yabahiro 40:7f93984e8ba1 2313 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2314 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2315 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2316 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2317 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2318 motor[TIRE_BL].pwm = 25;
yabahiro 40:7f93984e8ba1 2319 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2320 if(g[0]==0) {
yabahiro 40:7f93984e8ba1 2321 mode=10;
yabahiro 40:7f93984e8ba1 2322 count=100000;
yabahiro 40:7f93984e8ba1 2323 }
yabahiro 40:7f93984e8ba1 2324 } else if(mode==10) { //縦ライントレース
yabahiro 40:7f93984e8ba1 2325
yabahiro 40:7f93984e8ba1 2326 if(counts==false&&g[2]==0) {
yabahiro 40:7f93984e8ba1 2327 cros++;
yabahiro 40:7f93984e8ba1 2328 counts=true;
yabahiro 40:7f93984e8ba1 2329 }
yabahiro 40:7f93984e8ba1 2330 if(counts==true&&g[2]==99) {
yabahiro 40:7f93984e8ba1 2331 counts=false;
yabahiro 40:7f93984e8ba1 2332 }
yabahiro 40:7f93984e8ba1 2333
yabahiro 40:7f93984e8ba1 2334 if(cros==Twsh) {
yabahiro 40:7f93984e8ba1 2335 mode=11;
yabahiro 40:7f93984e8ba1 2336 count=0;
yabahiro 40:7f93984e8ba1 2337 cros=0;
yabahiro 40:7f93984e8ba1 2338 }//ゆっくりモードに入れなかった時の保険
yabahiro 40:7f93984e8ba1 2339 switch(g[1]) {
yabahiro 40:7f93984e8ba1 2340 case 98:
yabahiro 40:7f93984e8ba1 2341 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2342 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2343 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2344 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2345 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2346 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2347 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2348 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2349 mtc=true;
yabahiro 40:7f93984e8ba1 2350 if(count>80000) {
yabahiro 40:7f93984e8ba1 2351 cross++;
yabahiro 40:7f93984e8ba1 2352 count=0;
yabahiro 40:7f93984e8ba1 2353 }
yabahiro 40:7f93984e8ba1 2354 if(cross==Twsh) {
yabahiro 40:7f93984e8ba1 2355 mode=11;
yabahiro 40:7f93984e8ba1 2356 count=0;
yabahiro 40:7f93984e8ba1 2357 cross=0;
yabahiro 40:7f93984e8ba1 2358 }
yabahiro 40:7f93984e8ba1 2359 break;
yabahiro 40:7f93984e8ba1 2360 case 0:
yabahiro 40:7f93984e8ba1 2361 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2362 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2363 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2364 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2365 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2366 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2367 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2368 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2369 mtc=true;
yabahiro 40:7f93984e8ba1 2370 break;
yabahiro 40:7f93984e8ba1 2371 case 255:
yabahiro 40:7f93984e8ba1 2372 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2373 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2374 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2375 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2376 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2377 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2378 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2379 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2380 mtc=true;
yabahiro 40:7f93984e8ba1 2381 break;
yabahiro 40:7f93984e8ba1 2382 case 253:
yabahiro 40:7f93984e8ba1 2383 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2384 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2385 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2386 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2387 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2388 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2389 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2390 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2391 mtc=true;
yabahiro 40:7f93984e8ba1 2392 break;
yabahiro 40:7f93984e8ba1 2393 case 254:
yabahiro 40:7f93984e8ba1 2394 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2395 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2396 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2397 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2398 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2399 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2400 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2401 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2402 mtc=true;
yabahiro 40:7f93984e8ba1 2403 break;
yabahiro 40:7f93984e8ba1 2404 case 1:
yabahiro 40:7f93984e8ba1 2405 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2406 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2407 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2408 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2409 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2410 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2411 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2412 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2413 mtc=true;
yabahiro 40:7f93984e8ba1 2414 break;
yabahiro 40:7f93984e8ba1 2415 case 3:
yabahiro 40:7f93984e8ba1 2416 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2417 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2418 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2419 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2420 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2421 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2422 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2423 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2424 mtc=true;
yabahiro 40:7f93984e8ba1 2425 break;
yabahiro 40:7f93984e8ba1 2426 case 2:
yabahiro 40:7f93984e8ba1 2427 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2428 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2429 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2430 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2431 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2432 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2433 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2434 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2435 mtc=true;
yabahiro 40:7f93984e8ba1 2436 break;
yabahiro 40:7f93984e8ba1 2437 }
yabahiro 40:7f93984e8ba1 2438 if(g[0]!=98&&mtc==true) {
yabahiro 40:7f93984e8ba1 2439 switch(g[0]) {
yabahiro 40:7f93984e8ba1 2440 case 0:
yabahiro 40:7f93984e8ba1 2441 mtc=false;
yabahiro 40:7f93984e8ba1 2442 break;
yabahiro 40:7f93984e8ba1 2443 case 255:
yabahiro 40:7f93984e8ba1 2444 motor[TIRE_FL].pwm += 5;
yabahiro 40:7f93984e8ba1 2445 mtc=false;
yabahiro 40:7f93984e8ba1 2446 break;
yabahiro 40:7f93984e8ba1 2447 case 253:
yabahiro 40:7f93984e8ba1 2448 motor[TIRE_FL].pwm += 10;
yabahiro 40:7f93984e8ba1 2449 mtc=false;
yabahiro 40:7f93984e8ba1 2450 break;
yabahiro 40:7f93984e8ba1 2451 case 254:
yabahiro 40:7f93984e8ba1 2452 motor[TIRE_FL].pwm += 20;
yabahiro 40:7f93984e8ba1 2453 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2454 mtc=false;
yabahiro 40:7f93984e8ba1 2455 break;
yabahiro 40:7f93984e8ba1 2456 case 1:
yabahiro 40:7f93984e8ba1 2457 motor[TIRE_FR].pwm += 5;
yabahiro 40:7f93984e8ba1 2458 mtc=false;
yabahiro 40:7f93984e8ba1 2459 break;
yabahiro 40:7f93984e8ba1 2460 case 3:
yabahiro 40:7f93984e8ba1 2461 motor[TIRE_FR].pwm += 10;
yabahiro 40:7f93984e8ba1 2462 mtc=false;
yabahiro 40:7f93984e8ba1 2463 break;
yabahiro 40:7f93984e8ba1 2464 case 2:
yabahiro 40:7f93984e8ba1 2465 motor[TIRE_FR].pwm += 20;
yabahiro 40:7f93984e8ba1 2466 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2467 mtc=false;
yabahiro 40:7f93984e8ba1 2468 break;
yabahiro 40:7f93984e8ba1 2469 }
yabahiro 40:7f93984e8ba1 2470 }
yabahiro 40:7f93984e8ba1 2471 } else if(mode==11) {
yabahiro 40:7f93984e8ba1 2472 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2473 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2474 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2475 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2476 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2477 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2478 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2479 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2480 if(g[2]==0) {
yabahiro 40:7f93984e8ba1 2481 count=0;
yabahiro 40:7f93984e8ba1 2482 cross=0;
yabahiro 40:7f93984e8ba1 2483 mode=20;
yabahiro 40:7f93984e8ba1 2484 }
yabahiro 40:7f93984e8ba1 2485 } else if(mode==20) {
yabahiro 40:7f93984e8ba1 2486
yabahiro 40:7f93984e8ba1 2487 if(Twsh==2) {
yabahiro 40:7f93984e8ba1 2488 solenoid.solenoid2 = SOLENOID_ON;
yabahiro 40:7f93984e8ba1 2489 }
yabahiro 40:7f93984e8ba1 2490
yabahiro 40:7f93984e8ba1 2491 if(counts==false&&g[1]==0) {
yabahiro 40:7f93984e8ba1 2492 cros++;
yabahiro 40:7f93984e8ba1 2493 counts=true;
yabahiro 40:7f93984e8ba1 2494 }
yabahiro 40:7f93984e8ba1 2495 if(counts==true&&g[1]==99) {
yabahiro 40:7f93984e8ba1 2496 counts=false;
yabahiro 40:7f93984e8ba1 2497 }
yabahiro 40:7f93984e8ba1 2498
yabahiro 40:7f93984e8ba1 2499 if(cros==2) {
yabahiro 40:7f93984e8ba1 2500 //mode=21;
yabahiro 40:7f93984e8ba1 2501 //count=0;
yabahiro 40:7f93984e8ba1 2502 //cros=0;
yabahiro 40:7f93984e8ba1 2503 }//ゆっくりモードに入れなかった時の保険
yabahiro 40:7f93984e8ba1 2504
yabahiro 40:7f93984e8ba1 2505 switch(g[2]) {
yabahiro 40:7f93984e8ba1 2506 case 98:
yabahiro 40:7f93984e8ba1 2507 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2508 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2509 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2510 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2511 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2512 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2513 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2514 motor[TIRE_BL].dir = BACK;
yabahiro 41:e1dd6d97f17d 2515 if(count>50000) {
yabahiro 40:7f93984e8ba1 2516 cross++;
yabahiro 40:7f93984e8ba1 2517 count=0;
yabahiro 40:7f93984e8ba1 2518 }
yabahiro 40:7f93984e8ba1 2519 mtc2=true;
yabahiro 40:7f93984e8ba1 2520 if(cross==1) {
yabahiro 40:7f93984e8ba1 2521 fast=30;
yabahiro 40:7f93984e8ba1 2522 normal=20;
yabahiro 40:7f93984e8ba1 2523 slow=20;
yabahiro 40:7f93984e8ba1 2524 }
yabahiro 40:7f93984e8ba1 2525 if(cross==2) {
yabahiro 40:7f93984e8ba1 2526 mode=21;
yabahiro 40:7f93984e8ba1 2527 cross=0;
yabahiro 41:e1dd6d97f17d 2528 count=0;
yabahiro 40:7f93984e8ba1 2529 }
yabahiro 40:7f93984e8ba1 2530 break;
yabahiro 40:7f93984e8ba1 2531 case 0:
yabahiro 40:7f93984e8ba1 2532 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2533 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2534 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2535 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2536 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2537 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2538 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2539 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2540 mtc2=true;
yabahiro 40:7f93984e8ba1 2541 break;
yabahiro 40:7f93984e8ba1 2542 case 255:
yabahiro 40:7f93984e8ba1 2543 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2544 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2545 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2546 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2547 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2548 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2549 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2550 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2551 mtc2=true;
yabahiro 40:7f93984e8ba1 2552 break;
yabahiro 40:7f93984e8ba1 2553 case 253:
yabahiro 40:7f93984e8ba1 2554 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2555 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2556 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2557 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2558 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2559 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2560 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2561 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2562 mtc2=true;
yabahiro 40:7f93984e8ba1 2563 break;
yabahiro 40:7f93984e8ba1 2564 case 254:
yabahiro 40:7f93984e8ba1 2565 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2566 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2567 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2568 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2569 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2570 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2571 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2572 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2573 mtc2=true;
yabahiro 40:7f93984e8ba1 2574 break;
yabahiro 40:7f93984e8ba1 2575 case 1:
yabahiro 40:7f93984e8ba1 2576 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2577 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2578 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2579 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2580 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2581 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2582 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2583 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2584 mtc2=true;
yabahiro 40:7f93984e8ba1 2585 break;
yabahiro 40:7f93984e8ba1 2586 case 3:
yabahiro 40:7f93984e8ba1 2587 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2588 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2589 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2590 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2591 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2592 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2593 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2594 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2595 mtc2=true;
yabahiro 40:7f93984e8ba1 2596 break;
yabahiro 40:7f93984e8ba1 2597 case 2:
yabahiro 40:7f93984e8ba1 2598 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2599 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2600 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2601 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2602 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2603 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2604 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2605 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2606 mtc2=true;
yabahiro 40:7f93984e8ba1 2607 break;
yabahiro 40:7f93984e8ba1 2608 }
yabahiro 40:7f93984e8ba1 2609 if(g[2]!=98&&mtc2==true) {
yabahiro 40:7f93984e8ba1 2610 switch(g[3]) {
yabahiro 40:7f93984e8ba1 2611 case 0:
yabahiro 40:7f93984e8ba1 2612 mtc2=false;
yabahiro 40:7f93984e8ba1 2613 break;
yabahiro 40:7f93984e8ba1 2614 case 255:
yabahiro 40:7f93984e8ba1 2615 motor[TIRE_BL].pwm += 5;
yabahiro 40:7f93984e8ba1 2616 mtc2=false;
yabahiro 40:7f93984e8ba1 2617 break;
yabahiro 40:7f93984e8ba1 2618 case 253:
yabahiro 40:7f93984e8ba1 2619 motor[TIRE_BL].pwm += 10;
yabahiro 40:7f93984e8ba1 2620 mtc2=false;
yabahiro 40:7f93984e8ba1 2621 break;
yabahiro 40:7f93984e8ba1 2622 case 254:
yabahiro 40:7f93984e8ba1 2623 motor[TIRE_BL].pwm += 20;
yabahiro 40:7f93984e8ba1 2624 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2625 mtc2=false;
yabahiro 40:7f93984e8ba1 2626 break;
yabahiro 40:7f93984e8ba1 2627 case 1:
yabahiro 40:7f93984e8ba1 2628 motor[TIRE_FL].pwm += 5;
yabahiro 40:7f93984e8ba1 2629 mtc2=false;
yabahiro 40:7f93984e8ba1 2630 break;
yabahiro 40:7f93984e8ba1 2631 case 3:
yabahiro 40:7f93984e8ba1 2632 motor[TIRE_FL].pwm += 10;
yabahiro 40:7f93984e8ba1 2633 mtc2=false;
yabahiro 40:7f93984e8ba1 2634 break;
yabahiro 40:7f93984e8ba1 2635 case 2:
yabahiro 40:7f93984e8ba1 2636 motor[TIRE_FL].pwm += 20;
yabahiro 40:7f93984e8ba1 2637 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2638 mtc2=false;
yabahiro 40:7f93984e8ba1 2639 break;
yabahiro 40:7f93984e8ba1 2640 }
yabahiro 40:7f93984e8ba1 2641 }
yabahiro 42:4f9689c0a349 2642
yabahiro 40:7f93984e8ba1 2643 } else if(mode==21) {
yabahiro 40:7f93984e8ba1 2644 motor[TIRE_FR].pwm = 20;
yabahiro 41:e1dd6d97f17d 2645 motor[TIRE_FR].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 2646 motor[TIRE_FL].pwm = 20;
yabahiro 41:e1dd6d97f17d 2647 motor[TIRE_FL].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 2648 motor[TIRE_BR].pwm = 20;
yabahiro 41:e1dd6d97f17d 2649 motor[TIRE_BR].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 2650 motor[TIRE_BL].pwm = 20;
yabahiro 41:e1dd6d97f17d 2651 motor[TIRE_BL].dir = BRAKE;
yabahiro 41:e1dd6d97f17d 2652 if(count<=100000) {
yabahiro 41:e1dd6d97f17d 2653 mode=22;
yabahiro 41:e1dd6d97f17d 2654 }
yabahiro 42:4f9689c0a349 2655 }
yabahiro 42:4f9689c0a349 2656 if(YELLOW) {
yabahiro 42:4f9689c0a349 2657 if(mode==22) {
yabahiro 42:4f9689c0a349 2658 motor[TIRE_FR].pwm = 20;
yabahiro 42:4f9689c0a349 2659 motor[TIRE_FR].dir = BACK;
yabahiro 42:4f9689c0a349 2660 motor[TIRE_FL].pwm = 20;
yabahiro 42:4f9689c0a349 2661 motor[TIRE_FL].dir = BACK;
yabahiro 42:4f9689c0a349 2662 motor[TIRE_BR].pwm = 20;
yabahiro 42:4f9689c0a349 2663 motor[TIRE_BR].dir = FOR;
yabahiro 42:4f9689c0a349 2664 motor[TIRE_BL].pwm = 20;
yabahiro 42:4f9689c0a349 2665 motor[TIRE_BL].dir = FOR;
yabahiro 42:4f9689c0a349 2666 if(g[2]!=98) {
yabahiro 42:4f9689c0a349 2667 mode=30;
yabahiro 42:4f9689c0a349 2668 }
yabahiro 40:7f93984e8ba1 2669 }
yabahiro 42:4f9689c0a349 2670
yabahiro 42:4f9689c0a349 2671 } else {
yabahiro 42:4f9689c0a349 2672 if(mode==22) {
yabahiro 42:4f9689c0a349 2673 motor[TIRE_FR].pwm = normal;
yabahiro 42:4f9689c0a349 2674 motor[TIRE_FR].dir = FOR;
yabahiro 42:4f9689c0a349 2675 motor[TIRE_FL].pwm = 0;
yabahiro 42:4f9689c0a349 2676 motor[TIRE_FL].dir = FREE;
yabahiro 42:4f9689c0a349 2677 motor[TIRE_BR].pwm = 0;
yabahiro 42:4f9689c0a349 2678 motor[TIRE_BR].dir = FREE;
yabahiro 42:4f9689c0a349 2679 motor[TIRE_BL].pwm = normal;
yabahiro 42:4f9689c0a349 2680 motor[TIRE_BL].dir = BACK;
yabahiro 42:4f9689c0a349 2681 if(g[1]==0) {
yabahiro 42:4f9689c0a349 2682 count=0;
yabahiro 42:4f9689c0a349 2683 cross=0;
yabahiro 42:4f9689c0a349 2684 mode=30;
yabahiro 42:4f9689c0a349 2685 }
yabahiro 42:4f9689c0a349 2686 }
yabahiro 42:4f9689c0a349 2687 }
yabahiro 42:4f9689c0a349 2688 if(mode==30) {
yabahiro 41:e1dd6d97f17d 2689 motor[TIRE_FR].pwm = 64;
yabahiro 40:7f93984e8ba1 2690 motor[TIRE_FR].dir = FOR;
yabahiro 40:7f93984e8ba1 2691 motor[TIRE_FL].pwm = 60;
yabahiro 40:7f93984e8ba1 2692 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2693 motor[TIRE_BR].pwm = 60;
yabahiro 40:7f93984e8ba1 2694 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2695 motor[TIRE_BL].pwm = 60;
yabahiro 40:7f93984e8ba1 2696 motor[TIRE_BL].dir = BACK;
yabahiro 40:7f93984e8ba1 2697 if(g[1]==98) {
yabahiro 40:7f93984e8ba1 2698 mode=31;
yabahiro 41:e1dd6d97f17d 2699 count=0;
yabahiro 40:7f93984e8ba1 2700 }
yabahiro 40:7f93984e8ba1 2701 } else if(mode==31) {
yabahiro 42:4f9689c0a349 2702 motor[TIRE_FR].pwm = 0;
yabahiro 41:e1dd6d97f17d 2703 motor[TIRE_FR].dir = FREE;
yabahiro 42:4f9689c0a349 2704 motor[TIRE_FL].pwm = 0;
yabahiro 41:e1dd6d97f17d 2705 motor[TIRE_FL].dir = FREE;
yabahiro 42:4f9689c0a349 2706 motor[TIRE_BR].pwm = 0;
yabahiro 41:e1dd6d97f17d 2707 motor[TIRE_BR].dir = FREE;
yabahiro 42:4f9689c0a349 2708 motor[TIRE_BL].pwm = 0;
yabahiro 41:e1dd6d97f17d 2709 motor[TIRE_BL].dir = FREE;
yabahiro 42:4f9689c0a349 2710 if(count>=5000) {
yabahiro 41:e1dd6d97f17d 2711 mode=32;
yabahiro 40:7f93984e8ba1 2712 count=0;
yabahiro 40:7f93984e8ba1 2713 }
yabahiro 40:7f93984e8ba1 2714 } else if(mode==32) {
yabahiro 42:4f9689c0a349 2715 motor[TIRE_FR].pwm = 10;
yabahiro 41:e1dd6d97f17d 2716 motor[TIRE_FR].dir = BRAKE;
yabahiro 42:4f9689c0a349 2717 motor[TIRE_FL].pwm = 10;
yabahiro 41:e1dd6d97f17d 2718 motor[TIRE_FL].dir = BRAKE;
yabahiro 42:4f9689c0a349 2719 motor[TIRE_BR].pwm = 10;
yabahiro 41:e1dd6d97f17d 2720 motor[TIRE_BR].dir = BRAKE;
yabahiro 42:4f9689c0a349 2721 motor[TIRE_BL].pwm = 10;
yabahiro 41:e1dd6d97f17d 2722 motor[TIRE_BL].dir = BRAKE;
yabahiro 42:4f9689c0a349 2723 if(count>=30000) {
yabahiro 41:e1dd6d97f17d 2724 mode=33;
yabahiro 41:e1dd6d97f17d 2725 count=0;
yabahiro 41:e1dd6d97f17d 2726 }
yabahiro 41:e1dd6d97f17d 2727 } else if(mode==33) {
yabahiro 41:e1dd6d97f17d 2728 motor[TIRE_FR].pwm = 0;
yabahiro 41:e1dd6d97f17d 2729 motor[TIRE_FR].dir = FREE;
yabahiro 41:e1dd6d97f17d 2730 motor[TIRE_FL].pwm = 25;
yabahiro 41:e1dd6d97f17d 2731 motor[TIRE_FL].dir = BACK;
yabahiro 41:e1dd6d97f17d 2732 motor[TIRE_BR].pwm = 25;
yabahiro 41:e1dd6d97f17d 2733 motor[TIRE_BR].dir = FOR;
yabahiro 41:e1dd6d97f17d 2734 motor[TIRE_BL].pwm = 0;
yabahiro 41:e1dd6d97f17d 2735 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2736 if(g[3]==0) {
yabahiro 40:7f93984e8ba1 2737 count=0;
yabahiro 40:7f93984e8ba1 2738 if(Twsh==2) {
yabahiro 40:7f93984e8ba1 2739 solenoid.solenoid2 = SOLENOID_OFF;
yabahiro 40:7f93984e8ba1 2740 }
yabahiro 40:7f93984e8ba1 2741 mode=40;
yabahiro 40:7f93984e8ba1 2742 }
yabahiro 40:7f93984e8ba1 2743 } else if(mode==40) {
yabahiro 41:e1dd6d97f17d 2744 if(LIF||LIB) {
yabahiro 40:7f93984e8ba1 2745 mode=42;
yabahiro 40:7f93984e8ba1 2746 }
yabahiro 40:7f93984e8ba1 2747
yabahiro 40:7f93984e8ba1 2748 if(counts==false&&g[0]==0) {
yabahiro 40:7f93984e8ba1 2749 cros++;
yabahiro 40:7f93984e8ba1 2750 counts=true;
yabahiro 40:7f93984e8ba1 2751 }
yabahiro 40:7f93984e8ba1 2752 if(counts==true&&g[0]==99) {
yabahiro 40:7f93984e8ba1 2753 counts=false;
yabahiro 40:7f93984e8ba1 2754 }
yabahiro 40:7f93984e8ba1 2755
yabahiro 40:7f93984e8ba1 2756 if(cros==2) {
yabahiro 40:7f93984e8ba1 2757 // mode=41;
yabahiro 40:7f93984e8ba1 2758 //count=0;
yabahiro 40:7f93984e8ba1 2759 //cros=0;
yabahiro 40:7f93984e8ba1 2760 }//ゆっくりモードに入れなかった時の保険
yabahiro 40:7f93984e8ba1 2761
yabahiro 40:7f93984e8ba1 2762 switch(g[3]) {
yabahiro 40:7f93984e8ba1 2763 case 98:
yabahiro 40:7f93984e8ba1 2764 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2765 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2766 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2767 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2768 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2769 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2770 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2771 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2772 if(count>20000) {
yabahiro 40:7f93984e8ba1 2773 cross++;
yabahiro 40:7f93984e8ba1 2774 count=0;
yabahiro 40:7f93984e8ba1 2775 }
yabahiro 40:7f93984e8ba1 2776 mtc2=true;
yabahiro 40:7f93984e8ba1 2777 if(cross==1) {
yabahiro 40:7f93984e8ba1 2778 mode=41;
yabahiro 40:7f93984e8ba1 2779 count=0;
yabahiro 40:7f93984e8ba1 2780 cross=0;
yabahiro 40:7f93984e8ba1 2781 }
yabahiro 40:7f93984e8ba1 2782 break;
yabahiro 40:7f93984e8ba1 2783 case 0:
yabahiro 40:7f93984e8ba1 2784 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2785 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2786 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2787 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2788 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2789 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2790 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2791 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2792 mtc2=true;
yabahiro 40:7f93984e8ba1 2793 break;
yabahiro 40:7f93984e8ba1 2794 case 255:
yabahiro 40:7f93984e8ba1 2795 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2796 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2797 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2798 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2799 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2800 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2801 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2802 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2803 mtc2=true;
yabahiro 40:7f93984e8ba1 2804 break;
yabahiro 40:7f93984e8ba1 2805 case 253:
yabahiro 40:7f93984e8ba1 2806 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2807 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2808 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2809 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2810 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2811 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2812 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2813 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2814 mtc2=true;
yabahiro 40:7f93984e8ba1 2815 break;
yabahiro 40:7f93984e8ba1 2816 case 254:
yabahiro 40:7f93984e8ba1 2817 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2818 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2819 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2820 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2821 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2822 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2823 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 2824 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 2825 mtc2=true;
yabahiro 40:7f93984e8ba1 2826 break;
yabahiro 40:7f93984e8ba1 2827 case 1:
yabahiro 40:7f93984e8ba1 2828 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2829 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2830 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2831 motor[TIRE_FL].dir = BACK;
yabahiro 40:7f93984e8ba1 2832 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2833 motor[TIRE_BR].dir = FOR;
yabahiro 40:7f93984e8ba1 2834 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2835 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2836 mtc2=true;
yabahiro 40:7f93984e8ba1 2837 break;
yabahiro 40:7f93984e8ba1 2838 case 3:
yabahiro 40:7f93984e8ba1 2839 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2840 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2841 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2842 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2843 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2844 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2845 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2846 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2847 mtc2=true;
yabahiro 40:7f93984e8ba1 2848 break;
yabahiro 40:7f93984e8ba1 2849 case 2:
yabahiro 40:7f93984e8ba1 2850 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2851 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2852 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2853 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2854 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2855 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2856 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2857 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2858 mtc2=true;
yabahiro 40:7f93984e8ba1 2859 break;
yabahiro 40:7f93984e8ba1 2860 }
yabahiro 40:7f93984e8ba1 2861 if(g[2]!=98&&mtc2==true) {
yabahiro 40:7f93984e8ba1 2862 switch(g[2]) {
yabahiro 40:7f93984e8ba1 2863 case 0:
yabahiro 40:7f93984e8ba1 2864 mtc2=false;
yabahiro 40:7f93984e8ba1 2865 break;
yabahiro 40:7f93984e8ba1 2866 case 255:
yabahiro 40:7f93984e8ba1 2867 motor[TIRE_FR].pwm += 5;
yabahiro 40:7f93984e8ba1 2868 mtc2=false;
yabahiro 40:7f93984e8ba1 2869 break;
yabahiro 40:7f93984e8ba1 2870 case 253:
yabahiro 40:7f93984e8ba1 2871 motor[TIRE_FR].pwm += 10;
yabahiro 40:7f93984e8ba1 2872 mtc2=false;
yabahiro 40:7f93984e8ba1 2873 break;
yabahiro 40:7f93984e8ba1 2874 case 254:
yabahiro 40:7f93984e8ba1 2875 motor[TIRE_FR].pwm += 20;
yabahiro 40:7f93984e8ba1 2876 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2877 mtc2=false;
yabahiro 40:7f93984e8ba1 2878 break;
yabahiro 40:7f93984e8ba1 2879 case 1:
yabahiro 40:7f93984e8ba1 2880 motor[TIRE_BR].pwm += 5;
yabahiro 40:7f93984e8ba1 2881 mtc2=false;
yabahiro 40:7f93984e8ba1 2882 break;
yabahiro 40:7f93984e8ba1 2883 case 3:
yabahiro 40:7f93984e8ba1 2884 motor[TIRE_BR].pwm += 10;
yabahiro 40:7f93984e8ba1 2885 mtc2=false;
yabahiro 40:7f93984e8ba1 2886 break;
yabahiro 40:7f93984e8ba1 2887 case 2:
yabahiro 40:7f93984e8ba1 2888 motor[TIRE_BR].pwm += 20;
yabahiro 40:7f93984e8ba1 2889 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 2890 mtc2=false;
yabahiro 40:7f93984e8ba1 2891 break;
yabahiro 40:7f93984e8ba1 2892 }
yabahiro 40:7f93984e8ba1 2893 }
yabahiro 40:7f93984e8ba1 2894 } else if(mode==41) {
yabahiro 41:e1dd6d97f17d 2895 if(LIF||LIB) {
yabahiro 41:e1dd6d97f17d 2896 mode=42;
yabahiro 41:e1dd6d97f17d 2897 }
yabahiro 40:7f93984e8ba1 2898 motor[TIRE_FR].pwm = 30;
yabahiro 40:7f93984e8ba1 2899 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2900 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2901 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2902 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2903 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2904 motor[TIRE_BL].pwm = 30;
yabahiro 40:7f93984e8ba1 2905 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2906 if(g[1]==0) {
yabahiro 40:7f93984e8ba1 2907 count=0;
yabahiro 40:7f93984e8ba1 2908 mode=70;
yabahiro 40:7f93984e8ba1 2909 normal=60;
yabahiro 40:7f93984e8ba1 2910 fast=80;
yabahiro 40:7f93984e8ba1 2911 slow=40;
yabahiro 40:7f93984e8ba1 2912 }
yabahiro 40:7f93984e8ba1 2913 } else if(mode==42) {
yabahiro 41:e1dd6d97f17d 2914 motor[TIRE_FR].pwm = 0;
yabahiro 41:e1dd6d97f17d 2915 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 2916 motor[TIRE_FL].pwm = 30;
yabahiro 40:7f93984e8ba1 2917 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2918 motor[TIRE_BR].pwm = 30;
yabahiro 40:7f93984e8ba1 2919 motor[TIRE_BR].dir = BACK;
yabahiro 41:e1dd6d97f17d 2920 motor[TIRE_BL].pwm = 0;
yabahiro 41:e1dd6d97f17d 2921 motor[TIRE_BL].dir = FREE;
yabahiro 41:e1dd6d97f17d 2922 if(g[0]==0) {
yabahiro 40:7f93984e8ba1 2923 count=0;
yabahiro 40:7f93984e8ba1 2924 mode=70;
yabahiro 40:7f93984e8ba1 2925 normal=60;
yabahiro 40:7f93984e8ba1 2926 fast=80;
yabahiro 40:7f93984e8ba1 2927 slow=40;
yabahiro 40:7f93984e8ba1 2928 }
yabahiro 40:7f93984e8ba1 2929
yabahiro 40:7f93984e8ba1 2930 } else if(mode==70)
yabahiro 40:7f93984e8ba1 2931 switch(g[0]) {
yabahiro 40:7f93984e8ba1 2932 case 99:
yabahiro 40:7f93984e8ba1 2933 if(count>=100000) {
yabahiro 40:7f93984e8ba1 2934 cross++;
yabahiro 40:7f93984e8ba1 2935 mode=100;
yabahiro 40:7f93984e8ba1 2936 count=0;
yabahiro 40:7f93984e8ba1 2937 }
yabahiro 41:e1dd6d97f17d 2938 if(cross==2) {
yabahiro 40:7f93984e8ba1 2939 cross=0;
yabahiro 40:7f93984e8ba1 2940 mode=100;
yabahiro 40:7f93984e8ba1 2941 }
yabahiro 40:7f93984e8ba1 2942 break;
yabahiro 40:7f93984e8ba1 2943 case 98:
yabahiro 40:7f93984e8ba1 2944 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2945 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2946 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 2947 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2948 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 2949 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2950 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2951 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2952 break;
yabahiro 40:7f93984e8ba1 2953 case 0:
yabahiro 40:7f93984e8ba1 2954 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2955 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2956 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 2957 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2958 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 2959 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2960 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2961 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2962 mtc=true;
yabahiro 40:7f93984e8ba1 2963 break;
yabahiro 40:7f93984e8ba1 2964 case 255:
yabahiro 40:7f93984e8ba1 2965 motor[TIRE_FR].pwm = fast;
yabahiro 40:7f93984e8ba1 2966 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2967 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 2968 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 2969 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 2970 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 2971 motor[TIRE_BL].pwm = fast;
yabahiro 40:7f93984e8ba1 2972 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2973 mtc=true;
yabahiro 40:7f93984e8ba1 2974 break;
yabahiro 40:7f93984e8ba1 2975 case 253:
yabahiro 40:7f93984e8ba1 2976 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2977 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2978 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2979 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2980 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2981 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2982 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 2983 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2984 mtc=true;
yabahiro 40:7f93984e8ba1 2985 break;
yabahiro 40:7f93984e8ba1 2986 case 254:
yabahiro 40:7f93984e8ba1 2987 motor[TIRE_FR].pwm = normal;
yabahiro 40:7f93984e8ba1 2988 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 2989 motor[TIRE_FL].pwm = 0;
yabahiro 40:7f93984e8ba1 2990 motor[TIRE_FL].dir = FREE;
yabahiro 40:7f93984e8ba1 2991 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 2992 motor[TIRE_BR].dir = FREE;
yabahiro 40:7f93984e8ba1 2993 motor[TIRE_BL].pwm = normal;
yabahiro 40:7f93984e8ba1 2994 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 2995 mtc=true;
yabahiro 40:7f93984e8ba1 2996 break;
yabahiro 40:7f93984e8ba1 2997 case 1:
yabahiro 40:7f93984e8ba1 2998 motor[TIRE_FR].pwm = slow;
yabahiro 40:7f93984e8ba1 2999 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 3000 motor[TIRE_FL].pwm = fast;
yabahiro 40:7f93984e8ba1 3001 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 3002 motor[TIRE_BR].pwm = fast;
yabahiro 40:7f93984e8ba1 3003 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 3004 motor[TIRE_BL].pwm = slow;
yabahiro 40:7f93984e8ba1 3005 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 3006 mtc=true;
yabahiro 40:7f93984e8ba1 3007 break;
yabahiro 40:7f93984e8ba1 3008 case 3:
yabahiro 40:7f93984e8ba1 3009 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 3010 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 3011 motor[TIRE_FL].pwm = slow;
yabahiro 40:7f93984e8ba1 3012 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 3013 motor[TIRE_BR].pwm = slow;
yabahiro 40:7f93984e8ba1 3014 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 3015 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 3016 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 3017 mtc=true;
yabahiro 40:7f93984e8ba1 3018 break;
yabahiro 40:7f93984e8ba1 3019 case 2:
yabahiro 40:7f93984e8ba1 3020 motor[TIRE_FR].pwm = 0;
yabahiro 40:7f93984e8ba1 3021 motor[TIRE_FR].dir = FREE;
yabahiro 40:7f93984e8ba1 3022 motor[TIRE_FL].pwm = normal;
yabahiro 40:7f93984e8ba1 3023 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 3024 motor[TIRE_BR].pwm = normal;
yabahiro 40:7f93984e8ba1 3025 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 3026 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 3027 motor[TIRE_BL].dir = FREE;
yabahiro 40:7f93984e8ba1 3028 mtc=true;
yabahiro 40:7f93984e8ba1 3029 break;
yabahiro 40:7f93984e8ba1 3030 }
yabahiro 40:7f93984e8ba1 3031 if(g[1]!=98&&mtc==true) {
yabahiro 40:7f93984e8ba1 3032 switch(g[1]) {
yabahiro 40:7f93984e8ba1 3033 case 0:
yabahiro 40:7f93984e8ba1 3034 mtc=false;
yabahiro 40:7f93984e8ba1 3035 break;
yabahiro 40:7f93984e8ba1 3036 case 255:
yabahiro 40:7f93984e8ba1 3037 motor[TIRE_BR].pwm += 5;
yabahiro 40:7f93984e8ba1 3038 mtc=false;
yabahiro 40:7f93984e8ba1 3039 break;
yabahiro 40:7f93984e8ba1 3040 case 253:
yabahiro 40:7f93984e8ba1 3041 motor[TIRE_BR].pwm += 10;
yabahiro 40:7f93984e8ba1 3042 mtc=false;
yabahiro 40:7f93984e8ba1 3043 break;
yabahiro 40:7f93984e8ba1 3044 case 254:
yabahiro 40:7f93984e8ba1 3045 motor[TIRE_BR].pwm += 20;
yabahiro 40:7f93984e8ba1 3046 motor[TIRE_BL].pwm = 0;
yabahiro 40:7f93984e8ba1 3047 mtc=false;
yabahiro 40:7f93984e8ba1 3048 break;
yabahiro 40:7f93984e8ba1 3049 case 1:
yabahiro 40:7f93984e8ba1 3050 motor[TIRE_BL].pwm += 5;
yabahiro 40:7f93984e8ba1 3051 mtc=false;
yabahiro 40:7f93984e8ba1 3052 break;
yabahiro 40:7f93984e8ba1 3053 case 3:
yabahiro 40:7f93984e8ba1 3054 motor[TIRE_BL].pwm += 10;
yabahiro 40:7f93984e8ba1 3055 mtc=false;
yabahiro 40:7f93984e8ba1 3056 break;
yabahiro 40:7f93984e8ba1 3057 case 2:
yabahiro 40:7f93984e8ba1 3058 motor[TIRE_BL].pwm += 20;
yabahiro 40:7f93984e8ba1 3059 motor[TIRE_BR].pwm = 0;
yabahiro 40:7f93984e8ba1 3060 mtc=false;
yabahiro 40:7f93984e8ba1 3061 break;
yabahiro 40:7f93984e8ba1 3062 }
yabahiro 40:7f93984e8ba1 3063 } else if(mode==100) {
yabahiro 40:7f93984e8ba1 3064 motor[TIRE_FR].pwm = 20;
yabahiro 40:7f93984e8ba1 3065 motor[TIRE_FR].dir = BACK;
yabahiro 40:7f93984e8ba1 3066 motor[TIRE_FL].pwm = 20;
yabahiro 40:7f93984e8ba1 3067 motor[TIRE_FL].dir = FOR;
yabahiro 40:7f93984e8ba1 3068 motor[TIRE_BR].pwm = 23;
yabahiro 40:7f93984e8ba1 3069 motor[TIRE_BR].dir = BACK;
yabahiro 40:7f93984e8ba1 3070 motor[TIRE_BL].pwm = 20;
yabahiro 40:7f93984e8ba1 3071 motor[TIRE_BL].dir = FOR;
yabahiro 40:7f93984e8ba1 3072 if(StertSW) {
yabahiro 40:7f93984e8ba1 3073 SW_flag = true;
yabahiro 40:7f93984e8ba1 3074 }
yabahiro 40:7f93984e8ba1 3075 if(SW_flag ==true) {
yabahiro 40:7f93984e8ba1 3076 motor[TIRE_FR].pwm = 100;
yabahiro 40:7f93984e8ba1 3077 motor[TIRE_FR].dir = BRAKE;
yabahiro 40:7f93984e8ba1 3078 motor[TIRE_FL].pwm = 100;
yabahiro 40:7f93984e8ba1 3079 motor[TIRE_FL].dir = BRAKE;
yabahiro 40:7f93984e8ba1 3080 motor[TIRE_BR].pwm = 100;
yabahiro 40:7f93984e8ba1 3081 motor[TIRE_BR].dir = BRAKE;
yabahiro 40:7f93984e8ba1 3082 motor[TIRE_BL].pwm = 100;
yabahiro 40:7f93984e8ba1 3083 motor[TIRE_BL].dir = BRAKE;
yabahiro 40:7f93984e8ba1 3084 fast=60;
yabahiro 40:7f93984e8ba1 3085 normal=40;
yabahiro 40:7f93984e8ba1 3086 slow=20;
yabahiro 40:7f93984e8ba1 3087 mode =1;
yabahiro 40:7f93984e8ba1 3088 cross=0;//十字数える用
yabahiro 40:7f93984e8ba1 3089 cros=0;
yabahiro 40:7f93984e8ba1 3090 count=100000;//wait代替え
yabahiro 40:7f93984e8ba1 3091 UP_flag = false;
yabahiro 40:7f93984e8ba1 3092 SW_flag = false;
yabahiro 40:7f93984e8ba1 3093 Air_flag = false;
yabahiro 40:7f93984e8ba1 3094 zyouge=false;
yabahiro 40:7f93984e8ba1 3095 zyougedo=false;
yabahiro 40:7f93984e8ba1 3096 dz1=true;
yabahiro 40:7f93984e8ba1 3097 dz1i=false;
yabahiro 40:7f93984e8ba1 3098 dz2=true;
yabahiro 40:7f93984e8ba1 3099 dz2i=false;
yabahiro 40:7f93984e8ba1 3100 dz3=true;
yabahiro 40:7f93984e8ba1 3101 dz3i=false;
yabahiro 40:7f93984e8ba1 3102 dz4=true;
yabahiro 40:7f93984e8ba1 3103 dz4i=false;
yabahiro 40:7f93984e8ba1 3104 through=false;
yabahiro 40:7f93984e8ba1 3105 counts=false;
yabahiro 40:7f93984e8ba1 3106 mtc=false;
yabahiro 40:7f93984e8ba1 3107 mtc2 = false;
yabahiro 40:7f93984e8ba1 3108 current = 0;
yabahiro 40:7f93984e8ba1 3109 if(StertSW==false) {
yabahiro 40:7f93984e8ba1 3110 SW_flag = false;
yabahiro 40:7f93984e8ba1 3111 }
yabahiro 40:7f93984e8ba1 3112 }
yabahiro 40:7f93984e8ba1 3113 }
yabahiro 40:7f93984e8ba1 3114
yabahiro 40:7f93984e8ba1 3115
yabahiro 21:1f1e9c585da8 3116
kishibekairohan 13:b6e02d6261d7 3117 }
kishibekairohan 13:b6e02d6261d7 3118 #endif
kishibekairohan 13:b6e02d6261d7 3119
kishibekairohan 13:b6e02d6261d7 3120 #if USE_PROCESS_NUM>6
yabahiro 21:1f1e9c585da8 3121 static void Process6()
kishibekairohan 13:b6e02d6261d7 3122 {
yabahiro 21:1f1e9c585da8 3123
kishibekairohan 13:b6e02d6261d7 3124 }
kishibekairohan 13:b6e02d6261d7 3125 #endif
kishibekairohan 13:b6e02d6261d7 3126
kishibekairohan 13:b6e02d6261d7 3127 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 3128 static void Process7()
kishibekairohan 13:b6e02d6261d7 3129 {
yabahiro 21:1f1e9c585da8 3130
kishibekairohan 12:c09b3e08a316 3131 }
kishibekairohan 12:c09b3e08a316 3132 #endif
kishibekairohan 12:c09b3e08a316 3133
yabahiro 21:1f1e9c585da8 3134 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 3135 static void Process8()
kishibekairohan 12:c09b3e08a316 3136 {
yabahiro 21:1f1e9c585da8 3137
kishibekairohan 12:c09b3e08a316 3138 }
kishibekairohan 12:c09b3e08a316 3139 #endif
kishibekairohan 12:c09b3e08a316 3140
kishibekairohan 13:b6e02d6261d7 3141 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 3142 static void Process9()
kishibekairohan 12:c09b3e08a316 3143 {
yabahiro 21:1f1e9c585da8 3144
t_yamamoto 0:669ef71cba68 3145 }
t_yamamoto 0:669ef71cba68 3146 #endif
t_yamamoto 0:669ef71cba68 3147 #endif
t_yamamoto 0:669ef71cba68 3148 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 3149
t_yamamoto 0:669ef71cba68 3150 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 3151 {
t_yamamoto 0:669ef71cba68 3152
yabahiro 21:1f1e9c585da8 3153 #ifdef USE_SOLENOID
yabahiro 21:1f1e9c585da8 3154 solenoid.all = ALL_SOLENOID_OFF;
yabahiro 21:1f1e9c585da8 3155 #endif
t_yamamoto 0:669ef71cba68 3156
yabahiro 21:1f1e9c585da8 3157 #ifdef USE_MOTOR
yabahiro 21:1f1e9c585da8 3158 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
yabahiro 21:1f1e9c585da8 3159 motor[i].dir = FREE;
yabahiro 21:1f1e9c585da8 3160 motor[i].pwm = 0;
yabahiro 21:1f1e9c585da8 3161 }
yabahiro 21:1f1e9c585da8 3162 #endif
t_yamamoto 0:669ef71cba68 3163 }
t_yamamoto 0:669ef71cba68 3164
yabahiro 21:1f1e9c585da8 3165 /*
yabahiro 21:1f1e9c585da8 3166 void BuzzerTimer_func()
yabahiro 21:1f1e9c585da8 3167 {
kishibekairohan 16:3f2c2d89372b 3168 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 3169 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 3170 }
yabahiro 21:1f1e9c585da8 3171 */
yabahiro 21:1f1e9c585da8 3172 void TapeLedEms_func()
yabahiro 21:1f1e9c585da8 3173 {
kishibekairohan 16:3f2c2d89372b 3174 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 3175 }
kishibekairohan 16:3f2c2d89372b 3176
kishibekairohan 16:3f2c2d89372b 3177 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 16:3f2c2d89372b 3178
kishibekairohan 16:3f2c2d89372b 3179
t_yamamoto 0:669ef71cba68 3180 #pragma endregion