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