大季 矢花 / Mbed 2 deprecated 2019_Master_10_6_asaa

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@44:c208a908583c, 2019-10-03 (annotated)

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