大季 矢花 / Mbed 2 deprecated 2019_Master_10_6_asaa

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@48:d24c325fc0b4, 2019-10-28 (annotated)

Committer:
yabahiro
Date:
Mon Oct 28 11:11:33 2019 +0000
Revision:
48:d24c325fc0b4
Parent:
47:b6e5a6bfe7ab 
aa;

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