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