大季 矢花 / Mbed 2 deprecated 2019_Master_10_2_last

aa

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@43:677f7446612e, 2019-10-02 (annotated)

Committer:
yabahiro
Date:
Wed Oct 02 01:53:59 2019 +0000
Revision:
43:677f7446612e
Parent:
42:4f9689c0a349
Child:
44:c208a908583c
yey

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