![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aa
System/Process/Process.cpp@57:ccfd26b94d8a, 2019-11-19 (annotated)
- Committer:
- yabahiro
- Date:
- Tue Nov 19 14:21:38 2019 +0000
- Revision:
- 57:ccfd26b94d8a
- Parent:
- 56:61ebd4b759b4
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" |
Ryosei | 27:dd9f27fce7d1 | 3 | #include <stdlib.h> |
7ka884 | 4:ba9df71868df | 4 | #include "../../CommonLibraries/PID/PID.h" |
t_yamamoto | 0:669ef71cba68 | 5 | #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" |
M_souta | 21:e3b58d675c1c | 6 | #include "../../Communication/RS485/LineHub/LineHub.h" |
t_yamamoto | 0:669ef71cba68 | 7 | #include "../../Communication/Controller/Controller.h" |
M_souta | 30:8f092276b2ba | 8 | #include "../../Switch/Switch.h" |
Ryosei | 27:dd9f27fce7d1 | 9 | |
t_yamamoto | 0:669ef71cba68 | 10 | #include "../../LED/LED.h" |
t_yamamoto | 0:669ef71cba68 | 11 | #include "../../Safty/Safty.h" |
t_yamamoto | 0:669ef71cba68 | 12 | #include "../Using.h" |
t_yamamoto | 0:669ef71cba68 | 13 | |
t_yamamoto | 0:669ef71cba68 | 14 | using namespace SWITCH; |
7ka884 | 4:ba9df71868df | 15 | using namespace PID_SPACE; |
Ryosei | 27:dd9f27fce7d1 | 16 | |
M_souta | 21:e3b58d675c1c | 17 | using namespace LINEHUB; |
t_yamamoto | 0:669ef71cba68 | 18 | |
t_yamamoto | 0:669ef71cba68 | 19 | static CONTROLLER::ControllerData *controller; |
t_yamamoto | 0:669ef71cba68 | 20 | ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; |
t_yamamoto | 0:669ef71cba68 | 21 | ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; |
t_yamamoto | 0:669ef71cba68 | 22 | |
t_yamamoto | 0:669ef71cba68 | 23 | static bool lock; |
t_yamamoto | 0:669ef71cba68 | 24 | static bool processChangeComp; |
t_yamamoto | 0:669ef71cba68 | 25 | static int current; |
t_yamamoto | 0:669ef71cba68 | 26 | |
t_yamamoto | 0:669ef71cba68 | 27 | static void AllActuatorReset(); |
t_yamamoto | 0:669ef71cba68 | 28 | |
t_yamamoto | 0:669ef71cba68 | 29 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 30 | static void (*Process[USE_PROCESS_NUM])(void); |
t_yamamoto | 0:669ef71cba68 | 31 | #endif |
t_yamamoto | 0:669ef71cba68 | 32 | |
t_yamamoto | 0:669ef71cba68 | 33 | #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE |
t_yamamoto | 0:669ef71cba68 | 34 | |
t_yamamoto | 0:669ef71cba68 | 35 | /*Replace here with the definition code of your variables.*/ |
t_yamamoto | 0:669ef71cba68 | 36 | |
kishibekairohan | 2:c015739085d3 | 37 | |
M_souta | 30:8f092276b2ba | 38 | //Serial pc(USBTX, USBRX); |
kishibekairohan | 16:3f2c2d89372b | 39 | |
Ryosei | 27:dd9f27fce7d1 | 40 | const int omni[15][15] = { |
Ryosei | 27:dd9f27fce7d1 | 41 | { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 42 | { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 43 | { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 44 | { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 45 | { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 46 | { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 47 | { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 48 | { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, |
Ryosei | 27:dd9f27fce7d1 | 49 | { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, |
Ryosei | 27:dd9f27fce7d1 | 50 | { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, |
Ryosei | 27:dd9f27fce7d1 | 51 | { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, |
Ryosei | 27:dd9f27fce7d1 | 52 | { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, |
Ryosei | 27:dd9f27fce7d1 | 53 | { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, |
Ryosei | 27:dd9f27fce7d1 | 54 | { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, |
Ryosei | 27:dd9f27fce7d1 | 55 | { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } |
M_souta | 21:e3b58d675c1c | 56 | }; |
M_souta | 21:e3b58d675c1c | 57 | |
M_souta | 21:e3b58d675c1c | 58 | const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 }; |
M_souta | 21:e3b58d675c1c | 59 | |
kishibekairohan | 42:980af34f6fe3 | 60 | uint8_t SetStatus(int pwmVal); |
kishibekairohan | 42:980af34f6fe3 | 61 | uint8_t SetPWM(int pwmVal); |
yabahiro | 33:dd598bb23ad1 | 62 | |
kishibekairohan | 35:33c6cdba5630 | 63 | DigitalIn EMC(PC_13); //非常停止確認用 |
yabahiro | 33:dd598bb23ad1 | 64 | |
yabahiro | 33:dd598bb23ad1 | 65 | bool start_flag = true; |
yabahiro | 33:dd598bb23ad1 | 66 | bool UP_flag = false; |
yabahiro | 33:dd598bb23ad1 | 67 | bool SW_flag = false; |
yabahiro | 33:dd598bb23ad1 | 68 | bool Air_flag = false; |
yabahiro | 33:dd598bb23ad1 | 69 | bool zyouge; |
yabahiro | 33:dd598bb23ad1 | 70 | bool zyougedo; |
yabahiro | 33:dd598bb23ad1 | 71 | bool dz2=true; |
yabahiro | 33:dd598bb23ad1 | 72 | bool dz2i=false; |
yabahiro | 33:dd598bb23ad1 | 73 | bool dz3=true; |
yabahiro | 33:dd598bb23ad1 | 74 | bool dz3i=false; |
yabahiro | 33:dd598bb23ad1 | 75 | bool dz4=true; |
yabahiro | 33:dd598bb23ad1 | 76 | bool dz4i=false; |
yabahiro | 33:dd598bb23ad1 | 77 | bool zone; |
yabahiro | 33:dd598bb23ad1 | 78 | bool through=false; |
yabahiro | 33:dd598bb23ad1 | 79 | bool counts=false; |
yabahiro | 33:dd598bb23ad1 | 80 | bool mtc=false; |
yabahiro | 33:dd598bb23ad1 | 81 | bool mtc2 = false; |
yabahiro | 33:dd598bb23ad1 | 82 | bool emcs=false; |
yabahiro | 33:dd598bb23ad1 | 83 | bool kiri; |
yabahiro | 33:dd598bb23ad1 | 84 | |
yabahiro | 33:dd598bb23ad1 | 85 | int g[8]; |
yabahiro | 33:dd598bb23ad1 | 86 | int Twsh; |
yabahiro | 38:40e7fdf3c694 | 87 | int testmode =0; |
yabahiro | 33:dd598bb23ad1 | 88 | int mode =0; |
yabahiro | 33:dd598bb23ad1 | 89 | int cross=0;//十字数える用 |
yabahiro | 33:dd598bb23ad1 | 90 | int cros=0; |
yabahiro | 33:dd598bb23ad1 | 91 | int countss=100000;//wait代替え |
yabahiro | 55:844ac0974740 | 92 | int num=0; |
yabahiro | 33:dd598bb23ad1 | 93 | |
yabahiro | 33:dd598bb23ad1 | 94 | ///*********PWM調整用*********/// |
kishibekairohan | 41:99a1158f3eca | 95 | |
kishibekairohan | 42:980af34f6fe3 | 96 | int fast = 60; |
yabahiro | 33:dd598bb23ad1 | 97 | int normal = 40; |
yabahiro | 46:68296caae1af | 98 | int slow = 25; |
yabahiro | 33:dd598bb23ad1 | 99 | |
yabahiro | 55:844ac0974740 | 100 | /* |
yabahiro | 55:844ac0974740 | 101 | void Count(); |
yabahiro | 55:844ac0974740 | 102 | Ticker bunzin; |
yabahiro | 55:844ac0974740 | 103 | */ |
yabahiro | 55:844ac0974740 | 104 | |
yabahiro | 56:61ebd4b759b4 | 105 | Timer aime; |
yabahiro | 56:61ebd4b759b4 | 106 | int value; |
yabahiro | 55:844ac0974740 | 107 | |
t_yamamoto | 0:669ef71cba68 | 108 | #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE |
t_yamamoto | 0:669ef71cba68 | 109 | |
t_yamamoto | 0:669ef71cba68 | 110 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 111 | #if USE_PROCESS_NUM>0 |
t_yamamoto | 0:669ef71cba68 | 112 | static void Process0(void); |
t_yamamoto | 0:669ef71cba68 | 113 | #endif |
t_yamamoto | 0:669ef71cba68 | 114 | #if USE_PROCESS_NUM>1 |
t_yamamoto | 0:669ef71cba68 | 115 | static void Process1(void); |
t_yamamoto | 0:669ef71cba68 | 116 | #endif |
t_yamamoto | 0:669ef71cba68 | 117 | #if USE_PROCESS_NUM>2 |
t_yamamoto | 0:669ef71cba68 | 118 | static void Process2(void); |
t_yamamoto | 0:669ef71cba68 | 119 | #endif |
t_yamamoto | 0:669ef71cba68 | 120 | #if USE_PROCESS_NUM>3 |
t_yamamoto | 0:669ef71cba68 | 121 | static void Process3(void); |
t_yamamoto | 0:669ef71cba68 | 122 | #endif |
t_yamamoto | 0:669ef71cba68 | 123 | #if USE_PROCESS_NUM>4 |
t_yamamoto | 0:669ef71cba68 | 124 | static void Process4(void); |
t_yamamoto | 0:669ef71cba68 | 125 | #endif |
t_yamamoto | 0:669ef71cba68 | 126 | #if USE_PROCESS_NUM>5 |
t_yamamoto | 0:669ef71cba68 | 127 | static void Process5(void); |
t_yamamoto | 0:669ef71cba68 | 128 | #endif |
t_yamamoto | 0:669ef71cba68 | 129 | #if USE_PROCESS_NUM>6 |
t_yamamoto | 0:669ef71cba68 | 130 | static void Process6(void); |
t_yamamoto | 0:669ef71cba68 | 131 | #endif |
t_yamamoto | 0:669ef71cba68 | 132 | #if USE_PROCESS_NUM>7 |
t_yamamoto | 0:669ef71cba68 | 133 | static void Process7(void); |
t_yamamoto | 0:669ef71cba68 | 134 | #endif |
t_yamamoto | 0:669ef71cba68 | 135 | #if USE_PROCESS_NUM>8 |
t_yamamoto | 0:669ef71cba68 | 136 | static void Process8(void); |
t_yamamoto | 0:669ef71cba68 | 137 | #endif |
t_yamamoto | 0:669ef71cba68 | 138 | #if USE_PROCESS_NUM>9 |
t_yamamoto | 0:669ef71cba68 | 139 | static void Process9(void); |
t_yamamoto | 0:669ef71cba68 | 140 | #endif |
t_yamamoto | 0:669ef71cba68 | 141 | #endif |
t_yamamoto | 0:669ef71cba68 | 142 | |
t_yamamoto | 0:669ef71cba68 | 143 | void SystemProcessInitialize() |
t_yamamoto | 0:669ef71cba68 | 144 | { |
Ryosei | 27:dd9f27fce7d1 | 145 | #pragma region USER-DEFINED_VARIABLE_INIT |
Ryosei | 27:dd9f27fce7d1 | 146 | /*Replace here with the initialization code of your variables.*/ |
yabahiro | 55:844ac0974740 | 147 | //bunzin.attach(Count,0.001); |
Ryosei | 27:dd9f27fce7d1 | 148 | |
Ryosei | 27:dd9f27fce7d1 | 149 | #pragma endregion USER-DEFINED_VARIABLE_INIT |
Ryosei | 27:dd9f27fce7d1 | 150 | |
Ryosei | 27:dd9f27fce7d1 | 151 | lock = true; |
Ryosei | 27:dd9f27fce7d1 | 152 | processChangeComp = true; |
Ryosei | 27:dd9f27fce7d1 | 153 | current = DEFAULT_PROCESS; |
t_yamamoto | 0:669ef71cba68 | 154 | |
Ryosei | 27:dd9f27fce7d1 | 155 | #ifdef USE_SUBPROCESS |
Ryosei | 27:dd9f27fce7d1 | 156 | #if USE_PROCESS_NUM>0 |
Ryosei | 27:dd9f27fce7d1 | 157 | Process[0] = Process0; |
Ryosei | 27:dd9f27fce7d1 | 158 | #endif |
Ryosei | 27:dd9f27fce7d1 | 159 | #if USE_PROCESS_NUM>1 |
Ryosei | 27:dd9f27fce7d1 | 160 | Process[1] = Process1; |
Ryosei | 27:dd9f27fce7d1 | 161 | #endif |
Ryosei | 27:dd9f27fce7d1 | 162 | #if USE_PROCESS_NUM>2 |
Ryosei | 27:dd9f27fce7d1 | 163 | Process[2] = Process2; |
Ryosei | 27:dd9f27fce7d1 | 164 | #endif |
Ryosei | 27:dd9f27fce7d1 | 165 | #if USE_PROCESS_NUM>3 |
Ryosei | 27:dd9f27fce7d1 | 166 | Process[3] = Process3; |
Ryosei | 27:dd9f27fce7d1 | 167 | #endif |
Ryosei | 27:dd9f27fce7d1 | 168 | #if USE_PROCESS_NUM>4 |
Ryosei | 27:dd9f27fce7d1 | 169 | Process[4] = Process4; |
Ryosei | 27:dd9f27fce7d1 | 170 | #endif |
Ryosei | 27:dd9f27fce7d1 | 171 | #if USE_PROCESS_NUM>5 |
Ryosei | 27:dd9f27fce7d1 | 172 | Process[5] = Process5; |
Ryosei | 27:dd9f27fce7d1 | 173 | #endif |
Ryosei | 27:dd9f27fce7d1 | 174 | #if USE_PROCESS_NUM>6 |
Ryosei | 27:dd9f27fce7d1 | 175 | Process[6] = Process6; |
Ryosei | 27:dd9f27fce7d1 | 176 | #endif |
Ryosei | 27:dd9f27fce7d1 | 177 | #if USE_PROCESS_NUM>7 |
Ryosei | 27:dd9f27fce7d1 | 178 | Process[7] = Process7; |
Ryosei | 27:dd9f27fce7d1 | 179 | #endif |
Ryosei | 27:dd9f27fce7d1 | 180 | #if USE_PROCESS_NUM>8 |
Ryosei | 27:dd9f27fce7d1 | 181 | Process[8] = Process8; |
Ryosei | 27:dd9f27fce7d1 | 182 | #endif |
Ryosei | 27:dd9f27fce7d1 | 183 | #if USE_PROCESS_NUM>9 |
Ryosei | 27:dd9f27fce7d1 | 184 | Process[9] = Process9; |
Ryosei | 27:dd9f27fce7d1 | 185 | #endif |
Ryosei | 27:dd9f27fce7d1 | 186 | #endif |
t_yamamoto | 0:669ef71cba68 | 187 | } |
t_yamamoto | 0:669ef71cba68 | 188 | |
t_yamamoto | 0:669ef71cba68 | 189 | static void SystemProcessUpdate() |
t_yamamoto | 0:669ef71cba68 | 190 | { |
Ryosei | 27:dd9f27fce7d1 | 191 | #ifdef USE_SUBPROCESS |
Ryosei | 27:dd9f27fce7d1 | 192 | if(controller->Button.HOME) lock = false; |
Ryosei | 27:dd9f27fce7d1 | 193 | |
Ryosei | 27:dd9f27fce7d1 | 194 | if(controller->Button.START && processChangeComp) { |
Ryosei | 27:dd9f27fce7d1 | 195 | current++; |
Ryosei | 27:dd9f27fce7d1 | 196 | if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; |
Ryosei | 27:dd9f27fce7d1 | 197 | processChangeComp = false; |
Ryosei | 27:dd9f27fce7d1 | 198 | } else if(controller->Button.SELECT && processChangeComp) { |
Ryosei | 27:dd9f27fce7d1 | 199 | current--; |
Ryosei | 27:dd9f27fce7d1 | 200 | if (current < 0) current = 0; |
Ryosei | 27:dd9f27fce7d1 | 201 | processChangeComp = false; |
Ryosei | 27:dd9f27fce7d1 | 202 | } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; |
Ryosei | 27:dd9f27fce7d1 | 203 | #endif |
Ryosei | 27:dd9f27fce7d1 | 204 | |
Ryosei | 27:dd9f27fce7d1 | 205 | #ifdef USE_MOTOR |
Ryosei | 27:dd9f27fce7d1 | 206 | ACTUATORHUB::MOTOR::Motor::Update(motor); |
Ryosei | 27:dd9f27fce7d1 | 207 | #endif |
Ryosei | 27:dd9f27fce7d1 | 208 | |
Ryosei | 27:dd9f27fce7d1 | 209 | #ifdef USE_SOLENOID |
Ryosei | 27:dd9f27fce7d1 | 210 | ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); |
Ryosei | 27:dd9f27fce7d1 | 211 | #endif |
Ryosei | 27:dd9f27fce7d1 | 212 | |
Ryosei | 27:dd9f27fce7d1 | 213 | #ifdef USE_RS485 |
Ryosei | 27:dd9f27fce7d1 | 214 | ACTUATORHUB::ActuatorHub::Update(); |
Ryosei | 27:dd9f27fce7d1 | 215 | //LINEHUB::LineHub::Update(); |
Ryosei | 27:dd9f27fce7d1 | 216 | #endif |
Ryosei | 27:dd9f27fce7d1 | 217 | |
Ryosei | 27:dd9f27fce7d1 | 218 | } |
Ryosei | 27:dd9f27fce7d1 | 219 | |
yabahiro | 39:784552c26163 | 220 | int point1; |
Ryosei | 27:dd9f27fce7d1 | 221 | void SystemProcess() |
Ryosei | 27:dd9f27fce7d1 | 222 | { |
yabahiro | 56:61ebd4b759b4 | 223 | |
Ryosei | 27:dd9f27fce7d1 | 224 | SystemProcessInitialize(); |
yabahiro | 33:dd598bb23ad1 | 225 | |
Ryosei | 27:dd9f27fce7d1 | 226 | while(1) { |
yabahiro | 53:694dbd7dfa0e | 227 | //pc.printf("3:%d\n\r",g[3]); |
yabahiro | 46:68296caae1af | 228 | |
yabahiro | 33:dd598bb23ad1 | 229 | if(EMC==0) { |
yabahiro | 33:dd598bb23ad1 | 230 | fast=40; |
yabahiro | 33:dd598bb23ad1 | 231 | normal=30; |
yabahiro | 33:dd598bb23ad1 | 232 | slow=20; |
yabahiro | 33:dd598bb23ad1 | 233 | cross=0;//十字数える用 |
yabahiro | 33:dd598bb23ad1 | 234 | cros=0; |
yabahiro | 33:dd598bb23ad1 | 235 | countss=0;//wait代替え |
yabahiro | 33:dd598bb23ad1 | 236 | UP_flag = false; |
yabahiro | 33:dd598bb23ad1 | 237 | SW_flag = false; |
yabahiro | 33:dd598bb23ad1 | 238 | Air_flag = false; |
yabahiro | 33:dd598bb23ad1 | 239 | zyouge=false; |
yabahiro | 33:dd598bb23ad1 | 240 | zyougedo=false; |
yabahiro | 33:dd598bb23ad1 | 241 | dz2=true; |
yabahiro | 33:dd598bb23ad1 | 242 | dz2i=false; |
yabahiro | 33:dd598bb23ad1 | 243 | dz3=true; |
yabahiro | 33:dd598bb23ad1 | 244 | dz3i=false; |
yabahiro | 33:dd598bb23ad1 | 245 | dz4=true; |
yabahiro | 33:dd598bb23ad1 | 246 | dz4i=false; |
yabahiro | 33:dd598bb23ad1 | 247 | through=false; |
yabahiro | 33:dd598bb23ad1 | 248 | counts=false; |
yabahiro | 33:dd598bb23ad1 | 249 | mtc=false; |
yabahiro | 33:dd598bb23ad1 | 250 | mtc2 = false; |
yabahiro | 33:dd598bb23ad1 | 251 | current = 0; |
yabahiro | 33:dd598bb23ad1 | 252 | SystemProcessInitialize(); |
yabahiro | 33:dd598bb23ad1 | 253 | if(emcs==false) { |
yabahiro | 33:dd598bb23ad1 | 254 | AllActuatorReset(); |
yabahiro | 33:dd598bb23ad1 | 255 | emcs=true; |
yabahiro | 33:dd598bb23ad1 | 256 | } |
yabahiro | 33:dd598bb23ad1 | 257 | } else { |
yabahiro | 33:dd598bb23ad1 | 258 | emcs=false; |
yabahiro | 33:dd598bb23ad1 | 259 | } |
Ryosei | 27:dd9f27fce7d1 | 260 | |
yabahiro | 33:dd598bb23ad1 | 261 | if(mode==0) { |
yabahiro | 33:dd598bb23ad1 | 262 | if(ZoneSW) { |
kishibekairohan | 35:33c6cdba5630 | 263 | LED::LED::TapeledMode(1); |
yabahiro | 33:dd598bb23ad1 | 264 | } else { |
kishibekairohan | 35:33c6cdba5630 | 265 | LED::LED::TapeledMode(2); |
yabahiro | 33:dd598bb23ad1 | 266 | } |
yabahiro | 33:dd598bb23ad1 | 267 | } |
yabahiro | 33:dd598bb23ad1 | 268 | for(int i = 0; i < 8; i++) { |
yabahiro | 33:dd598bb23ad1 | 269 | g[i] = LineHub::GetPara(i); |
yabahiro | 33:dd598bb23ad1 | 270 | } |
yabahiro | 33:dd598bb23ad1 | 271 | if(ThSW) { |
yabahiro | 33:dd598bb23ad1 | 272 | Twsh=1; |
yabahiro | 33:dd598bb23ad1 | 273 | } else { |
yabahiro | 33:dd598bb23ad1 | 274 | Twsh=2; |
yabahiro | 33:dd598bb23ad1 | 275 | } |
yabahiro | 33:dd598bb23ad1 | 276 | |
yabahiro | 33:dd598bb23ad1 | 277 | if(StertSW && start_flag == true) { |
yabahiro | 33:dd598bb23ad1 | 278 | start_flag = false; |
yabahiro | 33:dd598bb23ad1 | 279 | lock = false; |
yabahiro | 33:dd598bb23ad1 | 280 | if(ZoneSW) { |
yabahiro | 33:dd598bb23ad1 | 281 | countss=0; |
yabahiro | 33:dd598bb23ad1 | 282 | mode=1; |
yabahiro | 57:ccfd26b94d8a | 283 | current = 3;//1119変更した |
yabahiro | 33:dd598bb23ad1 | 284 | |
yabahiro | 33:dd598bb23ad1 | 285 | } else { |
kishibekairohan | 34:2fce61e3ebc4 | 286 | countss = 0; |
yabahiro | 33:dd598bb23ad1 | 287 | mode=1; |
yabahiro | 56:61ebd4b759b4 | 288 | current = 2; |
yabahiro | 33:dd598bb23ad1 | 289 | } |
yabahiro | 33:dd598bb23ad1 | 290 | } |
yabahiro | 33:dd598bb23ad1 | 291 | if(DWSW||DOSW) { |
yabahiro | 33:dd598bb23ad1 | 292 | if(DWSW) { |
yabahiro | 33:dd598bb23ad1 | 293 | zyouge=true; |
yabahiro | 33:dd598bb23ad1 | 294 | } |
yabahiro | 33:dd598bb23ad1 | 295 | if(DOSW) { |
yabahiro | 33:dd598bb23ad1 | 296 | if(DOLS) { |
yabahiro | 33:dd598bb23ad1 | 297 | motor[MOTOR_0].pwm = 100; |
yabahiro | 33:dd598bb23ad1 | 298 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 299 | } else { |
yabahiro | 38:40e7fdf3c694 | 300 | motor[MOTOR_0].pwm = 200; |
yabahiro | 33:dd598bb23ad1 | 301 | motor[MOTOR_0].dir = BACK; |
yabahiro | 33:dd598bb23ad1 | 302 | } |
yabahiro | 33:dd598bb23ad1 | 303 | } |
yabahiro | 33:dd598bb23ad1 | 304 | } else if(zyouge==false) { |
yabahiro | 33:dd598bb23ad1 | 305 | motor[MOTOR_0].pwm = 100; |
yabahiro | 33:dd598bb23ad1 | 306 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 307 | } |
yabahiro | 33:dd598bb23ad1 | 308 | if(zyouge) { |
yabahiro | 33:dd598bb23ad1 | 309 | if(Twsh==1) { |
yabahiro | 38:40e7fdf3c694 | 310 | motor[MOTOR_0].pwm = 255; |
yabahiro | 33:dd598bb23ad1 | 311 | motor[MOTOR_0].dir = FOR; |
yabahiro | 33:dd598bb23ad1 | 312 | if(UPLS) { |
yabahiro | 33:dd598bb23ad1 | 313 | motor[MOTOR_0].pwm = 100; |
yabahiro | 33:dd598bb23ad1 | 314 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 315 | zyouge=false; |
yabahiro | 33:dd598bb23ad1 | 316 | } |
yabahiro | 33:dd598bb23ad1 | 317 | } |
yabahiro | 33:dd598bb23ad1 | 318 | if(Twsh==2) { |
yabahiro | 33:dd598bb23ad1 | 319 | motor[MOTOR_0].pwm = 245; |
yabahiro | 33:dd598bb23ad1 | 320 | motor[MOTOR_0].dir = FOR; |
yabahiro | 33:dd598bb23ad1 | 321 | if(UPLS2) { |
yabahiro | 33:dd598bb23ad1 | 322 | motor[MOTOR_0].pwm = 100; |
yabahiro | 33:dd598bb23ad1 | 323 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 324 | zyouge=false; |
yabahiro | 33:dd598bb23ad1 | 325 | } |
yabahiro | 33:dd598bb23ad1 | 326 | } |
yabahiro | 33:dd598bb23ad1 | 327 | } |
yabahiro | 33:dd598bb23ad1 | 328 | if(AIRSW) { |
yabahiro | 33:dd598bb23ad1 | 329 | if(SW_flag==false) { |
yabahiro | 33:dd598bb23ad1 | 330 | if(Air_flag==false) { |
yabahiro | 33:dd598bb23ad1 | 331 | solenoid.solenoid3 = SOLENOID_ON; |
yabahiro | 33:dd598bb23ad1 | 332 | Air_flag=true; |
yabahiro | 33:dd598bb23ad1 | 333 | SW_flag=true; |
yabahiro | 33:dd598bb23ad1 | 334 | } else { |
yabahiro | 33:dd598bb23ad1 | 335 | solenoid.solenoid3 = SOLENOID_OFF; |
yabahiro | 33:dd598bb23ad1 | 336 | Air_flag=false; |
yabahiro | 33:dd598bb23ad1 | 337 | SW_flag=true; |
yabahiro | 33:dd598bb23ad1 | 338 | } |
yabahiro | 33:dd598bb23ad1 | 339 | } |
yabahiro | 33:dd598bb23ad1 | 340 | } else { |
yabahiro | 33:dd598bb23ad1 | 341 | if(SW_flag==true) { |
yabahiro | 33:dd598bb23ad1 | 342 | countss++; |
yabahiro | 33:dd598bb23ad1 | 343 | if(countss>=2000) { |
yabahiro | 33:dd598bb23ad1 | 344 | SW_flag=false; |
yabahiro | 33:dd598bb23ad1 | 345 | countss=0; |
yabahiro | 33:dd598bb23ad1 | 346 | } |
yabahiro | 33:dd598bb23ad1 | 347 | } |
yabahiro | 33:dd598bb23ad1 | 348 | } |
yabahiro | 33:dd598bb23ad1 | 349 | |
yabahiro | 33:dd598bb23ad1 | 350 | |
yabahiro | 33:dd598bb23ad1 | 351 | #ifdef USE_MU |
Ryosei | 27:dd9f27fce7d1 | 352 | controller = CONTROLLER::Controller::GetData(); |
yabahiro | 33:dd598bb23ad1 | 353 | #endif |
yabahiro | 33:dd598bb23ad1 | 354 | |
Ryosei | 27:dd9f27fce7d1 | 355 | |
yabahiro | 33:dd598bb23ad1 | 356 | #ifdef USE_ERRORCHECK |
yabahiro | 38:40e7fdf3c694 | 357 | if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { |
Ryosei | 27:dd9f27fce7d1 | 358 | CONTROLLER::Controller::DataReset(); |
Ryosei | 27:dd9f27fce7d1 | 359 | AllActuatorReset(); |
Ryosei | 27:dd9f27fce7d1 | 360 | lock = true; |
Ryosei | 27:dd9f27fce7d1 | 361 | } else |
yabahiro | 38:40e7fdf3c694 | 362 | |
yabahiro | 33:dd598bb23ad1 | 363 | #endif |
Ryosei | 27:dd9f27fce7d1 | 364 | { |
Ryosei | 27:dd9f27fce7d1 | 365 | |
yabahiro | 33:dd598bb23ad1 | 366 | #ifdef USE_SUBPROCESS |
Ryosei | 27:dd9f27fce7d1 | 367 | if(!lock) { |
Ryosei | 27:dd9f27fce7d1 | 368 | Process[current](); |
Ryosei | 27:dd9f27fce7d1 | 369 | } else |
yabahiro | 33:dd598bb23ad1 | 370 | #endif |
Ryosei | 27:dd9f27fce7d1 | 371 | { |
Ryosei | 27:dd9f27fce7d1 | 372 | //ロック時の処理 |
Ryosei | 27:dd9f27fce7d1 | 373 | } |
Ryosei | 27:dd9f27fce7d1 | 374 | } |
yabahiro | 33:dd598bb23ad1 | 375 | |
Ryosei | 27:dd9f27fce7d1 | 376 | SystemProcessUpdate(); |
Ryosei | 27:dd9f27fce7d1 | 377 | } |
t_yamamoto | 0:669ef71cba68 | 378 | } |
t_yamamoto | 0:669ef71cba68 | 379 | |
kishibekairohan | 2:c015739085d3 | 380 | |
kishibekairohan | 2:c015739085d3 | 381 | |
kishibekairohan | 2:c015739085d3 | 382 | |
t_yamamoto | 0:669ef71cba68 | 383 | #pragma region PROCESS |
t_yamamoto | 0:669ef71cba68 | 384 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 385 | #if USE_PROCESS_NUM>0 |
Ryosei | 27:dd9f27fce7d1 | 386 | static void Process0() |
Ryosei | 27:dd9f27fce7d1 | 387 | { |
yabahiro | 33:dd598bb23ad1 | 388 | |
yabahiro | 33:dd598bb23ad1 | 389 | //AllActuatorReset(); |
t_yamamoto | 0:669ef71cba68 | 390 | } |
t_yamamoto | 0:669ef71cba68 | 391 | #endif |
t_yamamoto | 0:669ef71cba68 | 392 | |
t_yamamoto | 0:669ef71cba68 | 393 | #if USE_PROCESS_NUM>1 |
kishibekairohan | 16:3f2c2d89372b | 394 | static void Process1() |
t_yamamoto | 0:669ef71cba68 | 395 | { |
kishibekairohan | 35:33c6cdba5630 | 396 | LED::LED::TapeledMode(6); |
yabahiro | 33:dd598bb23ad1 | 397 | start_flag = true; |
yabahiro | 33:dd598bb23ad1 | 398 | mtc=false; |
yabahiro | 33:dd598bb23ad1 | 399 | mtc2 = false; |
yabahiro | 33:dd598bb23ad1 | 400 | mode=1; |
yabahiro | 33:dd598bb23ad1 | 401 | cross=0; |
yabahiro | 33:dd598bb23ad1 | 402 | Twsh=1; |
yabahiro | 33:dd598bb23ad1 | 403 | motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); |
yabahiro | 33:dd598bb23ad1 | 404 | motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); |
yabahiro | 33:dd598bb23ad1 | 405 | motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); |
yabahiro | 33:dd598bb23ad1 | 406 | motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); |
Ryosei | 27:dd9f27fce7d1 | 407 | |
kishibekairohan | 42:980af34f6fe3 | 408 | motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; |
kishibekairohan | 42:980af34f6fe3 | 409 | motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; |
kishibekairohan | 42:980af34f6fe3 | 410 | motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; |
kishibekairohan | 42:980af34f6fe3 | 411 | motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; |
yabahiro | 33:dd598bb23ad1 | 412 | |
yabahiro | 33:dd598bb23ad1 | 413 | if(controller->Button.UP||controller->Button.DOWN) { |
yabahiro | 33:dd598bb23ad1 | 414 | |
yabahiro | 33:dd598bb23ad1 | 415 | if(controller->Button.UP) { |
yabahiro | 33:dd598bb23ad1 | 416 | motor[MOTOR_0].pwm =200; |
yabahiro | 33:dd598bb23ad1 | 417 | motor[MOTOR_0].dir = FOR; |
yabahiro | 33:dd598bb23ad1 | 418 | if(UPLS) { |
yabahiro | 33:dd598bb23ad1 | 419 | motor[MOTOR_0].pwm = 0; |
yabahiro | 33:dd598bb23ad1 | 420 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 421 | } |
yabahiro | 33:dd598bb23ad1 | 422 | } |
yabahiro | 33:dd598bb23ad1 | 423 | if(controller->Button.DOWN) { |
yabahiro | 33:dd598bb23ad1 | 424 | motor[MOTOR_0].pwm = 50; |
yabahiro | 33:dd598bb23ad1 | 425 | motor[MOTOR_0].dir = BACK; |
yabahiro | 33:dd598bb23ad1 | 426 | } |
yabahiro | 33:dd598bb23ad1 | 427 | } else { |
yabahiro | 33:dd598bb23ad1 | 428 | motor[MOTOR_0].pwm = 0; |
yabahiro | 33:dd598bb23ad1 | 429 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 33:dd598bb23ad1 | 430 | } |
yabahiro | 33:dd598bb23ad1 | 431 | |
yabahiro | 33:dd598bb23ad1 | 432 | if(controller->Button.Y) { |
yabahiro | 33:dd598bb23ad1 | 433 | if(dz2==true) { |
yabahiro | 33:dd598bb23ad1 | 434 | if(dz2i==false) { |
yabahiro | 33:dd598bb23ad1 | 435 | solenoid.solenoid2 = SOLENOID_ON; |
yabahiro | 33:dd598bb23ad1 | 436 | dz2i=true; |
yabahiro | 33:dd598bb23ad1 | 437 | } else { |
yabahiro | 33:dd598bb23ad1 | 438 | solenoid.solenoid2 = SOLENOID_OFF; |
yabahiro | 33:dd598bb23ad1 | 439 | dz2i=false; |
yabahiro | 33:dd598bb23ad1 | 440 | } |
yabahiro | 33:dd598bb23ad1 | 441 | dz2=false; |
yabahiro | 33:dd598bb23ad1 | 442 | } |
yabahiro | 33:dd598bb23ad1 | 443 | } else { |
yabahiro | 33:dd598bb23ad1 | 444 | dz2=true; |
yabahiro | 33:dd598bb23ad1 | 445 | } |
yabahiro | 33:dd598bb23ad1 | 446 | |
yabahiro | 33:dd598bb23ad1 | 447 | if(controller->Button.A) { |
yabahiro | 33:dd598bb23ad1 | 448 | if(dz3==true) { |
yabahiro | 33:dd598bb23ad1 | 449 | if(dz3i==false) { |
kishibekairohan | 37:e77816810627 | 450 | solenoid.solenoid3 = SOLENOID_ON; |
yabahiro | 33:dd598bb23ad1 | 451 | dz3i=true; |
yabahiro | 33:dd598bb23ad1 | 452 | } else { |
yabahiro | 33:dd598bb23ad1 | 453 | solenoid.solenoid3 = SOLENOID_OFF; |
yabahiro | 33:dd598bb23ad1 | 454 | dz3i=false; |
yabahiro | 33:dd598bb23ad1 | 455 | } |
yabahiro | 33:dd598bb23ad1 | 456 | dz3=false; |
yabahiro | 33:dd598bb23ad1 | 457 | } |
yabahiro | 33:dd598bb23ad1 | 458 | } else { |
yabahiro | 33:dd598bb23ad1 | 459 | dz3=true; |
yabahiro | 33:dd598bb23ad1 | 460 | } |
yabahiro | 33:dd598bb23ad1 | 461 | |
yabahiro | 33:dd598bb23ad1 | 462 | if(controller->Button.B) { |
yabahiro | 33:dd598bb23ad1 | 463 | if(dz4==true) { |
yabahiro | 33:dd598bb23ad1 | 464 | if(dz4i==false) { |
yabahiro | 38:40e7fdf3c694 | 465 | solenoid.solenoid4 = SOLENOID_ON; |
yabahiro | 33:dd598bb23ad1 | 466 | dz4i=true; |
yabahiro | 33:dd598bb23ad1 | 467 | } else { |
yabahiro | 38:40e7fdf3c694 | 468 | solenoid.solenoid4 = SOLENOID_OFF; |
yabahiro | 33:dd598bb23ad1 | 469 | dz4i=false; |
yabahiro | 33:dd598bb23ad1 | 470 | } |
yabahiro | 33:dd598bb23ad1 | 471 | dz4=false; |
yabahiro | 33:dd598bb23ad1 | 472 | } |
yabahiro | 33:dd598bb23ad1 | 473 | } else { |
yabahiro | 33:dd598bb23ad1 | 474 | dz4=true; |
yabahiro | 33:dd598bb23ad1 | 475 | } |
t_yamamoto | 0:669ef71cba68 | 476 | } |
t_yamamoto | 0:669ef71cba68 | 477 | #endif |
t_yamamoto | 0:669ef71cba68 | 478 | |
t_yamamoto | 0:669ef71cba68 | 479 | #if USE_PROCESS_NUM>2 |
kishibekairohan | 16:3f2c2d89372b | 480 | static void Process2() |
Ryosei | 27:dd9f27fce7d1 | 481 | { |
yabahiro | 33:dd598bb23ad1 | 482 | //Oh So 青ゾーン |
yabahiro | 53:694dbd7dfa0e | 483 | LED::LED::TapeledMode(4); |
yabahiro | 33:dd598bb23ad1 | 484 | countss++; |
yabahiro | 50:87f5fa1bc460 | 485 | switch(mode) { |
yabahiro | 50:87f5fa1bc460 | 486 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 487 | fast=40; |
yabahiro | 51:a0bdbddd08f7 | 488 | normal=30; |
yabahiro | 51:a0bdbddd08f7 | 489 | slow=20; |
yabahiro | 51:a0bdbddd08f7 | 490 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 491 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 492 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 493 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 494 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 495 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 496 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 497 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 498 | if(g[2]==0&&countss>=300000) { |
yabahiro | 51:a0bdbddd08f7 | 499 | countss=100000; |
yabahiro | 51:a0bdbddd08f7 | 500 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 501 | mode=3; |
yabahiro | 51:a0bdbddd08f7 | 502 | } |
yabahiro | 50:87f5fa1bc460 | 503 | break; |
yabahiro | 51:a0bdbddd08f7 | 504 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 505 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 506 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 507 | motor[TIRE_FL].pwm = 255; |
yabahiro | 51:a0bdbddd08f7 | 508 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 509 | motor[TIRE_BR].pwm = 255; |
yabahiro | 51:a0bdbddd08f7 | 510 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 511 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 512 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 513 | if(g[1]==0) { |
yabahiro | 51:a0bdbddd08f7 | 514 | mode=10; |
yabahiro | 51:a0bdbddd08f7 | 515 | countss=100000; |
yabahiro | 55:844ac0974740 | 516 | fast=80; |
yabahiro | 55:844ac0974740 | 517 | normal=60; |
yabahiro | 55:844ac0974740 | 518 | slow=40; |
yabahiro | 51:a0bdbddd08f7 | 519 | } |
yabahiro | 50:87f5fa1bc460 | 520 | break; |
yabahiro | 50:87f5fa1bc460 | 521 | case 10: |
yabahiro | 51:a0bdbddd08f7 | 522 | switch(g[1]) { |
yabahiro | 51:a0bdbddd08f7 | 523 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 524 | motor[TIRE_FR].pwm = normal+55; |
yabahiro | 51:a0bdbddd08f7 | 525 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 526 | motor[TIRE_FL].pwm = normal+50; |
yabahiro | 51:a0bdbddd08f7 | 527 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 528 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 51:a0bdbddd08f7 | 529 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 530 | motor[TIRE_BL].pwm = normal+45; |
yabahiro | 51:a0bdbddd08f7 | 531 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 532 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 533 | if(countss>80000) { |
yabahiro | 51:a0bdbddd08f7 | 534 | cross++; |
yabahiro | 51:a0bdbddd08f7 | 535 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 536 | } |
yabahiro | 46:68296caae1af | 537 | |
yabahiro | 51:a0bdbddd08f7 | 538 | if(cross==2) { |
yabahiro | 51:a0bdbddd08f7 | 539 | fast = 10; |
yabahiro | 51:a0bdbddd08f7 | 540 | normal = 10; |
yabahiro | 51:a0bdbddd08f7 | 541 | slow = 5; |
yabahiro | 51:a0bdbddd08f7 | 542 | } |
yabahiro | 46:68296caae1af | 543 | |
kishibekairohan | 45:0cf48a0605cd | 544 | if(cross==3) { |
yabahiro | 51:a0bdbddd08f7 | 545 | fast=40; |
yabahiro | 51:a0bdbddd08f7 | 546 | normal=30; |
yabahiro | 51:a0bdbddd08f7 | 547 | slow=20; |
yabahiro | 51:a0bdbddd08f7 | 548 | mode=11; |
yabahiro | 39:784552c26163 | 549 | countss=0; |
yabahiro | 39:784552c26163 | 550 | cross=0; |
yabahiro | 39:784552c26163 | 551 | } |
yabahiro | 39:784552c26163 | 552 | break; |
yabahiro | 39:784552c26163 | 553 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 554 | motor[TIRE_FR].pwm = fast+55; |
yabahiro | 51:a0bdbddd08f7 | 555 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 556 | motor[TIRE_FL].pwm = fast+50; |
yabahiro | 51:a0bdbddd08f7 | 557 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 558 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 51:a0bdbddd08f7 | 559 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 560 | motor[TIRE_BL].pwm = fast+45; |
yabahiro | 51:a0bdbddd08f7 | 561 | motor[TIRE_BL].dir = BACK; |
yabahiro | 39:784552c26163 | 562 | mtc=true; |
yabahiro | 39:784552c26163 | 563 | break; |
yabahiro | 39:784552c26163 | 564 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 565 | motor[TIRE_FR].pwm = fast+55; |
yabahiro | 51:a0bdbddd08f7 | 566 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 567 | motor[TIRE_FL].pwm = slow+50; |
yabahiro | 51:a0bdbddd08f7 | 568 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 569 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 51:a0bdbddd08f7 | 570 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 571 | motor[TIRE_BL].pwm = fast+45; |
yabahiro | 51:a0bdbddd08f7 | 572 | motor[TIRE_BL].dir = BACK; |
yabahiro | 39:784552c26163 | 573 | mtc=true; |
yabahiro | 39:784552c26163 | 574 | break; |
yabahiro | 39:784552c26163 | 575 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 576 | motor[TIRE_FR].pwm = slow+55; |
yabahiro | 51:a0bdbddd08f7 | 577 | motor[TIRE_FR].dir = FOR; |
yabahiro | 39:784552c26163 | 578 | motor[TIRE_FL].pwm = 0; |
yabahiro | 39:784552c26163 | 579 | motor[TIRE_FL].dir = FREE; |
yabahiro | 39:784552c26163 | 580 | motor[TIRE_BR].pwm = 0; |
yabahiro | 39:784552c26163 | 581 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 582 | motor[TIRE_BL].pwm = slow+45; |
yabahiro | 51:a0bdbddd08f7 | 583 | motor[TIRE_BL].dir = BACK; |
yabahiro | 39:784552c26163 | 584 | mtc=true; |
yabahiro | 39:784552c26163 | 585 | break; |
yabahiro | 39:784552c26163 | 586 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 587 | motor[TIRE_FR].pwm = normal+55; |
yabahiro | 51:a0bdbddd08f7 | 588 | motor[TIRE_FR].dir = FOR; |
yabahiro | 39:784552c26163 | 589 | motor[TIRE_FL].pwm = 0; |
yabahiro | 39:784552c26163 | 590 | motor[TIRE_FL].dir = FREE; |
yabahiro | 39:784552c26163 | 591 | motor[TIRE_BR].pwm = 0; |
yabahiro | 39:784552c26163 | 592 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 593 | motor[TIRE_BL].pwm = normal+45; |
yabahiro | 51:a0bdbddd08f7 | 594 | motor[TIRE_BL].dir = BACK; |
yabahiro | 39:784552c26163 | 595 | mtc=true; |
yabahiro | 39:784552c26163 | 596 | break; |
yabahiro | 39:784552c26163 | 597 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 598 | motor[TIRE_FR].pwm = slow+55; |
yabahiro | 51:a0bdbddd08f7 | 599 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 600 | motor[TIRE_FL].pwm = fast+50; |
yabahiro | 51:a0bdbddd08f7 | 601 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 602 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 51:a0bdbddd08f7 | 603 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 604 | motor[TIRE_BL].pwm = slow+45; |
yabahiro | 51:a0bdbddd08f7 | 605 | motor[TIRE_BL].dir = BACK; |
yabahiro | 39:784552c26163 | 606 | mtc=true; |
yabahiro | 39:784552c26163 | 607 | break; |
yabahiro | 39:784552c26163 | 608 | case 3: |
yabahiro | 39:784552c26163 | 609 | motor[TIRE_FR].pwm = 0; |
yabahiro | 39:784552c26163 | 610 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 611 | motor[TIRE_FL].pwm = slow+50; |
yabahiro | 51:a0bdbddd08f7 | 612 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 613 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 51:a0bdbddd08f7 | 614 | motor[TIRE_BR].dir = FOR; |
yabahiro | 39:784552c26163 | 615 | motor[TIRE_BL].pwm = 0; |
yabahiro | 39:784552c26163 | 616 | motor[TIRE_BL].dir = FREE; |
yabahiro | 39:784552c26163 | 617 | mtc=true; |
yabahiro | 39:784552c26163 | 618 | break; |
yabahiro | 39:784552c26163 | 619 | case 2: |
yabahiro | 39:784552c26163 | 620 | motor[TIRE_FR].pwm = 0; |
yabahiro | 39:784552c26163 | 621 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 622 | motor[TIRE_FL].pwm = normal+50; |
yabahiro | 51:a0bdbddd08f7 | 623 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 624 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 51:a0bdbddd08f7 | 625 | motor[TIRE_BR].dir = FOR; |
yabahiro | 39:784552c26163 | 626 | motor[TIRE_BL].pwm = 0; |
yabahiro | 39:784552c26163 | 627 | motor[TIRE_BL].dir = FREE; |
yabahiro | 39:784552c26163 | 628 | mtc=true; |
yabahiro | 39:784552c26163 | 629 | break; |
yabahiro | 39:784552c26163 | 630 | } |
yabahiro | 51:a0bdbddd08f7 | 631 | if(g[1]!=98&&mtc==true) { |
yabahiro | 39:784552c26163 | 632 | switch(g[0]) { |
yabahiro | 39:784552c26163 | 633 | case 0: |
yabahiro | 39:784552c26163 | 634 | mtc=false; |
yabahiro | 39:784552c26163 | 635 | break; |
yabahiro | 39:784552c26163 | 636 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 637 | motor[TIRE_FL].pwm += 3; |
yabahiro | 39:784552c26163 | 638 | mtc=false; |
yabahiro | 39:784552c26163 | 639 | break; |
yabahiro | 39:784552c26163 | 640 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 641 | motor[TIRE_FL].pwm += 6; |
yabahiro | 39:784552c26163 | 642 | mtc=false; |
yabahiro | 39:784552c26163 | 643 | break; |
yabahiro | 39:784552c26163 | 644 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 645 | motor[TIRE_FL].pwm += 9; |
yabahiro | 51:a0bdbddd08f7 | 646 | motor[TIRE_FR].pwm = 0; |
yabahiro | 39:784552c26163 | 647 | mtc=false; |
yabahiro | 39:784552c26163 | 648 | break; |
yabahiro | 39:784552c26163 | 649 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 650 | motor[TIRE_FR].pwm += 3; |
yabahiro | 39:784552c26163 | 651 | mtc=false; |
yabahiro | 39:784552c26163 | 652 | break; |
yabahiro | 39:784552c26163 | 653 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 654 | motor[TIRE_FR].pwm += 6; |
yabahiro | 39:784552c26163 | 655 | mtc=false; |
yabahiro | 39:784552c26163 | 656 | break; |
yabahiro | 39:784552c26163 | 657 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 658 | motor[TIRE_FR].pwm += 9; |
yabahiro | 51:a0bdbddd08f7 | 659 | motor[TIRE_FL].pwm = 0; |
yabahiro | 39:784552c26163 | 660 | mtc=false; |
yabahiro | 39:784552c26163 | 661 | break; |
yabahiro | 39:784552c26163 | 662 | } |
yabahiro | 39:784552c26163 | 663 | } |
yabahiro | 51:a0bdbddd08f7 | 664 | break; |
yabahiro | 51:a0bdbddd08f7 | 665 | case 11: |
yabahiro | 53:694dbd7dfa0e | 666 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 667 | motor[TIRE_FR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 668 | motor[TIRE_FL].pwm = 15; |
yabahiro | 53:694dbd7dfa0e | 669 | motor[TIRE_FL].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 670 | motor[TIRE_BR].pwm = 15; |
yabahiro | 53:694dbd7dfa0e | 671 | motor[TIRE_BR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 672 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 673 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 674 | if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { |
yabahiro | 51:a0bdbddd08f7 | 675 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 676 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 677 | mode=20; |
yabahiro | 51:a0bdbddd08f7 | 678 | } |
yabahiro | 51:a0bdbddd08f7 | 679 | break; |
yabahiro | 51:a0bdbddd08f7 | 680 | case 20: |
yabahiro | 51:a0bdbddd08f7 | 681 | solenoid.solenoid2= SOLENOID_ON; |
yabahiro | 51:a0bdbddd08f7 | 682 | switch(g[2]) { |
yabahiro | 51:a0bdbddd08f7 | 683 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 684 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 685 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 686 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 687 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 688 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 689 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 690 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 691 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 692 | if(countss>30000) { |
yabahiro | 51:a0bdbddd08f7 | 693 | cross++; |
yabahiro | 51:a0bdbddd08f7 | 694 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 695 | } |
yabahiro | 51:a0bdbddd08f7 | 696 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 697 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 698 | if(cross==1) { |
yabahiro | 51:a0bdbddd08f7 | 699 | fast=20; |
yabahiro | 51:a0bdbddd08f7 | 700 | normal=15; |
yabahiro | 51:a0bdbddd08f7 | 701 | slow=15; |
yabahiro | 51:a0bdbddd08f7 | 702 | } |
yabahiro | 51:a0bdbddd08f7 | 703 | if(cross==2) { |
yabahiro | 51:a0bdbddd08f7 | 704 | mode=21; |
yabahiro | 51:a0bdbddd08f7 | 705 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 706 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 707 | } |
yabahiro | 51:a0bdbddd08f7 | 708 | } else { |
yabahiro | 51:a0bdbddd08f7 | 709 | if(cross==3) { |
yabahiro | 51:a0bdbddd08f7 | 710 | mode=21; |
yabahiro | 51:a0bdbddd08f7 | 711 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 712 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 713 | } |
yabahiro | 51:a0bdbddd08f7 | 714 | } |
yabahiro | 51:a0bdbddd08f7 | 715 | break; |
yabahiro | 51:a0bdbddd08f7 | 716 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 717 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 718 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 719 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 720 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 721 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 722 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 723 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 724 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 725 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 726 | break; |
yabahiro | 51:a0bdbddd08f7 | 727 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 728 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 729 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 730 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 731 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 732 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 733 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 734 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 735 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 736 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 737 | break; |
yabahiro | 51:a0bdbddd08f7 | 738 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 739 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 740 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 741 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 742 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 743 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 744 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 745 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 746 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 747 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 748 | break; |
yabahiro | 51:a0bdbddd08f7 | 749 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 750 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 751 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 752 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 753 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 754 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 755 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 756 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 757 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 758 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 759 | break; |
yabahiro | 51:a0bdbddd08f7 | 760 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 761 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 762 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 763 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 764 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 765 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 766 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 767 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 51:a0bdbddd08f7 | 768 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 769 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 770 | break; |
yabahiro | 51:a0bdbddd08f7 | 771 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 772 | motor[TIRE_FR].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 773 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 774 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 775 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 776 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 777 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 778 | motor[TIRE_BL].pwm = slow+20; |
yabahiro | 51:a0bdbddd08f7 | 779 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 780 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 781 | break; |
yabahiro | 51:a0bdbddd08f7 | 782 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 783 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 784 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 785 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 786 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 787 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 788 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 789 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 51:a0bdbddd08f7 | 790 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 791 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 792 | break; |
yabahiro | 51:a0bdbddd08f7 | 793 | } |
yabahiro | 51:a0bdbddd08f7 | 794 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 51:a0bdbddd08f7 | 795 | switch(g[3]) { |
yabahiro | 51:a0bdbddd08f7 | 796 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 797 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 798 | break; |
yabahiro | 51:a0bdbddd08f7 | 799 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 800 | motor[TIRE_BL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 801 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 802 | break; |
yabahiro | 51:a0bdbddd08f7 | 803 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 804 | motor[TIRE_BL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 805 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 806 | break; |
yabahiro | 51:a0bdbddd08f7 | 807 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 808 | motor[TIRE_BL].pwm += 15; |
yabahiro | 51:a0bdbddd08f7 | 809 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 810 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 811 | break; |
yabahiro | 51:a0bdbddd08f7 | 812 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 813 | motor[TIRE_FL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 814 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 815 | break; |
yabahiro | 51:a0bdbddd08f7 | 816 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 817 | motor[TIRE_FL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 818 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 819 | break; |
yabahiro | 51:a0bdbddd08f7 | 820 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 821 | motor[TIRE_FL].pwm += 15; |
yabahiro | 51:a0bdbddd08f7 | 822 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 823 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 824 | break; |
yabahiro | 51:a0bdbddd08f7 | 825 | } |
yabahiro | 51:a0bdbddd08f7 | 826 | } |
yabahiro | 51:a0bdbddd08f7 | 827 | break; |
yabahiro | 51:a0bdbddd08f7 | 828 | case 21: |
yabahiro | 51:a0bdbddd08f7 | 829 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 830 | motor[TIRE_FR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 831 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 832 | motor[TIRE_FL].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 833 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 834 | motor[TIRE_BR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 835 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 836 | motor[TIRE_BL].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 837 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 838 | if(g[3]==98&&countss>=10000) { |
yabahiro | 51:a0bdbddd08f7 | 839 | mode=22; |
yabahiro | 51:a0bdbddd08f7 | 840 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 841 | fast=60; |
yabahiro | 51:a0bdbddd08f7 | 842 | normal=40; |
yabahiro | 51:a0bdbddd08f7 | 843 | slow=20; |
yabahiro | 51:a0bdbddd08f7 | 844 | } |
yabahiro | 51:a0bdbddd08f7 | 845 | } else { |
yabahiro | 51:a0bdbddd08f7 | 846 | motor[TIRE_FR].pwm = 255; |
yabahiro | 51:a0bdbddd08f7 | 847 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 848 | motor[TIRE_FL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 849 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 850 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 851 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 852 | motor[TIRE_BL].pwm = 255; |
yabahiro | 51:a0bdbddd08f7 | 853 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 854 | if(g[0]==0) { |
yabahiro | 51:a0bdbddd08f7 | 855 | mode=22; |
yabahiro | 51:a0bdbddd08f7 | 856 | fast=60; |
yabahiro | 51:a0bdbddd08f7 | 857 | normal=40; |
yabahiro | 51:a0bdbddd08f7 | 858 | slow=25; |
yabahiro | 51:a0bdbddd08f7 | 859 | } |
yabahiro | 51:a0bdbddd08f7 | 860 | } |
yabahiro | 51:a0bdbddd08f7 | 861 | break; |
yabahiro | 51:a0bdbddd08f7 | 862 | case 22: |
yabahiro | 51:a0bdbddd08f7 | 863 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 864 | if(g[4]==3||g[4]==2) { |
yabahiro | 51:a0bdbddd08f7 | 865 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 866 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 867 | motor[TIRE_FL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 868 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 869 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 870 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 871 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 872 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 873 | } |
yabahiro | 51:a0bdbddd08f7 | 874 | if(g[4]==253||g[4]==254) { |
yabahiro | 51:a0bdbddd08f7 | 875 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 876 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 877 | motor[TIRE_FL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 878 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 879 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 880 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 881 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 882 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 883 | } |
yabahiro | 51:a0bdbddd08f7 | 884 | switch(g[2]) { |
yabahiro | 51:a0bdbddd08f7 | 885 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 886 | motor[TIRE_FR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 887 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 888 | motor[TIRE_BR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 889 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 890 | break; |
yabahiro | 51:a0bdbddd08f7 | 891 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 892 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 893 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 894 | motor[TIRE_BR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 895 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 896 | break; |
yabahiro | 51:a0bdbddd08f7 | 897 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 898 | motor[TIRE_FR].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 899 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 900 | motor[TIRE_BR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 901 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 902 | break; |
yabahiro | 51:a0bdbddd08f7 | 903 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 904 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 905 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 906 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 907 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 908 | break; |
yabahiro | 51:a0bdbddd08f7 | 909 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 910 | motor[TIRE_FR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 911 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 912 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 913 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 914 | break; |
yabahiro | 51:a0bdbddd08f7 | 915 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 916 | motor[TIRE_FR].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 917 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 918 | motor[TIRE_BR].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 919 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 920 | break; |
yabahiro | 51:a0bdbddd08f7 | 921 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 922 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 923 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 924 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 925 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 926 | break; |
yabahiro | 51:a0bdbddd08f7 | 927 | } |
yabahiro | 51:a0bdbddd08f7 | 928 | switch(g[3]) { |
yabahiro | 51:a0bdbddd08f7 | 929 | case 0: |
yabahiro | 53:694dbd7dfa0e | 930 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 931 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 932 | motor[TIRE_BL].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 933 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 934 | break; |
yabahiro | 51:a0bdbddd08f7 | 935 | case 255: |
yabahiro | 53:694dbd7dfa0e | 936 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 937 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 938 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 939 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 940 | case 253: |
yabahiro | 53:694dbd7dfa0e | 941 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 942 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 943 | motor[TIRE_BL].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 944 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 945 | break; |
yabahiro | 51:a0bdbddd08f7 | 946 | case 254: |
yabahiro | 53:694dbd7dfa0e | 947 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 948 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 949 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 950 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 951 | break; |
yabahiro | 51:a0bdbddd08f7 | 952 | case 1: |
yabahiro | 53:694dbd7dfa0e | 953 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 954 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 955 | motor[TIRE_BL].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 956 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 957 | break; |
yabahiro | 51:a0bdbddd08f7 | 958 | case 3: |
yabahiro | 53:694dbd7dfa0e | 959 | motor[TIRE_FL].pwm = 45; |
yabahiro | 51:a0bdbddd08f7 | 960 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 961 | motor[TIRE_BL].pwm = 20; |
yabahiro | 51:a0bdbddd08f7 | 962 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 963 | break; |
yabahiro | 51:a0bdbddd08f7 | 964 | case 2: |
yabahiro | 53:694dbd7dfa0e | 965 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 966 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 967 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 968 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 969 | break; |
yabahiro | 51:a0bdbddd08f7 | 970 | } |
yabahiro | 51:a0bdbddd08f7 | 971 | if(g[3]==0&&g[2]==0) { |
yabahiro | 51:a0bdbddd08f7 | 972 | mode=23; |
yabahiro | 51:a0bdbddd08f7 | 973 | countss=0; |
yabahiro | 54:4167f10ecad0 | 974 | fast=80; |
yabahiro | 54:4167f10ecad0 | 975 | normal=60; |
yabahiro | 54:4167f10ecad0 | 976 | slow=30; |
yabahiro | 51:a0bdbddd08f7 | 977 | } |
yabahiro | 51:a0bdbddd08f7 | 978 | } else { |
yabahiro | 51:a0bdbddd08f7 | 979 | if(g[0]==98) { |
yabahiro | 51:a0bdbddd08f7 | 980 | slow=25; |
yabahiro | 51:a0bdbddd08f7 | 981 | normal=30; |
yabahiro | 53:694dbd7dfa0e | 982 | fast=30; |
yabahiro | 51:a0bdbddd08f7 | 983 | } |
yabahiro | 51:a0bdbddd08f7 | 984 | if(g[3]==0) { |
yabahiro | 51:a0bdbddd08f7 | 985 | slow=25; |
yabahiro | 51:a0bdbddd08f7 | 986 | normal=40; |
yabahiro | 51:a0bdbddd08f7 | 987 | fast=60; |
yabahiro | 51:a0bdbddd08f7 | 988 | mode=23; |
yabahiro | 51:a0bdbddd08f7 | 989 | } |
yabahiro | 51:a0bdbddd08f7 | 990 | switch(g[0]) { |
yabahiro | 51:a0bdbddd08f7 | 991 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 992 | solenoid.solenoid2 = SOLENOID_OFF; |
yabahiro | 51:a0bdbddd08f7 | 993 | fast=40; |
yabahiro | 51:a0bdbddd08f7 | 994 | normal=30; |
yabahiro | 51:a0bdbddd08f7 | 995 | slow=25; |
yabahiro | 51:a0bdbddd08f7 | 996 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 997 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 998 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 51:a0bdbddd08f7 | 999 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1000 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1001 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1002 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1003 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1004 | break; |
yabahiro | 51:a0bdbddd08f7 | 1005 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1006 | motor[TIRE_FR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1007 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1008 | motor[TIRE_FL].pwm = fast+5; |
yabahiro | 51:a0bdbddd08f7 | 1009 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1010 | motor[TIRE_BR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1011 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1012 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1013 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1014 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1015 | break; |
yabahiro | 51:a0bdbddd08f7 | 1016 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1017 | motor[TIRE_FR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1018 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1019 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 51:a0bdbddd08f7 | 1020 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1021 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1022 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1023 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1024 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1025 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1026 | break; |
yabahiro | 51:a0bdbddd08f7 | 1027 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1028 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1029 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1030 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1031 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1032 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1033 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1034 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1035 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1036 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1037 | break; |
yabahiro | 51:a0bdbddd08f7 | 1038 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1039 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1040 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1041 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1042 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1043 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1044 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1045 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1046 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1047 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1048 | break; |
yabahiro | 51:a0bdbddd08f7 | 1049 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1050 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1051 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1052 | motor[TIRE_FL].pwm = fast+5; |
yabahiro | 51:a0bdbddd08f7 | 1053 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1054 | motor[TIRE_BR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1055 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1056 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1057 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1058 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1059 | break; |
yabahiro | 51:a0bdbddd08f7 | 1060 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1061 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1062 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1063 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 51:a0bdbddd08f7 | 1064 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1065 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1066 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1067 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1068 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1069 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1070 | break; |
yabahiro | 51:a0bdbddd08f7 | 1071 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1072 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1073 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1074 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 51:a0bdbddd08f7 | 1075 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1076 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1077 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1078 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1079 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1080 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1081 | break; |
yabahiro | 51:a0bdbddd08f7 | 1082 | } |
yabahiro | 51:a0bdbddd08f7 | 1083 | if(g[0]!=98&&mtc==true) { |
yabahiro | 51:a0bdbddd08f7 | 1084 | switch(g[0]) { |
yabahiro | 51:a0bdbddd08f7 | 1085 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1086 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1087 | break; |
yabahiro | 51:a0bdbddd08f7 | 1088 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1089 | motor[TIRE_BR].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1090 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1091 | break; |
yabahiro | 51:a0bdbddd08f7 | 1092 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1093 | motor[TIRE_BR].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1094 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1095 | break; |
yabahiro | 51:a0bdbddd08f7 | 1096 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1097 | motor[TIRE_BR].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1098 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1099 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1100 | break; |
yabahiro | 51:a0bdbddd08f7 | 1101 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1102 | motor[TIRE_BL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1103 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1104 | break; |
yabahiro | 51:a0bdbddd08f7 | 1105 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1106 | motor[TIRE_BL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1107 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1108 | break; |
yabahiro | 51:a0bdbddd08f7 | 1109 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1110 | motor[TIRE_BL].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1111 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1112 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1113 | break; |
yabahiro | 51:a0bdbddd08f7 | 1114 | } |
yabahiro | 51:a0bdbddd08f7 | 1115 | } |
yabahiro | 51:a0bdbddd08f7 | 1116 | } |
yabahiro | 51:a0bdbddd08f7 | 1117 | break; |
yabahiro | 51:a0bdbddd08f7 | 1118 | case 23: |
yabahiro | 51:a0bdbddd08f7 | 1119 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 1120 | switch(g[4]) { |
yabahiro | 51:a0bdbddd08f7 | 1121 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 1122 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1123 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1124 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1125 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1126 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1127 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1128 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1129 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1130 | break; |
yabahiro | 51:a0bdbddd08f7 | 1131 | case 99: |
yabahiro | 51:a0bdbddd08f7 | 1132 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1133 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1134 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1135 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1136 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1137 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1138 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1139 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1140 | break; |
yabahiro | 51:a0bdbddd08f7 | 1141 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1142 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1143 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1144 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1145 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1146 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1147 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1148 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1149 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1150 | break; |
yabahiro | 51:a0bdbddd08f7 | 1151 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1152 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1153 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1154 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1155 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1156 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1157 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1158 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1159 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1160 | break; |
yabahiro | 51:a0bdbddd08f7 | 1161 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1162 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1163 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1164 | motor[TIRE_FL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1165 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1166 | motor[TIRE_BR].pwm = slow-5; |
yabahiro | 51:a0bdbddd08f7 | 1167 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1168 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1169 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1170 | break; |
yabahiro | 51:a0bdbddd08f7 | 1171 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1172 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1173 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1174 | motor[TIRE_FL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1175 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1176 | motor[TIRE_BR].pwm = normal-5; |
yabahiro | 51:a0bdbddd08f7 | 1177 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1178 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1179 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1180 | break; |
yabahiro | 51:a0bdbddd08f7 | 1181 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1182 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1183 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1184 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1185 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1186 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1187 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1188 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1189 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1190 | break; |
yabahiro | 51:a0bdbddd08f7 | 1191 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1192 | motor[TIRE_FR].pwm = slow-5; |
yabahiro | 51:a0bdbddd08f7 | 1193 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1194 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1195 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1196 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1197 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1198 | motor[TIRE_BL].pwm = slow-5; |
yabahiro | 51:a0bdbddd08f7 | 1199 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1200 | break; |
yabahiro | 51:a0bdbddd08f7 | 1201 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1202 | motor[TIRE_FR].pwm = normal-5; |
yabahiro | 51:a0bdbddd08f7 | 1203 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1204 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1205 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1206 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 51:a0bdbddd08f7 | 1207 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1208 | motor[TIRE_BL].pwm = normal-5; |
yabahiro | 51:a0bdbddd08f7 | 1209 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1210 | break; |
yabahiro | 51:a0bdbddd08f7 | 1211 | } |
yabahiro | 51:a0bdbddd08f7 | 1212 | if(g[0]==98) { |
yabahiro | 51:a0bdbddd08f7 | 1213 | fast=20; |
yabahiro | 51:a0bdbddd08f7 | 1214 | normal=15; |
yabahiro | 51:a0bdbddd08f7 | 1215 | slow=10; |
yabahiro | 51:a0bdbddd08f7 | 1216 | } |
yabahiro | 51:a0bdbddd08f7 | 1217 | if(g[2]==254&&countss>=100000) { |
yabahiro | 51:a0bdbddd08f7 | 1218 | mode=24; |
yabahiro | 51:a0bdbddd08f7 | 1219 | solenoid.solenoid2 = SOLENOID_OFF; |
yabahiro | 51:a0bdbddd08f7 | 1220 | countss=0; |
yabahiro | 54:4167f10ecad0 | 1221 | fast=80; |
yabahiro | 54:4167f10ecad0 | 1222 | normal=60; |
yabahiro | 54:4167f10ecad0 | 1223 | slow=30; |
yabahiro | 51:a0bdbddd08f7 | 1224 | } |
yabahiro | 51:a0bdbddd08f7 | 1225 | } else { |
yabahiro | 51:a0bdbddd08f7 | 1226 | if(UPLS==true||DOLS==true) { |
yabahiro | 51:a0bdbddd08f7 | 1227 | if(UPLS==true||DOLS==true) { |
yabahiro | 51:a0bdbddd08f7 | 1228 | motor[MOTOR_0].pwm = 200; |
yabahiro | 51:a0bdbddd08f7 | 1229 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1230 | fast=60; |
yabahiro | 51:a0bdbddd08f7 | 1231 | normal=40; |
yabahiro | 53:694dbd7dfa0e | 1232 | slow=30; |
yabahiro | 51:a0bdbddd08f7 | 1233 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1234 | mode=24; |
yabahiro | 51:a0bdbddd08f7 | 1235 | } |
yabahiro | 51:a0bdbddd08f7 | 1236 | } else { |
yabahiro | 51:a0bdbddd08f7 | 1237 | motor[MOTOR_0].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1238 | motor[MOTOR_0].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1239 | } |
yabahiro | 51:a0bdbddd08f7 | 1240 | motor[TIRE_FR].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1241 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1242 | motor[TIRE_FL].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1243 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1244 | motor[TIRE_BR].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1245 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1246 | motor[TIRE_BL].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1247 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1248 | } |
yabahiro | 51:a0bdbddd08f7 | 1249 | break; |
yabahiro | 51:a0bdbddd08f7 | 1250 | case 24: |
yabahiro | 51:a0bdbddd08f7 | 1251 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 1252 | if(g[4]==3||g[4]==2) { |
yabahiro | 51:a0bdbddd08f7 | 1253 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1254 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1255 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1256 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1257 | motor[TIRE_BR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1258 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1259 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1260 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1261 | } |
yabahiro | 51:a0bdbddd08f7 | 1262 | if(g[4]==253||g[4]==254) { |
yabahiro | 54:4167f10ecad0 | 1263 | motor[TIRE_FR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1264 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1265 | motor[TIRE_FL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1266 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1267 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1268 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1269 | motor[TIRE_BL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1270 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1271 | } |
yabahiro | 51:a0bdbddd08f7 | 1272 | switch(g[2]) { |
yabahiro | 51:a0bdbddd08f7 | 1273 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1274 | motor[TIRE_FR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1275 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1276 | motor[TIRE_BR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1277 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1278 | break; |
yabahiro | 51:a0bdbddd08f7 | 1279 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1280 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1281 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1282 | motor[TIRE_BR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1283 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1284 | break; |
yabahiro | 51:a0bdbddd08f7 | 1285 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1286 | motor[TIRE_FR].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 1287 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1288 | motor[TIRE_BR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1289 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1290 | break; |
yabahiro | 51:a0bdbddd08f7 | 1291 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1292 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1293 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1294 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1295 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1296 | break; |
yabahiro | 51:a0bdbddd08f7 | 1297 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1298 | motor[TIRE_FR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1299 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1300 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1301 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1302 | break; |
yabahiro | 51:a0bdbddd08f7 | 1303 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1304 | motor[TIRE_FR].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1305 | motor[TIRE_FR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1306 | motor[TIRE_BR].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 1307 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1308 | break; |
yabahiro | 51:a0bdbddd08f7 | 1309 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1310 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1311 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1312 | motor[TIRE_BR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1313 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1314 | break; |
yabahiro | 51:a0bdbddd08f7 | 1315 | } |
yabahiro | 51:a0bdbddd08f7 | 1316 | switch(g[3]) { |
yabahiro | 51:a0bdbddd08f7 | 1317 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1318 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1319 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1320 | motor[TIRE_BL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1321 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1322 | break; |
yabahiro | 51:a0bdbddd08f7 | 1323 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1324 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1325 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1326 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1327 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1328 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1329 | motor[TIRE_FL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1330 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1331 | motor[TIRE_BL].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 1332 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1333 | break; |
yabahiro | 51:a0bdbddd08f7 | 1334 | case 254: |
yabahiro | 53:694dbd7dfa0e | 1335 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 1336 | motor[TIRE_FL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1337 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1338 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1339 | break; |
yabahiro | 51:a0bdbddd08f7 | 1340 | case 1: |
yabahiro | 53:694dbd7dfa0e | 1341 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 1342 | motor[TIRE_FL].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1343 | motor[TIRE_BL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1344 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1345 | break; |
yabahiro | 51:a0bdbddd08f7 | 1346 | case 3: |
yabahiro | 53:694dbd7dfa0e | 1347 | motor[TIRE_FL].pwm = 45; |
yabahiro | 51:a0bdbddd08f7 | 1348 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1349 | motor[TIRE_BL].pwm = 25; |
yabahiro | 51:a0bdbddd08f7 | 1350 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1351 | break; |
yabahiro | 51:a0bdbddd08f7 | 1352 | case 2: |
yabahiro | 53:694dbd7dfa0e | 1353 | motor[TIRE_FL].pwm = 35; |
yabahiro | 51:a0bdbddd08f7 | 1354 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1355 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1356 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1357 | break; |
yabahiro | 51:a0bdbddd08f7 | 1358 | } |
yabahiro | 51:a0bdbddd08f7 | 1359 | if(g[3]==0&&g[2]==0) { |
yabahiro | 51:a0bdbddd08f7 | 1360 | mode=25; |
yabahiro | 51:a0bdbddd08f7 | 1361 | } |
yabahiro | 51:a0bdbddd08f7 | 1362 | } else { |
yabahiro | 53:694dbd7dfa0e | 1363 | if(g[0]==98&&countss>=40000) { |
yabahiro | 51:a0bdbddd08f7 | 1364 | mode=31; |
yabahiro | 53:694dbd7dfa0e | 1365 | fast=60; |
yabahiro | 53:694dbd7dfa0e | 1366 | normal=40; |
yabahiro | 53:694dbd7dfa0e | 1367 | slow=30; |
yabahiro | 51:a0bdbddd08f7 | 1368 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1369 | } |
yabahiro | 51:a0bdbddd08f7 | 1370 | switch(g[0]) { |
yabahiro | 51:a0bdbddd08f7 | 1371 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 1372 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1373 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1374 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 51:a0bdbddd08f7 | 1375 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1376 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1377 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1378 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1379 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1380 | break; |
yabahiro | 51:a0bdbddd08f7 | 1381 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1382 | motor[TIRE_FR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1383 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1384 | motor[TIRE_FL].pwm = fast+5; |
yabahiro | 51:a0bdbddd08f7 | 1385 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1386 | motor[TIRE_BR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1387 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1388 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1389 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1390 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1391 | break; |
yabahiro | 51:a0bdbddd08f7 | 1392 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1393 | motor[TIRE_FR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1394 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1395 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 51:a0bdbddd08f7 | 1396 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1397 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1398 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1399 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1400 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1401 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1402 | break; |
yabahiro | 51:a0bdbddd08f7 | 1403 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1404 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1405 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1406 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1407 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1408 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1409 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1410 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1411 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1412 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1413 | break; |
yabahiro | 51:a0bdbddd08f7 | 1414 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1415 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1416 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1417 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1418 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1419 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1420 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1421 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1422 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1423 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1424 | break; |
yabahiro | 51:a0bdbddd08f7 | 1425 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1426 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1427 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1428 | motor[TIRE_FL].pwm = fast+5; |
yabahiro | 51:a0bdbddd08f7 | 1429 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1430 | motor[TIRE_BR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1431 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1432 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1433 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1434 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1435 | break; |
yabahiro | 51:a0bdbddd08f7 | 1436 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1437 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1438 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1439 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 51:a0bdbddd08f7 | 1440 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1441 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1442 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1443 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1444 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1445 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1446 | break; |
yabahiro | 51:a0bdbddd08f7 | 1447 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1448 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1449 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1450 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 51:a0bdbddd08f7 | 1451 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1452 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1453 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1454 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1455 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1456 | mtc=true; |
yabahiro | 51:a0bdbddd08f7 | 1457 | break; |
yabahiro | 51:a0bdbddd08f7 | 1458 | } |
yabahiro | 51:a0bdbddd08f7 | 1459 | if(g[1]!=98&&mtc==true) { |
yabahiro | 51:a0bdbddd08f7 | 1460 | switch(g[1]) { |
yabahiro | 51:a0bdbddd08f7 | 1461 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1462 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1463 | break; |
yabahiro | 51:a0bdbddd08f7 | 1464 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1465 | motor[TIRE_BR].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1466 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1467 | break; |
yabahiro | 51:a0bdbddd08f7 | 1468 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1469 | motor[TIRE_BR].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1470 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1471 | break; |
yabahiro | 51:a0bdbddd08f7 | 1472 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1473 | motor[TIRE_BR].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1474 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1475 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1476 | break; |
yabahiro | 51:a0bdbddd08f7 | 1477 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1478 | motor[TIRE_BL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1479 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1480 | break; |
yabahiro | 51:a0bdbddd08f7 | 1481 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1482 | motor[TIRE_BL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1483 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1484 | break; |
yabahiro | 51:a0bdbddd08f7 | 1485 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1486 | motor[TIRE_BL].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1487 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1488 | mtc=false; |
yabahiro | 51:a0bdbddd08f7 | 1489 | break; |
yabahiro | 51:a0bdbddd08f7 | 1490 | } |
yabahiro | 51:a0bdbddd08f7 | 1491 | } |
yabahiro | 51:a0bdbddd08f7 | 1492 | } |
yabahiro | 51:a0bdbddd08f7 | 1493 | break; |
yabahiro | 51:a0bdbddd08f7 | 1494 | case 25: |
yabahiro | 51:a0bdbddd08f7 | 1495 | motor[TIRE_FR].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1496 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1497 | motor[TIRE_FL].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1498 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1499 | motor[TIRE_BR].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1500 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1501 | motor[TIRE_BL].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1502 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1503 | if(DOLS==true||UPLS==true) { |
yabahiro | 51:a0bdbddd08f7 | 1504 | if(DOLS==true||UPLS==true) { |
yabahiro | 51:a0bdbddd08f7 | 1505 | motor[MOTOR_0].pwm = 200; |
yabahiro | 51:a0bdbddd08f7 | 1506 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1507 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1508 | mode=26; |
yabahiro | 51:a0bdbddd08f7 | 1509 | } |
yabahiro | 51:a0bdbddd08f7 | 1510 | } else { |
yabahiro | 51:a0bdbddd08f7 | 1511 | motor[MOTOR_0].pwm = 100; |
yabahiro | 51:a0bdbddd08f7 | 1512 | motor[MOTOR_0].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1513 | } |
yabahiro | 51:a0bdbddd08f7 | 1514 | break; |
yabahiro | 51:a0bdbddd08f7 | 1515 | case 26: |
yabahiro | 53:694dbd7dfa0e | 1516 | if(g[0]==98) { |
yabahiro | 53:694dbd7dfa0e | 1517 | mode=31; |
yabahiro | 53:694dbd7dfa0e | 1518 | } |
yabahiro | 50:87f5fa1bc460 | 1519 | switch(g[4]) { |
yabahiro | 50:87f5fa1bc460 | 1520 | case 98: |
yabahiro | 51:a0bdbddd08f7 | 1521 | motor[TIRE_FR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1522 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1523 | motor[TIRE_FL].pwm = 65; |
yabahiro | 50:87f5fa1bc460 | 1524 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1525 | motor[TIRE_BR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1526 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1527 | motor[TIRE_BL].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1528 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1529 | break; |
yabahiro | 50:87f5fa1bc460 | 1530 | case 99: |
yabahiro | 50:87f5fa1bc460 | 1531 | motor[TIRE_FR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1532 | motor[TIRE_FR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1533 | motor[TIRE_FL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1534 | motor[TIRE_FL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1535 | motor[TIRE_BR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1536 | motor[TIRE_BR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1537 | motor[TIRE_BL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1538 | motor[TIRE_BL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1539 | break; |
yabahiro | 50:87f5fa1bc460 | 1540 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1541 | motor[TIRE_FR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1542 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1543 | motor[TIRE_FL].pwm = 65; |
yabahiro | 50:87f5fa1bc460 | 1544 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1545 | motor[TIRE_BR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1546 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1547 | motor[TIRE_BL].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1548 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1549 | break; |
yabahiro | 50:87f5fa1bc460 | 1550 | case 255: |
yabahiro | 50:87f5fa1bc460 | 1551 | motor[TIRE_FR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1552 | motor[TIRE_FR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1553 | motor[TIRE_FL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1554 | motor[TIRE_FL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1555 | motor[TIRE_BR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1556 | motor[TIRE_BR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1557 | motor[TIRE_BL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1558 | motor[TIRE_BL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1559 | break; |
yabahiro | 50:87f5fa1bc460 | 1560 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1561 | motor[TIRE_FR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1562 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1563 | motor[TIRE_FL].pwm = 30; |
yabahiro | 50:87f5fa1bc460 | 1564 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1565 | motor[TIRE_BR].pwm = 25; |
yabahiro | 50:87f5fa1bc460 | 1566 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1567 | motor[TIRE_BL].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1568 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1569 | break; |
yabahiro | 50:87f5fa1bc460 | 1570 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1571 | motor[TIRE_FR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1572 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1573 | motor[TIRE_FL].pwm = 45; |
yabahiro | 50:87f5fa1bc460 | 1574 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1575 | motor[TIRE_BR].pwm = 40; |
yabahiro | 50:87f5fa1bc460 | 1576 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1577 | motor[TIRE_BL].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1578 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1579 | break; |
yabahiro | 50:87f5fa1bc460 | 1580 | case 1: |
yabahiro | 50:87f5fa1bc460 | 1581 | motor[TIRE_FR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1582 | motor[TIRE_FR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1583 | motor[TIRE_FL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1584 | motor[TIRE_FL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1585 | motor[TIRE_BR].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1586 | motor[TIRE_BR].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1587 | motor[TIRE_BL].pwm = 0; |
yabahiro | 50:87f5fa1bc460 | 1588 | motor[TIRE_BL].dir = FREE; |
yabahiro | 50:87f5fa1bc460 | 1589 | break; |
yabahiro | 50:87f5fa1bc460 | 1590 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1591 | motor[TIRE_FR].pwm = 25; |
yabahiro | 50:87f5fa1bc460 | 1592 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1593 | motor[TIRE_FL].pwm = 65; |
yabahiro | 50:87f5fa1bc460 | 1594 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1595 | motor[TIRE_BR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1596 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1597 | motor[TIRE_BL].pwm = 25; |
yabahiro | 50:87f5fa1bc460 | 1598 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1599 | break; |
yabahiro | 50:87f5fa1bc460 | 1600 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1601 | motor[TIRE_FR].pwm = 40; |
yabahiro | 50:87f5fa1bc460 | 1602 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1603 | motor[TIRE_FL].pwm = 65; |
yabahiro | 50:87f5fa1bc460 | 1604 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1605 | motor[TIRE_BR].pwm = 60; |
yabahiro | 50:87f5fa1bc460 | 1606 | motor[TIRE_BR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1607 | motor[TIRE_BL].pwm = 40; |
yabahiro | 50:87f5fa1bc460 | 1608 | motor[TIRE_BL].dir = FOR; |
yabahiro | 50:87f5fa1bc460 | 1609 | break; |
yabahiro | 50:87f5fa1bc460 | 1610 | } |
yabahiro | 51:a0bdbddd08f7 | 1611 | break; |
yabahiro | 51:a0bdbddd08f7 | 1612 | case 31: |
yabahiro | 53:694dbd7dfa0e | 1613 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1614 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1615 | motor[TIRE_FL].pwm = 5; |
yabahiro | 53:694dbd7dfa0e | 1616 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1617 | motor[TIRE_BR].pwm = 5; |
yabahiro | 53:694dbd7dfa0e | 1618 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1619 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1620 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1621 | if(g[2]==0) { |
yabahiro | 51:a0bdbddd08f7 | 1622 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 1623 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1624 | mode=40; |
yabahiro | 55:844ac0974740 | 1625 | fast=70; |
yabahiro | 55:844ac0974740 | 1626 | normal=50; |
yabahiro | 55:844ac0974740 | 1627 | slow=40; |
yabahiro | 51:a0bdbddd08f7 | 1628 | } |
yabahiro | 51:a0bdbddd08f7 | 1629 | break; |
yabahiro | 51:a0bdbddd08f7 | 1630 | case 40: |
yabahiro | 51:a0bdbddd08f7 | 1631 | switch(g[3]) { |
yabahiro | 51:a0bdbddd08f7 | 1632 | case 98: |
yabahiro | 53:694dbd7dfa0e | 1633 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1634 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1635 | motor[TIRE_FL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1636 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1637 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1638 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1639 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1640 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1641 | if(countss>30000) { |
yabahiro | 51:a0bdbddd08f7 | 1642 | cross++; |
yabahiro | 51:a0bdbddd08f7 | 1643 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1644 | } |
yabahiro | 51:a0bdbddd08f7 | 1645 | mtc2=true; |
yabahiro | 51:a0bdbddd08f7 | 1646 | if(YELLOW) { |
yabahiro | 51:a0bdbddd08f7 | 1647 | if(cross==1) { |
yabahiro | 51:a0bdbddd08f7 | 1648 | mode=41; |
yabahiro | 51:a0bdbddd08f7 | 1649 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1650 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 1651 | } |
yabahiro | 51:a0bdbddd08f7 | 1652 | } else { |
yabahiro | 51:a0bdbddd08f7 | 1653 | if(cross==3) { |
yabahiro | 53:694dbd7dfa0e | 1654 | fast=60; |
yabahiro | 53:694dbd7dfa0e | 1655 | normal=40; |
yabahiro | 53:694dbd7dfa0e | 1656 | slow=30; |
yabahiro | 51:a0bdbddd08f7 | 1657 | mode=41; |
yabahiro | 51:a0bdbddd08f7 | 1658 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1659 | cross=0; |
yabahiro | 51:a0bdbddd08f7 | 1660 | } |
yabahiro | 51:a0bdbddd08f7 | 1661 | } |
yabahiro | 51:a0bdbddd08f7 | 1662 | break; |
yabahiro | 50:87f5fa1bc460 | 1663 | case 0: |
yabahiro | 53:694dbd7dfa0e | 1664 | motor[TIRE_FR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1665 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1666 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1667 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1668 | motor[TIRE_BR].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1669 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1670 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1671 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1672 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1673 | break; |
yabahiro | 50:87f5fa1bc460 | 1674 | case 255: |
yabahiro | 53:694dbd7dfa0e | 1675 | motor[TIRE_FR].pwm = slow; |
yabahiro | 50:87f5fa1bc460 | 1676 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1677 | motor[TIRE_FL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1678 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1679 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1680 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1681 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1682 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1683 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1684 | break; |
yabahiro | 50:87f5fa1bc460 | 1685 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1686 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1687 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1688 | motor[TIRE_FL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1689 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1690 | motor[TIRE_BR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1691 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1692 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1693 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1694 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1695 | break; |
yabahiro | 50:87f5fa1bc460 | 1696 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1697 | motor[TIRE_FR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1698 | motor[TIRE_FR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1699 | motor[TIRE_FL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1700 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1701 | motor[TIRE_BR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1702 | motor[TIRE_BR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1703 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1704 | motor[TIRE_BL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1705 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1706 | break; |
yabahiro | 50:87f5fa1bc460 | 1707 | case 1: |
yabahiro | 53:694dbd7dfa0e | 1708 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1709 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1710 | motor[TIRE_FL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1711 | motor[TIRE_FL].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1712 | motor[TIRE_BR].pwm = slow; |
yabahiro | 50:87f5fa1bc460 | 1713 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1714 | motor[TIRE_BL].pwm = fast; |
yabahiro | 51:a0bdbddd08f7 | 1715 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1716 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1717 | break; |
yabahiro | 50:87f5fa1bc460 | 1718 | case 3: |
yabahiro | 53:694dbd7dfa0e | 1719 | motor[TIRE_FR].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1720 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1721 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1722 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1723 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1724 | motor[TIRE_BR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1725 | motor[TIRE_BL].pwm = slow; |
yabahiro | 51:a0bdbddd08f7 | 1726 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1727 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1728 | break; |
yabahiro | 50:87f5fa1bc460 | 1729 | case 2: |
yabahiro | 53:694dbd7dfa0e | 1730 | motor[TIRE_FR].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1731 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1732 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1733 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1734 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1735 | motor[TIRE_BR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1736 | motor[TIRE_BL].pwm = normal; |
yabahiro | 51:a0bdbddd08f7 | 1737 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1738 | mtc2=true; |
yabahiro | 50:87f5fa1bc460 | 1739 | break; |
yabahiro | 50:87f5fa1bc460 | 1740 | } |
yabahiro | 51:a0bdbddd08f7 | 1741 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 51:a0bdbddd08f7 | 1742 | switch(g[2]) { |
yabahiro | 51:a0bdbddd08f7 | 1743 | case 0: |
yabahiro | 51:a0bdbddd08f7 | 1744 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1745 | break; |
yabahiro | 51:a0bdbddd08f7 | 1746 | case 255: |
yabahiro | 51:a0bdbddd08f7 | 1747 | motor[TIRE_FL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1748 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1749 | break; |
yabahiro | 51:a0bdbddd08f7 | 1750 | case 253: |
yabahiro | 51:a0bdbddd08f7 | 1751 | motor[TIRE_FL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1752 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1753 | break; |
yabahiro | 51:a0bdbddd08f7 | 1754 | case 254: |
yabahiro | 51:a0bdbddd08f7 | 1755 | motor[TIRE_FL].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1756 | motor[TIRE_BL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1757 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1758 | break; |
yabahiro | 51:a0bdbddd08f7 | 1759 | case 1: |
yabahiro | 51:a0bdbddd08f7 | 1760 | motor[TIRE_BL].pwm += 5; |
yabahiro | 51:a0bdbddd08f7 | 1761 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1762 | break; |
yabahiro | 51:a0bdbddd08f7 | 1763 | case 3: |
yabahiro | 51:a0bdbddd08f7 | 1764 | motor[TIRE_BL].pwm += 10; |
yabahiro | 51:a0bdbddd08f7 | 1765 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1766 | break; |
yabahiro | 51:a0bdbddd08f7 | 1767 | case 2: |
yabahiro | 51:a0bdbddd08f7 | 1768 | motor[TIRE_BL].pwm += 20; |
yabahiro | 51:a0bdbddd08f7 | 1769 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1770 | mtc2=false; |
yabahiro | 51:a0bdbddd08f7 | 1771 | break; |
yabahiro | 51:a0bdbddd08f7 | 1772 | } |
yabahiro | 51:a0bdbddd08f7 | 1773 | } |
yabahiro | 51:a0bdbddd08f7 | 1774 | break; |
yabahiro | 51:a0bdbddd08f7 | 1775 | case 41: |
yabahiro | 53:694dbd7dfa0e | 1776 | /* |
yabahiro | 53:694dbd7dfa0e | 1777 | if(LIF||LIB) { |
yabahiro | 53:694dbd7dfa0e | 1778 | mode=42; |
yabahiro | 53:694dbd7dfa0e | 1779 | } |
yabahiro | 53:694dbd7dfa0e | 1780 | */ |
yabahiro | 53:694dbd7dfa0e | 1781 | motor[TIRE_FR].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1782 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1783 | motor[TIRE_FL].pwm = 0; |
yabahiro | 53:694dbd7dfa0e | 1784 | motor[TIRE_FL].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1785 | motor[TIRE_BR].pwm = 0; |
yabahiro | 53:694dbd7dfa0e | 1786 | motor[TIRE_BR].dir = FREE; |
yabahiro | 53:694dbd7dfa0e | 1787 | motor[TIRE_BL].pwm = 30; |
yabahiro | 51:a0bdbddd08f7 | 1788 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1789 | if(g[1]==0) { |
yabahiro | 51:a0bdbddd08f7 | 1790 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1791 | mode=70; |
yabahiro | 55:844ac0974740 | 1792 | normal=120; |
yabahiro | 55:844ac0974740 | 1793 | fast=140; |
yabahiro | 55:844ac0974740 | 1794 | slow=100; |
yabahiro | 51:a0bdbddd08f7 | 1795 | } |
yabahiro | 51:a0bdbddd08f7 | 1796 | break; |
yabahiro | 51:a0bdbddd08f7 | 1797 | case 42: |
yabahiro | 51:a0bdbddd08f7 | 1798 | motor[TIRE_FR].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 1799 | motor[TIRE_FR].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1800 | motor[TIRE_FL].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1801 | motor[TIRE_FL].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1802 | motor[TIRE_BR].pwm = 0; |
yabahiro | 51:a0bdbddd08f7 | 1803 | motor[TIRE_BR].dir = FREE; |
yabahiro | 51:a0bdbddd08f7 | 1804 | motor[TIRE_BL].pwm = 40; |
yabahiro | 51:a0bdbddd08f7 | 1805 | motor[TIRE_BL].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1806 | if(g[1]==0) { |
yabahiro | 51:a0bdbddd08f7 | 1807 | countss=0; |
yabahiro | 51:a0bdbddd08f7 | 1808 | mode=70; |
yabahiro | 51:a0bdbddd08f7 | 1809 | } |
yabahiro | 51:a0bdbddd08f7 | 1810 | break; |
yabahiro | 51:a0bdbddd08f7 | 1811 | case 70: |
yabahiro | 51:a0bdbddd08f7 | 1812 | switch(g[0]) { |
yabahiro | 51:a0bdbddd08f7 | 1813 | case 99: |
yabahiro | 51:a0bdbddd08f7 | 1814 | if(countss>=100000) { |
yabahiro | 51:a0bdbddd08f7 | 1815 | mode=100; |
yabahiro | 51:a0bdbddd08f7 | 1816 | } |
yabahiro | 50:87f5fa1bc460 | 1817 | break; |
yabahiro | 39:784552c26163 | 1818 | case 98: |
yabahiro | 39:784552c26163 | 1819 | motor[TIRE_FR].pwm = normal; |
yabahiro | 39:784552c26163 | 1820 | motor[TIRE_FR].dir = BACK; |
yabahiro | 39:784552c26163 | 1821 | motor[TIRE_FL].pwm = normal; |
yabahiro | 39:784552c26163 | 1822 | motor[TIRE_FL].dir = FOR; |
yabahiro | 39:784552c26163 | 1823 | motor[TIRE_BR].pwm = normal; |
yabahiro | 39:784552c26163 | 1824 | motor[TIRE_BR].dir = BACK; |
yabahiro | 39:784552c26163 | 1825 | motor[TIRE_BL].pwm = normal; |
yabahiro | 39:784552c26163 | 1826 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1827 | break; |
yabahiro | 39:784552c26163 | 1828 | case 0: |
yabahiro | 53:694dbd7dfa0e | 1829 | motor[TIRE_FR].pwm = 80; |
yabahiro | 39:784552c26163 | 1830 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1831 | motor[TIRE_FL].pwm = 80; |
yabahiro | 39:784552c26163 | 1832 | motor[TIRE_FL].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 1833 | motor[TIRE_BR].pwm = 80; |
yabahiro | 39:784552c26163 | 1834 | motor[TIRE_BR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 1835 | motor[TIRE_BL].pwm = 80; |
yabahiro | 39:784552c26163 | 1836 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1837 | mtc=true; |
yabahiro | 39:784552c26163 | 1838 | break; |
yabahiro | 39:784552c26163 | 1839 | case 255: |
yabahiro | 39:784552c26163 | 1840 | motor[TIRE_FR].pwm = fast; |
yabahiro | 39:784552c26163 | 1841 | motor[TIRE_FR].dir = BACK; |
yabahiro | 39:784552c26163 | 1842 | motor[TIRE_FL].pwm = slow; |
yabahiro | 39:784552c26163 | 1843 | motor[TIRE_FL].dir = FOR; |
yabahiro | 39:784552c26163 | 1844 | motor[TIRE_BR].pwm = slow; |
yabahiro | 39:784552c26163 | 1845 | motor[TIRE_BR].dir = BACK; |
yabahiro | 39:784552c26163 | 1846 | motor[TIRE_BL].pwm = fast; |
yabahiro | 39:784552c26163 | 1847 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1848 | mtc=true; |
yabahiro | 39:784552c26163 | 1849 | break; |
yabahiro | 39:784552c26163 | 1850 | case 253: |
yabahiro | 39:784552c26163 | 1851 | motor[TIRE_FR].pwm = slow; |
yabahiro | 39:784552c26163 | 1852 | motor[TIRE_FR].dir = BACK; |
yabahiro | 39:784552c26163 | 1853 | motor[TIRE_FL].pwm = 0; |
yabahiro | 39:784552c26163 | 1854 | motor[TIRE_FL].dir = FREE; |
yabahiro | 39:784552c26163 | 1855 | motor[TIRE_BR].pwm = 0; |
yabahiro | 39:784552c26163 | 1856 | motor[TIRE_BR].dir = FREE; |
yabahiro | 39:784552c26163 | 1857 | motor[TIRE_BL].pwm = slow; |
yabahiro | 39:784552c26163 | 1858 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1859 | mtc=true; |
yabahiro | 39:784552c26163 | 1860 | break; |
yabahiro | 39:784552c26163 | 1861 | case 254: |
yabahiro | 39:784552c26163 | 1862 | motor[TIRE_FR].pwm = normal; |
yabahiro | 39:784552c26163 | 1863 | motor[TIRE_FR].dir = BACK; |
yabahiro | 39:784552c26163 | 1864 | motor[TIRE_FL].pwm = 0; |
yabahiro | 39:784552c26163 | 1865 | motor[TIRE_FL].dir = FREE; |
yabahiro | 39:784552c26163 | 1866 | motor[TIRE_BR].pwm = 0; |
yabahiro | 39:784552c26163 | 1867 | motor[TIRE_BR].dir = FREE; |
yabahiro | 39:784552c26163 | 1868 | motor[TIRE_BL].pwm = normal; |
yabahiro | 39:784552c26163 | 1869 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1870 | mtc=true; |
yabahiro | 39:784552c26163 | 1871 | break; |
yabahiro | 39:784552c26163 | 1872 | case 1: |
yabahiro | 39:784552c26163 | 1873 | motor[TIRE_FR].pwm = slow; |
yabahiro | 39:784552c26163 | 1874 | motor[TIRE_FR].dir = BACK; |
yabahiro | 39:784552c26163 | 1875 | motor[TIRE_FL].pwm = fast; |
yabahiro | 39:784552c26163 | 1876 | motor[TIRE_FL].dir = FOR; |
yabahiro | 39:784552c26163 | 1877 | motor[TIRE_BR].pwm = fast; |
yabahiro | 39:784552c26163 | 1878 | motor[TIRE_BR].dir = BACK; |
yabahiro | 39:784552c26163 | 1879 | motor[TIRE_BL].pwm = slow; |
yabahiro | 39:784552c26163 | 1880 | motor[TIRE_BL].dir = FOR; |
yabahiro | 39:784552c26163 | 1881 | mtc=true; |
yabahiro | 39:784552c26163 | 1882 | break; |
yabahiro | 39:784552c26163 | 1883 | case 3: |
yabahiro | 39:784552c26163 | 1884 | motor[TIRE_FR].pwm = 0; |
yabahiro | 39:784552c26163 | 1885 | motor[TIRE_FR].dir = FREE; |
yabahiro | 39:784552c26163 | 1886 | motor[TIRE_FL].pwm = slow; |
yabahiro | 39:784552c26163 | 1887 | motor[TIRE_FL].dir = FOR; |
yabahiro | 39:784552c26163 | 1888 | motor[TIRE_BR].pwm = slow; |
yabahiro | 39:784552c26163 | 1889 | motor[TIRE_BR].dir = BACK; |
yabahiro | 39:784552c26163 | 1890 | motor[TIRE_BL].pwm = 0; |
yabahiro | 39:784552c26163 | 1891 | motor[TIRE_BL].dir = FREE; |
yabahiro | 39:784552c26163 | 1892 | mtc=true; |
yabahiro | 39:784552c26163 | 1893 | break; |
yabahiro | 39:784552c26163 | 1894 | case 2: |
yabahiro | 39:784552c26163 | 1895 | motor[TIRE_FR].pwm = 0; |
yabahiro | 39:784552c26163 | 1896 | motor[TIRE_FR].dir = FREE; |
yabahiro | 39:784552c26163 | 1897 | motor[TIRE_FL].pwm = normal; |
yabahiro | 39:784552c26163 | 1898 | motor[TIRE_FL].dir = FOR; |
yabahiro | 39:784552c26163 | 1899 | motor[TIRE_BR].pwm = normal; |
yabahiro | 39:784552c26163 | 1900 | motor[TIRE_BR].dir = BACK; |
yabahiro | 39:784552c26163 | 1901 | motor[TIRE_BL].pwm = 0; |
yabahiro | 39:784552c26163 | 1902 | motor[TIRE_BL].dir = FREE; |
yabahiro | 39:784552c26163 | 1903 | mtc=true; |
yabahiro | 39:784552c26163 | 1904 | break; |
yabahiro | 39:784552c26163 | 1905 | } |
yabahiro | 39:784552c26163 | 1906 | if(g[1]!=98&&mtc==true) { |
yabahiro | 39:784552c26163 | 1907 | switch(g[1]) { |
yabahiro | 39:784552c26163 | 1908 | case 0: |
yabahiro | 39:784552c26163 | 1909 | mtc=false; |
yabahiro | 39:784552c26163 | 1910 | break; |
yabahiro | 39:784552c26163 | 1911 | case 255: |
yabahiro | 39:784552c26163 | 1912 | motor[TIRE_BR].pwm += 5; |
yabahiro | 39:784552c26163 | 1913 | mtc=false; |
yabahiro | 39:784552c26163 | 1914 | break; |
yabahiro | 39:784552c26163 | 1915 | case 253: |
yabahiro | 39:784552c26163 | 1916 | motor[TIRE_BR].pwm += 10; |
yabahiro | 39:784552c26163 | 1917 | mtc=false; |
yabahiro | 39:784552c26163 | 1918 | break; |
yabahiro | 39:784552c26163 | 1919 | case 254: |
yabahiro | 39:784552c26163 | 1920 | motor[TIRE_BR].pwm += 20; |
yabahiro | 39:784552c26163 | 1921 | motor[TIRE_BL].pwm = 0; |
yabahiro | 39:784552c26163 | 1922 | mtc=false; |
yabahiro | 39:784552c26163 | 1923 | break; |
yabahiro | 39:784552c26163 | 1924 | case 1: |
yabahiro | 39:784552c26163 | 1925 | motor[TIRE_BL].pwm += 5; |
yabahiro | 39:784552c26163 | 1926 | mtc=false; |
yabahiro | 39:784552c26163 | 1927 | break; |
yabahiro | 39:784552c26163 | 1928 | case 3: |
yabahiro | 39:784552c26163 | 1929 | motor[TIRE_BL].pwm += 10; |
yabahiro | 39:784552c26163 | 1930 | mtc=false; |
yabahiro | 39:784552c26163 | 1931 | break; |
yabahiro | 39:784552c26163 | 1932 | case 2: |
yabahiro | 39:784552c26163 | 1933 | motor[TIRE_BL].pwm += 20; |
yabahiro | 39:784552c26163 | 1934 | motor[TIRE_BR].pwm = 0; |
yabahiro | 39:784552c26163 | 1935 | mtc=false; |
yabahiro | 39:784552c26163 | 1936 | break; |
yabahiro | 39:784552c26163 | 1937 | } |
yabahiro | 33:dd598bb23ad1 | 1938 | } |
yabahiro | 51:a0bdbddd08f7 | 1939 | break; |
yabahiro | 51:a0bdbddd08f7 | 1940 | case 100: |
yabahiro | 54:4167f10ecad0 | 1941 | motor[TIRE_FR].pwm = 47; |
yabahiro | 51:a0bdbddd08f7 | 1942 | motor[TIRE_FR].dir = BACK; |
yabahiro | 51:a0bdbddd08f7 | 1943 | motor[TIRE_FL].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1944 | motor[TIRE_FL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1945 | motor[TIRE_BR].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1946 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 1947 | motor[TIRE_BL].pwm = 55; |
yabahiro | 51:a0bdbddd08f7 | 1948 | motor[TIRE_BL].dir = FOR; |
yabahiro | 51:a0bdbddd08f7 | 1949 | break; |
yabahiro | 51:a0bdbddd08f7 | 1950 | default: |
yabahiro | 51:a0bdbddd08f7 | 1951 | motor[TIRE_FR].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1952 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1953 | motor[TIRE_FL].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1954 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1955 | motor[TIRE_BR].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1956 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1957 | motor[TIRE_BL].pwm = 50; |
yabahiro | 51:a0bdbddd08f7 | 1958 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 51:a0bdbddd08f7 | 1959 | break; |
yabahiro | 33:dd598bb23ad1 | 1960 | } |
kishibekairohan | 13:b6e02d6261d7 | 1961 | } |
kishibekairohan | 13:b6e02d6261d7 | 1962 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1963 | |
kishibekairohan | 13:b6e02d6261d7 | 1964 | #if USE_PROCESS_NUM>3 |
Ryosei | 27:dd9f27fce7d1 | 1965 | static void Process3() |
kishibekairohan | 13:b6e02d6261d7 | 1966 | { |
yabahiro | 33:dd598bb23ad1 | 1967 | //Clearly 赤ゾーン |
kishibekairohan | 35:33c6cdba5630 | 1968 | LED::LED::TapeledMode(3); |
yabahiro | 33:dd598bb23ad1 | 1969 | countss++; |
yabahiro | 54:4167f10ecad0 | 1970 | switch(mode) { |
yabahiro | 54:4167f10ecad0 | 1971 | case 1: |
yabahiro | 54:4167f10ecad0 | 1972 | fast=40; |
yabahiro | 54:4167f10ecad0 | 1973 | normal=30; |
yabahiro | 54:4167f10ecad0 | 1974 | slow=20; |
yabahiro | 54:4167f10ecad0 | 1975 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 1976 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 1977 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 1978 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1979 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 1980 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 1981 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 1982 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1983 | if(g[2]==0&&countss>=300000) { |
yabahiro | 54:4167f10ecad0 | 1984 | countss=100000; |
yabahiro | 54:4167f10ecad0 | 1985 | cross=0; |
yabahiro | 54:4167f10ecad0 | 1986 | mode=3; |
yabahiro | 54:4167f10ecad0 | 1987 | } |
yabahiro | 54:4167f10ecad0 | 1988 | break; |
yabahiro | 54:4167f10ecad0 | 1989 | case 3: |
yabahiro | 54:4167f10ecad0 | 1990 | motor[TIRE_FR].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 1991 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 1992 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 1993 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 1994 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 1995 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 1996 | motor[TIRE_BL].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 1997 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 1998 | if(g[0]==0) { |
yabahiro | 54:4167f10ecad0 | 1999 | mode=10; |
yabahiro | 54:4167f10ecad0 | 2000 | countss=100000; |
yabahiro | 56:61ebd4b759b4 | 2001 | fast = 80; |
yabahiro | 56:61ebd4b759b4 | 2002 | normal = 60; |
yabahiro | 56:61ebd4b759b4 | 2003 | slow = 40; |
yabahiro | 54:4167f10ecad0 | 2004 | } |
yabahiro | 54:4167f10ecad0 | 2005 | break; |
yabahiro | 54:4167f10ecad0 | 2006 | case 10: |
yabahiro | 54:4167f10ecad0 | 2007 | switch(g[0]) { |
yabahiro | 54:4167f10ecad0 | 2008 | case 98: |
yabahiro | 54:4167f10ecad0 | 2009 | motor[TIRE_FR].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2010 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2011 | motor[TIRE_FL].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2012 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2013 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2014 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2015 | motor[TIRE_BL].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2016 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2017 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2018 | if(countss>80000) { |
yabahiro | 54:4167f10ecad0 | 2019 | cross++; |
yabahiro | 54:4167f10ecad0 | 2020 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2021 | } |
yabahiro | 54:4167f10ecad0 | 2022 | if(cross==2) { |
yabahiro | 54:4167f10ecad0 | 2023 | fast = 10; |
yabahiro | 54:4167f10ecad0 | 2024 | normal = 10; |
yabahiro | 54:4167f10ecad0 | 2025 | slow = 5; |
yabahiro | 54:4167f10ecad0 | 2026 | } |
yabahiro | 46:68296caae1af | 2027 | |
yabahiro | 53:694dbd7dfa0e | 2028 | if(cross==3) { |
yabahiro | 54:4167f10ecad0 | 2029 | fast=40; |
yabahiro | 54:4167f10ecad0 | 2030 | normal=30; |
yabahiro | 54:4167f10ecad0 | 2031 | slow=20; |
yabahiro | 54:4167f10ecad0 | 2032 | mode=11; |
yabahiro | 53:694dbd7dfa0e | 2033 | countss=0; |
yabahiro | 53:694dbd7dfa0e | 2034 | cross=0; |
yabahiro | 54:4167f10ecad0 | 2035 | |
yabahiro | 53:694dbd7dfa0e | 2036 | } |
yabahiro | 54:4167f10ecad0 | 2037 | break; |
yabahiro | 38:40e7fdf3c694 | 2038 | case 0: |
yabahiro | 54:4167f10ecad0 | 2039 | motor[TIRE_FR].pwm = fast+50; |
yabahiro | 54:4167f10ecad0 | 2040 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2041 | motor[TIRE_FL].pwm = fast+60; |
yabahiro | 54:4167f10ecad0 | 2042 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2043 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 54:4167f10ecad0 | 2044 | motor[TIRE_BR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2045 | motor[TIRE_BL].pwm = fast+45; |
yabahiro | 54:4167f10ecad0 | 2046 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2047 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 2048 | break; |
yabahiro | 38:40e7fdf3c694 | 2049 | case 255: |
yabahiro | 54:4167f10ecad0 | 2050 | motor[TIRE_FR].pwm = fast+50; |
yabahiro | 54:4167f10ecad0 | 2051 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2052 | motor[TIRE_FL].pwm = slow+60; |
yabahiro | 54:4167f10ecad0 | 2053 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2054 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 54:4167f10ecad0 | 2055 | motor[TIRE_BR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2056 | motor[TIRE_BL].pwm = fast+45; |
yabahiro | 54:4167f10ecad0 | 2057 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2058 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2059 | break; |
yabahiro | 53:694dbd7dfa0e | 2060 | case 253: |
yabahiro | 54:4167f10ecad0 | 2061 | motor[TIRE_FR].pwm = slow+50; |
yabahiro | 54:4167f10ecad0 | 2062 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2063 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2064 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2065 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2066 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 2067 | motor[TIRE_BL].pwm = slow+45; |
yabahiro | 54:4167f10ecad0 | 2068 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2069 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2070 | break; |
yabahiro | 53:694dbd7dfa0e | 2071 | case 254: |
yabahiro | 54:4167f10ecad0 | 2072 | motor[TIRE_FR].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2073 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 2074 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2075 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2076 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2077 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 2078 | motor[TIRE_BL].pwm = normal+45; |
yabahiro | 54:4167f10ecad0 | 2079 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2080 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2081 | break; |
yabahiro | 53:694dbd7dfa0e | 2082 | case 1: |
yabahiro | 54:4167f10ecad0 | 2083 | motor[TIRE_FR].pwm = slow+50; |
yabahiro | 54:4167f10ecad0 | 2084 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2085 | motor[TIRE_FL].pwm = fast+60; |
yabahiro | 54:4167f10ecad0 | 2086 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2087 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 54:4167f10ecad0 | 2088 | motor[TIRE_BR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 2089 | motor[TIRE_BL].pwm = slow+45; |
yabahiro | 54:4167f10ecad0 | 2090 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2091 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2092 | break; |
yabahiro | 53:694dbd7dfa0e | 2093 | case 3: |
yabahiro | 54:4167f10ecad0 | 2094 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2095 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 2096 | motor[TIRE_FL].pwm = slow+60; |
yabahiro | 54:4167f10ecad0 | 2097 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2098 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 54:4167f10ecad0 | 2099 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2100 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2101 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2102 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2103 | break; |
yabahiro | 53:694dbd7dfa0e | 2104 | case 2: |
yabahiro | 54:4167f10ecad0 | 2105 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2106 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 2107 | motor[TIRE_FL].pwm = normal+60; |
yabahiro | 54:4167f10ecad0 | 2108 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2109 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 54:4167f10ecad0 | 2110 | motor[TIRE_BR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 2111 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2112 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2113 | mtc=true; |
yabahiro | 53:694dbd7dfa0e | 2114 | break; |
yabahiro | 53:694dbd7dfa0e | 2115 | } |
yabahiro | 54:4167f10ecad0 | 2116 | if(g[0]!=98&&mtc==true) { |
yabahiro | 54:4167f10ecad0 | 2117 | switch(g[1]) { |
yabahiro | 54:4167f10ecad0 | 2118 | case 0: |
yabahiro | 54:4167f10ecad0 | 2119 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2120 | break; |
yabahiro | 54:4167f10ecad0 | 2121 | case 255: |
yabahiro | 54:4167f10ecad0 | 2122 | motor[TIRE_BR].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2123 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2124 | break; |
yabahiro | 54:4167f10ecad0 | 2125 | case 253: |
yabahiro | 54:4167f10ecad0 | 2126 | motor[TIRE_BR].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2127 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2128 | break; |
yabahiro | 54:4167f10ecad0 | 2129 | case 254: |
yabahiro | 54:4167f10ecad0 | 2130 | motor[TIRE_BR].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2131 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2132 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2133 | break; |
yabahiro | 54:4167f10ecad0 | 2134 | case 1: |
yabahiro | 54:4167f10ecad0 | 2135 | motor[TIRE_BL].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2136 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2137 | break; |
yabahiro | 54:4167f10ecad0 | 2138 | case 3: |
yabahiro | 54:4167f10ecad0 | 2139 | motor[TIRE_BL].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2140 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2141 | break; |
yabahiro | 54:4167f10ecad0 | 2142 | case 2: |
yabahiro | 54:4167f10ecad0 | 2143 | motor[TIRE_BL].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2144 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2145 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2146 | break; |
yabahiro | 54:4167f10ecad0 | 2147 | } |
yabahiro | 54:4167f10ecad0 | 2148 | } |
yabahiro | 54:4167f10ecad0 | 2149 | break; |
yabahiro | 54:4167f10ecad0 | 2150 | case 11: |
yabahiro | 54:4167f10ecad0 | 2151 | motor[TIRE_FR].pwm = 15; |
yabahiro | 54:4167f10ecad0 | 2152 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2153 | motor[TIRE_FL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2154 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2155 | motor[TIRE_BR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2156 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2157 | motor[TIRE_BL].pwm = 15; |
yabahiro | 54:4167f10ecad0 | 2158 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2159 | if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { |
yabahiro | 53:694dbd7dfa0e | 2160 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2161 | cross=0; |
yabahiro | 54:4167f10ecad0 | 2162 | mode=20; |
yabahiro | 53:694dbd7dfa0e | 2163 | } |
yabahiro | 54:4167f10ecad0 | 2164 | break; |
yabahiro | 54:4167f10ecad0 | 2165 | case 20: |
yabahiro | 54:4167f10ecad0 | 2166 | solenoid.solenoid4= SOLENOID_ON; |
yabahiro | 53:694dbd7dfa0e | 2167 | switch(g[2]) { |
yabahiro | 54:4167f10ecad0 | 2168 | case 98: |
yabahiro | 54:4167f10ecad0 | 2169 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2170 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2171 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2172 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2173 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2174 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2175 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2176 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2177 | if(countss>30000) { |
yabahiro | 54:4167f10ecad0 | 2178 | cross++; |
yabahiro | 54:4167f10ecad0 | 2179 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2180 | } |
yabahiro | 54:4167f10ecad0 | 2181 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 2182 | |
yabahiro | 54:4167f10ecad0 | 2183 | if(YELLOW) { |
yabahiro | 54:4167f10ecad0 | 2184 | if(cross==1) { |
yabahiro | 54:4167f10ecad0 | 2185 | fast=20; |
yabahiro | 54:4167f10ecad0 | 2186 | normal=15; |
yabahiro | 54:4167f10ecad0 | 2187 | slow=15; |
yabahiro | 54:4167f10ecad0 | 2188 | } else if(cross==2) { |
yabahiro | 54:4167f10ecad0 | 2189 | mode=21; |
yabahiro | 54:4167f10ecad0 | 2190 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2191 | cross=0; |
yabahiro | 54:4167f10ecad0 | 2192 | } |
yabahiro | 54:4167f10ecad0 | 2193 | } else { |
yabahiro | 54:4167f10ecad0 | 2194 | if(cross==3) { |
yabahiro | 54:4167f10ecad0 | 2195 | mode=21; |
yabahiro | 54:4167f10ecad0 | 2196 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2197 | cross=0; |
yabahiro | 54:4167f10ecad0 | 2198 | } |
yabahiro | 54:4167f10ecad0 | 2199 | } |
yabahiro | 54:4167f10ecad0 | 2200 | break; |
yabahiro | 53:694dbd7dfa0e | 2201 | case 0: |
yabahiro | 54:4167f10ecad0 | 2202 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2203 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2204 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2205 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2206 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2207 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2208 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2209 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2210 | mtc2=true; |
yabahiro | 53:694dbd7dfa0e | 2211 | break; |
yabahiro | 53:694dbd7dfa0e | 2212 | case 255: |
yabahiro | 54:4167f10ecad0 | 2213 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2214 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2215 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2216 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2217 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2218 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2219 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2220 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2221 | mtc2=true; |
yabahiro | 38:40e7fdf3c694 | 2222 | break; |
yabahiro | 38:40e7fdf3c694 | 2223 | case 253: |
yabahiro | 54:4167f10ecad0 | 2224 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2225 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2226 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2227 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2228 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2229 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2230 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2231 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2232 | mtc2=true; |
yabahiro | 38:40e7fdf3c694 | 2233 | break; |
yabahiro | 38:40e7fdf3c694 | 2234 | case 254: |
yabahiro | 54:4167f10ecad0 | 2235 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2236 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2237 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 54:4167f10ecad0 | 2238 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2239 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 53:694dbd7dfa0e | 2240 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2241 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2242 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2243 | mtc2=true; |
yabahiro | 53:694dbd7dfa0e | 2244 | break; |
yabahiro | 53:694dbd7dfa0e | 2245 | case 1: |
yabahiro | 54:4167f10ecad0 | 2246 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2247 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2248 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2249 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2250 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2251 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2252 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 54:4167f10ecad0 | 2253 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2254 | mtc2=true; |
yabahiro | 53:694dbd7dfa0e | 2255 | break; |
yabahiro | 53:694dbd7dfa0e | 2256 | case 3: |
yabahiro | 54:4167f10ecad0 | 2257 | motor[TIRE_FR].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2258 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2259 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2260 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2261 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2262 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2263 | motor[TIRE_BL].pwm = slow+20; |
yabahiro | 54:4167f10ecad0 | 2264 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2265 | mtc2=true; |
yabahiro | 53:694dbd7dfa0e | 2266 | break; |
yabahiro | 53:694dbd7dfa0e | 2267 | case 2: |
yabahiro | 54:4167f10ecad0 | 2268 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 53:694dbd7dfa0e | 2269 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 2270 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 2271 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 2272 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 2273 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2274 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 38:40e7fdf3c694 | 2275 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2276 | mtc2=true; |
yabahiro | 38:40e7fdf3c694 | 2277 | break; |
yabahiro | 33:dd598bb23ad1 | 2278 | } |
yabahiro | 54:4167f10ecad0 | 2279 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 54:4167f10ecad0 | 2280 | switch(g[3]) { |
yabahiro | 54:4167f10ecad0 | 2281 | case 0: |
yabahiro | 54:4167f10ecad0 | 2282 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2283 | break; |
yabahiro | 54:4167f10ecad0 | 2284 | case 255: |
yabahiro | 54:4167f10ecad0 | 2285 | motor[TIRE_BL].pwm += 5; |
yabahiro | 54:4167f10ecad0 | 2286 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2287 | break; |
yabahiro | 54:4167f10ecad0 | 2288 | case 253: |
yabahiro | 54:4167f10ecad0 | 2289 | motor[TIRE_BL].pwm += 10; |
yabahiro | 54:4167f10ecad0 | 2290 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2291 | break; |
yabahiro | 54:4167f10ecad0 | 2292 | case 254: |
yabahiro | 54:4167f10ecad0 | 2293 | motor[TIRE_BL].pwm += 15; |
yabahiro | 54:4167f10ecad0 | 2294 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2295 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2296 | break; |
yabahiro | 54:4167f10ecad0 | 2297 | case 1: |
yabahiro | 54:4167f10ecad0 | 2298 | motor[TIRE_FL].pwm += 5; |
yabahiro | 54:4167f10ecad0 | 2299 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2300 | break; |
yabahiro | 54:4167f10ecad0 | 2301 | case 3: |
yabahiro | 54:4167f10ecad0 | 2302 | motor[TIRE_FL].pwm += 10; |
yabahiro | 54:4167f10ecad0 | 2303 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2304 | break; |
yabahiro | 54:4167f10ecad0 | 2305 | case 2: |
yabahiro | 54:4167f10ecad0 | 2306 | motor[TIRE_FL].pwm += 15; |
yabahiro | 54:4167f10ecad0 | 2307 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2308 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 2309 | break; |
yabahiro | 54:4167f10ecad0 | 2310 | } |
yabahiro | 38:40e7fdf3c694 | 2311 | } |
yabahiro | 54:4167f10ecad0 | 2312 | break; |
yabahiro | 54:4167f10ecad0 | 2313 | case 21: |
yabahiro | 54:4167f10ecad0 | 2314 | if(YELLOW) { |
yabahiro | 54:4167f10ecad0 | 2315 | motor[TIRE_FR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2316 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2317 | motor[TIRE_FL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2318 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2319 | motor[TIRE_BR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2320 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2321 | motor[TIRE_BL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2322 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2323 | if(g[3]==98&&countss>=10000) { |
yabahiro | 54:4167f10ecad0 | 2324 | mode=22; |
yabahiro | 54:4167f10ecad0 | 2325 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2326 | fast=60; |
yabahiro | 54:4167f10ecad0 | 2327 | normal=40; |
yabahiro | 54:4167f10ecad0 | 2328 | slow=20; |
yabahiro | 54:4167f10ecad0 | 2329 | } |
yabahiro | 54:4167f10ecad0 | 2330 | } else { |
yabahiro | 38:40e7fdf3c694 | 2331 | motor[TIRE_FR].pwm = 30; |
yabahiro | 38:40e7fdf3c694 | 2332 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2333 | motor[TIRE_FL].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 2334 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2335 | motor[TIRE_BR].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 2336 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 38:40e7fdf3c694 | 2337 | motor[TIRE_BL].pwm = 30; |
yabahiro | 38:40e7fdf3c694 | 2338 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2339 | if(g[1]==0) { |
yabahiro | 54:4167f10ecad0 | 2340 | mode=22; |
yabahiro | 54:4167f10ecad0 | 2341 | fast=60; |
yabahiro | 54:4167f10ecad0 | 2342 | normal=40; |
yabahiro | 54:4167f10ecad0 | 2343 | slow=25; |
yabahiro | 54:4167f10ecad0 | 2344 | } |
yabahiro | 38:40e7fdf3c694 | 2345 | } |
yabahiro | 54:4167f10ecad0 | 2346 | break; |
yabahiro | 54:4167f10ecad0 | 2347 | case 22: |
yabahiro | 54:4167f10ecad0 | 2348 | if(YELLOW) { |
yabahiro | 54:4167f10ecad0 | 2349 | if(g[4]==3||g[4]==2) { |
yabahiro | 38:40e7fdf3c694 | 2350 | motor[TIRE_FR].pwm = 30; |
yabahiro | 38:40e7fdf3c694 | 2351 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2352 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2353 | motor[TIRE_FL].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 2354 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2355 | motor[TIRE_BR].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 2356 | motor[TIRE_BL].pwm = 30; |
yabahiro | 38:40e7fdf3c694 | 2357 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2358 | } |
yabahiro | 54:4167f10ecad0 | 2359 | if(g[4]==253||g[4]==254) { |
yabahiro | 54:4167f10ecad0 | 2360 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2361 | motor[TIRE_FR].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 2362 | motor[TIRE_FL].pwm = 30; |
yabahiro | 38:40e7fdf3c694 | 2363 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2364 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2365 | motor[TIRE_BR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 2366 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2367 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2368 | } |
yabahiro | 54:4167f10ecad0 | 2369 | switch(g[2]) { |
yabahiro | 54:4167f10ecad0 | 2370 | case 0: |
yabahiro | 54:4167f10ecad0 | 2371 | motor[TIRE_FR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2372 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2373 | motor[TIRE_BR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2374 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2375 | break; |
yabahiro | 54:4167f10ecad0 | 2376 | case 255: |
yabahiro | 54:4167f10ecad0 | 2377 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2378 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2379 | motor[TIRE_BR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2380 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2381 | break; |
yabahiro | 54:4167f10ecad0 | 2382 | case 253: |
yabahiro | 54:4167f10ecad0 | 2383 | motor[TIRE_FR].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2384 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2385 | motor[TIRE_BR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2386 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2387 | break; |
yabahiro | 54:4167f10ecad0 | 2388 | case 254: |
yabahiro | 54:4167f10ecad0 | 2389 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2390 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2391 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2392 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2393 | break; |
yabahiro | 54:4167f10ecad0 | 2394 | case 1: |
yabahiro | 54:4167f10ecad0 | 2395 | motor[TIRE_FR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2396 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2397 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2398 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2399 | break; |
yabahiro | 54:4167f10ecad0 | 2400 | case 3: |
yabahiro | 54:4167f10ecad0 | 2401 | motor[TIRE_FR].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2402 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2403 | motor[TIRE_BR].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2404 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2405 | break; |
yabahiro | 54:4167f10ecad0 | 2406 | case 2: |
yabahiro | 54:4167f10ecad0 | 2407 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2408 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2409 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2410 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2411 | break; |
yabahiro | 54:4167f10ecad0 | 2412 | } |
yabahiro | 54:4167f10ecad0 | 2413 | switch(g[3]) { |
yabahiro | 54:4167f10ecad0 | 2414 | case 0: |
yabahiro | 54:4167f10ecad0 | 2415 | motor[TIRE_FL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2416 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2417 | motor[TIRE_BL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2418 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2419 | break; |
yabahiro | 54:4167f10ecad0 | 2420 | case 255: |
yabahiro | 54:4167f10ecad0 | 2421 | motor[TIRE_FL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2422 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2423 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2424 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2425 | case 253: |
yabahiro | 54:4167f10ecad0 | 2426 | motor[TIRE_FL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2427 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2428 | motor[TIRE_BL].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2429 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2430 | break; |
yabahiro | 54:4167f10ecad0 | 2431 | case 254: |
yabahiro | 54:4167f10ecad0 | 2432 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2433 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2434 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2435 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2436 | break; |
yabahiro | 54:4167f10ecad0 | 2437 | case 1: |
yabahiro | 54:4167f10ecad0 | 2438 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2439 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2440 | motor[TIRE_BL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2441 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2442 | break; |
yabahiro | 54:4167f10ecad0 | 2443 | case 3: |
yabahiro | 54:4167f10ecad0 | 2444 | motor[TIRE_FL].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2445 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2446 | motor[TIRE_BL].pwm = 20; |
yabahiro | 54:4167f10ecad0 | 2447 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2448 | break; |
yabahiro | 54:4167f10ecad0 | 2449 | case 2: |
yabahiro | 54:4167f10ecad0 | 2450 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2451 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2452 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2453 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2454 | break; |
yabahiro | 54:4167f10ecad0 | 2455 | } |
yabahiro | 54:4167f10ecad0 | 2456 | if(g[3]==0&&g[2]==0) { |
yabahiro | 54:4167f10ecad0 | 2457 | mode=23; |
yabahiro | 54:4167f10ecad0 | 2458 | countss=0; |
yabahiro | 56:61ebd4b759b4 | 2459 | fast = 60; |
yabahiro | 56:61ebd4b759b4 | 2460 | normal = 40; |
yabahiro | 54:4167f10ecad0 | 2461 | slow = 30; |
yabahiro | 54:4167f10ecad0 | 2462 | } |
yabahiro | 54:4167f10ecad0 | 2463 | } else { |
yabahiro | 54:4167f10ecad0 | 2464 | if(g[1]==98) { |
yabahiro | 54:4167f10ecad0 | 2465 | slow=25; |
yabahiro | 54:4167f10ecad0 | 2466 | normal=30; |
yabahiro | 54:4167f10ecad0 | 2467 | fast=40; |
yabahiro | 54:4167f10ecad0 | 2468 | } |
yabahiro | 54:4167f10ecad0 | 2469 | if(g[3]==0) { |
yabahiro | 54:4167f10ecad0 | 2470 | slow=25; |
yabahiro | 54:4167f10ecad0 | 2471 | normal=40; |
yabahiro | 54:4167f10ecad0 | 2472 | fast=60; |
yabahiro | 54:4167f10ecad0 | 2473 | mode=23; |
yabahiro | 54:4167f10ecad0 | 2474 | } |
yabahiro | 54:4167f10ecad0 | 2475 | switch(g[1]) { |
yabahiro | 54:4167f10ecad0 | 2476 | case 98: |
yabahiro | 54:4167f10ecad0 | 2477 | solenoid.solenoid4 = SOLENOID_OFF; |
yabahiro | 54:4167f10ecad0 | 2478 | fast=40; |
yabahiro | 54:4167f10ecad0 | 2479 | normal=30; |
yabahiro | 54:4167f10ecad0 | 2480 | slow=25; |
yabahiro | 54:4167f10ecad0 | 2481 | motor[TIRE_FR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2482 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2483 | motor[TIRE_FL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2484 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2485 | motor[TIRE_BR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2486 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2487 | motor[TIRE_BL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2488 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2489 | break; |
yabahiro | 54:4167f10ecad0 | 2490 | case 0: |
yabahiro | 54:4167f10ecad0 | 2491 | motor[TIRE_FR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2492 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2493 | motor[TIRE_FL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2494 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2495 | motor[TIRE_BR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2496 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2497 | motor[TIRE_BL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2498 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2499 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2500 | break; |
yabahiro | 54:4167f10ecad0 | 2501 | case 255: |
yabahiro | 54:4167f10ecad0 | 2502 | motor[TIRE_FR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2503 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2504 | motor[TIRE_FL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2505 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2506 | motor[TIRE_BR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2507 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2508 | motor[TIRE_BL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2509 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2510 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2511 | break; |
yabahiro | 54:4167f10ecad0 | 2512 | case 253: |
yabahiro | 54:4167f10ecad0 | 2513 | motor[TIRE_FR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2514 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2515 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2516 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2517 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2518 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2519 | motor[TIRE_BL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2520 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2521 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2522 | break; |
yabahiro | 54:4167f10ecad0 | 2523 | case 254: |
yabahiro | 54:4167f10ecad0 | 2524 | motor[TIRE_FR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2525 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2526 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2527 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2528 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2529 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2530 | motor[TIRE_BL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2531 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2532 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2533 | break; |
yabahiro | 54:4167f10ecad0 | 2534 | case 1: |
yabahiro | 54:4167f10ecad0 | 2535 | motor[TIRE_FR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2536 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2537 | motor[TIRE_FL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2538 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2539 | motor[TIRE_BR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2540 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2541 | motor[TIRE_BL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2542 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2543 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2544 | break; |
yabahiro | 54:4167f10ecad0 | 2545 | case 3: |
yabahiro | 54:4167f10ecad0 | 2546 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2547 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2548 | motor[TIRE_FL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2549 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2550 | motor[TIRE_BR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2551 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2552 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2553 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2554 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2555 | break; |
yabahiro | 54:4167f10ecad0 | 2556 | case 2: |
yabahiro | 54:4167f10ecad0 | 2557 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2558 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2559 | motor[TIRE_FL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2560 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2561 | motor[TIRE_BR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2562 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2563 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2564 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2565 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2566 | break; |
yabahiro | 54:4167f10ecad0 | 2567 | } |
yabahiro | 54:4167f10ecad0 | 2568 | if(g[1]!=98&&mtc==true) { |
yabahiro | 54:4167f10ecad0 | 2569 | switch(g[0]) { |
yabahiro | 54:4167f10ecad0 | 2570 | case 0: |
yabahiro | 54:4167f10ecad0 | 2571 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2572 | break; |
yabahiro | 54:4167f10ecad0 | 2573 | case 255: |
yabahiro | 54:4167f10ecad0 | 2574 | motor[TIRE_FL].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2575 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2576 | break; |
yabahiro | 54:4167f10ecad0 | 2577 | case 253: |
yabahiro | 54:4167f10ecad0 | 2578 | motor[TIRE_FL].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2579 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2580 | break; |
yabahiro | 54:4167f10ecad0 | 2581 | case 254: |
yabahiro | 54:4167f10ecad0 | 2582 | motor[TIRE_FL].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2583 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2584 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2585 | break; |
yabahiro | 54:4167f10ecad0 | 2586 | case 1: |
yabahiro | 54:4167f10ecad0 | 2587 | motor[TIRE_FR].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2588 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2589 | break; |
yabahiro | 54:4167f10ecad0 | 2590 | case 3: |
yabahiro | 54:4167f10ecad0 | 2591 | motor[TIRE_FR].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2592 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2593 | break; |
yabahiro | 54:4167f10ecad0 | 2594 | case 2: |
yabahiro | 54:4167f10ecad0 | 2595 | motor[TIRE_FR].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2596 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2597 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2598 | break; |
yabahiro | 54:4167f10ecad0 | 2599 | } |
yabahiro | 54:4167f10ecad0 | 2600 | } |
yabahiro | 54:4167f10ecad0 | 2601 | } |
yabahiro | 54:4167f10ecad0 | 2602 | break; |
yabahiro | 54:4167f10ecad0 | 2603 | case 23: |
yabahiro | 54:4167f10ecad0 | 2604 | if(YELLOW) { |
yabahiro | 54:4167f10ecad0 | 2605 | switch(g[4]) { |
yabahiro | 54:4167f10ecad0 | 2606 | case 98: |
yabahiro | 54:4167f10ecad0 | 2607 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2608 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2609 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2610 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2611 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2612 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2613 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2614 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2615 | break; |
yabahiro | 54:4167f10ecad0 | 2616 | case 99: |
yabahiro | 54:4167f10ecad0 | 2617 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2618 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2619 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2620 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2621 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2622 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2623 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2624 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2625 | break; |
yabahiro | 54:4167f10ecad0 | 2626 | case 0: |
yabahiro | 54:4167f10ecad0 | 2627 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2628 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2629 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2630 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2631 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2632 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2633 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2634 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2635 | break; |
yabahiro | 54:4167f10ecad0 | 2636 | case 255: |
yabahiro | 54:4167f10ecad0 | 2637 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2638 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2639 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2640 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2641 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2642 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2643 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2644 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2645 | break; |
yabahiro | 54:4167f10ecad0 | 2646 | case 253: |
yabahiro | 54:4167f10ecad0 | 2647 | motor[TIRE_FR].pwm = slow+5; |
yabahiro | 54:4167f10ecad0 | 2648 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2649 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2650 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2651 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2652 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2653 | motor[TIRE_BL].pwm = slow+5; |
yabahiro | 54:4167f10ecad0 | 2654 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2655 | break; |
yabahiro | 54:4167f10ecad0 | 2656 | case 254: |
yabahiro | 54:4167f10ecad0 | 2657 | motor[TIRE_FR].pwm = normal+5; |
yabahiro | 54:4167f10ecad0 | 2658 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2659 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2660 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2661 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2662 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2663 | motor[TIRE_BL].pwm = normal+5; |
yabahiro | 54:4167f10ecad0 | 2664 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2665 | break; |
yabahiro | 54:4167f10ecad0 | 2666 | case 1: |
yabahiro | 54:4167f10ecad0 | 2667 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2668 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2669 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2670 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2671 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2672 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2673 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2674 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2675 | break; |
yabahiro | 54:4167f10ecad0 | 2676 | case 3: |
yabahiro | 54:4167f10ecad0 | 2677 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2678 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2679 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 54:4167f10ecad0 | 2680 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2681 | motor[TIRE_BR].pwm = slow+5; |
yabahiro | 54:4167f10ecad0 | 2682 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2683 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2684 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2685 | break; |
yabahiro | 54:4167f10ecad0 | 2686 | case 2: |
yabahiro | 54:4167f10ecad0 | 2687 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2688 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2689 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 54:4167f10ecad0 | 2690 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2691 | motor[TIRE_BR].pwm = normal+5; |
yabahiro | 54:4167f10ecad0 | 2692 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2693 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 54:4167f10ecad0 | 2694 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2695 | break; |
yabahiro | 54:4167f10ecad0 | 2696 | } |
yabahiro | 57:ccfd26b94d8a | 2697 | if(g[1]==98) { |
yabahiro | 56:61ebd4b759b4 | 2698 | fast=10; |
yabahiro | 56:61ebd4b759b4 | 2699 | normal=10; |
yabahiro | 54:4167f10ecad0 | 2700 | slow=10; |
yabahiro | 54:4167f10ecad0 | 2701 | } |
yabahiro | 54:4167f10ecad0 | 2702 | if(g[3]==254&&countss>=100000) { |
yabahiro | 54:4167f10ecad0 | 2703 | mode=24; |
yabahiro | 54:4167f10ecad0 | 2704 | solenoid.solenoid4 = SOLENOID_OFF; |
yabahiro | 56:61ebd4b759b4 | 2705 | aime.start(); |
yabahiro | 56:61ebd4b759b4 | 2706 | aime.reset(); |
yabahiro | 54:4167f10ecad0 | 2707 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2708 | fast=80; |
yabahiro | 54:4167f10ecad0 | 2709 | normal=60; |
yabahiro | 54:4167f10ecad0 | 2710 | slow=30; |
yabahiro | 54:4167f10ecad0 | 2711 | } |
yabahiro | 54:4167f10ecad0 | 2712 | } else { |
yabahiro | 54:4167f10ecad0 | 2713 | if(UPLS||DOLS) { |
yabahiro | 54:4167f10ecad0 | 2714 | if(UPLS||DOLS) { |
yabahiro | 54:4167f10ecad0 | 2715 | motor[MOTOR_0].pwm = 200; |
yabahiro | 54:4167f10ecad0 | 2716 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2717 | fast=60; |
yabahiro | 54:4167f10ecad0 | 2718 | normal=40; |
yabahiro | 54:4167f10ecad0 | 2719 | slow=30; |
yabahiro | 54:4167f10ecad0 | 2720 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2721 | mode=24; |
yabahiro | 54:4167f10ecad0 | 2722 | } |
yabahiro | 54:4167f10ecad0 | 2723 | } else { |
yabahiro | 54:4167f10ecad0 | 2724 | motor[MOTOR_0].pwm = 100; |
yabahiro | 54:4167f10ecad0 | 2725 | motor[MOTOR_0].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2726 | } |
yabahiro | 54:4167f10ecad0 | 2727 | motor[TIRE_FR].pwm = 100; |
yabahiro | 54:4167f10ecad0 | 2728 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2729 | motor[TIRE_FL].pwm = 100; |
yabahiro | 54:4167f10ecad0 | 2730 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2731 | motor[TIRE_BR].pwm = 100; |
yabahiro | 54:4167f10ecad0 | 2732 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2733 | motor[TIRE_BL].pwm = 100; |
yabahiro | 54:4167f10ecad0 | 2734 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 2735 | } |
yabahiro | 54:4167f10ecad0 | 2736 | break; |
yabahiro | 54:4167f10ecad0 | 2737 | case 24: |
yabahiro | 54:4167f10ecad0 | 2738 | if(YELLOW) { |
yabahiro | 56:61ebd4b759b4 | 2739 | value=aime.read_ms(); |
yabahiro | 56:61ebd4b759b4 | 2740 | if(value>=4000) { |
yabahiro | 56:61ebd4b759b4 | 2741 | mode=25; |
yabahiro | 56:61ebd4b759b4 | 2742 | } |
yabahiro | 54:4167f10ecad0 | 2743 | if(g[4]==3||g[4]==2) { |
yabahiro | 54:4167f10ecad0 | 2744 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2745 | motor[TIRE_FR].dir = FOR; |
yabahiro | 53:694dbd7dfa0e | 2746 | motor[TIRE_FL].pwm = 30; |
yabahiro | 53:694dbd7dfa0e | 2747 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2748 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2749 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2750 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2751 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2752 | } |
yabahiro | 54:4167f10ecad0 | 2753 | if(g[4]==253||g[4]==254) { |
yabahiro | 54:4167f10ecad0 | 2754 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2755 | motor[TIRE_FR].dir = BACK; |
yabahiro | 53:694dbd7dfa0e | 2756 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2757 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2758 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2759 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 2760 | motor[TIRE_BL].pwm = 30; |
yabahiro | 52:4394a1debceb | 2761 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2762 | } |
yabahiro | 54:4167f10ecad0 | 2763 | switch(g[2]) { |
yabahiro | 54:4167f10ecad0 | 2764 | case 0: |
yabahiro | 54:4167f10ecad0 | 2765 | motor[TIRE_FR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2766 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2767 | motor[TIRE_BR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2768 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2769 | break; |
yabahiro | 54:4167f10ecad0 | 2770 | case 255: |
yabahiro | 54:4167f10ecad0 | 2771 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2772 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2773 | motor[TIRE_BR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2774 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2775 | break; |
yabahiro | 54:4167f10ecad0 | 2776 | case 253: |
yabahiro | 54:4167f10ecad0 | 2777 | motor[TIRE_FR].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2778 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2779 | motor[TIRE_BR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2780 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2781 | break; |
yabahiro | 54:4167f10ecad0 | 2782 | case 254: |
yabahiro | 54:4167f10ecad0 | 2783 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2784 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2785 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2786 | motor[TIRE_BR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2787 | break; |
yabahiro | 54:4167f10ecad0 | 2788 | case 1: |
yabahiro | 54:4167f10ecad0 | 2789 | motor[TIRE_FR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2790 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2791 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2792 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2793 | break; |
yabahiro | 54:4167f10ecad0 | 2794 | case 3: |
yabahiro | 54:4167f10ecad0 | 2795 | motor[TIRE_FR].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2796 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2797 | motor[TIRE_BR].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2798 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2799 | break; |
yabahiro | 54:4167f10ecad0 | 2800 | case 2: |
yabahiro | 54:4167f10ecad0 | 2801 | motor[TIRE_FR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2802 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2803 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2804 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2805 | break; |
yabahiro | 54:4167f10ecad0 | 2806 | } |
yabahiro | 54:4167f10ecad0 | 2807 | switch(g[3]) { |
yabahiro | 54:4167f10ecad0 | 2808 | case 0: |
yabahiro | 54:4167f10ecad0 | 2809 | motor[TIRE_FL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2810 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2811 | motor[TIRE_BL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2812 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2813 | break; |
yabahiro | 54:4167f10ecad0 | 2814 | case 255: |
yabahiro | 54:4167f10ecad0 | 2815 | motor[TIRE_FL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2816 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2817 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2818 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2819 | case 253: |
yabahiro | 54:4167f10ecad0 | 2820 | motor[TIRE_FL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2821 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2822 | motor[TIRE_BL].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2823 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2824 | break; |
yabahiro | 54:4167f10ecad0 | 2825 | case 254: |
yabahiro | 54:4167f10ecad0 | 2826 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2827 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2828 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2829 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2830 | break; |
yabahiro | 54:4167f10ecad0 | 2831 | case 1: |
yabahiro | 54:4167f10ecad0 | 2832 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2833 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2834 | motor[TIRE_BL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2835 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2836 | break; |
yabahiro | 54:4167f10ecad0 | 2837 | case 3: |
yabahiro | 54:4167f10ecad0 | 2838 | motor[TIRE_FL].pwm = 40; |
yabahiro | 54:4167f10ecad0 | 2839 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2840 | motor[TIRE_BL].pwm = 25; |
yabahiro | 54:4167f10ecad0 | 2841 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2842 | break; |
yabahiro | 54:4167f10ecad0 | 2843 | case 2: |
yabahiro | 54:4167f10ecad0 | 2844 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2845 | motor[TIRE_FL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2846 | motor[TIRE_BL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 2847 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2848 | break; |
yabahiro | 54:4167f10ecad0 | 2849 | } |
yabahiro | 54:4167f10ecad0 | 2850 | if(g[3]==0&&g[2]==0) { |
yabahiro | 54:4167f10ecad0 | 2851 | mode=25; |
yabahiro | 54:4167f10ecad0 | 2852 | } |
yabahiro | 54:4167f10ecad0 | 2853 | } else { |
yabahiro | 54:4167f10ecad0 | 2854 | if(g[1]==98&&countss>=40000) { |
yabahiro | 54:4167f10ecad0 | 2855 | mode=31; |
yabahiro | 54:4167f10ecad0 | 2856 | fast = 60; |
yabahiro | 54:4167f10ecad0 | 2857 | normal = 40; |
yabahiro | 54:4167f10ecad0 | 2858 | slow = 30; |
yabahiro | 54:4167f10ecad0 | 2859 | countss=0; |
yabahiro | 54:4167f10ecad0 | 2860 | } |
yabahiro | 54:4167f10ecad0 | 2861 | switch(g[1]) { |
yabahiro | 54:4167f10ecad0 | 2862 | case 98: |
yabahiro | 54:4167f10ecad0 | 2863 | fast=40; |
yabahiro | 54:4167f10ecad0 | 2864 | normal=30; |
yabahiro | 54:4167f10ecad0 | 2865 | slow=25; |
yabahiro | 54:4167f10ecad0 | 2866 | motor[TIRE_FR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2867 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2868 | motor[TIRE_FL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2869 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2870 | motor[TIRE_BR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2871 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2872 | motor[TIRE_BL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2873 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2874 | break; |
yabahiro | 54:4167f10ecad0 | 2875 | case 0: |
yabahiro | 54:4167f10ecad0 | 2876 | motor[TIRE_FR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2877 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2878 | motor[TIRE_FL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2879 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2880 | motor[TIRE_BR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2881 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2882 | motor[TIRE_BL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2883 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2884 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2885 | break; |
yabahiro | 54:4167f10ecad0 | 2886 | case 255: |
yabahiro | 54:4167f10ecad0 | 2887 | motor[TIRE_FR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2888 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2889 | motor[TIRE_FL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2890 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2891 | motor[TIRE_BR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2892 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2893 | motor[TIRE_BL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2894 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2895 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2896 | break; |
yabahiro | 54:4167f10ecad0 | 2897 | case 253: |
yabahiro | 54:4167f10ecad0 | 2898 | motor[TIRE_FR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2899 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2900 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2901 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2902 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2903 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2904 | motor[TIRE_BL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2905 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2906 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2907 | break; |
yabahiro | 54:4167f10ecad0 | 2908 | case 254: |
yabahiro | 54:4167f10ecad0 | 2909 | motor[TIRE_FR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2910 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2911 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2912 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2913 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2914 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2915 | motor[TIRE_BL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2916 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2917 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2918 | break; |
yabahiro | 54:4167f10ecad0 | 2919 | case 1: |
yabahiro | 54:4167f10ecad0 | 2920 | motor[TIRE_FR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2921 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2922 | motor[TIRE_FL].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2923 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2924 | motor[TIRE_BR].pwm = fast; |
yabahiro | 54:4167f10ecad0 | 2925 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2926 | motor[TIRE_BL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2927 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2928 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2929 | break; |
yabahiro | 54:4167f10ecad0 | 2930 | case 3: |
yabahiro | 54:4167f10ecad0 | 2931 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2932 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2933 | motor[TIRE_FL].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2934 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2935 | motor[TIRE_BR].pwm = slow; |
yabahiro | 54:4167f10ecad0 | 2936 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2937 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2938 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2939 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2940 | break; |
yabahiro | 54:4167f10ecad0 | 2941 | case 2: |
yabahiro | 54:4167f10ecad0 | 2942 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2943 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2944 | motor[TIRE_FL].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2945 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 2946 | motor[TIRE_BR].pwm = normal; |
yabahiro | 54:4167f10ecad0 | 2947 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 2948 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2949 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 2950 | mtc=true; |
yabahiro | 54:4167f10ecad0 | 2951 | break; |
yabahiro | 54:4167f10ecad0 | 2952 | } |
yabahiro | 54:4167f10ecad0 | 2953 | if(g[1]!=98&&mtc==true) { |
yabahiro | 54:4167f10ecad0 | 2954 | switch(g[0]) { |
yabahiro | 54:4167f10ecad0 | 2955 | case 0: |
yabahiro | 54:4167f10ecad0 | 2956 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2957 | break; |
yabahiro | 54:4167f10ecad0 | 2958 | case 255: |
yabahiro | 54:4167f10ecad0 | 2959 | motor[TIRE_FL].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2960 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2961 | break; |
yabahiro | 54:4167f10ecad0 | 2962 | case 253: |
yabahiro | 54:4167f10ecad0 | 2963 | motor[TIRE_FL].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2964 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2965 | break; |
yabahiro | 54:4167f10ecad0 | 2966 | case 254: |
yabahiro | 54:4167f10ecad0 | 2967 | motor[TIRE_FL].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2968 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2969 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2970 | break; |
yabahiro | 54:4167f10ecad0 | 2971 | case 1: |
yabahiro | 54:4167f10ecad0 | 2972 | motor[TIRE_FR].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 2973 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2974 | break; |
yabahiro | 54:4167f10ecad0 | 2975 | case 3: |
yabahiro | 54:4167f10ecad0 | 2976 | motor[TIRE_FR].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 2977 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2978 | break; |
yabahiro | 54:4167f10ecad0 | 2979 | case 2: |
yabahiro | 54:4167f10ecad0 | 2980 | motor[TIRE_FR].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 2981 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 2982 | mtc=false; |
yabahiro | 54:4167f10ecad0 | 2983 | break; |
yabahiro | 54:4167f10ecad0 | 2984 | } |
yabahiro | 54:4167f10ecad0 | 2985 | } |
yabahiro | 52:4394a1debceb | 2986 | } |
yabahiro | 54:4167f10ecad0 | 2987 | break; |
yabahiro | 54:4167f10ecad0 | 2988 | case 25: |
kishibekairohan | 47:a9cad8502999 | 2989 | motor[TIRE_FR].pwm = 100; |
kishibekairohan | 47:a9cad8502999 | 2990 | motor[TIRE_FR].dir = BRAKE; |
kishibekairohan | 47:a9cad8502999 | 2991 | motor[TIRE_FL].pwm = 100; |
kishibekairohan | 47:a9cad8502999 | 2992 | motor[TIRE_FL].dir = BRAKE; |
kishibekairohan | 47:a9cad8502999 | 2993 | motor[TIRE_BR].pwm = 100; |
kishibekairohan | 47:a9cad8502999 | 2994 | motor[TIRE_BR].dir = BRAKE; |
kishibekairohan | 47:a9cad8502999 | 2995 | motor[TIRE_BL].pwm = 100; |
kishibekairohan | 47:a9cad8502999 | 2996 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 46:68296caae1af | 2997 | if(UPLS==true||DOLS==true) { |
yabahiro | 54:4167f10ecad0 | 2998 | if(UPLS==true||DOLS==true) { |
yabahiro | 54:4167f10ecad0 | 2999 | motor[MOTOR_0].pwm = 200; |
yabahiro | 54:4167f10ecad0 | 3000 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3001 | countss = 0; |
yabahiro | 54:4167f10ecad0 | 3002 | mode=26; |
yabahiro | 54:4167f10ecad0 | 3003 | } |
yabahiro | 46:68296caae1af | 3004 | } else { |
yabahiro | 46:68296caae1af | 3005 | motor[MOTOR_0].pwm = 100; |
yabahiro | 46:68296caae1af | 3006 | motor[MOTOR_0].dir = BACK; |
yabahiro | 46:68296caae1af | 3007 | } |
yabahiro | 54:4167f10ecad0 | 3008 | break; |
yabahiro | 54:4167f10ecad0 | 3009 | case 26: |
yabahiro | 56:61ebd4b759b4 | 3010 | /* |
yabahiro | 56:61ebd4b759b4 | 3011 | if (g[3] == 0&&countss>=30000) { |
yabahiro | 56:61ebd4b759b4 | 3012 | mode = 40; |
yabahiro | 56:61ebd4b759b4 | 3013 | } |
yabahiro | 56:61ebd4b759b4 | 3014 | */ |
yabahiro | 56:61ebd4b759b4 | 3015 | if (g[1] != 99) { |
yabahiro | 54:4167f10ecad0 | 3016 | mode = 31; |
yabahiro | 54:4167f10ecad0 | 3017 | } |
yabahiro | 38:40e7fdf3c694 | 3018 | switch(g[4]) { |
yabahiro | 38:40e7fdf3c694 | 3019 | case 98: |
yabahiro | 38:40e7fdf3c694 | 3020 | motor[TIRE_FR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3021 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3022 | motor[TIRE_FL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3023 | motor[TIRE_FL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3024 | motor[TIRE_BR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3025 | motor[TIRE_BR].dir = FOR; |
kishibekairohan | 41:99a1158f3eca | 3026 | motor[TIRE_BL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3027 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3028 | break; |
yabahiro | 38:40e7fdf3c694 | 3029 | case 99: |
yabahiro | 38:40e7fdf3c694 | 3030 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3031 | motor[TIRE_FR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3032 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3033 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3034 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3035 | motor[TIRE_BR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3036 | motor[TIRE_BL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3037 | motor[TIRE_BL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3038 | break; |
yabahiro | 38:40e7fdf3c694 | 3039 | case 0: |
yabahiro | 38:40e7fdf3c694 | 3040 | motor[TIRE_FR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3041 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3042 | motor[TIRE_FL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3043 | motor[TIRE_FL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3044 | motor[TIRE_BR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3045 | motor[TIRE_BR].dir = FOR; |
kishibekairohan | 41:99a1158f3eca | 3046 | motor[TIRE_BL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3047 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3048 | break; |
yabahiro | 38:40e7fdf3c694 | 3049 | case 255: |
yabahiro | 38:40e7fdf3c694 | 3050 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3051 | motor[TIRE_FR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3052 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3053 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3054 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3055 | motor[TIRE_BR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3056 | motor[TIRE_BL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3057 | motor[TIRE_BL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3058 | break; |
yabahiro | 38:40e7fdf3c694 | 3059 | case 253: |
yabahiro | 46:68296caae1af | 3060 | motor[TIRE_FR].pwm = 25; |
yabahiro | 38:40e7fdf3c694 | 3061 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3062 | motor[TIRE_FL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3063 | motor[TIRE_FL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3064 | motor[TIRE_BR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3065 | motor[TIRE_BR].dir = FOR; |
yabahiro | 46:68296caae1af | 3066 | motor[TIRE_BL].pwm = 25; |
yabahiro | 38:40e7fdf3c694 | 3067 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3068 | break; |
yabahiro | 38:40e7fdf3c694 | 3069 | case 254: |
yabahiro | 38:40e7fdf3c694 | 3070 | motor[TIRE_FR].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3071 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3072 | motor[TIRE_FL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3073 | motor[TIRE_FL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3074 | motor[TIRE_BR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3075 | motor[TIRE_BR].dir = FOR; |
kishibekairohan | 41:99a1158f3eca | 3076 | motor[TIRE_BL].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3077 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3078 | break; |
yabahiro | 38:40e7fdf3c694 | 3079 | case 1: |
yabahiro | 38:40e7fdf3c694 | 3080 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3081 | motor[TIRE_FR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3082 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3083 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3084 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3085 | motor[TIRE_BR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3086 | motor[TIRE_BL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3087 | motor[TIRE_BL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3088 | break; |
yabahiro | 38:40e7fdf3c694 | 3089 | case 3: |
yabahiro | 38:40e7fdf3c694 | 3090 | motor[TIRE_FR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3091 | motor[TIRE_FR].dir = FOR; |
yabahiro | 46:68296caae1af | 3092 | motor[TIRE_FL].pwm = 25; |
yabahiro | 38:40e7fdf3c694 | 3093 | motor[TIRE_FL].dir = BACK; |
yabahiro | 46:68296caae1af | 3094 | motor[TIRE_BR].pwm = 25; |
yabahiro | 38:40e7fdf3c694 | 3095 | motor[TIRE_BR].dir = FOR; |
kishibekairohan | 41:99a1158f3eca | 3096 | motor[TIRE_BL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3097 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3098 | break; |
yabahiro | 38:40e7fdf3c694 | 3099 | case 2: |
yabahiro | 38:40e7fdf3c694 | 3100 | motor[TIRE_FR].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3101 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3102 | motor[TIRE_FL].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3103 | motor[TIRE_FL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3104 | motor[TIRE_BR].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3105 | motor[TIRE_BR].dir = FOR; |
kishibekairohan | 41:99a1158f3eca | 3106 | motor[TIRE_BL].pwm = 60; |
yabahiro | 38:40e7fdf3c694 | 3107 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3108 | break; |
yabahiro | 38:40e7fdf3c694 | 3109 | } |
yabahiro | 54:4167f10ecad0 | 3110 | break; |
yabahiro | 54:4167f10ecad0 | 3111 | case 31: |
yabahiro | 54:4167f10ecad0 | 3112 | motor[TIRE_FR].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 3113 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3114 | motor[TIRE_FL].pwm = 45; |
yabahiro | 54:4167f10ecad0 | 3115 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3116 | motor[TIRE_BR].pwm = 45; |
yabahiro | 54:4167f10ecad0 | 3117 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3118 | motor[TIRE_BL].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 3119 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 3120 | if(g[3]==0) { |
yabahiro | 54:4167f10ecad0 | 3121 | cross=0; |
yabahiro | 54:4167f10ecad0 | 3122 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3123 | mode=40; |
yabahiro | 56:61ebd4b759b4 | 3124 | fast = 70; |
yabahiro | 56:61ebd4b759b4 | 3125 | normal = 50; |
yabahiro | 54:4167f10ecad0 | 3126 | slow = 30; |
yabahiro | 54:4167f10ecad0 | 3127 | } |
yabahiro | 54:4167f10ecad0 | 3128 | break; |
yabahiro | 54:4167f10ecad0 | 3129 | case 40: |
yabahiro | 54:4167f10ecad0 | 3130 | if(YELLOW) { |
yabahiro | 54:4167f10ecad0 | 3131 | if(cross==1) { |
yabahiro | 54:4167f10ecad0 | 3132 | mode=41; |
yabahiro | 54:4167f10ecad0 | 3133 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3134 | cross=0; |
yabahiro | 54:4167f10ecad0 | 3135 | } |
yabahiro | 54:4167f10ecad0 | 3136 | } else { |
yabahiro | 54:4167f10ecad0 | 3137 | if(cross==3) { |
yabahiro | 54:4167f10ecad0 | 3138 | fast = 60; |
yabahiro | 54:4167f10ecad0 | 3139 | normal = 40; |
yabahiro | 54:4167f10ecad0 | 3140 | slow = 30; |
yabahiro | 54:4167f10ecad0 | 3141 | mode=41; |
yabahiro | 54:4167f10ecad0 | 3142 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3143 | cross=0; |
yabahiro | 54:4167f10ecad0 | 3144 | } |
yabahiro | 54:4167f10ecad0 | 3145 | } |
yabahiro | 54:4167f10ecad0 | 3146 | /* |
yabahiro | 54:4167f10ecad0 | 3147 | if(LIF||LIB) { |
yabahiro | 54:4167f10ecad0 | 3148 | mode=42; |
yabahiro | 33:dd598bb23ad1 | 3149 | } |
yabahiro | 54:4167f10ecad0 | 3150 | */ |
yabahiro | 54:4167f10ecad0 | 3151 | switch(g[3]) { |
yabahiro | 54:4167f10ecad0 | 3152 | case 98: |
yabahiro | 54:4167f10ecad0 | 3153 | motor[TIRE_FR].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3154 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3155 | motor[TIRE_FL].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3156 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3157 | motor[TIRE_BR].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3158 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3159 | motor[TIRE_BL].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3160 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3161 | if(countss>30000) { |
yabahiro | 54:4167f10ecad0 | 3162 | cross++; |
yabahiro | 54:4167f10ecad0 | 3163 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3164 | } |
yabahiro | 54:4167f10ecad0 | 3165 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3166 | break; |
yabahiro | 54:4167f10ecad0 | 3167 | case 0: |
yabahiro | 54:4167f10ecad0 | 3168 | motor[TIRE_FR].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3169 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3170 | motor[TIRE_FL].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3171 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3172 | motor[TIRE_BR].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3173 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3174 | motor[TIRE_BL].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3175 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3176 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3177 | break; |
yabahiro | 54:4167f10ecad0 | 3178 | case 255: |
yabahiro | 54:4167f10ecad0 | 3179 | motor[TIRE_FR].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3180 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3181 | motor[TIRE_FL].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3182 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3183 | motor[TIRE_BR].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3184 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3185 | motor[TIRE_BL].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3186 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3187 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3188 | break; |
yabahiro | 54:4167f10ecad0 | 3189 | case 253: |
yabahiro | 54:4167f10ecad0 | 3190 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3191 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3192 | motor[TIRE_FL].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3193 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3194 | motor[TIRE_BR].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3195 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3196 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3197 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3198 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3199 | break; |
yabahiro | 54:4167f10ecad0 | 3200 | case 254: |
yabahiro | 54:4167f10ecad0 | 3201 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3202 | motor[TIRE_FR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3203 | motor[TIRE_FL].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3204 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3205 | motor[TIRE_BR].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3206 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3207 | motor[TIRE_BL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3208 | motor[TIRE_BL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3209 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3210 | break; |
yabahiro | 54:4167f10ecad0 | 3211 | case 1: |
yabahiro | 54:4167f10ecad0 | 3212 | motor[TIRE_FR].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3213 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3214 | motor[TIRE_FL].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3215 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3216 | motor[TIRE_BR].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3217 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3218 | motor[TIRE_BL].pwm = fast+15; |
yabahiro | 54:4167f10ecad0 | 3219 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3220 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3221 | break; |
yabahiro | 54:4167f10ecad0 | 3222 | case 3: |
yabahiro | 54:4167f10ecad0 | 3223 | motor[TIRE_FR].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3224 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3225 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3226 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3227 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3228 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3229 | motor[TIRE_BL].pwm = slow+15; |
yabahiro | 54:4167f10ecad0 | 3230 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3231 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3232 | break; |
yabahiro | 54:4167f10ecad0 | 3233 | case 2: |
yabahiro | 54:4167f10ecad0 | 3234 | motor[TIRE_FR].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3235 | motor[TIRE_FR].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3236 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3237 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3238 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3239 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3240 | motor[TIRE_BL].pwm = normal+15; |
yabahiro | 54:4167f10ecad0 | 3241 | motor[TIRE_BL].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3242 | mtc2=true; |
yabahiro | 54:4167f10ecad0 | 3243 | break; |
yabahiro | 54:4167f10ecad0 | 3244 | } |
yabahiro | 54:4167f10ecad0 | 3245 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 54:4167f10ecad0 | 3246 | switch(g[2]) { |
yabahiro | 54:4167f10ecad0 | 3247 | case 0: |
yabahiro | 54:4167f10ecad0 | 3248 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3249 | break; |
yabahiro | 54:4167f10ecad0 | 3250 | case 255: |
yabahiro | 54:4167f10ecad0 | 3251 | motor[TIRE_FR].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 3252 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3253 | break; |
yabahiro | 54:4167f10ecad0 | 3254 | case 253: |
yabahiro | 54:4167f10ecad0 | 3255 | motor[TIRE_FR].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 3256 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3257 | break; |
yabahiro | 54:4167f10ecad0 | 3258 | case 254: |
yabahiro | 54:4167f10ecad0 | 3259 | motor[TIRE_FR].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 3260 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3261 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3262 | break; |
yabahiro | 54:4167f10ecad0 | 3263 | case 1: |
yabahiro | 54:4167f10ecad0 | 3264 | motor[TIRE_BR].pwm += 3; |
yabahiro | 54:4167f10ecad0 | 3265 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3266 | break; |
yabahiro | 54:4167f10ecad0 | 3267 | case 3: |
yabahiro | 54:4167f10ecad0 | 3268 | motor[TIRE_BR].pwm += 6; |
yabahiro | 54:4167f10ecad0 | 3269 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3270 | break; |
yabahiro | 54:4167f10ecad0 | 3271 | case 2: |
yabahiro | 54:4167f10ecad0 | 3272 | motor[TIRE_BR].pwm += 9; |
yabahiro | 54:4167f10ecad0 | 3273 | motor[TIRE_FR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3274 | mtc2=false; |
yabahiro | 54:4167f10ecad0 | 3275 | break; |
yabahiro | 54:4167f10ecad0 | 3276 | } |
yabahiro | 54:4167f10ecad0 | 3277 | } |
yabahiro | 54:4167f10ecad0 | 3278 | break; |
yabahiro | 54:4167f10ecad0 | 3279 | case 41: |
yabahiro | 54:4167f10ecad0 | 3280 | /* |
yabahiro | 54:4167f10ecad0 | 3281 | if(LIF||LIB) { |
yabahiro | 54:4167f10ecad0 | 3282 | mode=42; |
yabahiro | 54:4167f10ecad0 | 3283 | } |
yabahiro | 54:4167f10ecad0 | 3284 | */ |
yabahiro | 54:4167f10ecad0 | 3285 | motor[TIRE_FR].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 3286 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3287 | motor[TIRE_FL].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 3288 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3289 | motor[TIRE_BR].pwm = 30; |
yabahiro | 54:4167f10ecad0 | 3290 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3291 | motor[TIRE_BL].pwm = 255; |
yabahiro | 54:4167f10ecad0 | 3292 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3293 | if(g[1]==0) { |
yabahiro | 54:4167f10ecad0 | 3294 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3295 | mode=70; |
yabahiro | 56:61ebd4b759b4 | 3296 | normal = 120; |
yabahiro | 56:61ebd4b759b4 | 3297 | fast = 140; |
yabahiro | 56:61ebd4b759b4 | 3298 | slow = 100; |
yabahiro | 54:4167f10ecad0 | 3299 | } |
yabahiro | 54:4167f10ecad0 | 3300 | break; |
yabahiro | 54:4167f10ecad0 | 3301 | case 42: |
yabahiro | 54:4167f10ecad0 | 3302 | motor[TIRE_FR].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3303 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3304 | motor[TIRE_FL].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3305 | motor[TIRE_FL].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3306 | motor[TIRE_BR].pwm = 0; |
yabahiro | 54:4167f10ecad0 | 3307 | motor[TIRE_BR].dir = FREE; |
yabahiro | 54:4167f10ecad0 | 3308 | motor[TIRE_BL].pwm = 40; |
yabahiro | 38:40e7fdf3c694 | 3309 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3310 | if(g[1]==0) { |
yabahiro | 54:4167f10ecad0 | 3311 | countss=0; |
yabahiro | 54:4167f10ecad0 | 3312 | mode=70; |
yabahiro | 53:694dbd7dfa0e | 3313 | } |
yabahiro | 54:4167f10ecad0 | 3314 | break; |
yabahiro | 54:4167f10ecad0 | 3315 | case 70: |
yabahiro | 38:40e7fdf3c694 | 3316 | switch(g[1]) { |
yabahiro | 54:4167f10ecad0 | 3317 | case 99: |
yabahiro | 54:4167f10ecad0 | 3318 | if(countss>=100000) { |
yabahiro | 54:4167f10ecad0 | 3319 | mode=100; |
yabahiro | 54:4167f10ecad0 | 3320 | } |
yabahiro | 54:4167f10ecad0 | 3321 | break; |
yabahiro | 38:40e7fdf3c694 | 3322 | case 98: |
yabahiro | 38:40e7fdf3c694 | 3323 | motor[TIRE_FR].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3324 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3325 | motor[TIRE_FL].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3326 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3327 | motor[TIRE_BR].pwm = normal+5; |
yabahiro | 38:40e7fdf3c694 | 3328 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3329 | motor[TIRE_BL].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3330 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3331 | break; |
yabahiro | 38:40e7fdf3c694 | 3332 | case 0: |
yabahiro | 54:4167f10ecad0 | 3333 | motor[TIRE_FR].pwm = 100; |
yabahiro | 38:40e7fdf3c694 | 3334 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3335 | motor[TIRE_FL].pwm = 100; |
yabahiro | 38:40e7fdf3c694 | 3336 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3337 | motor[TIRE_BR].pwm = 105; |
yabahiro | 38:40e7fdf3c694 | 3338 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3339 | motor[TIRE_BL].pwm = 100; |
yabahiro | 38:40e7fdf3c694 | 3340 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3341 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3342 | break; |
yabahiro | 38:40e7fdf3c694 | 3343 | case 255: |
yabahiro | 38:40e7fdf3c694 | 3344 | motor[TIRE_FR].pwm = fast; |
yabahiro | 38:40e7fdf3c694 | 3345 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3346 | motor[TIRE_FL].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3347 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3348 | motor[TIRE_BR].pwm = slow+5; |
yabahiro | 38:40e7fdf3c694 | 3349 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3350 | motor[TIRE_BL].pwm = fast; |
yabahiro | 38:40e7fdf3c694 | 3351 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3352 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3353 | break; |
yabahiro | 38:40e7fdf3c694 | 3354 | case 253: |
yabahiro | 38:40e7fdf3c694 | 3355 | motor[TIRE_FR].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3356 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3357 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3358 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3359 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3360 | motor[TIRE_BR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3361 | motor[TIRE_BL].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3362 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3363 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3364 | break; |
yabahiro | 38:40e7fdf3c694 | 3365 | case 254: |
yabahiro | 38:40e7fdf3c694 | 3366 | motor[TIRE_FR].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3367 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3368 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3369 | motor[TIRE_FL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3370 | motor[TIRE_BR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3371 | motor[TIRE_BR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3372 | motor[TIRE_BL].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3373 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3374 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3375 | break; |
yabahiro | 38:40e7fdf3c694 | 3376 | case 1: |
yabahiro | 38:40e7fdf3c694 | 3377 | motor[TIRE_FR].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3378 | motor[TIRE_FR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3379 | motor[TIRE_FL].pwm = fast; |
yabahiro | 38:40e7fdf3c694 | 3380 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3381 | motor[TIRE_BR].pwm = fast+5; |
yabahiro | 38:40e7fdf3c694 | 3382 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3383 | motor[TIRE_BL].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3384 | motor[TIRE_BL].dir = BACK; |
yabahiro | 38:40e7fdf3c694 | 3385 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3386 | break; |
yabahiro | 38:40e7fdf3c694 | 3387 | case 3: |
yabahiro | 38:40e7fdf3c694 | 3388 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3389 | motor[TIRE_FR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3390 | motor[TIRE_FL].pwm = slow; |
yabahiro | 38:40e7fdf3c694 | 3391 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3392 | motor[TIRE_BR].pwm = slow+5; |
yabahiro | 38:40e7fdf3c694 | 3393 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3394 | motor[TIRE_BL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3395 | motor[TIRE_BL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3396 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3397 | break; |
yabahiro | 38:40e7fdf3c694 | 3398 | case 2: |
yabahiro | 38:40e7fdf3c694 | 3399 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3400 | motor[TIRE_FR].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3401 | motor[TIRE_FL].pwm = normal; |
yabahiro | 38:40e7fdf3c694 | 3402 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3403 | motor[TIRE_BR].pwm = normal+5; |
yabahiro | 38:40e7fdf3c694 | 3404 | motor[TIRE_BR].dir = FOR; |
yabahiro | 38:40e7fdf3c694 | 3405 | motor[TIRE_BL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3406 | motor[TIRE_BL].dir = FREE; |
yabahiro | 38:40e7fdf3c694 | 3407 | mtc=true; |
yabahiro | 38:40e7fdf3c694 | 3408 | break; |
yabahiro | 38:40e7fdf3c694 | 3409 | } |
yabahiro | 38:40e7fdf3c694 | 3410 | if(g[1]!=98&&mtc==true) { |
yabahiro | 38:40e7fdf3c694 | 3411 | switch(g[0]) { |
yabahiro | 38:40e7fdf3c694 | 3412 | case 0: |
yabahiro | 38:40e7fdf3c694 | 3413 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3414 | break; |
yabahiro | 38:40e7fdf3c694 | 3415 | case 255: |
kishibekairohan | 44:2aff0da5ebdb | 3416 | motor[TIRE_FL].pwm += 3; |
yabahiro | 38:40e7fdf3c694 | 3417 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3418 | break; |
yabahiro | 38:40e7fdf3c694 | 3419 | case 253: |
kishibekairohan | 44:2aff0da5ebdb | 3420 | motor[TIRE_FL].pwm += 6; |
yabahiro | 38:40e7fdf3c694 | 3421 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3422 | break; |
yabahiro | 38:40e7fdf3c694 | 3423 | case 254: |
kishibekairohan | 44:2aff0da5ebdb | 3424 | motor[TIRE_FL].pwm += 9; |
yabahiro | 38:40e7fdf3c694 | 3425 | motor[TIRE_FR].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3426 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3427 | break; |
yabahiro | 38:40e7fdf3c694 | 3428 | case 1: |
kishibekairohan | 44:2aff0da5ebdb | 3429 | motor[TIRE_FR].pwm += 3; |
yabahiro | 38:40e7fdf3c694 | 3430 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3431 | break; |
yabahiro | 38:40e7fdf3c694 | 3432 | case 3: |
kishibekairohan | 44:2aff0da5ebdb | 3433 | motor[TIRE_FR].pwm += 6; |
yabahiro | 38:40e7fdf3c694 | 3434 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3435 | break; |
yabahiro | 38:40e7fdf3c694 | 3436 | case 2: |
kishibekairohan | 44:2aff0da5ebdb | 3437 | motor[TIRE_FR].pwm += 9; |
yabahiro | 38:40e7fdf3c694 | 3438 | motor[TIRE_FL].pwm = 0; |
yabahiro | 38:40e7fdf3c694 | 3439 | mtc=false; |
yabahiro | 38:40e7fdf3c694 | 3440 | break; |
yabahiro | 38:40e7fdf3c694 | 3441 | } |
yabahiro | 33:dd598bb23ad1 | 3442 | } |
yabahiro | 54:4167f10ecad0 | 3443 | break; |
yabahiro | 54:4167f10ecad0 | 3444 | case 100: |
yabahiro | 54:4167f10ecad0 | 3445 | motor[TIRE_FR].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3446 | motor[TIRE_FR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3447 | motor[TIRE_FL].pwm = 55; |
yabahiro | 54:4167f10ecad0 | 3448 | motor[TIRE_FL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3449 | motor[TIRE_BR].pwm = 60; |
yabahiro | 54:4167f10ecad0 | 3450 | motor[TIRE_BR].dir = FOR; |
yabahiro | 54:4167f10ecad0 | 3451 | motor[TIRE_BL].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3452 | motor[TIRE_BL].dir = BACK; |
yabahiro | 54:4167f10ecad0 | 3453 | break; |
yabahiro | 54:4167f10ecad0 | 3454 | default: |
yabahiro | 54:4167f10ecad0 | 3455 | motor[TIRE_FR].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3456 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3457 | motor[TIRE_FL].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3458 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3459 | motor[TIRE_BR].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3460 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3461 | motor[TIRE_BL].pwm = 50; |
yabahiro | 54:4167f10ecad0 | 3462 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 54:4167f10ecad0 | 3463 | break; |
yabahiro | 53:694dbd7dfa0e | 3464 | } |
kishibekairohan | 13:b6e02d6261d7 | 3465 | } |
kishibekairohan | 13:b6e02d6261d7 | 3466 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 3467 | |
kishibekairohan | 13:b6e02d6261d7 | 3468 | #if USE_PROCESS_NUM>4 |
Ryosei | 27:dd9f27fce7d1 | 3469 | static void Process4() |
M_souta | 22:7d93f79a3686 | 3470 | { |
yabahiro | 48:5562a99479e5 | 3471 | //Oh So 決勝青ゾーン |
yabahiro | 48:5562a99479e5 | 3472 | LED::LED::TapeledMode(2); |
yabahiro | 48:5562a99479e5 | 3473 | countss++; |
yabahiro | 48:5562a99479e5 | 3474 | if(mode==1) {//スタートゾーンから白線検知 |
yabahiro | 48:5562a99479e5 | 3475 | fast=40; |
yabahiro | 48:5562a99479e5 | 3476 | normal=30; |
yabahiro | 48:5562a99479e5 | 3477 | slow=20; |
yabahiro | 48:5562a99479e5 | 3478 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3479 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3480 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3481 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3482 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3483 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3484 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3485 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3486 | if(g[2]==0&&countss>=100000) { |
yabahiro | 48:5562a99479e5 | 3487 | countss=100000; |
yabahiro | 48:5562a99479e5 | 3488 | cross=0; |
yabahiro | 48:5562a99479e5 | 3489 | mode=3; |
yabahiro | 48:5562a99479e5 | 3490 | } |
yabahiro | 48:5562a99479e5 | 3491 | } else if(mode==3) { //横ライントレースから縦ライントレースへ |
yabahiro | 48:5562a99479e5 | 3492 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3493 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3494 | motor[TIRE_FL].pwm = 255; |
yabahiro | 48:5562a99479e5 | 3495 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 3496 | motor[TIRE_BR].pwm = 255; |
yabahiro | 48:5562a99479e5 | 3497 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 3498 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3499 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3500 | if(g[1]==0) { |
yabahiro | 48:5562a99479e5 | 3501 | mode=10; |
yabahiro | 48:5562a99479e5 | 3502 | countss=100000; |
yabahiro | 48:5562a99479e5 | 3503 | } |
yabahiro | 48:5562a99479e5 | 3504 | } else if(mode==10) { //縦ライントレース |
yabahiro | 48:5562a99479e5 | 3505 | switch(g[1]) { |
yabahiro | 48:5562a99479e5 | 3506 | case 98: |
yabahiro | 48:5562a99479e5 | 3507 | motor[TIRE_FR].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3508 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3509 | motor[TIRE_FL].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3510 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3511 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3512 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3513 | motor[TIRE_BL].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3514 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3515 | mtc=true; |
yabahiro | 49:697b4e9ce884 | 3516 | if(YELLOW) { |
yabahiro | 49:697b4e9ce884 | 3517 | if(countss>80000) { |
yabahiro | 49:697b4e9ce884 | 3518 | cross++; |
yabahiro | 49:697b4e9ce884 | 3519 | countss=0; |
yabahiro | 49:697b4e9ce884 | 3520 | } |
yabahiro | 48:5562a99479e5 | 3521 | |
yabahiro | 49:697b4e9ce884 | 3522 | if(cross==2) { |
yabahiro | 49:697b4e9ce884 | 3523 | fast = 10; |
yabahiro | 49:697b4e9ce884 | 3524 | normal = 10; |
yabahiro | 49:697b4e9ce884 | 3525 | slow = 5; |
yabahiro | 49:697b4e9ce884 | 3526 | } |
yabahiro | 48:5562a99479e5 | 3527 | |
yabahiro | 49:697b4e9ce884 | 3528 | if(cross==3) { |
yabahiro | 49:697b4e9ce884 | 3529 | fast=40; |
yabahiro | 49:697b4e9ce884 | 3530 | normal=30; |
yabahiro | 49:697b4e9ce884 | 3531 | slow=20; |
yabahiro | 49:697b4e9ce884 | 3532 | mode=11; |
yabahiro | 49:697b4e9ce884 | 3533 | countss=0; |
yabahiro | 49:697b4e9ce884 | 3534 | cross=0; |
yabahiro | 49:697b4e9ce884 | 3535 | } |
yabahiro | 49:697b4e9ce884 | 3536 | } else { |
yabahiro | 48:5562a99479e5 | 3537 | mode=11; |
yabahiro | 48:5562a99479e5 | 3538 | } |
yabahiro | 48:5562a99479e5 | 3539 | break; |
yabahiro | 48:5562a99479e5 | 3540 | case 0: |
yabahiro | 48:5562a99479e5 | 3541 | motor[TIRE_FR].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3542 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3543 | motor[TIRE_FL].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3544 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3545 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3546 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3547 | motor[TIRE_BL].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3548 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3549 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3550 | break; |
yabahiro | 48:5562a99479e5 | 3551 | case 255: |
yabahiro | 48:5562a99479e5 | 3552 | motor[TIRE_FR].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3553 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3554 | motor[TIRE_FL].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3555 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3556 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3557 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3558 | motor[TIRE_BL].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3559 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3560 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3561 | break; |
yabahiro | 48:5562a99479e5 | 3562 | case 253: |
yabahiro | 48:5562a99479e5 | 3563 | motor[TIRE_FR].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3564 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3565 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3566 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3567 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3568 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3569 | motor[TIRE_BL].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3570 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3571 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3572 | break; |
yabahiro | 48:5562a99479e5 | 3573 | case 254: |
yabahiro | 48:5562a99479e5 | 3574 | motor[TIRE_FR].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3575 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3576 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3577 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3578 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3579 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3580 | motor[TIRE_BL].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3581 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3582 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3583 | break; |
yabahiro | 48:5562a99479e5 | 3584 | case 1: |
yabahiro | 48:5562a99479e5 | 3585 | motor[TIRE_FR].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3586 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3587 | motor[TIRE_FL].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3588 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3589 | motor[TIRE_BR].pwm = fast+50; |
yabahiro | 48:5562a99479e5 | 3590 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3591 | motor[TIRE_BL].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3592 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3593 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3594 | break; |
yabahiro | 48:5562a99479e5 | 3595 | case 3: |
yabahiro | 48:5562a99479e5 | 3596 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3597 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3598 | motor[TIRE_FL].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3599 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3600 | motor[TIRE_BR].pwm = slow+50; |
yabahiro | 48:5562a99479e5 | 3601 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3602 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3603 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3604 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3605 | break; |
yabahiro | 48:5562a99479e5 | 3606 | case 2: |
yabahiro | 48:5562a99479e5 | 3607 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3608 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3609 | motor[TIRE_FL].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3610 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3611 | motor[TIRE_BR].pwm = normal+50; |
yabahiro | 48:5562a99479e5 | 3612 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3613 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3614 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3615 | mtc=true; |
yabahiro | 48:5562a99479e5 | 3616 | break; |
yabahiro | 48:5562a99479e5 | 3617 | } |
yabahiro | 48:5562a99479e5 | 3618 | if(g[1]!=98&&mtc==true) { |
yabahiro | 48:5562a99479e5 | 3619 | switch(g[0]) { |
yabahiro | 48:5562a99479e5 | 3620 | case 0: |
yabahiro | 48:5562a99479e5 | 3621 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3622 | break; |
yabahiro | 48:5562a99479e5 | 3623 | case 255: |
yabahiro | 48:5562a99479e5 | 3624 | motor[TIRE_FL].pwm += 3; |
yabahiro | 48:5562a99479e5 | 3625 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3626 | break; |
yabahiro | 48:5562a99479e5 | 3627 | case 253: |
yabahiro | 48:5562a99479e5 | 3628 | motor[TIRE_FL].pwm += 6; |
yabahiro | 48:5562a99479e5 | 3629 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3630 | break; |
yabahiro | 48:5562a99479e5 | 3631 | case 254: |
yabahiro | 48:5562a99479e5 | 3632 | motor[TIRE_FL].pwm += 9; |
yabahiro | 48:5562a99479e5 | 3633 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3634 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3635 | break; |
yabahiro | 48:5562a99479e5 | 3636 | case 1: |
yabahiro | 48:5562a99479e5 | 3637 | motor[TIRE_FR].pwm += 3; |
yabahiro | 48:5562a99479e5 | 3638 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3639 | break; |
yabahiro | 48:5562a99479e5 | 3640 | case 3: |
yabahiro | 48:5562a99479e5 | 3641 | motor[TIRE_FR].pwm += 6; |
yabahiro | 48:5562a99479e5 | 3642 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3643 | break; |
yabahiro | 48:5562a99479e5 | 3644 | case 2: |
yabahiro | 48:5562a99479e5 | 3645 | motor[TIRE_FR].pwm += 9; |
yabahiro | 48:5562a99479e5 | 3646 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3647 | mtc=false; |
yabahiro | 48:5562a99479e5 | 3648 | break; |
yabahiro | 48:5562a99479e5 | 3649 | } |
yabahiro | 48:5562a99479e5 | 3650 | } |
yabahiro | 48:5562a99479e5 | 3651 | } else if(mode==11) { |
yabahiro | 48:5562a99479e5 | 3652 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 3653 | motor[TIRE_FR].dir = FOR; |
yabahiro | 49:697b4e9ce884 | 3654 | motor[TIRE_FL].pwm = 50; |
yabahiro | 49:697b4e9ce884 | 3655 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 49:697b4e9ce884 | 3656 | motor[TIRE_BR].pwm = 50; |
yabahiro | 49:697b4e9ce884 | 3657 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 3658 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 3659 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3660 | if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { |
yabahiro | 48:5562a99479e5 | 3661 | countss=0; |
yabahiro | 48:5562a99479e5 | 3662 | cross=0; |
yabahiro | 48:5562a99479e5 | 3663 | mode=20; |
yabahiro | 48:5562a99479e5 | 3664 | } |
yabahiro | 48:5562a99479e5 | 3665 | } else if(mode==20) { |
yabahiro | 48:5562a99479e5 | 3666 | switch(g[2]) { |
yabahiro | 48:5562a99479e5 | 3667 | case 98: |
yabahiro | 48:5562a99479e5 | 3668 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3669 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3670 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3671 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3672 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3673 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3674 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3675 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3676 | if(countss>30000) { |
yabahiro | 48:5562a99479e5 | 3677 | cross++; |
yabahiro | 48:5562a99479e5 | 3678 | countss=0; |
yabahiro | 48:5562a99479e5 | 3679 | } |
yabahiro | 48:5562a99479e5 | 3680 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3681 | |
yabahiro | 48:5562a99479e5 | 3682 | if(YELLOW) { |
yabahiro | 49:697b4e9ce884 | 3683 | solenoid.solenoid4= SOLENOID_ON; |
yabahiro | 48:5562a99479e5 | 3684 | if(cross==1) { |
yabahiro | 48:5562a99479e5 | 3685 | fast=20; |
yabahiro | 48:5562a99479e5 | 3686 | normal=15; |
yabahiro | 48:5562a99479e5 | 3687 | slow=15; |
yabahiro | 48:5562a99479e5 | 3688 | } else if(cross==2) { |
yabahiro | 48:5562a99479e5 | 3689 | mode=21; |
yabahiro | 48:5562a99479e5 | 3690 | countss=0; |
yabahiro | 48:5562a99479e5 | 3691 | cross=0; |
yabahiro | 48:5562a99479e5 | 3692 | } |
yabahiro | 48:5562a99479e5 | 3693 | } else { |
yabahiro | 49:697b4e9ce884 | 3694 | if(cross==2) { |
yabahiro | 49:697b4e9ce884 | 3695 | fast=20; |
yabahiro | 49:697b4e9ce884 | 3696 | normal=15; |
yabahiro | 49:697b4e9ce884 | 3697 | slow=15; |
yabahiro | 49:697b4e9ce884 | 3698 | } |
yabahiro | 48:5562a99479e5 | 3699 | if(cross==3) { |
yabahiro | 48:5562a99479e5 | 3700 | mode=21; |
yabahiro | 48:5562a99479e5 | 3701 | countss=0; |
yabahiro | 48:5562a99479e5 | 3702 | cross=0; |
yabahiro | 48:5562a99479e5 | 3703 | } |
yabahiro | 48:5562a99479e5 | 3704 | } |
yabahiro | 48:5562a99479e5 | 3705 | break; |
yabahiro | 48:5562a99479e5 | 3706 | case 0: |
yabahiro | 48:5562a99479e5 | 3707 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3708 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3709 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3710 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3711 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3712 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3713 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3714 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3715 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3716 | break; |
yabahiro | 48:5562a99479e5 | 3717 | case 255: |
yabahiro | 48:5562a99479e5 | 3718 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3719 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3720 | motor[TIRE_FL].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3721 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3722 | motor[TIRE_BR].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3723 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3724 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3725 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3726 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3727 | break; |
yabahiro | 48:5562a99479e5 | 3728 | case 253: |
yabahiro | 48:5562a99479e5 | 3729 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3730 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3731 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3732 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3733 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3734 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3735 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3736 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3737 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3738 | break; |
yabahiro | 48:5562a99479e5 | 3739 | case 254: |
yabahiro | 48:5562a99479e5 | 3740 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3741 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3742 | motor[TIRE_FL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3743 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3744 | motor[TIRE_BR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3745 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3746 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3747 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3748 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3749 | break; |
yabahiro | 48:5562a99479e5 | 3750 | case 1: |
yabahiro | 48:5562a99479e5 | 3751 | motor[TIRE_FR].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3752 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3753 | motor[TIRE_FL].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3754 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3755 | motor[TIRE_BR].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3756 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3757 | motor[TIRE_BL].pwm = fast+20; |
yabahiro | 48:5562a99479e5 | 3758 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3759 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3760 | break; |
yabahiro | 48:5562a99479e5 | 3761 | case 3: |
yabahiro | 48:5562a99479e5 | 3762 | motor[TIRE_FR].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3763 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3764 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3765 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3766 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3767 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3768 | motor[TIRE_BL].pwm = slow+20; |
yabahiro | 48:5562a99479e5 | 3769 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3770 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3771 | break; |
yabahiro | 48:5562a99479e5 | 3772 | case 2: |
yabahiro | 48:5562a99479e5 | 3773 | motor[TIRE_FR].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3774 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3775 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3776 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3777 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3778 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3779 | motor[TIRE_BL].pwm = normal+20; |
yabahiro | 48:5562a99479e5 | 3780 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3781 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 3782 | break; |
yabahiro | 48:5562a99479e5 | 3783 | } |
yabahiro | 48:5562a99479e5 | 3784 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 48:5562a99479e5 | 3785 | switch(g[3]) { |
yabahiro | 48:5562a99479e5 | 3786 | case 0: |
yabahiro | 48:5562a99479e5 | 3787 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3788 | break; |
yabahiro | 48:5562a99479e5 | 3789 | case 255: |
yabahiro | 48:5562a99479e5 | 3790 | motor[TIRE_BL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 3791 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3792 | break; |
yabahiro | 48:5562a99479e5 | 3793 | case 253: |
yabahiro | 48:5562a99479e5 | 3794 | motor[TIRE_BL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 3795 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3796 | break; |
yabahiro | 48:5562a99479e5 | 3797 | case 254: |
yabahiro | 48:5562a99479e5 | 3798 | motor[TIRE_BL].pwm += 15; |
yabahiro | 48:5562a99479e5 | 3799 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3800 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3801 | break; |
yabahiro | 48:5562a99479e5 | 3802 | case 1: |
yabahiro | 48:5562a99479e5 | 3803 | motor[TIRE_FL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 3804 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3805 | break; |
yabahiro | 48:5562a99479e5 | 3806 | case 3: |
yabahiro | 48:5562a99479e5 | 3807 | motor[TIRE_FL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 3808 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3809 | break; |
yabahiro | 48:5562a99479e5 | 3810 | case 2: |
yabahiro | 48:5562a99479e5 | 3811 | motor[TIRE_FL].pwm += 15; |
yabahiro | 48:5562a99479e5 | 3812 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3813 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 3814 | break; |
yabahiro | 48:5562a99479e5 | 3815 | } |
yabahiro | 48:5562a99479e5 | 3816 | } |
yabahiro | 48:5562a99479e5 | 3817 | } |
yabahiro | 48:5562a99479e5 | 3818 | if(YELLOW) { |
yabahiro | 48:5562a99479e5 | 3819 | if(mode==21) { |
yabahiro | 48:5562a99479e5 | 3820 | motor[TIRE_FR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3821 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3822 | motor[TIRE_FL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3823 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3824 | motor[TIRE_BR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3825 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3826 | motor[TIRE_BL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3827 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3828 | if(g[3]==98&&countss>=10000) { |
yabahiro | 48:5562a99479e5 | 3829 | mode=22; |
yabahiro | 48:5562a99479e5 | 3830 | countss=0; |
yabahiro | 48:5562a99479e5 | 3831 | fast=60; |
yabahiro | 48:5562a99479e5 | 3832 | normal=40; |
yabahiro | 48:5562a99479e5 | 3833 | slow=20; |
yabahiro | 48:5562a99479e5 | 3834 | } |
yabahiro | 48:5562a99479e5 | 3835 | } else if(mode==22) { |
yabahiro | 48:5562a99479e5 | 3836 | if(g[4]==3||g[4]==2) { |
yabahiro | 48:5562a99479e5 | 3837 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3838 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3839 | motor[TIRE_FL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3840 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3841 | motor[TIRE_BR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3842 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3843 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3844 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3845 | } |
yabahiro | 48:5562a99479e5 | 3846 | if(g[4]==253||g[4]==254) { |
yabahiro | 48:5562a99479e5 | 3847 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3848 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3849 | motor[TIRE_FL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3850 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3851 | motor[TIRE_BR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3852 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3853 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3854 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3855 | } |
yabahiro | 48:5562a99479e5 | 3856 | switch(g[2]) { |
yabahiro | 48:5562a99479e5 | 3857 | case 0: |
yabahiro | 48:5562a99479e5 | 3858 | motor[TIRE_FR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3859 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3860 | motor[TIRE_BR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3861 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3862 | break; |
yabahiro | 48:5562a99479e5 | 3863 | case 255: |
yabahiro | 48:5562a99479e5 | 3864 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3865 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3866 | motor[TIRE_BR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3867 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3868 | break; |
yabahiro | 48:5562a99479e5 | 3869 | case 253: |
yabahiro | 48:5562a99479e5 | 3870 | motor[TIRE_FR].pwm = 40; |
yabahiro | 48:5562a99479e5 | 3871 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3872 | motor[TIRE_BR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3873 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3874 | break; |
yabahiro | 48:5562a99479e5 | 3875 | case 254: |
yabahiro | 48:5562a99479e5 | 3876 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3877 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3878 | motor[TIRE_BR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3879 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3880 | break; |
yabahiro | 48:5562a99479e5 | 3881 | case 1: |
yabahiro | 48:5562a99479e5 | 3882 | motor[TIRE_FR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3883 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3884 | motor[TIRE_BR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3885 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3886 | break; |
yabahiro | 48:5562a99479e5 | 3887 | case 3: |
yabahiro | 48:5562a99479e5 | 3888 | motor[TIRE_FR].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3889 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3890 | motor[TIRE_BR].pwm = 40; |
yabahiro | 48:5562a99479e5 | 3891 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3892 | break; |
yabahiro | 48:5562a99479e5 | 3893 | case 2: |
yabahiro | 48:5562a99479e5 | 3894 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3895 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3896 | motor[TIRE_BR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3897 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3898 | break; |
yabahiro | 48:5562a99479e5 | 3899 | } |
yabahiro | 48:5562a99479e5 | 3900 | switch(g[3]) { |
yabahiro | 48:5562a99479e5 | 3901 | case 0: |
yabahiro | 48:5562a99479e5 | 3902 | motor[TIRE_FL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3903 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3904 | motor[TIRE_BL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3905 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3906 | break; |
yabahiro | 48:5562a99479e5 | 3907 | case 255: |
yabahiro | 48:5562a99479e5 | 3908 | motor[TIRE_FL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3909 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3910 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3911 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3912 | case 253: |
yabahiro | 48:5562a99479e5 | 3913 | motor[TIRE_FL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3914 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3915 | motor[TIRE_BL].pwm = 40; |
yabahiro | 48:5562a99479e5 | 3916 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3917 | break; |
yabahiro | 48:5562a99479e5 | 3918 | case 254: |
yabahiro | 48:5562a99479e5 | 3919 | motor[TIRE_FL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3920 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3921 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3922 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3923 | break; |
yabahiro | 48:5562a99479e5 | 3924 | case 1: |
yabahiro | 48:5562a99479e5 | 3925 | motor[TIRE_FL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3926 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3927 | motor[TIRE_BL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3928 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3929 | break; |
yabahiro | 48:5562a99479e5 | 3930 | case 3: |
yabahiro | 48:5562a99479e5 | 3931 | motor[TIRE_FL].pwm = 40; |
yabahiro | 48:5562a99479e5 | 3932 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3933 | motor[TIRE_BL].pwm = 20; |
yabahiro | 48:5562a99479e5 | 3934 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3935 | break; |
yabahiro | 48:5562a99479e5 | 3936 | case 2: |
yabahiro | 48:5562a99479e5 | 3937 | motor[TIRE_FL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3938 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3939 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 3940 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3941 | break; |
yabahiro | 48:5562a99479e5 | 3942 | } |
yabahiro | 48:5562a99479e5 | 3943 | if(g[3]==0&&g[2]==0) { |
yabahiro | 48:5562a99479e5 | 3944 | mode=23; |
yabahiro | 48:5562a99479e5 | 3945 | countss=0; |
yabahiro | 48:5562a99479e5 | 3946 | } |
yabahiro | 48:5562a99479e5 | 3947 | } else if(mode==23) { |
yabahiro | 48:5562a99479e5 | 3948 | switch(g[4]) { |
yabahiro | 48:5562a99479e5 | 3949 | case 98: |
yabahiro | 48:5562a99479e5 | 3950 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3951 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3952 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3953 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3954 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3955 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3956 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3957 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3958 | break; |
yabahiro | 48:5562a99479e5 | 3959 | case 99: |
yabahiro | 48:5562a99479e5 | 3960 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3961 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3962 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3963 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3964 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3965 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3966 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3967 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3968 | break; |
yabahiro | 48:5562a99479e5 | 3969 | case 0: |
yabahiro | 48:5562a99479e5 | 3970 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3971 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3972 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3973 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3974 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3975 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3976 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3977 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3978 | break; |
yabahiro | 48:5562a99479e5 | 3979 | case 255: |
yabahiro | 48:5562a99479e5 | 3980 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3981 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3982 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3983 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3984 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3985 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3986 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 3987 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 3988 | break; |
yabahiro | 48:5562a99479e5 | 3989 | case 253: |
yabahiro | 48:5562a99479e5 | 3990 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3991 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3992 | motor[TIRE_FL].pwm = slow-5; |
yabahiro | 48:5562a99479e5 | 3993 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3994 | motor[TIRE_BR].pwm = slow-5; |
yabahiro | 48:5562a99479e5 | 3995 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 3996 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 3997 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 3998 | break; |
yabahiro | 48:5562a99479e5 | 3999 | case 254: |
yabahiro | 48:5562a99479e5 | 4000 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4001 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4002 | motor[TIRE_FL].pwm = normal-5; |
yabahiro | 48:5562a99479e5 | 4003 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4004 | motor[TIRE_BR].pwm = normal-5; |
yabahiro | 48:5562a99479e5 | 4005 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4006 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4007 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4008 | break; |
yabahiro | 48:5562a99479e5 | 4009 | case 1: |
yabahiro | 48:5562a99479e5 | 4010 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4011 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4012 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4013 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4014 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4015 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4016 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4017 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4018 | break; |
yabahiro | 48:5562a99479e5 | 4019 | case 3: |
yabahiro | 48:5562a99479e5 | 4020 | motor[TIRE_FR].pwm = slow-5; |
yabahiro | 48:5562a99479e5 | 4021 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4022 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4023 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4024 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4025 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4026 | motor[TIRE_BL].pwm = slow-5; |
yabahiro | 48:5562a99479e5 | 4027 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4028 | break; |
yabahiro | 48:5562a99479e5 | 4029 | case 2: |
yabahiro | 48:5562a99479e5 | 4030 | motor[TIRE_FR].pwm = normal-5; |
yabahiro | 48:5562a99479e5 | 4031 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4032 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4033 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4034 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 48:5562a99479e5 | 4035 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4036 | motor[TIRE_BL].pwm = normal-5; |
yabahiro | 48:5562a99479e5 | 4037 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4038 | break; |
yabahiro | 48:5562a99479e5 | 4039 | } |
yabahiro | 48:5562a99479e5 | 4040 | if(g[2]==254&&countss>=100000) { |
yabahiro | 48:5562a99479e5 | 4041 | mode=24; |
yabahiro | 48:5562a99479e5 | 4042 | solenoid.solenoid4 = SOLENOID_OFF; |
yabahiro | 48:5562a99479e5 | 4043 | countss=0; |
yabahiro | 48:5562a99479e5 | 4044 | fast=60; |
yabahiro | 48:5562a99479e5 | 4045 | normal=40; |
yabahiro | 48:5562a99479e5 | 4046 | slow=20; |
yabahiro | 48:5562a99479e5 | 4047 | } |
yabahiro | 48:5562a99479e5 | 4048 | } else if(mode==24) { |
yabahiro | 48:5562a99479e5 | 4049 | motor[TIRE_FR].pwm = 60; |
yabahiro | 48:5562a99479e5 | 4050 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4051 | motor[TIRE_FL].pwm = 60; |
yabahiro | 48:5562a99479e5 | 4052 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4053 | motor[TIRE_BR].pwm = 60; |
yabahiro | 48:5562a99479e5 | 4054 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4055 | motor[TIRE_BL].pwm = 60; |
yabahiro | 48:5562a99479e5 | 4056 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4057 | if(g[0]==0) { |
yabahiro | 48:5562a99479e5 | 4058 | mode=25; |
yabahiro | 48:5562a99479e5 | 4059 | } |
yabahiro | 48:5562a99479e5 | 4060 | } else if(mode==25) { |
yabahiro | 48:5562a99479e5 | 4061 | motor[TIRE_FR].pwm = 100; |
yabahiro | 48:5562a99479e5 | 4062 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4063 | motor[TIRE_FL].pwm = 100; |
yabahiro | 48:5562a99479e5 | 4064 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4065 | motor[TIRE_BR].pwm = 100; |
yabahiro | 48:5562a99479e5 | 4066 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4067 | motor[TIRE_BL].pwm = 100; |
yabahiro | 48:5562a99479e5 | 4068 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4069 | if(DOLS==true||UPLS==true) { |
yabahiro | 48:5562a99479e5 | 4070 | motor[MOTOR_0].pwm = 200; |
yabahiro | 48:5562a99479e5 | 4071 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4072 | mode=26; |
yabahiro | 48:5562a99479e5 | 4073 | countss=0; |
yabahiro | 48:5562a99479e5 | 4074 | } else { |
yabahiro | 48:5562a99479e5 | 4075 | motor[MOTOR_0].pwm = 100; |
yabahiro | 48:5562a99479e5 | 4076 | motor[MOTOR_0].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4077 | } |
yabahiro | 48:5562a99479e5 | 4078 | } else if(mode==26) { |
yabahiro | 48:5562a99479e5 | 4079 | if(g[0]==98&&countss>=40000) { |
yabahiro | 48:5562a99479e5 | 4080 | mode=31; |
yabahiro | 48:5562a99479e5 | 4081 | countss=0; |
yabahiro | 48:5562a99479e5 | 4082 | } |
yabahiro | 48:5562a99479e5 | 4083 | switch(g[0]) { |
yabahiro | 48:5562a99479e5 | 4084 | case 98: |
yabahiro | 48:5562a99479e5 | 4085 | fast=40; |
yabahiro | 48:5562a99479e5 | 4086 | normal=30; |
yabahiro | 48:5562a99479e5 | 4087 | slow=25; |
yabahiro | 48:5562a99479e5 | 4088 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4089 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4090 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4091 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4092 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4093 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4094 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4095 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4096 | break; |
yabahiro | 48:5562a99479e5 | 4097 | case 0: |
yabahiro | 48:5562a99479e5 | 4098 | motor[TIRE_FR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4099 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4100 | motor[TIRE_FL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4101 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4102 | motor[TIRE_BR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4103 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4104 | motor[TIRE_BL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4105 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4106 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4107 | break; |
yabahiro | 48:5562a99479e5 | 4108 | case 255: |
yabahiro | 48:5562a99479e5 | 4109 | motor[TIRE_FR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4110 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4111 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4112 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4113 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4114 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4115 | motor[TIRE_BL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4116 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4117 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4118 | break; |
yabahiro | 48:5562a99479e5 | 4119 | case 253: |
yabahiro | 48:5562a99479e5 | 4120 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4121 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4122 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4123 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4124 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4125 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4126 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4127 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4128 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4129 | break; |
yabahiro | 48:5562a99479e5 | 4130 | case 254: |
yabahiro | 48:5562a99479e5 | 4131 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4132 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4133 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4134 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4135 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4136 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4137 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4138 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4139 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4140 | break; |
yabahiro | 48:5562a99479e5 | 4141 | case 1: |
yabahiro | 48:5562a99479e5 | 4142 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4143 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4144 | motor[TIRE_FL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4145 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4146 | motor[TIRE_BR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4147 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4148 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4149 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4150 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4151 | break; |
yabahiro | 48:5562a99479e5 | 4152 | case 3: |
yabahiro | 48:5562a99479e5 | 4153 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4154 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4155 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4156 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4157 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4158 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4159 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4160 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4161 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4162 | break; |
yabahiro | 48:5562a99479e5 | 4163 | case 2: |
yabahiro | 48:5562a99479e5 | 4164 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4165 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4166 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4167 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4168 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4169 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4170 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4171 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4172 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4173 | break; |
yabahiro | 48:5562a99479e5 | 4174 | } |
yabahiro | 48:5562a99479e5 | 4175 | if(g[1]!=98&&mtc==true) { |
yabahiro | 48:5562a99479e5 | 4176 | switch(g[1]) { |
yabahiro | 48:5562a99479e5 | 4177 | case 0: |
yabahiro | 48:5562a99479e5 | 4178 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4179 | break; |
yabahiro | 48:5562a99479e5 | 4180 | case 255: |
yabahiro | 48:5562a99479e5 | 4181 | motor[TIRE_BR].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4182 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4183 | break; |
yabahiro | 48:5562a99479e5 | 4184 | case 253: |
yabahiro | 48:5562a99479e5 | 4185 | motor[TIRE_BR].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4186 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4187 | break; |
yabahiro | 48:5562a99479e5 | 4188 | case 254: |
yabahiro | 48:5562a99479e5 | 4189 | motor[TIRE_BR].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4190 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4191 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4192 | break; |
yabahiro | 48:5562a99479e5 | 4193 | case 1: |
yabahiro | 48:5562a99479e5 | 4194 | motor[TIRE_BL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4195 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4196 | break; |
yabahiro | 48:5562a99479e5 | 4197 | case 3: |
yabahiro | 48:5562a99479e5 | 4198 | motor[TIRE_BL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4199 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4200 | break; |
yabahiro | 48:5562a99479e5 | 4201 | case 2: |
yabahiro | 48:5562a99479e5 | 4202 | motor[TIRE_BL].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4203 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4204 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4205 | break; |
yabahiro | 48:5562a99479e5 | 4206 | } |
yabahiro | 48:5562a99479e5 | 4207 | } |
yabahiro | 48:5562a99479e5 | 4208 | } |
yabahiro | 48:5562a99479e5 | 4209 | } else { |
yabahiro | 48:5562a99479e5 | 4210 | if(mode==21) { |
yabahiro | 49:697b4e9ce884 | 4211 | if(g[0]==98&&countss>=40000) { |
yabahiro | 48:5562a99479e5 | 4212 | fast=60; |
yabahiro | 48:5562a99479e5 | 4213 | normal=30; |
yabahiro | 48:5562a99479e5 | 4214 | slow=25; |
yabahiro | 48:5562a99479e5 | 4215 | mode=31; |
yabahiro | 48:5562a99479e5 | 4216 | countss=0; |
yabahiro | 48:5562a99479e5 | 4217 | } |
yabahiro | 48:5562a99479e5 | 4218 | switch(g[0]) { |
yabahiro | 48:5562a99479e5 | 4219 | case 98: |
yabahiro | 48:5562a99479e5 | 4220 | fast=40; |
yabahiro | 48:5562a99479e5 | 4221 | normal=30; |
yabahiro | 48:5562a99479e5 | 4222 | slow=25; |
yabahiro | 48:5562a99479e5 | 4223 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4224 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4225 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4226 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4227 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4228 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4229 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4230 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4231 | break; |
yabahiro | 48:5562a99479e5 | 4232 | case 0: |
yabahiro | 48:5562a99479e5 | 4233 | motor[TIRE_FR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4234 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4235 | motor[TIRE_FL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4236 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4237 | motor[TIRE_BR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4238 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4239 | motor[TIRE_BL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4240 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4241 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4242 | break; |
yabahiro | 48:5562a99479e5 | 4243 | case 255: |
yabahiro | 48:5562a99479e5 | 4244 | motor[TIRE_FR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4245 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4246 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4247 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4248 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4249 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4250 | motor[TIRE_BL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4251 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4252 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4253 | break; |
yabahiro | 48:5562a99479e5 | 4254 | case 253: |
yabahiro | 48:5562a99479e5 | 4255 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4256 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4257 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4258 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4259 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4260 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4261 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4262 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4263 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4264 | break; |
yabahiro | 48:5562a99479e5 | 4265 | case 254: |
yabahiro | 48:5562a99479e5 | 4266 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4267 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4268 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4269 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4270 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4271 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4272 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4273 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4274 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4275 | break; |
yabahiro | 48:5562a99479e5 | 4276 | case 1: |
yabahiro | 48:5562a99479e5 | 4277 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4278 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4279 | motor[TIRE_FL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4280 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4281 | motor[TIRE_BR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4282 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4283 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4284 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4285 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4286 | break; |
yabahiro | 48:5562a99479e5 | 4287 | case 3: |
yabahiro | 48:5562a99479e5 | 4288 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4289 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4290 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4291 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4292 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4293 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4294 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4295 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4296 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4297 | break; |
yabahiro | 48:5562a99479e5 | 4298 | case 2: |
yabahiro | 48:5562a99479e5 | 4299 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4300 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4301 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4302 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4303 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4304 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4305 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4306 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4307 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4308 | break; |
yabahiro | 48:5562a99479e5 | 4309 | } |
yabahiro | 48:5562a99479e5 | 4310 | if(g[1]!=98&&mtc==true) { |
yabahiro | 48:5562a99479e5 | 4311 | switch(g[1]) { |
yabahiro | 48:5562a99479e5 | 4312 | case 0: |
yabahiro | 48:5562a99479e5 | 4313 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4314 | break; |
yabahiro | 48:5562a99479e5 | 4315 | case 255: |
yabahiro | 48:5562a99479e5 | 4316 | motor[TIRE_BR].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4317 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4318 | break; |
yabahiro | 48:5562a99479e5 | 4319 | case 253: |
yabahiro | 48:5562a99479e5 | 4320 | motor[TIRE_BR].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4321 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4322 | break; |
yabahiro | 48:5562a99479e5 | 4323 | case 254: |
yabahiro | 48:5562a99479e5 | 4324 | motor[TIRE_BR].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4325 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4326 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4327 | break; |
yabahiro | 48:5562a99479e5 | 4328 | case 1: |
yabahiro | 48:5562a99479e5 | 4329 | motor[TIRE_BL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4330 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4331 | break; |
yabahiro | 48:5562a99479e5 | 4332 | case 3: |
yabahiro | 48:5562a99479e5 | 4333 | motor[TIRE_BL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4334 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4335 | break; |
yabahiro | 48:5562a99479e5 | 4336 | case 2: |
yabahiro | 48:5562a99479e5 | 4337 | motor[TIRE_BL].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4338 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4339 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4340 | break; |
yabahiro | 48:5562a99479e5 | 4341 | } |
yabahiro | 48:5562a99479e5 | 4342 | } |
yabahiro | 48:5562a99479e5 | 4343 | } |
yabahiro | 48:5562a99479e5 | 4344 | } |
yabahiro | 48:5562a99479e5 | 4345 | |
yabahiro | 48:5562a99479e5 | 4346 | /* |
yabahiro | 48:5562a99479e5 | 4347 | if(mode==31) { |
yabahiro | 48:5562a99479e5 | 4348 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4349 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4350 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4351 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4352 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4353 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4354 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4355 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4356 | if(countss>=5000) { |
yabahiro | 48:5562a99479e5 | 4357 | countss=0; |
yabahiro | 48:5562a99479e5 | 4358 | mode=33; |
yabahiro | 48:5562a99479e5 | 4359 | } |
yabahiro | 48:5562a99479e5 | 4360 | } else if(mode==33) { |
yabahiro | 48:5562a99479e5 | 4361 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 4362 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4363 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4364 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4365 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4366 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4367 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 4368 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4369 | if(g[3]==0) { |
yabahiro | 48:5562a99479e5 | 4370 | cross=0; |
yabahiro | 48:5562a99479e5 | 4371 | countss=0; |
yabahiro | 48:5562a99479e5 | 4372 | mode=40; |
yabahiro | 48:5562a99479e5 | 4373 | } |
yabahiro | 48:5562a99479e5 | 4374 | } else |
yabahiro | 48:5562a99479e5 | 4375 | */ |
yabahiro | 48:5562a99479e5 | 4376 | if(mode==31) { |
yabahiro | 48:5562a99479e5 | 4377 | motor[TIRE_FR].pwm = 45; |
yabahiro | 48:5562a99479e5 | 4378 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4379 | motor[TIRE_FL].pwm = 255; |
yabahiro | 48:5562a99479e5 | 4380 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4381 | motor[TIRE_BR].pwm = 255; |
yabahiro | 48:5562a99479e5 | 4382 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 48:5562a99479e5 | 4383 | motor[TIRE_BL].pwm = 45; |
yabahiro | 48:5562a99479e5 | 4384 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4385 | if(g[3]==0) { |
yabahiro | 48:5562a99479e5 | 4386 | cross=0; |
yabahiro | 48:5562a99479e5 | 4387 | countss=0; |
yabahiro | 48:5562a99479e5 | 4388 | mode=40; |
yabahiro | 48:5562a99479e5 | 4389 | } |
yabahiro | 48:5562a99479e5 | 4390 | } else if(mode==40) { |
yabahiro | 48:5562a99479e5 | 4391 | switch(g[3]) { |
yabahiro | 48:5562a99479e5 | 4392 | case 98: |
yabahiro | 48:5562a99479e5 | 4393 | motor[TIRE_FR].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4394 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4395 | motor[TIRE_FL].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4396 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4397 | motor[TIRE_BR].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4398 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4399 | motor[TIRE_BL].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4400 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4401 | if(countss>10000) { |
yabahiro | 48:5562a99479e5 | 4402 | cross++; |
yabahiro | 48:5562a99479e5 | 4403 | countss=0; |
yabahiro | 48:5562a99479e5 | 4404 | } |
yabahiro | 48:5562a99479e5 | 4405 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4406 | if(cross==1) { |
yabahiro | 48:5562a99479e5 | 4407 | mode=41; |
yabahiro | 48:5562a99479e5 | 4408 | countss=0; |
yabahiro | 48:5562a99479e5 | 4409 | cross=0; |
yabahiro | 48:5562a99479e5 | 4410 | } |
yabahiro | 48:5562a99479e5 | 4411 | break; |
yabahiro | 48:5562a99479e5 | 4412 | case 0: |
yabahiro | 48:5562a99479e5 | 4413 | motor[TIRE_FR].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4414 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4415 | motor[TIRE_FL].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4416 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4417 | motor[TIRE_BR].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4418 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4419 | motor[TIRE_BL].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4420 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4421 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4422 | break; |
yabahiro | 48:5562a99479e5 | 4423 | case 255: |
yabahiro | 48:5562a99479e5 | 4424 | motor[TIRE_FR].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4425 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4426 | motor[TIRE_FL].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4427 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4428 | motor[TIRE_BR].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4429 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4430 | motor[TIRE_BL].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4431 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4432 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4433 | break; |
yabahiro | 48:5562a99479e5 | 4434 | case 253: |
yabahiro | 48:5562a99479e5 | 4435 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4436 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4437 | motor[TIRE_FL].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4438 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4439 | motor[TIRE_BR].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4440 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4441 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4442 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4443 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4444 | break; |
yabahiro | 48:5562a99479e5 | 4445 | case 254: |
yabahiro | 48:5562a99479e5 | 4446 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4447 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4448 | motor[TIRE_FL].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4449 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4450 | motor[TIRE_BR].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4451 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4452 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4453 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4454 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4455 | break; |
yabahiro | 48:5562a99479e5 | 4456 | case 1: |
yabahiro | 48:5562a99479e5 | 4457 | motor[TIRE_FR].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4458 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4459 | motor[TIRE_FL].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4460 | motor[TIRE_FL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4461 | motor[TIRE_BR].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4462 | motor[TIRE_BR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4463 | motor[TIRE_BL].pwm = fast+15; |
yabahiro | 48:5562a99479e5 | 4464 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4465 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4466 | break; |
yabahiro | 48:5562a99479e5 | 4467 | case 3: |
yabahiro | 48:5562a99479e5 | 4468 | motor[TIRE_FR].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4469 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4470 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4471 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4472 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4473 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4474 | motor[TIRE_BL].pwm = slow+15; |
yabahiro | 48:5562a99479e5 | 4475 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4476 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4477 | break; |
yabahiro | 48:5562a99479e5 | 4478 | case 2: |
yabahiro | 48:5562a99479e5 | 4479 | motor[TIRE_FR].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4480 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4481 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4482 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4483 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4484 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4485 | motor[TIRE_BL].pwm = normal+15; |
yabahiro | 48:5562a99479e5 | 4486 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4487 | mtc2=true; |
yabahiro | 48:5562a99479e5 | 4488 | break; |
yabahiro | 48:5562a99479e5 | 4489 | } |
yabahiro | 48:5562a99479e5 | 4490 | if(g[2]!=98&&mtc2==true) { |
yabahiro | 48:5562a99479e5 | 4491 | switch(g[2]) { |
yabahiro | 48:5562a99479e5 | 4492 | case 0: |
yabahiro | 48:5562a99479e5 | 4493 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4494 | break; |
yabahiro | 48:5562a99479e5 | 4495 | case 255: |
yabahiro | 48:5562a99479e5 | 4496 | motor[TIRE_FL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4497 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4498 | break; |
yabahiro | 48:5562a99479e5 | 4499 | case 253: |
yabahiro | 48:5562a99479e5 | 4500 | motor[TIRE_FL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4501 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4502 | break; |
yabahiro | 48:5562a99479e5 | 4503 | case 254: |
yabahiro | 48:5562a99479e5 | 4504 | motor[TIRE_FL].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4505 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4506 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4507 | break; |
yabahiro | 48:5562a99479e5 | 4508 | case 1: |
yabahiro | 48:5562a99479e5 | 4509 | motor[TIRE_BL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4510 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4511 | break; |
yabahiro | 48:5562a99479e5 | 4512 | case 3: |
yabahiro | 48:5562a99479e5 | 4513 | motor[TIRE_BL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4514 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4515 | break; |
yabahiro | 48:5562a99479e5 | 4516 | case 2: |
yabahiro | 48:5562a99479e5 | 4517 | motor[TIRE_BL].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4518 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4519 | mtc2=false; |
yabahiro | 48:5562a99479e5 | 4520 | break; |
yabahiro | 48:5562a99479e5 | 4521 | } |
yabahiro | 48:5562a99479e5 | 4522 | } |
yabahiro | 48:5562a99479e5 | 4523 | } else if(mode==41) { |
yabahiro | 48:5562a99479e5 | 4524 | if(LIF||LIB) { |
yabahiro | 48:5562a99479e5 | 4525 | mode=42; |
yabahiro | 48:5562a99479e5 | 4526 | } |
yabahiro | 48:5562a99479e5 | 4527 | motor[TIRE_FR].pwm = 30; |
yabahiro | 48:5562a99479e5 | 4528 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4529 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4530 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4531 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4532 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4533 | motor[TIRE_BL].pwm = 30; |
yabahiro | 48:5562a99479e5 | 4534 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4535 | if(g[1]==0) { |
yabahiro | 48:5562a99479e5 | 4536 | countss=0; |
yabahiro | 48:5562a99479e5 | 4537 | mode=70; |
yabahiro | 48:5562a99479e5 | 4538 | normal=60; |
yabahiro | 48:5562a99479e5 | 4539 | fast=70; |
yabahiro | 48:5562a99479e5 | 4540 | slow=40; |
yabahiro | 48:5562a99479e5 | 4541 | } |
yabahiro | 48:5562a99479e5 | 4542 | } else if(mode==42) { |
yabahiro | 48:5562a99479e5 | 4543 | motor[TIRE_FR].pwm = 40; |
yabahiro | 48:5562a99479e5 | 4544 | motor[TIRE_FR].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4545 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4546 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4547 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4548 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4549 | motor[TIRE_BL].pwm = 40; |
yabahiro | 48:5562a99479e5 | 4550 | motor[TIRE_BL].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4551 | if(g[1]==0) { |
yabahiro | 48:5562a99479e5 | 4552 | countss=0; |
yabahiro | 48:5562a99479e5 | 4553 | mode=70; |
yabahiro | 48:5562a99479e5 | 4554 | } |
yabahiro | 48:5562a99479e5 | 4555 | } else if(mode==70) { |
yabahiro | 48:5562a99479e5 | 4556 | switch(g[0]) { |
yabahiro | 48:5562a99479e5 | 4557 | case 99: |
yabahiro | 48:5562a99479e5 | 4558 | if(countss>=100000) { |
yabahiro | 48:5562a99479e5 | 4559 | mode=100; |
yabahiro | 48:5562a99479e5 | 4560 | } |
yabahiro | 48:5562a99479e5 | 4561 | break; |
yabahiro | 48:5562a99479e5 | 4562 | case 98: |
yabahiro | 48:5562a99479e5 | 4563 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4564 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4565 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4566 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4567 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4568 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4569 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4570 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4571 | break; |
yabahiro | 48:5562a99479e5 | 4572 | case 0: |
yabahiro | 48:5562a99479e5 | 4573 | motor[TIRE_FR].pwm = 80; |
yabahiro | 48:5562a99479e5 | 4574 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4575 | motor[TIRE_FL].pwm = 80; |
yabahiro | 48:5562a99479e5 | 4576 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4577 | motor[TIRE_BR].pwm = 80; |
yabahiro | 48:5562a99479e5 | 4578 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4579 | motor[TIRE_BL].pwm = 80; |
yabahiro | 48:5562a99479e5 | 4580 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4581 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4582 | break; |
yabahiro | 48:5562a99479e5 | 4583 | case 255: |
yabahiro | 48:5562a99479e5 | 4584 | motor[TIRE_FR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4585 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4586 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4587 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4588 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4589 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4590 | motor[TIRE_BL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4591 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4592 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4593 | break; |
yabahiro | 48:5562a99479e5 | 4594 | case 253: |
yabahiro | 48:5562a99479e5 | 4595 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4596 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4597 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4598 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4599 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4600 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4601 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4602 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4603 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4604 | break; |
yabahiro | 48:5562a99479e5 | 4605 | case 254: |
yabahiro | 48:5562a99479e5 | 4606 | motor[TIRE_FR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4607 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4608 | motor[TIRE_FL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4609 | motor[TIRE_FL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4610 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4611 | motor[TIRE_BR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4612 | motor[TIRE_BL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4613 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4614 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4615 | break; |
yabahiro | 48:5562a99479e5 | 4616 | case 1: |
yabahiro | 48:5562a99479e5 | 4617 | motor[TIRE_FR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4618 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4619 | motor[TIRE_FL].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4620 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4621 | motor[TIRE_BR].pwm = fast; |
yabahiro | 48:5562a99479e5 | 4622 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4623 | motor[TIRE_BL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4624 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4625 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4626 | break; |
yabahiro | 48:5562a99479e5 | 4627 | case 3: |
yabahiro | 48:5562a99479e5 | 4628 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4629 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4630 | motor[TIRE_FL].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4631 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4632 | motor[TIRE_BR].pwm = slow; |
yabahiro | 48:5562a99479e5 | 4633 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4634 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4635 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4636 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4637 | break; |
yabahiro | 48:5562a99479e5 | 4638 | case 2: |
yabahiro | 48:5562a99479e5 | 4639 | motor[TIRE_FR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4640 | motor[TIRE_FR].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4641 | motor[TIRE_FL].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4642 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4643 | motor[TIRE_BR].pwm = normal; |
yabahiro | 48:5562a99479e5 | 4644 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4645 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4646 | motor[TIRE_BL].dir = FREE; |
yabahiro | 48:5562a99479e5 | 4647 | mtc=true; |
yabahiro | 48:5562a99479e5 | 4648 | break; |
yabahiro | 48:5562a99479e5 | 4649 | } |
yabahiro | 48:5562a99479e5 | 4650 | if(g[1]!=98&&mtc==true) { |
yabahiro | 48:5562a99479e5 | 4651 | switch(g[1]) { |
yabahiro | 48:5562a99479e5 | 4652 | case 0: |
yabahiro | 48:5562a99479e5 | 4653 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4654 | break; |
yabahiro | 48:5562a99479e5 | 4655 | case 255: |
yabahiro | 48:5562a99479e5 | 4656 | motor[TIRE_BR].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4657 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4658 | break; |
yabahiro | 48:5562a99479e5 | 4659 | case 253: |
yabahiro | 48:5562a99479e5 | 4660 | motor[TIRE_BR].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4661 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4662 | break; |
yabahiro | 48:5562a99479e5 | 4663 | case 254: |
yabahiro | 48:5562a99479e5 | 4664 | motor[TIRE_BR].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4665 | motor[TIRE_BL].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4666 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4667 | break; |
yabahiro | 48:5562a99479e5 | 4668 | case 1: |
yabahiro | 48:5562a99479e5 | 4669 | motor[TIRE_BL].pwm += 5; |
yabahiro | 48:5562a99479e5 | 4670 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4671 | break; |
yabahiro | 48:5562a99479e5 | 4672 | case 3: |
yabahiro | 48:5562a99479e5 | 4673 | motor[TIRE_BL].pwm += 10; |
yabahiro | 48:5562a99479e5 | 4674 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4675 | break; |
yabahiro | 48:5562a99479e5 | 4676 | case 2: |
yabahiro | 48:5562a99479e5 | 4677 | motor[TIRE_BL].pwm += 20; |
yabahiro | 48:5562a99479e5 | 4678 | motor[TIRE_BR].pwm = 0; |
yabahiro | 48:5562a99479e5 | 4679 | mtc=false; |
yabahiro | 48:5562a99479e5 | 4680 | break; |
yabahiro | 48:5562a99479e5 | 4681 | } |
yabahiro | 48:5562a99479e5 | 4682 | } |
yabahiro | 48:5562a99479e5 | 4683 | } else if(mode==100) { |
yabahiro | 48:5562a99479e5 | 4684 | motor[TIRE_FR].pwm = 50; |
yabahiro | 48:5562a99479e5 | 4685 | motor[TIRE_FR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4686 | motor[TIRE_FL].pwm = 50; |
yabahiro | 48:5562a99479e5 | 4687 | motor[TIRE_FL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4688 | motor[TIRE_BR].pwm = 50; |
yabahiro | 48:5562a99479e5 | 4689 | motor[TIRE_BR].dir = BACK; |
yabahiro | 48:5562a99479e5 | 4690 | motor[TIRE_BL].pwm = 50; |
yabahiro | 48:5562a99479e5 | 4691 | motor[TIRE_BL].dir = FOR; |
yabahiro | 48:5562a99479e5 | 4692 | } else { |
yabahiro | 48:5562a99479e5 | 4693 | } |
yabahiro | 33:dd598bb23ad1 | 4694 | |
kishibekairohan | 13:b6e02d6261d7 | 4695 | } |
kishibekairohan | 13:b6e02d6261d7 | 4696 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 4697 | |
kishibekairohan | 13:b6e02d6261d7 | 4698 | #if USE_PROCESS_NUM>5 |
Ryosei | 27:dd9f27fce7d1 | 4699 | static void Process5() |
Ryosei | 27:dd9f27fce7d1 | 4700 | { |
yabahiro | 56:61ebd4b759b4 | 4701 | countss++; |
yabahiro | 56:61ebd4b759b4 | 4702 | switch(mode) { |
yabahiro | 56:61ebd4b759b4 | 4703 | case 1: |
yabahiro | 56:61ebd4b759b4 | 4704 | mode=2; |
yabahiro | 56:61ebd4b759b4 | 4705 | countss=0; |
yabahiro | 56:61ebd4b759b4 | 4706 | fast = 60; |
yabahiro | 56:61ebd4b759b4 | 4707 | normal = 40; |
yabahiro | 56:61ebd4b759b4 | 4708 | slow = 30; |
yabahiro | 56:61ebd4b759b4 | 4709 | break; |
yabahiro | 56:61ebd4b759b4 | 4710 | case 2: |
yabahiro | 56:61ebd4b759b4 | 4711 | if(YELLOW) { |
yabahiro | 56:61ebd4b759b4 | 4712 | switch(g[4]) { |
yabahiro | 56:61ebd4b759b4 | 4713 | case 98: |
yabahiro | 56:61ebd4b759b4 | 4714 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4715 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4716 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4717 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4718 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4719 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4720 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4721 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4722 | break; |
yabahiro | 56:61ebd4b759b4 | 4723 | case 99: |
yabahiro | 56:61ebd4b759b4 | 4724 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4725 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4726 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4727 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4728 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4729 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4730 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4731 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4732 | break; |
yabahiro | 56:61ebd4b759b4 | 4733 | case 0: |
yabahiro | 56:61ebd4b759b4 | 4734 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4735 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4736 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4737 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4738 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4739 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4740 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4741 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4742 | break; |
yabahiro | 56:61ebd4b759b4 | 4743 | case 255: |
yabahiro | 56:61ebd4b759b4 | 4744 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4745 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4746 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4747 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4748 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4749 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4750 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4751 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4752 | break; |
yabahiro | 56:61ebd4b759b4 | 4753 | case 253: |
yabahiro | 56:61ebd4b759b4 | 4754 | motor[TIRE_FR].pwm = slow+5; |
yabahiro | 56:61ebd4b759b4 | 4755 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4756 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4757 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4758 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4759 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4760 | motor[TIRE_BL].pwm = slow+5; |
yabahiro | 56:61ebd4b759b4 | 4761 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4762 | break; |
yabahiro | 56:61ebd4b759b4 | 4763 | case 254: |
yabahiro | 56:61ebd4b759b4 | 4764 | motor[TIRE_FR].pwm = normal+5; |
yabahiro | 56:61ebd4b759b4 | 4765 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4766 | motor[TIRE_FL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4767 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4768 | motor[TIRE_BR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4769 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4770 | motor[TIRE_BL].pwm = normal+5; |
yabahiro | 56:61ebd4b759b4 | 4771 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4772 | break; |
yabahiro | 56:61ebd4b759b4 | 4773 | case 1: |
yabahiro | 56:61ebd4b759b4 | 4774 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4775 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4776 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4777 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4778 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4779 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4780 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 4781 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4782 | break; |
yabahiro | 56:61ebd4b759b4 | 4783 | case 3: |
yabahiro | 56:61ebd4b759b4 | 4784 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4785 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4786 | motor[TIRE_FL].pwm = slow+5; |
yabahiro | 56:61ebd4b759b4 | 4787 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4788 | motor[TIRE_BR].pwm = slow+5; |
yabahiro | 56:61ebd4b759b4 | 4789 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4790 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4791 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4792 | break; |
yabahiro | 56:61ebd4b759b4 | 4793 | case 2: |
yabahiro | 56:61ebd4b759b4 | 4794 | motor[TIRE_FR].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4795 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4796 | motor[TIRE_FL].pwm = normal+5; |
yabahiro | 56:61ebd4b759b4 | 4797 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4798 | motor[TIRE_BR].pwm = normal+5; |
yabahiro | 56:61ebd4b759b4 | 4799 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4800 | motor[TIRE_BL].pwm = fast-5; |
yabahiro | 56:61ebd4b759b4 | 4801 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4802 | break; |
yabahiro | 56:61ebd4b759b4 | 4803 | } |
yabahiro | 56:61ebd4b759b4 | 4804 | if(g[1]==98||g[1]==0||g[1]==1||g[1]==2||g[1]==3||g[1]==254||g[1]==253||g[1]==255) { |
yabahiro | 56:61ebd4b759b4 | 4805 | fast=10; |
yabahiro | 56:61ebd4b759b4 | 4806 | normal=10; |
yabahiro | 56:61ebd4b759b4 | 4807 | slow=10; |
yabahiro | 56:61ebd4b759b4 | 4808 | } |
yabahiro | 56:61ebd4b759b4 | 4809 | if(g[3]==254&&countss>=100000) { |
yabahiro | 56:61ebd4b759b4 | 4810 | mode=3; |
yabahiro | 56:61ebd4b759b4 | 4811 | solenoid.solenoid4 = SOLENOID_OFF; |
yabahiro | 56:61ebd4b759b4 | 4812 | aime.start(); |
yabahiro | 56:61ebd4b759b4 | 4813 | aime.reset(); |
yabahiro | 56:61ebd4b759b4 | 4814 | countss=0; |
yabahiro | 56:61ebd4b759b4 | 4815 | fast=80; |
yabahiro | 56:61ebd4b759b4 | 4816 | normal=60; |
yabahiro | 56:61ebd4b759b4 | 4817 | slow=30; |
yabahiro | 56:61ebd4b759b4 | 4818 | } |
yabahiro | 56:61ebd4b759b4 | 4819 | } else { |
yabahiro | 56:61ebd4b759b4 | 4820 | if(UPLS||DOLS) { |
yabahiro | 56:61ebd4b759b4 | 4821 | if(UPLS||DOLS) { |
yabahiro | 56:61ebd4b759b4 | 4822 | motor[MOTOR_0].pwm = 200; |
yabahiro | 56:61ebd4b759b4 | 4823 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 4824 | fast=60; |
yabahiro | 56:61ebd4b759b4 | 4825 | normal=40; |
yabahiro | 56:61ebd4b759b4 | 4826 | slow=30; |
yabahiro | 56:61ebd4b759b4 | 4827 | countss=0; |
yabahiro | 56:61ebd4b759b4 | 4828 | mode=24; |
yabahiro | 56:61ebd4b759b4 | 4829 | } |
yabahiro | 56:61ebd4b759b4 | 4830 | } else { |
yabahiro | 56:61ebd4b759b4 | 4831 | motor[MOTOR_0].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 4832 | motor[MOTOR_0].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4833 | } |
yabahiro | 56:61ebd4b759b4 | 4834 | motor[TIRE_FR].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 4835 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 4836 | motor[TIRE_FL].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 4837 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 4838 | motor[TIRE_BR].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 4839 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 4840 | motor[TIRE_BL].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 4841 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 4842 | } |
yabahiro | 56:61ebd4b759b4 | 4843 | break; |
yabahiro | 56:61ebd4b759b4 | 4844 | case 3: |
yabahiro | 56:61ebd4b759b4 | 4845 | if(YELLOW) { |
yabahiro | 56:61ebd4b759b4 | 4846 | value=aime.read_ms(); |
yabahiro | 56:61ebd4b759b4 | 4847 | if(value>=5000) { |
yabahiro | 56:61ebd4b759b4 | 4848 | mode=4; |
yabahiro | 56:61ebd4b759b4 | 4849 | } |
yabahiro | 56:61ebd4b759b4 | 4850 | if(g[4]==3||g[4]==2) { |
yabahiro | 56:61ebd4b759b4 | 4851 | motor[TIRE_FR].pwm = 20; |
yabahiro | 56:61ebd4b759b4 | 4852 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4853 | motor[TIRE_FL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4854 | motor[TIRE_FL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4855 | motor[TIRE_BR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4856 | motor[TIRE_BR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4857 | motor[TIRE_BL].pwm = 20; |
yabahiro | 56:61ebd4b759b4 | 4858 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4859 | } |
yabahiro | 56:61ebd4b759b4 | 4860 | if(g[4]==253||g[4]==254) { |
yabahiro | 56:61ebd4b759b4 | 4861 | motor[TIRE_FR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4862 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4863 | motor[TIRE_FL].pwm = 20; |
yabahiro | 56:61ebd4b759b4 | 4864 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4865 | motor[TIRE_BR].pwm = 20; |
yabahiro | 56:61ebd4b759b4 | 4866 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4867 | motor[TIRE_BL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4868 | motor[TIRE_BL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4869 | } |
yabahiro | 56:61ebd4b759b4 | 4870 | switch(g[2]) { |
yabahiro | 56:61ebd4b759b4 | 4871 | case 0: |
yabahiro | 56:61ebd4b759b4 | 4872 | motor[TIRE_FR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4873 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4874 | motor[TIRE_BR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4875 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4876 | break; |
yabahiro | 56:61ebd4b759b4 | 4877 | case 255: |
yabahiro | 56:61ebd4b759b4 | 4878 | motor[TIRE_FR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4879 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4880 | motor[TIRE_BR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4881 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4882 | break; |
yabahiro | 56:61ebd4b759b4 | 4883 | case 253: |
yabahiro | 56:61ebd4b759b4 | 4884 | motor[TIRE_FR].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 4885 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4886 | motor[TIRE_BR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4887 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4888 | break; |
yabahiro | 56:61ebd4b759b4 | 4889 | case 254: |
yabahiro | 56:61ebd4b759b4 | 4890 | motor[TIRE_FR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4891 | motor[TIRE_FR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4892 | motor[TIRE_BR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4893 | motor[TIRE_BR].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4894 | break; |
yabahiro | 56:61ebd4b759b4 | 4895 | case 1: |
yabahiro | 56:61ebd4b759b4 | 4896 | motor[TIRE_FR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4897 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4898 | motor[TIRE_BR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4899 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4900 | break; |
yabahiro | 56:61ebd4b759b4 | 4901 | case 3: |
yabahiro | 56:61ebd4b759b4 | 4902 | motor[TIRE_FR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4903 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4904 | motor[TIRE_BR].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 4905 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4906 | break; |
yabahiro | 56:61ebd4b759b4 | 4907 | case 2: |
yabahiro | 56:61ebd4b759b4 | 4908 | motor[TIRE_FR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4909 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4910 | motor[TIRE_BR].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4911 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4912 | break; |
yabahiro | 56:61ebd4b759b4 | 4913 | } |
yabahiro | 56:61ebd4b759b4 | 4914 | switch(g[3]) { |
yabahiro | 56:61ebd4b759b4 | 4915 | case 0: |
yabahiro | 56:61ebd4b759b4 | 4916 | motor[TIRE_FL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4917 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4918 | motor[TIRE_BL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4919 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4920 | break; |
yabahiro | 56:61ebd4b759b4 | 4921 | case 255: |
yabahiro | 56:61ebd4b759b4 | 4922 | motor[TIRE_FL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4923 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4924 | motor[TIRE_BL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4925 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4926 | case 253: |
yabahiro | 56:61ebd4b759b4 | 4927 | motor[TIRE_FL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4928 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4929 | motor[TIRE_BL].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 4930 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4931 | break; |
yabahiro | 56:61ebd4b759b4 | 4932 | case 254: |
yabahiro | 56:61ebd4b759b4 | 4933 | motor[TIRE_FL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4934 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4935 | motor[TIRE_BL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4936 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4937 | break; |
yabahiro | 56:61ebd4b759b4 | 4938 | case 1: |
yabahiro | 56:61ebd4b759b4 | 4939 | motor[TIRE_FL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4940 | motor[TIRE_FL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4941 | motor[TIRE_BL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4942 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4943 | break; |
yabahiro | 56:61ebd4b759b4 | 4944 | case 3: |
yabahiro | 56:61ebd4b759b4 | 4945 | motor[TIRE_FL].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 4946 | motor[TIRE_FL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4947 | motor[TIRE_BL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 4948 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 4949 | break; |
yabahiro | 56:61ebd4b759b4 | 4950 | case 2: |
yabahiro | 56:61ebd4b759b4 | 4951 | motor[TIRE_FL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4952 | motor[TIRE_FL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4953 | motor[TIRE_BL].pwm = 30; |
yabahiro | 56:61ebd4b759b4 | 4954 | motor[TIRE_BL].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4955 | break; |
yabahiro | 56:61ebd4b759b4 | 4956 | } |
yabahiro | 56:61ebd4b759b4 | 4957 | if(g[3]==0&&g[2]==0) { |
yabahiro | 56:61ebd4b759b4 | 4958 | mode=25; |
yabahiro | 56:61ebd4b759b4 | 4959 | } |
yabahiro | 56:61ebd4b759b4 | 4960 | } else { |
yabahiro | 56:61ebd4b759b4 | 4961 | if(g[1]==98&&countss>=40000) { |
yabahiro | 56:61ebd4b759b4 | 4962 | mode=31; |
yabahiro | 56:61ebd4b759b4 | 4963 | fast = 60; |
yabahiro | 56:61ebd4b759b4 | 4964 | normal = 40; |
yabahiro | 56:61ebd4b759b4 | 4965 | slow = 30; |
yabahiro | 56:61ebd4b759b4 | 4966 | countss=0; |
yabahiro | 56:61ebd4b759b4 | 4967 | } |
yabahiro | 56:61ebd4b759b4 | 4968 | switch(g[1]) { |
yabahiro | 56:61ebd4b759b4 | 4969 | case 98: |
yabahiro | 56:61ebd4b759b4 | 4970 | fast=40; |
yabahiro | 56:61ebd4b759b4 | 4971 | normal=30; |
yabahiro | 56:61ebd4b759b4 | 4972 | slow=25; |
yabahiro | 56:61ebd4b759b4 | 4973 | motor[TIRE_FR].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 4974 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4975 | motor[TIRE_FL].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 4976 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4977 | motor[TIRE_BR].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 4978 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4979 | motor[TIRE_BL].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 4980 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4981 | break; |
yabahiro | 56:61ebd4b759b4 | 4982 | case 0: |
yabahiro | 56:61ebd4b759b4 | 4983 | motor[TIRE_FR].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 4984 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4985 | motor[TIRE_FL].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 4986 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4987 | motor[TIRE_BR].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 4988 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4989 | motor[TIRE_BL].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 4990 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4991 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 4992 | break; |
yabahiro | 56:61ebd4b759b4 | 4993 | case 255: |
yabahiro | 56:61ebd4b759b4 | 4994 | motor[TIRE_FR].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 4995 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 4996 | motor[TIRE_FL].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 4997 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 4998 | motor[TIRE_BR].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 4999 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5000 | motor[TIRE_BL].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 5001 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5002 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5003 | break; |
yabahiro | 56:61ebd4b759b4 | 5004 | case 253: |
yabahiro | 56:61ebd4b759b4 | 5005 | motor[TIRE_FR].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5006 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5007 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5008 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5009 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5010 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5011 | motor[TIRE_BL].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5012 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5013 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5014 | break; |
yabahiro | 56:61ebd4b759b4 | 5015 | case 254: |
yabahiro | 56:61ebd4b759b4 | 5016 | motor[TIRE_FR].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 5017 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5018 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5019 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5020 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5021 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5022 | motor[TIRE_BL].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 5023 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5024 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5025 | break; |
yabahiro | 56:61ebd4b759b4 | 5026 | case 1: |
yabahiro | 56:61ebd4b759b4 | 5027 | motor[TIRE_FR].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5028 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5029 | motor[TIRE_FL].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 5030 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5031 | motor[TIRE_BR].pwm = fast; |
yabahiro | 56:61ebd4b759b4 | 5032 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5033 | motor[TIRE_BL].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5034 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5035 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5036 | break; |
yabahiro | 56:61ebd4b759b4 | 5037 | case 3: |
yabahiro | 56:61ebd4b759b4 | 5038 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5039 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5040 | motor[TIRE_FL].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5041 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5042 | motor[TIRE_BR].pwm = slow; |
yabahiro | 56:61ebd4b759b4 | 5043 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5044 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5045 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5046 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5047 | break; |
yabahiro | 56:61ebd4b759b4 | 5048 | case 2: |
yabahiro | 56:61ebd4b759b4 | 5049 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5050 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5051 | motor[TIRE_FL].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 5052 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5053 | motor[TIRE_BR].pwm = normal; |
yabahiro | 56:61ebd4b759b4 | 5054 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5055 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5056 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5057 | mtc=true; |
yabahiro | 56:61ebd4b759b4 | 5058 | break; |
yabahiro | 56:61ebd4b759b4 | 5059 | } |
yabahiro | 56:61ebd4b759b4 | 5060 | if(g[1]!=98&&mtc==true) { |
yabahiro | 56:61ebd4b759b4 | 5061 | switch(g[0]) { |
yabahiro | 56:61ebd4b759b4 | 5062 | case 0: |
yabahiro | 56:61ebd4b759b4 | 5063 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5064 | break; |
yabahiro | 56:61ebd4b759b4 | 5065 | case 255: |
yabahiro | 56:61ebd4b759b4 | 5066 | motor[TIRE_FL].pwm += 3; |
yabahiro | 56:61ebd4b759b4 | 5067 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5068 | break; |
yabahiro | 56:61ebd4b759b4 | 5069 | case 253: |
yabahiro | 56:61ebd4b759b4 | 5070 | motor[TIRE_FL].pwm += 6; |
yabahiro | 56:61ebd4b759b4 | 5071 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5072 | break; |
yabahiro | 56:61ebd4b759b4 | 5073 | case 254: |
yabahiro | 56:61ebd4b759b4 | 5074 | motor[TIRE_FL].pwm += 9; |
yabahiro | 56:61ebd4b759b4 | 5075 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5076 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5077 | break; |
yabahiro | 56:61ebd4b759b4 | 5078 | case 1: |
yabahiro | 56:61ebd4b759b4 | 5079 | motor[TIRE_FR].pwm += 3; |
yabahiro | 56:61ebd4b759b4 | 5080 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5081 | break; |
yabahiro | 56:61ebd4b759b4 | 5082 | case 3: |
yabahiro | 56:61ebd4b759b4 | 5083 | motor[TIRE_FR].pwm += 6; |
yabahiro | 56:61ebd4b759b4 | 5084 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5085 | break; |
yabahiro | 56:61ebd4b759b4 | 5086 | case 2: |
yabahiro | 56:61ebd4b759b4 | 5087 | motor[TIRE_FR].pwm += 9; |
yabahiro | 56:61ebd4b759b4 | 5088 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5089 | mtc=false; |
yabahiro | 56:61ebd4b759b4 | 5090 | break; |
yabahiro | 56:61ebd4b759b4 | 5091 | } |
yabahiro | 56:61ebd4b759b4 | 5092 | } |
yabahiro | 56:61ebd4b759b4 | 5093 | } |
yabahiro | 56:61ebd4b759b4 | 5094 | break; |
yabahiro | 56:61ebd4b759b4 | 5095 | case 4: |
yabahiro | 56:61ebd4b759b4 | 5096 | motor[TIRE_FR].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 5097 | motor[TIRE_FR].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 5098 | motor[TIRE_FL].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 5099 | motor[TIRE_FL].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 5100 | motor[TIRE_BR].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 5101 | motor[TIRE_BR].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 5102 | motor[TIRE_BL].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 5103 | motor[TIRE_BL].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 5104 | if(UPLS==true||DOLS==true) { |
yabahiro | 56:61ebd4b759b4 | 5105 | if(UPLS==true||DOLS==true) { |
yabahiro | 56:61ebd4b759b4 | 5106 | motor[MOTOR_0].pwm = 200; |
yabahiro | 56:61ebd4b759b4 | 5107 | motor[MOTOR_0].dir = BRAKE; |
yabahiro | 56:61ebd4b759b4 | 5108 | countss = 0; |
yabahiro | 56:61ebd4b759b4 | 5109 | mode=26; |
yabahiro | 56:61ebd4b759b4 | 5110 | } |
yabahiro | 56:61ebd4b759b4 | 5111 | } else { |
yabahiro | 56:61ebd4b759b4 | 5112 | motor[MOTOR_0].pwm = 100; |
yabahiro | 56:61ebd4b759b4 | 5113 | motor[MOTOR_0].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5114 | } |
yabahiro | 56:61ebd4b759b4 | 5115 | break; |
yabahiro | 56:61ebd4b759b4 | 5116 | case 5: |
yabahiro | 56:61ebd4b759b4 | 5117 | /* |
yabahiro | 56:61ebd4b759b4 | 5118 | if (g[3] == 0&&countss>=30000) { |
yabahiro | 56:61ebd4b759b4 | 5119 | mode = 40; |
yabahiro | 56:61ebd4b759b4 | 5120 | } |
yabahiro | 56:61ebd4b759b4 | 5121 | */ |
yabahiro | 56:61ebd4b759b4 | 5122 | if (g[1] != 99) { |
yabahiro | 56:61ebd4b759b4 | 5123 | mode = 31; |
yabahiro | 56:61ebd4b759b4 | 5124 | } |
yabahiro | 56:61ebd4b759b4 | 5125 | switch(g[4]) { |
yabahiro | 56:61ebd4b759b4 | 5126 | case 98: |
yabahiro | 56:61ebd4b759b4 | 5127 | motor[TIRE_FR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5128 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5129 | motor[TIRE_FL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5130 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5131 | motor[TIRE_BR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5132 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5133 | motor[TIRE_BL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5134 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5135 | break; |
yabahiro | 56:61ebd4b759b4 | 5136 | case 99: |
yabahiro | 56:61ebd4b759b4 | 5137 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5138 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5139 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5140 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5141 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5142 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5143 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5144 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5145 | break; |
yabahiro | 56:61ebd4b759b4 | 5146 | case 0: |
yabahiro | 56:61ebd4b759b4 | 5147 | motor[TIRE_FR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5148 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5149 | motor[TIRE_FL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5150 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5151 | motor[TIRE_BR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5152 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5153 | motor[TIRE_BL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5154 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5155 | break; |
yabahiro | 56:61ebd4b759b4 | 5156 | case 255: |
yabahiro | 56:61ebd4b759b4 | 5157 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5158 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5159 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5160 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5161 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5162 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5163 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5164 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5165 | break; |
yabahiro | 56:61ebd4b759b4 | 5166 | case 253: |
yabahiro | 56:61ebd4b759b4 | 5167 | motor[TIRE_FR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 5168 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5169 | motor[TIRE_FL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5170 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5171 | motor[TIRE_BR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5172 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5173 | motor[TIRE_BL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 5174 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5175 | break; |
yabahiro | 56:61ebd4b759b4 | 5176 | case 254: |
yabahiro | 56:61ebd4b759b4 | 5177 | motor[TIRE_FR].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 5178 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5179 | motor[TIRE_FL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5180 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5181 | motor[TIRE_BR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5182 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5183 | motor[TIRE_BL].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 5184 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5185 | break; |
yabahiro | 56:61ebd4b759b4 | 5186 | case 1: |
yabahiro | 56:61ebd4b759b4 | 5187 | motor[TIRE_FR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5188 | motor[TIRE_FR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5189 | motor[TIRE_FL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5190 | motor[TIRE_FL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5191 | motor[TIRE_BR].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5192 | motor[TIRE_BR].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5193 | motor[TIRE_BL].pwm = 0; |
yabahiro | 56:61ebd4b759b4 | 5194 | motor[TIRE_BL].dir = FREE; |
yabahiro | 56:61ebd4b759b4 | 5195 | break; |
yabahiro | 56:61ebd4b759b4 | 5196 | case 3: |
yabahiro | 56:61ebd4b759b4 | 5197 | motor[TIRE_FR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5198 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5199 | motor[TIRE_FL].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 5200 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5201 | motor[TIRE_BR].pwm = 25; |
yabahiro | 56:61ebd4b759b4 | 5202 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5203 | motor[TIRE_BL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5204 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5205 | break; |
yabahiro | 56:61ebd4b759b4 | 5206 | case 2: |
yabahiro | 56:61ebd4b759b4 | 5207 | motor[TIRE_FR].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5208 | motor[TIRE_FR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5209 | motor[TIRE_FL].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 5210 | motor[TIRE_FL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5211 | motor[TIRE_BR].pwm = 40; |
yabahiro | 56:61ebd4b759b4 | 5212 | motor[TIRE_BR].dir = FOR; |
yabahiro | 56:61ebd4b759b4 | 5213 | motor[TIRE_BL].pwm = 60; |
yabahiro | 56:61ebd4b759b4 | 5214 | motor[TIRE_BL].dir = BACK; |
yabahiro | 56:61ebd4b759b4 | 5215 | break; |
yabahiro | 56:61ebd4b759b4 | 5216 | } |
yabahiro | 56:61ebd4b759b4 | 5217 | break; |
yabahiro | 56:61ebd4b759b4 | 5218 | } |
yabahiro | 46:68296caae1af | 5219 | |
kishibekairohan | 13:b6e02d6261d7 | 5220 | } |
kishibekairohan | 13:b6e02d6261d7 | 5221 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 5222 | |
kishibekairohan | 13:b6e02d6261d7 | 5223 | #if USE_PROCESS_NUM>6 |
Ryosei | 27:dd9f27fce7d1 | 5224 | static void Process6() |
kishibekairohan | 13:b6e02d6261d7 | 5225 | { |
Ryosei | 27:dd9f27fce7d1 | 5226 | |
kishibekairohan | 13:b6e02d6261d7 | 5227 | } |
kishibekairohan | 13:b6e02d6261d7 | 5228 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 5229 | |
kishibekairohan | 13:b6e02d6261d7 | 5230 | #if USE_PROCESS_NUM>7 |
kishibekairohan | 13:b6e02d6261d7 | 5231 | static void Process7() |
kishibekairohan | 13:b6e02d6261d7 | 5232 | { |
yabahiro | 33:dd598bb23ad1 | 5233 | |
kishibekairohan | 12:c09b3e08a316 | 5234 | } |
kishibekairohan | 12:c09b3e08a316 | 5235 | #endif |
kishibekairohan | 12:c09b3e08a316 | 5236 | |
Ryosei | 27:dd9f27fce7d1 | 5237 | #if USE_PROCESS_NUM>8 |
kishibekairohan | 13:b6e02d6261d7 | 5238 | static void Process8() |
kishibekairohan | 12:c09b3e08a316 | 5239 | { |
yabahiro | 33:dd598bb23ad1 | 5240 | |
kishibekairohan | 12:c09b3e08a316 | 5241 | } |
kishibekairohan | 12:c09b3e08a316 | 5242 | #endif |
kishibekairohan | 12:c09b3e08a316 | 5243 | |
kishibekairohan | 13:b6e02d6261d7 | 5244 | #if USE_PROCESS_NUM>9 |
kishibekairohan | 13:b6e02d6261d7 | 5245 | static void Process9() |
kishibekairohan | 12:c09b3e08a316 | 5246 | { |
Ryosei | 27:dd9f27fce7d1 | 5247 | |
t_yamamoto | 0:669ef71cba68 | 5248 | } |
t_yamamoto | 0:669ef71cba68 | 5249 | #endif |
t_yamamoto | 0:669ef71cba68 | 5250 | #endif |
t_yamamoto | 0:669ef71cba68 | 5251 | #pragma endregion PROCESS |
t_yamamoto | 0:669ef71cba68 | 5252 | |
t_yamamoto | 0:669ef71cba68 | 5253 | static void AllActuatorReset() |
t_yamamoto | 0:669ef71cba68 | 5254 | { |
yabahiro | 33:dd598bb23ad1 | 5255 | #ifdef USE_SOLENOID |
yabahiro | 33:dd598bb23ad1 | 5256 | solenoid.all = ALL_SOLENOID_OFF; |
yabahiro | 33:dd598bb23ad1 | 5257 | #endif |
yabahiro | 33:dd598bb23ad1 | 5258 | |
yabahiro | 33:dd598bb23ad1 | 5259 | #ifdef USE_MOTOR |
yabahiro | 33:dd598bb23ad1 | 5260 | for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { |
yabahiro | 33:dd598bb23ad1 | 5261 | motor[i].dir = FREE; |
yabahiro | 33:dd598bb23ad1 | 5262 | motor[i].pwm = 0; |
yabahiro | 33:dd598bb23ad1 | 5263 | } |
yabahiro | 33:dd598bb23ad1 | 5264 | #endif |
t_yamamoto | 0:669ef71cba68 | 5265 | } |
t_yamamoto | 0:669ef71cba68 | 5266 | |
kishibekairohan | 16:3f2c2d89372b | 5267 | #pragma region USER-DEFINED-FUNCTIONS |
M_souta | 29:44d5454ce8fa | 5268 | |
M_souta | 29:44d5454ce8fa | 5269 | uint8_t SetStatus(int pwmVal) |
Ryosei | 27:dd9f27fce7d1 | 5270 | { |
M_souta | 29:44d5454ce8fa | 5271 | if (pwmVal < 0) return BACK; |
M_souta | 29:44d5454ce8fa | 5272 | else if (pwmVal > 0) return FOR; |
M_souta | 29:44d5454ce8fa | 5273 | else if (pwmVal == 0) return BRAKE; |
M_souta | 29:44d5454ce8fa | 5274 | else return BRAKE; |
M_souta | 22:7d93f79a3686 | 5275 | } |
kishibekairohan | 16:3f2c2d89372b | 5276 | |
M_souta | 29:44d5454ce8fa | 5277 | uint8_t SetPWM(int pwmVal) |
Ryosei | 27:dd9f27fce7d1 | 5278 | { |
M_souta | 29:44d5454ce8fa | 5279 | if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; |
M_souta | 29:44d5454ce8fa | 5280 | else return abs(pwmVal); |
M_souta | 22:7d93f79a3686 | 5281 | } |
yabahiro | 55:844ac0974740 | 5282 | /* |
yabahiro | 55:844ac0974740 | 5283 | void Count(){ |
yabahiro | 55:844ac0974740 | 5284 | num++; |
yabahiro | 55:844ac0974740 | 5285 | } |
yabahiro | 55:844ac0974740 | 5286 | */ |
t_yamamoto | 0:669ef71cba68 | 5287 | #pragma endregion |