三浦 颯太 / Mbed 2 deprecated MainBoard2019_Auto_B_3

ttt

Dependencies:   mbed TrapezoidControl QEI Pulse LM61CIZ

System/Process/Process.cpp@27:dd6c9feb7493, 2019-10-02 (annotated)

Committer:
M_souta
Date:
Wed Oct 02 11:08:26 2019 +0000
Revision:
27:dd6c9feb7493
Parent:
26:4c0ce2f05688 
pp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
t_yamamoto 0:669ef71cba68 3
7ka884 4:ba9df71868df 4 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 5 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 6 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/Switch/Switch.h"
kishibekairohan 16:3f2c2d89372b 10 #include "../../Input/Encoder/Encoder.h"
M_souta 22:7d93f79a3686 11 #include "../../Input/Ultrasonic/Ultrasonic.h"
t_yamamoto 0:669ef71cba68 12 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 13 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 14 #include "../Using.h"
t_yamamoto 0:669ef71cba68 15
t_yamamoto 0:669ef71cba68 16 using namespace SWITCH;
7ka884 4:ba9df71868df 17 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 18 using namespace ENCODER;
M_souta 22:7d93f79a3686 19 using namespace ULTRASONIC;
M_souta 21:e3b58d675c1c 20 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 21
M_souta 22:7d93f79a3686 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
M_souta 22:7d93f79a3686 41 USS ultrasonic[] = {
M_souta 22:7d93f79a3686 42 USS(ECHO_0,TRIG_0,TEMP),
M_souta 22:7d93f79a3686 43 USS(ECHO_1,TRIG_1,TEMP),
M_souta 22:7d93f79a3686 44 };
kishibekairohan 2:c015739085d3 45
M_souta 22:7d93f79a3686 46 Serial pc(USBTX, USBRX);
kishibekairohan 7:e88c5d47a3be 47
kishibekairohan 16:3f2c2d89372b 48 //**************Buzzer****************
kishibekairohan 11:028a150943b5 49 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 50 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 51 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 52 bool EMGflag = false;
kishibekairohan 16:3f2c2d89372b 53 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 54 //**************Buzzer****************
kishibekairohan 11:028a150943b5 55
kishibekairohan 11:028a150943b5 56 //************TapeLed*****************
kishibekairohan 11:028a150943b5 57 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 58 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 59 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 60 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 61 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 62 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 63
M_souta 22:7d93f79a3686 64 //*************** lift ***************
M_souta 22:7d93f79a3686 65 #define LOWER 1
M_souta 22:7d93f79a3686 66 #define MIDDLRE 2
M_souta 22:7d93f79a3686 67 #define UPPER 3
M_souta 22:7d93f79a3686 68 uint8_t liftState = LOWER;
M_souta 22:7d93f79a3686 69 bool moving = false;
M_souta 22:7d93f79a3686 70 bool switchFlag_LB = false;
M_souta 22:7d93f79a3686 71 bool switchFlag_RB = false;
M_souta 22:7d93f79a3686 72
M_souta 22:7d93f79a3686 73 //*************** lift ***************
M_souta 22:7d93f79a3686 74
M_souta 22:7d93f79a3686 75 //*************tire*************
M_souta 22:7d93f79a3686 76 PID rotaconPID[] = {
M_souta 22:7d93f79a3686 77 PID(0.0001,-1,1,0.05,0,0), //LF
M_souta 22:7d93f79a3686 78 PID(0.0001,-1,1,0.05,0,0), //LB
M_souta 22:7d93f79a3686 79 PID(0.0001,-1,1,0.05,0,0), //RB
M_souta 22:7d93f79a3686 80 PID(0.0001,-1,1,0.05,0,0), //RF
M_souta 22:7d93f79a3686 81 };
M_souta 22:7d93f79a3686 82
M_souta 22:7d93f79a3686 83 #define FL 0
M_souta 22:7d93f79a3686 84 #define BL 1
M_souta 22:7d93f79a3686 85 #define BR 2
M_souta 22:7d93f79a3686 86 #define FR 3
M_souta 22:7d93f79a3686 87
M_souta 22:7d93f79a3686 88 #define PI 3.141592
M_souta 22:7d93f79a3686 89
M_souta 22:7d93f79a3686 90 const float tireR = 101.6; //タイヤの半径
M_souta 22:7d93f79a3686 91 const float ucR = 420.0; //中心からのタイヤの距離
M_souta 22:7d93f79a3686 92
M_souta 22:7d93f79a3686 93 typedef struct {
M_souta 22:7d93f79a3686 94 float Vx; //X方向の速度
M_souta 22:7d93f79a3686 95 float Vy; //Y方向の速度
M_souta 22:7d93f79a3686 96 float Va; //角速度
M_souta 22:7d93f79a3686 97 } Vvector;
M_souta 22:7d93f79a3686 98
M_souta 22:7d93f79a3686 99 Vvector move; //進む速度
M_souta 22:7d93f79a3686 100 Vvector correction_LT; //ライントレースの補正速度
M_souta 22:7d93f79a3686 101 Vvector synthetic; //合成速度
M_souta 22:7d93f79a3686 102
M_souta 22:7d93f79a3686 103 float sita = 0;
M_souta 22:7d93f79a3686 104
M_souta 22:7d93f79a3686 105 bool PIDflag = false;
M_souta 22:7d93f79a3686 106
M_souta 22:7d93f79a3686 107 int linePara[8];
M_souta 22:7d93f79a3686 108 int linePara_U;
M_souta 22:7d93f79a3686 109 int linePara_B;
M_souta 22:7d93f79a3686 110 int linePara_L;
M_souta 22:7d93f79a3686 111 int linePara_R;
M_souta 22:7d93f79a3686 112
M_souta 22:7d93f79a3686 113 #define FL 0
M_souta 22:7d93f79a3686 114 #define BL 1
M_souta 22:7d93f79a3686 115 #define BR 2
M_souta 22:7d93f79a3686 116 #define FR 3
M_souta 22:7d93f79a3686 117
M_souta 22:7d93f79a3686 118 float tireProcessRPM[4];
M_souta 22:7d93f79a3686 119 float tireTargetMaxRPM[4];
M_souta 22:7d93f79a3686 120 float tireTargetRPM[4];
M_souta 22:7d93f79a3686 121
M_souta 22:7d93f79a3686 122 float tirePWM[4];
M_souta 22:7d93f79a3686 123
M_souta 22:7d93f79a3686 124 float timePV[4];
M_souta 22:7d93f79a3686 125 float timeCV[4];
M_souta 22:7d93f79a3686 126 float pulsePV[4];
M_souta 22:7d93f79a3686 127 float pulseCV[4];
M_souta 22:7d93f79a3686 128
M_souta 22:7d93f79a3686 129 void tirePID();
M_souta 22:7d93f79a3686 130 int lineCast(char k);
M_souta 22:7d93f79a3686 131
M_souta 22:7d93f79a3686 132 Timer rotaconSampling;
M_souta 22:7d93f79a3686 133 Ticker rotaconPIDtimer;
M_souta 22:7d93f79a3686 134
M_souta 22:7d93f79a3686 135 bool countFlag;
M_souta 22:7d93f79a3686 136 //*************tire**************//
M_souta 22:7d93f79a3686 137
M_souta 22:7d93f79a3686 138 // ************* Line ************** //
M_souta 22:7d93f79a3686 139
M_souta 26:4c0ce2f05688 140 Timer tow_stop;
M_souta 26:4c0ce2f05688 141
M_souta 22:7d93f79a3686 142 float pw = 0;
M_souta 22:7d93f79a3686 143 int lineFase = 0;
M_souta 22:7d93f79a3686 144 bool lineCheck = false;
M_souta 25:b3a9f34b201d 145 bool lineFlag = false;
M_souta 25:b3a9f34b201d 146 bool adjAnable = false;
M_souta 25:b3a9f34b201d 147 int adj = 0;
M_souta 25:b3a9f34b201d 148
M_souta 26:4c0ce2f05688 149 int countW = 0;
M_souta 25:b3a9f34b201d 150 int lineCount = 0;
M_souta 25:b3a9f34b201d 151 int targetCount = 0;
M_souta 25:b3a9f34b201d 152
M_souta 25:b3a9f34b201d 153 bool startFlag = true;
M_souta 25:b3a9f34b201d 154
M_souta 22:7d93f79a3686 155 int linePWM;
M_souta 22:7d93f79a3686 156 int adj_F;
M_souta 22:7d93f79a3686 157 int adj_B;
M_souta 22:7d93f79a3686 158
M_souta 22:7d93f79a3686 159 int mode = 0;
M_souta 22:7d93f79a3686 160
M_souta 22:7d93f79a3686 161 // ************* Line ************** //
M_souta 22:7d93f79a3686 162
M_souta 27:dd6c9feb7493 163 // ************* Air *********** //
M_souta 27:dd6c9feb7493 164
M_souta 27:dd6c9feb7493 165 #define AIR_NUM 4
M_souta 27:dd6c9feb7493 166
M_souta 27:dd6c9feb7493 167 DigitalOut air[] = {
M_souta 27:dd6c9feb7493 168 DigitalOut (ECD_A_0),
M_souta 27:dd6c9feb7493 169 DigitalOut (ECD_B_0),
M_souta 27:dd6c9feb7493 170 DigitalOut (ECD_A_1),
M_souta 27:dd6c9feb7493 171 DigitalOut (ECD_B_1),
M_souta 27:dd6c9feb7493 172 };
M_souta 27:dd6c9feb7493 173
M_souta 27:dd6c9feb7493 174 bool Air[AIR_NUM];
M_souta 27:dd6c9feb7493 175
M_souta 27:dd6c9feb7493 176 void AirUpdate() {
M_souta 27:dd6c9feb7493 177 for(int i = 0; i < AIR_NUM; i++) {
M_souta 27:dd6c9feb7493 178 air[i] = Air[i];
M_souta 27:dd6c9feb7493 179 }
M_souta 27:dd6c9feb7493 180 }
M_souta 27:dd6c9feb7493 181 // ************* Air ************ //
M_souta 27:dd6c9feb7493 182
M_souta 21:e3b58d675c1c 183 const int omni[15][15] =
M_souta 20:eae8c84f318c 184 {
M_souta 21:e3b58d675c1c 185 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 186 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 187 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta 21:e3b58d675c1c 188 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta 21:e3b58d675c1c 189 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta 21:e3b58d675c1c 190 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta 21:e3b58d675c1c 191 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta 21:e3b58d675c1c 192 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta 21:e3b58d675c1c 193 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta 21:e3b58d675c1c 194 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta 21:e3b58d675c1c 195 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta 21:e3b58d675c1c 196 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta 21:e3b58d675c1c 197 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta 21:e3b58d675c1c 198 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta 21:e3b58d675c1c 199 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 200 };
M_souta 21:e3b58d675c1c 201
M_souta 21:e3b58d675c1c 202 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 203
M_souta 21:e3b58d675c1c 204 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 205 uint8_t SetStatus(int pwmVal) {
M_souta 21:e3b58d675c1c 206 if (pwmVal < 0) return BACK;
M_souta 21:e3b58d675c1c 207 else if (pwmVal > 0) return FOR;
M_souta 21:e3b58d675c1c 208 else if (pwmVal == 0) return BRAKE;
M_souta 21:e3b58d675c1c 209 else return BRAKE;
M_souta 20:eae8c84f318c 210 }
M_souta 21:e3b58d675c1c 211 uint8_t SetPWM(int);
M_souta 21:e3b58d675c1c 212 uint8_t SetPWM(int pwmVal) {
M_souta 21:e3b58d675c1c 213 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 21:e3b58d675c1c 214 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 215 }
M_souta 18:c694bae76e51 216
t_yamamoto 0:669ef71cba68 217 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 218
t_yamamoto 0:669ef71cba68 219 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 220 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 221 static void Process0(void);
t_yamamoto 0:669ef71cba68 222 #endif
t_yamamoto 0:669ef71cba68 223 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 224 static void Process1(void);
t_yamamoto 0:669ef71cba68 225 #endif
t_yamamoto 0:669ef71cba68 226 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 227 static void Process2(void);
t_yamamoto 0:669ef71cba68 228 #endif
t_yamamoto 0:669ef71cba68 229 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 230 static void Process3(void);
t_yamamoto 0:669ef71cba68 231 #endif
t_yamamoto 0:669ef71cba68 232 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 233 static void Process4(void);
t_yamamoto 0:669ef71cba68 234 #endif
t_yamamoto 0:669ef71cba68 235 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 236 static void Process5(void);
t_yamamoto 0:669ef71cba68 237 #endif
t_yamamoto 0:669ef71cba68 238 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 239 static void Process6(void);
t_yamamoto 0:669ef71cba68 240 #endif
t_yamamoto 0:669ef71cba68 241 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 242 static void Process7(void);
t_yamamoto 0:669ef71cba68 243 #endif
t_yamamoto 0:669ef71cba68 244 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 245 static void Process8(void);
t_yamamoto 0:669ef71cba68 246 #endif
t_yamamoto 0:669ef71cba68 247 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 248 static void Process9(void);
t_yamamoto 0:669ef71cba68 249 #endif
t_yamamoto 0:669ef71cba68 250 #endif
t_yamamoto 0:669ef71cba68 251
t_yamamoto 0:669ef71cba68 252 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 253 {
t_yamamoto 0:669ef71cba68 254 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 16:3f2c2d89372b 255 /*Replace here with the initialization code of your variables.*/
M_souta 25:b3a9f34b201d 256 //rotaconPIDtimer.attach(tirePID,0.1);
M_souta 25:b3a9f34b201d 257
t_yamamoto 0:669ef71cba68 258 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 259
t_yamamoto 0:669ef71cba68 260 lock = true;
t_yamamoto 0:669ef71cba68 261 processChangeComp = true;
t_yamamoto 0:669ef71cba68 262 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 263
t_yamamoto 0:669ef71cba68 264 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 265 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 266 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 267 #endif
t_yamamoto 0:669ef71cba68 268 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 269 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 270 #endif
t_yamamoto 0:669ef71cba68 271 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 272 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 273 #endif
t_yamamoto 0:669ef71cba68 274 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 275 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 276 #endif
t_yamamoto 0:669ef71cba68 277 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 278 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 279 #endif
t_yamamoto 0:669ef71cba68 280 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 281 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 282 #endif
t_yamamoto 0:669ef71cba68 283 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 284 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 285 #endif
t_yamamoto 0:669ef71cba68 286 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 287 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 288 #endif
t_yamamoto 0:669ef71cba68 289 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 290 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 291 #endif
t_yamamoto 0:669ef71cba68 292 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 293 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 294 #endif
t_yamamoto 0:669ef71cba68 295 #endif
t_yamamoto 0:669ef71cba68 296 }
t_yamamoto 0:669ef71cba68 297
t_yamamoto 0:669ef71cba68 298 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 299 {
t_yamamoto 0:669ef71cba68 300 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 301 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 302
t_yamamoto 0:669ef71cba68 303 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 304 {
t_yamamoto 0:669ef71cba68 305 current++;
t_yamamoto 0:669ef71cba68 306 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 307 processChangeComp = false;
t_yamamoto 0:669ef71cba68 308 }
t_yamamoto 0:669ef71cba68 309 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 310 {
t_yamamoto 0:669ef71cba68 311 current--;
t_yamamoto 0:669ef71cba68 312 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 313 processChangeComp = false;
t_yamamoto 0:669ef71cba68 314 }
t_yamamoto 0:669ef71cba68 315 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 316 #endif
t_yamamoto 0:669ef71cba68 317
t_yamamoto 0:669ef71cba68 318 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 319 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 320 #endif
t_yamamoto 0:669ef71cba68 321
t_yamamoto 0:669ef71cba68 322 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 323 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 324 #endif
t_yamamoto 0:669ef71cba68 325
t_yamamoto 0:669ef71cba68 326 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 327 ACTUATORHUB::ActuatorHub::Update();
M_souta 21:e3b58d675c1c 328 //LINEHUB::LineHub::Update();
t_yamamoto 0:669ef71cba68 329 #endif
t_yamamoto 0:669ef71cba68 330
t_yamamoto 0:669ef71cba68 331 }
t_yamamoto 0:669ef71cba68 332
kishibekairohan 2:c015739085d3 333
kishibekairohan 2:c015739085d3 334
t_yamamoto 0:669ef71cba68 335 void SystemProcess()
t_yamamoto 0:669ef71cba68 336 {
t_yamamoto 0:669ef71cba68 337 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 338
t_yamamoto 0:669ef71cba68 339 while(1)
kishibekairohan 15:dfcec98f5aa9 340 {
M_souta 22:7d93f79a3686 341
M_souta 27:dd6c9feb7493 342 int g[8];
M_souta 27:dd6c9feb7493 343 for(int i = 0; i < 8; i++){
M_souta 27:dd6c9feb7493 344 g[i] = lineCast(LineHub::GetPara(i));
M_souta 27:dd6c9feb7493 345 }
M_souta 27:dd6c9feb7493 346 pc.printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
M_souta 27:dd6c9feb7493 347 /*
M_souta 27:dd6c9feb7493 348 if(LimitSw::IsPressed(UNFOLD_SW) && startFlag) {
M_souta 27:dd6c9feb7493 349 startFlag = false;
M_souta 27:dd6c9feb7493 350 lock = false;
M_souta 27:dd6c9feb7493 351 current = 6;
M_souta 27:dd6c9feb7493 352
M_souta 27:dd6c9feb7493 353 }
M_souta 27:dd6c9feb7493 354 */
M_souta 27:dd6c9feb7493 355
M_souta 27:dd6c9feb7493 356 /*
M_souta 25:b3a9f34b201d 357 if(LimitSw::IsPressed(START_SW) && startFlag == true) {
M_souta 27:dd6c9feb7493 358 startFlag = false;
M_souta 25:b3a9f34b201d 359 lock = false;
M_souta 25:b3a9f34b201d 360 lineFase = 0;
M_souta 25:b3a9f34b201d 361 lineCount = 0;
M_souta 25:b3a9f34b201d 362 lineCheck = false;
M_souta 26:4c0ce2f05688 363 countW = 0;
M_souta 27:dd6c9feb7493 364 for(int i = 0; i < 4; i++) {
M_souta 27:dd6c9feb7493 365 Air[i] = 0;
M_souta 27:dd6c9feb7493 366 }
M_souta 26:4c0ce2f05688 367 if(LimitSw::IsPressed(REDBLUE_SW)) {
M_souta 25:b3a9f34b201d 368 current = 4;
M_souta 25:b3a9f34b201d 369 } else {
M_souta 25:b3a9f34b201d 370 current = 5;
M_souta 25:b3a9f34b201d 371 }
M_souta 25:b3a9f34b201d 372 }
M_souta 27:dd6c9feb7493 373 */
M_souta 27:dd6c9feb7493 374
M_souta 27:dd6c9feb7493 375 AirUpdate(); // エアー 更新
M_souta 20:eae8c84f318c 376
kishibekairohan 17:50dc4b449e69 377 buzzer.period(1.0/800);
kishibekairohan 15:dfcec98f5aa9 378
t_yamamoto 0:669ef71cba68 379 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 380 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 381 #endif
t_yamamoto 0:669ef71cba68 382
t_yamamoto 0:669ef71cba68 383 #ifdef USE_ERRORCHECK
M_souta 25:b3a9f34b201d 384 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
t_yamamoto 0:669ef71cba68 385 {
t_yamamoto 0:669ef71cba68 386 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 387 AllActuatorReset();
t_yamamoto 0:669ef71cba68 388 lock = true;
t_yamamoto 0:669ef71cba68 389 }
t_yamamoto 0:669ef71cba68 390 else
t_yamamoto 0:669ef71cba68 391 #endif
t_yamamoto 0:669ef71cba68 392 {
t_yamamoto 0:669ef71cba68 393
t_yamamoto 0:669ef71cba68 394 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 395 if(!lock)
t_yamamoto 0:669ef71cba68 396 {
t_yamamoto 0:669ef71cba68 397 Process[current]();
t_yamamoto 0:669ef71cba68 398 }
t_yamamoto 0:669ef71cba68 399 else
t_yamamoto 0:669ef71cba68 400 #endif
t_yamamoto 0:669ef71cba68 401 {
t_yamamoto 0:669ef71cba68 402 //ロック時の処理
t_yamamoto 0:669ef71cba68 403 }
t_yamamoto 0:669ef71cba68 404 }
t_yamamoto 0:669ef71cba68 405
M_souta 22:7d93f79a3686 406
kishibekairohan 16:3f2c2d89372b 407 //Emergency!
M_souta 22:7d93f79a3686 408 /*
kishibekairohan 17:50dc4b449e69 409 if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan 17:50dc4b449e69 410 buzzer = 0;
kishibekairohan 17:50dc4b449e69 411 BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan 17:50dc4b449e69 412 EMGflag = true;
kishibekairohan 17:50dc4b449e69 413 LED_DEBUG0 = 1;
kishibekairohan 9:f93fc79a49ea 414 }
kishibekairohan 17:50dc4b449e69 415 if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan 17:50dc4b449e69 416 buzzer = 1;
kishibekairohan 9:f93fc79a49ea 417 BuzzerTimer.detach();
kishibekairohan 17:50dc4b449e69 418 EMGflag = false;
kishibekairohan 9:f93fc79a49ea 419 }
M_souta 22:7d93f79a3686 420 */
t_yamamoto 0:669ef71cba68 421 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 422 }
t_yamamoto 0:669ef71cba68 423 }
t_yamamoto 0:669ef71cba68 424
kishibekairohan 2:c015739085d3 425
kishibekairohan 2:c015739085d3 426
kishibekairohan 2:c015739085d3 427
t_yamamoto 0:669ef71cba68 428 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 429 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 430 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 431 static void Process0()
kishibekairohan 10:1295d39fec3a 432 {
M_souta 21:e3b58d675c1c 433 AllActuatorReset();
M_souta 27:dd6c9feb7493 434
t_yamamoto 0:669ef71cba68 435 }
t_yamamoto 0:669ef71cba68 436 #endif
t_yamamoto 0:669ef71cba68 437
t_yamamoto 0:669ef71cba68 438 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 439 static void Process1()
t_yamamoto 0:669ef71cba68 440 {
M_souta 22:7d93f79a3686 441
M_souta 22:7d93f79a3686 442 PIDflag = false;
M_souta 22:7d93f79a3686 443
M_souta 21:e3b58d675c1c 444 if(controller->Button.UP) {
M_souta 21:e3b58d675c1c 445 motor[LIFT_LB].dir = FOR;
M_souta 26:4c0ce2f05688 446 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 447 motor[LIFT_RB].dir = BACK;
M_souta 26:4c0ce2f05688 448 motor[LIFT_RB].pwm = 200;
M_souta 21:e3b58d675c1c 449 } else if(controller->Button.DOWN) {
M_souta 21:e3b58d675c1c 450 motor[LIFT_LB].dir = BACK;
M_souta 21:e3b58d675c1c 451 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 452 motor[LIFT_RB].dir = FOR;
M_souta 26:4c0ce2f05688 453 motor[LIFT_RB].pwm = 200;
M_souta 23:c853372cf626 454 } else if(controller->Button.LEFT) {
M_souta 23:c853372cf626 455 motor[LIFT_LB].dir = FOR;
M_souta 23:c853372cf626 456 motor[LIFT_LB].pwm = 180;
M_souta 23:c853372cf626 457 } else if(controller->Button.RIGHT) {
M_souta 23:c853372cf626 458 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 459 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 460 } else {
M_souta 25:b3a9f34b201d 461 motor[LIFT_LB].dir = BRAKE;
M_souta 23:c853372cf626 462 motor[LIFT_LB].pwm = 255;
M_souta 26:4c0ce2f05688 463 motor[LIFT_RB].dir = BRAKE;
M_souta 26:4c0ce2f05688 464 motor[LIFT_RB].pwm = 255;
M_souta 21:e3b58d675c1c 465 }
M_souta 22:7d93f79a3686 466
7ka884 4:ba9df71868df 467
M_souta 21:e3b58d675c1c 468 if(controller->Button.X) {
M_souta 21:e3b58d675c1c 469 motor[LIFT_U].dir = FOR;
M_souta 21:e3b58d675c1c 470 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 471 } else if(controller->Button.Y) {
M_souta 21:e3b58d675c1c 472 motor[LIFT_U].dir = BACK;
M_souta 25:b3a9f34b201d 473 motor[LIFT_U].pwm = 230;
M_souta 21:e3b58d675c1c 474 } else {
M_souta 21:e3b58d675c1c 475 motor[LIFT_U].dir = BRAKE;
M_souta 25:b3a9f34b201d 476 motor[LIFT_U].pwm = 255;
M_souta 21:e3b58d675c1c 477 }
M_souta 21:e3b58d675c1c 478
M_souta 21:e3b58d675c1c 479 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
M_souta 25:b3a9f34b201d 480 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 481 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 482 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 25:b3a9f34b201d 483 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 484
M_souta 25:b3a9f34b201d 485 motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 486 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 487 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 25:b3a9f34b201d 488 motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 21:e3b58d675c1c 489 } else {
M_souta 25:b3a9f34b201d 490 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 491 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 492 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 25:b3a9f34b201d 493 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 494
M_souta 21:e3b58d675c1c 495 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 496 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 497 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 498 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 499 }
M_souta 25:b3a9f34b201d 500
M_souta 27:dd6c9feb7493 501 if(controller->Button.L) {
M_souta 27:dd6c9feb7493 502 air[0] = SOLENOID_ON;
M_souta 27:dd6c9feb7493 503 } else {
M_souta 27:dd6c9feb7493 504 air[0] = SOLENOID_OFF;
M_souta 26:4c0ce2f05688 505 }
M_souta 26:4c0ce2f05688 506 if(controller->Button.ZL) {
M_souta 27:dd6c9feb7493 507 air[1] = SOLENOID_ON;
M_souta 27:dd6c9feb7493 508 } else {
M_souta 27:dd6c9feb7493 509 air[1] = SOLENOID_OFF;
M_souta 25:b3a9f34b201d 510 }
M_souta 27:dd6c9feb7493 511 if(controller->Button.R) {
M_souta 27:dd6c9feb7493 512 air[2] = SOLENOID_ON;
M_souta 27:dd6c9feb7493 513 } else {
M_souta 27:dd6c9feb7493 514 air[2] = SOLENOID_OFF;
M_souta 27:dd6c9feb7493 515 }
M_souta 27:dd6c9feb7493 516 if(controller->Button.ZR) {
M_souta 27:dd6c9feb7493 517 air[3] = SOLENOID_ON;
M_souta 27:dd6c9feb7493 518 } else {
M_souta 27:dd6c9feb7493 519 air[3] = SOLENOID_OFF;
M_souta 27:dd6c9feb7493 520 }
M_souta 27:dd6c9feb7493 521
M_souta 27:dd6c9feb7493 522
t_yamamoto 0:669ef71cba68 523 }
t_yamamoto 0:669ef71cba68 524 #endif
t_yamamoto 0:669ef71cba68 525
t_yamamoto 0:669ef71cba68 526 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 527 static void Process2()
kishibekairohan 13:b6e02d6261d7 528 {
M_souta 25:b3a9f34b201d 529
M_souta 25:b3a9f34b201d 530
M_souta 25:b3a9f34b201d 531
M_souta 25:b3a9f34b201d 532
M_souta 22:7d93f79a3686 533 /*
M_souta 22:7d93f79a3686 534 if(moving) {
M_souta 26:4c0ce2f05688 535 if(LimitSw::IsPressed(LSW_LB)) {
M_souta 22:7d93f79a3686 536 if(switchFlag_LB) {
M_souta 22:7d93f79a3686 537 switchFlag_LB = false;
M_souta 22:7d93f79a3686 538 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 539 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 540 } else {
M_souta 26:4c0ce2f05688 541 switchFlag_LB = true;
M_souta 22:7d93f79a3686 542 }
M_souta 22:7d93f79a3686 543 }
M_souta 26:4c0ce2f05688 544 if(LimitSw::IsPressed(LSW_RB)) {
M_souta 22:7d93f79a3686 545 if(switchFlag_RB) {
M_souta 22:7d93f79a3686 546 switchFlag_RB = false;
M_souta 22:7d93f79a3686 547 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 548 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 549 } else {
M_souta 26:4c0ce2f05688 550 switchFlag_RB = true;
M_souta 22:7d93f79a3686 551 }
M_souta 22:7d93f79a3686 552 }
M_souta 22:7d93f79a3686 553 if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
M_souta 22:7d93f79a3686 554 } else {
M_souta 22:7d93f79a3686 555 if(controller->Button.UP) {
M_souta 26:4c0ce2f05688 556 if(!liftSstate == UPPER)) {
M_souta 26:4c0ce2f05688 557 liftState++;
M_souta 26:4c0ce2f05688 558 motor[LIFT_LB].dir = FOR;
M_souta 26:4c0ce2f05688 559 motor[LIFT_RB].dir = BACK;
M_souta 26:4c0ce2f05688 560 motor[LIFT_LB].pwm = 185;
M_souta 26:4c0ce2f05688 561 motor[LIFT_RB].pwm = 180;
M_souta 22:7d93f79a3686 562 }
M_souta 22:7d93f79a3686 563 } else if(controller->Button.DOWN) {
M_souta 26:4c0ce2f05688 564 if(!(liftstate == LOWER)) {
M_souta 26:4c0ce2f05688 565 liftState--;
M_souta 22:7d93f79a3686 566 moving = true;
M_souta 26:4c0ce2f05688 567 motor[LIFT_LB].dir = BACK;
M_souta 26:4c0ce2f05688 568 motor[LIFT_RB].dir = FOR;
M_souta 26:4c0ce2f05688 569 motor[LIFT_LB].pwm = 180;
M_souta 26:4c0ce2f05688 570 motor[LIFT_RB].pwm = 185;
M_souta 22:7d93f79a3686 571 }
M_souta 22:7d93f79a3686 572 } else {
M_souta 22:7d93f79a3686 573 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 574 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 575 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 576 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 577 }
M_souta 22:7d93f79a3686 578 }
M_souta 22:7d93f79a3686 579 */
M_souta 26:4c0ce2f05688 580
kishibekairohan 13:b6e02d6261d7 581 }
kishibekairohan 13:b6e02d6261d7 582 #endif
kishibekairohan 13:b6e02d6261d7 583
kishibekairohan 13:b6e02d6261d7 584 #if USE_PROCESS_NUM>3
kishibekairohan 16:3f2c2d89372b 585 static void Process3()
kishibekairohan 13:b6e02d6261d7 586 {
M_souta 25:b3a9f34b201d 587 startFlag = true;
M_souta 22:7d93f79a3686 588 AllActuatorReset();
M_souta 23:c853372cf626 589 lineFase = 0;
M_souta 25:b3a9f34b201d 590 lineCheck = false;
M_souta 25:b3a9f34b201d 591 lineCount = 0;
M_souta 26:4c0ce2f05688 592 countW = 0;
M_souta 27:dd6c9feb7493 593 for(int i = 0; i < 4; i++) {
M_souta 27:dd6c9feb7493 594 air[i] = 0;
M_souta 27:dd6c9feb7493 595 Air[i] = 0;
M_souta 27:dd6c9feb7493 596 }
kishibekairohan 13:b6e02d6261d7 597 }
kishibekairohan 13:b6e02d6261d7 598 #endif
kishibekairohan 13:b6e02d6261d7 599
kishibekairohan 13:b6e02d6261d7 600 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 601 static void Process4()
M_souta 22:7d93f79a3686 602 {
M_souta 22:7d93f79a3686 603
M_souta 25:b3a9f34b201d 604 /* ************************************** //
M_souta 25:b3a9f34b201d 605
M_souta 25:b3a9f34b201d 606 赤ゾーン 赤ゾーン 赤ゾーン
M_souta 25:b3a9f34b201d 607
M_souta 25:b3a9f34b201d 608 // ************************************** */
M_souta 25:b3a9f34b201d 609
M_souta 25:b3a9f34b201d 610
M_souta 25:b3a9f34b201d 611 for(int i = 0; i < 8; i++) {
M_souta 25:b3a9f34b201d 612 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 25:b3a9f34b201d 613 }
M_souta 22:7d93f79a3686 614
M_souta 25:b3a9f34b201d 615 if(lineFase == 0) {
M_souta 25:b3a9f34b201d 616 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 617 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 618 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 619 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 620 if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta 25:b3a9f34b201d 621 lineFase = 1;
M_souta 25:b3a9f34b201d 622 }
M_souta 25:b3a9f34b201d 623 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 624 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 625 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 626 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 627
M_souta 25:b3a9f34b201d 628 } else if(lineFase == 1) { // 前 ライントレース
M_souta 25:b3a9f34b201d 629 switch(linePara[0]) {
M_souta 22:7d93f79a3686 630 case -2:
M_souta 25:b3a9f34b201d 631 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 632 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 633 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 634 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 635 adjAnable = true;
M_souta 25:b3a9f34b201d 636 break;
M_souta 25:b3a9f34b201d 637 case -3:
M_souta 25:b3a9f34b201d 638 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 639 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 640 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 641 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 642 adjAnable = true;
M_souta 25:b3a9f34b201d 643 break;
M_souta 25:b3a9f34b201d 644 case -1:
M_souta 25:b3a9f34b201d 645 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 646 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 647 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 648 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 649 adjAnable = true;
M_souta 25:b3a9f34b201d 650 break;
M_souta 25:b3a9f34b201d 651 case 0:
M_souta 25:b3a9f34b201d 652 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 653 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 654 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 655 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 656 adjAnable = true;
M_souta 25:b3a9f34b201d 657 break;
M_souta 25:b3a9f34b201d 658 case 1:
M_souta 25:b3a9f34b201d 659 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 660 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 661 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 662 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 663 adjAnable = true;
M_souta 25:b3a9f34b201d 664 break;
M_souta 25:b3a9f34b201d 665 case 3:
M_souta 25:b3a9f34b201d 666 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 667 tirePWM[TIRE_BL] = 10;
M_souta 25:b3a9f34b201d 668 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 669 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 670 adjAnable = true;
M_souta 25:b3a9f34b201d 671 break;
M_souta 25:b3a9f34b201d 672 case 2:
M_souta 25:b3a9f34b201d 673 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 674 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 675 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 676 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 677 adjAnable = true;
M_souta 25:b3a9f34b201d 678 break;
M_souta 25:b3a9f34b201d 679 case 'A':
M_souta 25:b3a9f34b201d 680 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 681 lineCheck = true;
M_souta 25:b3a9f34b201d 682 lineCount = 0;
M_souta 26:4c0ce2f05688 683 countW++;
M_souta 25:b3a9f34b201d 684 }
M_souta 25:b3a9f34b201d 685 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 686 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 687 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 688 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 689 adjAnable = true;
M_souta 25:b3a9f34b201d 690 break;
M_souta 25:b3a9f34b201d 691 case 'N':
M_souta 25:b3a9f34b201d 692 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 25:b3a9f34b201d 693 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 25:b3a9f34b201d 694 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 25:b3a9f34b201d 695 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 25:b3a9f34b201d 696 adjAnable = false;
M_souta 25:b3a9f34b201d 697 break;
M_souta 25:b3a9f34b201d 698 default:
M_souta 25:b3a9f34b201d 699 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 700 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 701 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 702 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 703 adjAnable = false;
M_souta 25:b3a9f34b201d 704 }
M_souta 25:b3a9f34b201d 705
M_souta 25:b3a9f34b201d 706 if(adjAnable){
M_souta 25:b3a9f34b201d 707 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta 25:b3a9f34b201d 708 } else {
M_souta 25:b3a9f34b201d 709 adj = 0;
M_souta 25:b3a9f34b201d 710 }
M_souta 25:b3a9f34b201d 711
M_souta 25:b3a9f34b201d 712 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 713 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 714 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 715 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 716 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 717 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 718 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 719 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 720
M_souta 25:b3a9f34b201d 721 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 722 lineCount++;
M_souta 25:b3a9f34b201d 723 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 724 }
M_souta 26:4c0ce2f05688 725 if(countW == 3) {
M_souta 26:4c0ce2f05688 726 countW = 0;
M_souta 25:b3a9f34b201d 727 lineFase = 2;
M_souta 25:b3a9f34b201d 728 lineCount = 0;
M_souta 25:b3a9f34b201d 729 lineCheck = false;
M_souta 25:b3a9f34b201d 730 }
M_souta 25:b3a9f34b201d 731 } else if(lineFase == 2) { // 前 低速
M_souta 25:b3a9f34b201d 732 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 733 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 734 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 735 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 736 if(linePara[4] >= -1 && linePara[4] <= 1){
M_souta 25:b3a9f34b201d 737 lineFase = 3;
M_souta 25:b3a9f34b201d 738 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 739 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 740 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 741 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 742 }
M_souta 27:dd6c9feb7493 743 motor[TIRE_FL].pwm = 20;
M_souta 27:dd6c9feb7493 744 motor[TIRE_BL].pwm = 20;
M_souta 27:dd6c9feb7493 745 motor[TIRE_BR].pwm = 20;
M_souta 27:dd6c9feb7493 746 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 747 } else if(lineFase == 3) { // 右 ライントレース
M_souta 25:b3a9f34b201d 748 switch(linePara[4]) {
M_souta 25:b3a9f34b201d 749 case -2:
M_souta 25:b3a9f34b201d 750 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 751 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 752 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 753 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 754 adjAnable = true;
M_souta 22:7d93f79a3686 755 break;
M_souta 22:7d93f79a3686 756 case -3:
M_souta 25:b3a9f34b201d 757 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 758 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 759 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 760 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 761 adjAnable = true;
M_souta 22:7d93f79a3686 762 break;
M_souta 22:7d93f79a3686 763 case -1:
M_souta 25:b3a9f34b201d 764 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 765 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 766 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 767 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 768 adjAnable = true;
M_souta 22:7d93f79a3686 769 break;
M_souta 22:7d93f79a3686 770 case 0:
M_souta 25:b3a9f34b201d 771 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 772 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 773 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 774 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 775 adjAnable = true;
M_souta 22:7d93f79a3686 776 break;
M_souta 22:7d93f79a3686 777 case 1:
M_souta 25:b3a9f34b201d 778 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 779 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 780 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 781 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 782 adjAnable = true;
M_souta 22:7d93f79a3686 783 break;
M_souta 22:7d93f79a3686 784 case 3:
M_souta 25:b3a9f34b201d 785 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 786 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 787 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 788 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 789 adjAnable = true;
M_souta 22:7d93f79a3686 790 break;
M_souta 22:7d93f79a3686 791 case 2:
M_souta 25:b3a9f34b201d 792 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 793 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 794 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 795 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 796 adjAnable = true;
M_souta 22:7d93f79a3686 797 break;
M_souta 22:7d93f79a3686 798 case 'A':
M_souta 25:b3a9f34b201d 799 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 800 lineCheck = true;
M_souta 25:b3a9f34b201d 801 lineCount = 0;
M_souta 26:4c0ce2f05688 802 countW++;
M_souta 25:b3a9f34b201d 803 }
M_souta 25:b3a9f34b201d 804 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 805 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 806 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 807 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 808 adjAnable = true;
M_souta 25:b3a9f34b201d 809 break;
M_souta 25:b3a9f34b201d 810 case 'N':
M_souta 25:b3a9f34b201d 811 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 812 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 813 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 814 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 815 adjAnable = false;
M_souta 25:b3a9f34b201d 816 break;
M_souta 25:b3a9f34b201d 817 default:
M_souta 25:b3a9f34b201d 818 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 819 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 820 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 821 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 822 adjAnable = false;
M_souta 25:b3a9f34b201d 823 }
M_souta 25:b3a9f34b201d 824
M_souta 25:b3a9f34b201d 825 if(adjAnable){
M_souta 25:b3a9f34b201d 826 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta 25:b3a9f34b201d 827 } else {
M_souta 25:b3a9f34b201d 828 adj = 0;
M_souta 25:b3a9f34b201d 829 }
M_souta 25:b3a9f34b201d 830
M_souta 25:b3a9f34b201d 831 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 832 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 833 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 834 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 835 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 836 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 837 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 838 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 839
M_souta 25:b3a9f34b201d 840 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 841 lineCount++;
M_souta 25:b3a9f34b201d 842 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 843 }
M_souta 25:b3a9f34b201d 844
M_souta 27:dd6c9feb7493 845 targetCount = 1;
M_souta 25:b3a9f34b201d 846
M_souta 26:4c0ce2f05688 847 if(countW == targetCount) {
M_souta 26:4c0ce2f05688 848 countW = 0;
M_souta 25:b3a9f34b201d 849 lineFase = 4;
M_souta 25:b3a9f34b201d 850 lineCount = 0;
M_souta 25:b3a9f34b201d 851 lineCheck = false;
M_souta 25:b3a9f34b201d 852 }
M_souta 25:b3a9f34b201d 853 } else if(lineFase == 4) { // 右 低速
M_souta 25:b3a9f34b201d 854 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 855 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 856 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 857 motor[TIRE_FR].dir = FOR;
M_souta 27:dd6c9feb7493 858 if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
M_souta 25:b3a9f34b201d 859 lineFase = 6;
M_souta 25:b3a9f34b201d 860 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 861 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 862 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 863 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 864 }
M_souta 27:dd6c9feb7493 865 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 866 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 867 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 868 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 869 } else if (lineFase == 5) {
M_souta 25:b3a9f34b201d 870 lineFase = 6;
M_souta 25:b3a9f34b201d 871 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 872 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 873 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 874 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 875 } else if(lineFase == 6) { // タオル1 検知
M_souta 25:b3a9f34b201d 876 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 877 lineFase = 7;
M_souta 25:b3a9f34b201d 878 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 879 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 880 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 881 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 882 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 883 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 884 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 885 motor[TIRE_FR].pwm = 50;
M_souta 27:dd6c9feb7493 886 } else if(LimitSw::IsPressed(TOW_1L)) {
M_souta 27:dd6c9feb7493 887 motor[TIRE_FL].dir = FOR;
M_souta 27:dd6c9feb7493 888 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 889 motor[TIRE_BL].dir = FOR;
M_souta 27:dd6c9feb7493 890 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 891 motor[TIRE_BR].dir = FOR;
M_souta 27:dd6c9feb7493 892 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 893 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 894 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 895 } else if(LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 896 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 897 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 898 motor[TIRE_BL].dir = BACK;
M_souta 27:dd6c9feb7493 899 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 900 motor[TIRE_BR].dir = BACK;
M_souta 27:dd6c9feb7493 901 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 902 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 903 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 904 } else {
M_souta 27:dd6c9feb7493 905 if(LimitSw::IsPressed(QF_SW)) {
M_souta 27:dd6c9feb7493 906 switch(linePara[LINE_TOW_2]) {
M_souta 27:dd6c9feb7493 907 case -2:
M_souta 27:dd6c9feb7493 908 tirePWM[TIRE_FL] = -10;
M_souta 27:dd6c9feb7493 909 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 910 tirePWM[TIRE_BR] = 10;
M_souta 27:dd6c9feb7493 911 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 912 adjAnable = true;
M_souta 27:dd6c9feb7493 913 break;
M_souta 27:dd6c9feb7493 914 case -3:
M_souta 27:dd6c9feb7493 915 tirePWM[TIRE_FL] = -14;
M_souta 27:dd6c9feb7493 916 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 917 tirePWM[TIRE_BR] = 14;
M_souta 27:dd6c9feb7493 918 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 919 adjAnable = true;
M_souta 27:dd6c9feb7493 920 break;
M_souta 27:dd6c9feb7493 921 case -1:
M_souta 27:dd6c9feb7493 922 tirePWM[TIRE_FL] = -17;
M_souta 27:dd6c9feb7493 923 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 924 tirePWM[TIRE_BR] = 17;
M_souta 27:dd6c9feb7493 925 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 926 adjAnable = true;
M_souta 27:dd6c9feb7493 927 break;
M_souta 27:dd6c9feb7493 928 case 0:
M_souta 27:dd6c9feb7493 929 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 930 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 931 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 932 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 933 adjAnable = true;
M_souta 27:dd6c9feb7493 934 break;
M_souta 27:dd6c9feb7493 935 case 1:
M_souta 27:dd6c9feb7493 936 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 937 tirePWM[TIRE_BL] = -17;
M_souta 27:dd6c9feb7493 938 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 939 tirePWM[TIRE_FR] = 17;
M_souta 27:dd6c9feb7493 940 adjAnable = true;
M_souta 27:dd6c9feb7493 941 break;
M_souta 27:dd6c9feb7493 942 case 3:
M_souta 27:dd6c9feb7493 943 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 944 tirePWM[TIRE_BL] = -14;
M_souta 27:dd6c9feb7493 945 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 946 tirePWM[TIRE_FR] = 14;
M_souta 27:dd6c9feb7493 947 adjAnable = true;
M_souta 27:dd6c9feb7493 948 break;
M_souta 27:dd6c9feb7493 949 case 2:
M_souta 27:dd6c9feb7493 950 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 951 tirePWM[TIRE_BL] = -10;
M_souta 27:dd6c9feb7493 952 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 953 tirePWM[TIRE_FR] = 10;
M_souta 27:dd6c9feb7493 954 adjAnable = true;
M_souta 27:dd6c9feb7493 955 break;
M_souta 27:dd6c9feb7493 956 case 'A':
M_souta 27:dd6c9feb7493 957 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 958 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 959 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 960 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 961 adjAnable = true;
M_souta 27:dd6c9feb7493 962 break;
M_souta 27:dd6c9feb7493 963 case 'N':
M_souta 27:dd6c9feb7493 964 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 965 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 966 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 967 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 27:dd6c9feb7493 968 adjAnable = false;
M_souta 27:dd6c9feb7493 969 break;
M_souta 27:dd6c9feb7493 970 default:
M_souta 27:dd6c9feb7493 971 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 972 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 973 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 974 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 975 adjAnable = false;
M_souta 27:dd6c9feb7493 976 }
M_souta 27:dd6c9feb7493 977 } else {
M_souta 27:dd6c9feb7493 978 switch(linePara[LINE_TOW_2]) {
M_souta 27:dd6c9feb7493 979 case 2:
M_souta 27:dd6c9feb7493 980 tirePWM[TIRE_FL] = -10;
M_souta 27:dd6c9feb7493 981 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 982 tirePWM[TIRE_BR] = 10;
M_souta 27:dd6c9feb7493 983 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 984 adjAnable = true;
M_souta 27:dd6c9feb7493 985 break;
M_souta 27:dd6c9feb7493 986 case 3:
M_souta 27:dd6c9feb7493 987 tirePWM[TIRE_FL] = -14;
M_souta 27:dd6c9feb7493 988 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 989 tirePWM[TIRE_BR] = 14;
M_souta 27:dd6c9feb7493 990 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 991 adjAnable = true;
M_souta 27:dd6c9feb7493 992 break;
M_souta 27:dd6c9feb7493 993 case 1:
M_souta 27:dd6c9feb7493 994 tirePWM[TIRE_FL] = -17;
M_souta 27:dd6c9feb7493 995 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 996 tirePWM[TIRE_BR] = 17;
M_souta 27:dd6c9feb7493 997 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 998 adjAnable = true;
M_souta 27:dd6c9feb7493 999 break;
M_souta 27:dd6c9feb7493 1000 case 0:
M_souta 27:dd6c9feb7493 1001 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1002 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1003 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1004 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1005 adjAnable = true;
M_souta 27:dd6c9feb7493 1006 break;
M_souta 27:dd6c9feb7493 1007 case -1:
M_souta 27:dd6c9feb7493 1008 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1009 tirePWM[TIRE_BL] = -17;
M_souta 27:dd6c9feb7493 1010 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1011 tirePWM[TIRE_FR] = 17;
M_souta 27:dd6c9feb7493 1012 adjAnable = true;
M_souta 27:dd6c9feb7493 1013 break;
M_souta 27:dd6c9feb7493 1014 case -3:
M_souta 27:dd6c9feb7493 1015 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1016 tirePWM[TIRE_BL] = -14;
M_souta 27:dd6c9feb7493 1017 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1018 tirePWM[TIRE_FR] = 14;
M_souta 27:dd6c9feb7493 1019 adjAnable = true;
M_souta 27:dd6c9feb7493 1020 break;
M_souta 27:dd6c9feb7493 1021 case -2:
M_souta 27:dd6c9feb7493 1022 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1023 tirePWM[TIRE_BL] = -10;
M_souta 27:dd6c9feb7493 1024 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1025 tirePWM[TIRE_FR] = 10;
M_souta 27:dd6c9feb7493 1026 adjAnable = true;
M_souta 27:dd6c9feb7493 1027 break;
M_souta 27:dd6c9feb7493 1028 case 'A':
M_souta 27:dd6c9feb7493 1029 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1030 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1031 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1032 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1033 adjAnable = true;
M_souta 27:dd6c9feb7493 1034 break;
M_souta 27:dd6c9feb7493 1035 case 'N':
M_souta 27:dd6c9feb7493 1036 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 1037 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 1038 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 1039 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 27:dd6c9feb7493 1040 adjAnable = false;
M_souta 27:dd6c9feb7493 1041 break;
M_souta 27:dd6c9feb7493 1042 default:
M_souta 27:dd6c9feb7493 1043 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1044 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1045 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1046 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1047 adjAnable = false;
M_souta 27:dd6c9feb7493 1048 }
M_souta 27:dd6c9feb7493 1049 }
M_souta 27:dd6c9feb7493 1050
M_souta 27:dd6c9feb7493 1051 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1052 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1053 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1054 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1055 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1056 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1057 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1058 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1059 }
M_souta 27:dd6c9feb7493 1060 } else if(lineFase == 7) { // ライン 修正
M_souta 27:dd6c9feb7493 1061 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
M_souta 27:dd6c9feb7493 1062 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1063 motor[TIRE_FL].pwm = 50;
M_souta 27:dd6c9feb7493 1064 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1065 motor[TIRE_BL].pwm = 50;
M_souta 27:dd6c9feb7493 1066 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1067 motor[TIRE_BR].pwm = 50;
M_souta 27:dd6c9feb7493 1068 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1069 motor[TIRE_FR].pwm = 50;
M_souta 27:dd6c9feb7493 1070 } else if(linePara[LINE_TOW_2] > 0) {
M_souta 27:dd6c9feb7493 1071 motor[TIRE_FL].dir = BACK;
M_souta 27:dd6c9feb7493 1072 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 1073 motor[TIRE_BL].dir = FOR;
M_souta 27:dd6c9feb7493 1074 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 1075 motor[TIRE_BR].dir = FOR;
M_souta 27:dd6c9feb7493 1076 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 1077 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 1078 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 1079 } else if(linePara[LINE_TOW_2] < 0) {
M_souta 27:dd6c9feb7493 1080 motor[TIRE_FL].dir = FOR;
M_souta 27:dd6c9feb7493 1081 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 1082 motor[TIRE_BL].dir = BACK;
M_souta 27:dd6c9feb7493 1083 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 1084 motor[TIRE_BR].dir = BACK;
M_souta 27:dd6c9feb7493 1085 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 1086 motor[TIRE_FR].dir = FOR;
M_souta 27:dd6c9feb7493 1087 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 1088 } else if(linePara[LINE_TOW_2] == 0) {
M_souta 27:dd6c9feb7493 1089 lineFase = 8;
M_souta 27:dd6c9feb7493 1090 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1091 motor[TIRE_FL].pwm = 50;
M_souta 27:dd6c9feb7493 1092 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1093 motor[TIRE_BL].pwm = 50;
M_souta 27:dd6c9feb7493 1094 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1095 motor[TIRE_BR].pwm = 50;
M_souta 27:dd6c9feb7493 1096 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1097 motor[TIRE_FR].pwm = 50;
M_souta 27:dd6c9feb7493 1098 } else {
M_souta 27:dd6c9feb7493 1099 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1100 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1101 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1102 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1103 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1104 motor[TIRE_BR].pwm = 50;
M_souta 27:dd6c9feb7493 1105 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1106 motor[TIRE_FR].pwm = 50;
M_souta 27:dd6c9feb7493 1107 }
M_souta 27:dd6c9feb7493 1108 } else if(lineFase == 8) { // タオル1 解放
M_souta 27:dd6c9feb7493 1109 Air[0] = SOLENOID_ON;
M_souta 27:dd6c9feb7493 1110 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1111 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1112 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1113 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1114 lineFase = 9;
M_souta 27:dd6c9feb7493 1115 } else if(lineFase == 9) { // 前 ライントレース
M_souta 27:dd6c9feb7493 1116 if(LimitSw::IsPressed(QF_SW)) {
M_souta 27:dd6c9feb7493 1117 switch(linePara[LINE_TOW_2]) {
M_souta 27:dd6c9feb7493 1118 case -2:
M_souta 27:dd6c9feb7493 1119 tirePWM[TIRE_FL] = 10;
M_souta 27:dd6c9feb7493 1120 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1121 tirePWM[TIRE_BR] = -10;
M_souta 27:dd6c9feb7493 1122 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1123 adjAnable = true;
M_souta 27:dd6c9feb7493 1124 break;
M_souta 27:dd6c9feb7493 1125 case -3:
M_souta 27:dd6c9feb7493 1126 tirePWM[TIRE_FL] = 14;
M_souta 27:dd6c9feb7493 1127 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1128 tirePWM[TIRE_BR] = -14;
M_souta 27:dd6c9feb7493 1129 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1130 adjAnable = true;
M_souta 27:dd6c9feb7493 1131 break;
M_souta 27:dd6c9feb7493 1132 case -1:
M_souta 27:dd6c9feb7493 1133 tirePWM[TIRE_FL] = 17;
M_souta 27:dd6c9feb7493 1134 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1135 tirePWM[TIRE_BR] = -17;
M_souta 27:dd6c9feb7493 1136 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1137 adjAnable = true;
M_souta 27:dd6c9feb7493 1138 break;
M_souta 27:dd6c9feb7493 1139 case 0:
M_souta 27:dd6c9feb7493 1140 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1141 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1142 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1143 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1144 adjAnable = true;
M_souta 27:dd6c9feb7493 1145 break;
M_souta 27:dd6c9feb7493 1146 case 1:
M_souta 27:dd6c9feb7493 1147 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1148 tirePWM[TIRE_BL] = 17;
M_souta 27:dd6c9feb7493 1149 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1150 tirePWM[TIRE_FR] = -17;
M_souta 27:dd6c9feb7493 1151 adjAnable = true;
M_souta 27:dd6c9feb7493 1152 break;
M_souta 27:dd6c9feb7493 1153 case 3:
M_souta 27:dd6c9feb7493 1154 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1155 tirePWM[TIRE_BL] = 14;
M_souta 27:dd6c9feb7493 1156 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1157 tirePWM[TIRE_FR] = -14;
M_souta 27:dd6c9feb7493 1158 adjAnable = true;
M_souta 27:dd6c9feb7493 1159 break;
M_souta 27:dd6c9feb7493 1160 case 2:
M_souta 27:dd6c9feb7493 1161 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1162 tirePWM[TIRE_BL] = 10;
M_souta 27:dd6c9feb7493 1163 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1164 tirePWM[TIRE_FR] = -10;
M_souta 27:dd6c9feb7493 1165 adjAnable = true;
M_souta 27:dd6c9feb7493 1166 break;
M_souta 27:dd6c9feb7493 1167 case 'A':
M_souta 27:dd6c9feb7493 1168 if(lineCheck == false) {
M_souta 27:dd6c9feb7493 1169 lineCheck = true;
M_souta 27:dd6c9feb7493 1170 lineCount = 0;
M_souta 27:dd6c9feb7493 1171 countW++;
M_souta 27:dd6c9feb7493 1172 }
M_souta 27:dd6c9feb7493 1173 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1174 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1175 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1176 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1177 adjAnable = true;
M_souta 27:dd6c9feb7493 1178 break;
M_souta 27:dd6c9feb7493 1179 case 'N':
M_souta 27:dd6c9feb7493 1180 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1181 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1182 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1183 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1184 adjAnable = false;
M_souta 27:dd6c9feb7493 1185 break;
M_souta 27:dd6c9feb7493 1186 default:
M_souta 27:dd6c9feb7493 1187 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1188 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1189 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1190 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1191 adjAnable = false;
M_souta 27:dd6c9feb7493 1192 }
M_souta 25:b3a9f34b201d 1193 } else {
M_souta 27:dd6c9feb7493 1194 switch(linePara[LINE_TOW_2]) {
M_souta 27:dd6c9feb7493 1195 case 2:
M_souta 27:dd6c9feb7493 1196 tirePWM[TIRE_FL] = 10;
M_souta 27:dd6c9feb7493 1197 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1198 tirePWM[TIRE_BR] = -10;
M_souta 27:dd6c9feb7493 1199 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1200 adjAnable = true;
M_souta 27:dd6c9feb7493 1201 break;
M_souta 27:dd6c9feb7493 1202 case 3:
M_souta 27:dd6c9feb7493 1203 tirePWM[TIRE_FL] = 14;
M_souta 27:dd6c9feb7493 1204 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1205 tirePWM[TIRE_BR] = -14;
M_souta 27:dd6c9feb7493 1206 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1207 adjAnable = true;
M_souta 27:dd6c9feb7493 1208 break;
M_souta 27:dd6c9feb7493 1209 case 1:
M_souta 27:dd6c9feb7493 1210 tirePWM[TIRE_FL] = 17;
M_souta 27:dd6c9feb7493 1211 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1212 tirePWM[TIRE_BR] = -17;
M_souta 27:dd6c9feb7493 1213 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1214 adjAnable = true;
M_souta 27:dd6c9feb7493 1215 break;
M_souta 27:dd6c9feb7493 1216 case 0:
M_souta 27:dd6c9feb7493 1217 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1218 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1219 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1220 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1221 adjAnable = true;
M_souta 27:dd6c9feb7493 1222 break;
M_souta 27:dd6c9feb7493 1223 case -1:
M_souta 27:dd6c9feb7493 1224 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1225 tirePWM[TIRE_BL] = 17;
M_souta 27:dd6c9feb7493 1226 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1227 tirePWM[TIRE_FR] = -17;
M_souta 27:dd6c9feb7493 1228 adjAnable = true;
M_souta 27:dd6c9feb7493 1229 break;
M_souta 27:dd6c9feb7493 1230 case -3:
M_souta 27:dd6c9feb7493 1231 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1232 tirePWM[TIRE_BL] = 14;
M_souta 27:dd6c9feb7493 1233 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1234 tirePWM[TIRE_FR] = -14;
M_souta 27:dd6c9feb7493 1235 adjAnable = true;
M_souta 27:dd6c9feb7493 1236 break;
M_souta 27:dd6c9feb7493 1237 case -2:
M_souta 27:dd6c9feb7493 1238 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1239 tirePWM[TIRE_BL] = 10;
M_souta 27:dd6c9feb7493 1240 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1241 tirePWM[TIRE_FR] = -10;
M_souta 27:dd6c9feb7493 1242 adjAnable = true;
M_souta 27:dd6c9feb7493 1243 break;
M_souta 27:dd6c9feb7493 1244 case 'A':
M_souta 27:dd6c9feb7493 1245 if(lineCheck == false) {
M_souta 27:dd6c9feb7493 1246 lineCheck = true;
M_souta 27:dd6c9feb7493 1247 lineCount = 0;
M_souta 27:dd6c9feb7493 1248 countW++;
M_souta 27:dd6c9feb7493 1249 }
M_souta 27:dd6c9feb7493 1250 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1251 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1252 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1253 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1254 adjAnable = true;
M_souta 27:dd6c9feb7493 1255 break;
M_souta 27:dd6c9feb7493 1256 case 'N':
M_souta 27:dd6c9feb7493 1257 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1258 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1259 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1260 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1261 adjAnable = false;
M_souta 27:dd6c9feb7493 1262 break;
M_souta 27:dd6c9feb7493 1263 default:
M_souta 27:dd6c9feb7493 1264 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1265 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1266 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1267 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1268 adjAnable = false;
M_souta 27:dd6c9feb7493 1269 }
M_souta 27:dd6c9feb7493 1270 }
M_souta 27:dd6c9feb7493 1271
M_souta 27:dd6c9feb7493 1272 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1273 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1274 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1275 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1276 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1277 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1278 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1279 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1280
M_souta 27:dd6c9feb7493 1281 if(lineCheck == true) {
M_souta 27:dd6c9feb7493 1282 lineCount++;
M_souta 27:dd6c9feb7493 1283 if(lineCount > 20) lineCheck = false;
M_souta 27:dd6c9feb7493 1284 }
M_souta 27:dd6c9feb7493 1285 if(countW == 1) {
M_souta 27:dd6c9feb7493 1286 countW = 0;
M_souta 27:dd6c9feb7493 1287 lineFase = 10;
M_souta 27:dd6c9feb7493 1288 lineCount = 0;
M_souta 27:dd6c9feb7493 1289 lineCheck = false;
M_souta 27:dd6c9feb7493 1290 }
M_souta 27:dd6c9feb7493 1291 } else if(lineFase == 10) { // 前 低速
M_souta 27:dd6c9feb7493 1292 motor[TIRE_FL].dir = FOR;
M_souta 27:dd6c9feb7493 1293 motor[TIRE_BL].dir = FOR;
M_souta 27:dd6c9feb7493 1294 motor[TIRE_BR].dir = BACK;
M_souta 27:dd6c9feb7493 1295 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 1296 if(linePara[4] == 0) {
M_souta 27:dd6c9feb7493 1297 lineFase = 11;
M_souta 27:dd6c9feb7493 1298 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1299 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1300 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1301 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1302 }
M_souta 27:dd6c9feb7493 1303 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 1304 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 1305 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 1306 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 1307 } else if(lineFase == 11) {
M_souta 27:dd6c9feb7493 1308 switch(linePara[4]) { // 右 ライントレース
M_souta 27:dd6c9feb7493 1309 case -2:
M_souta 27:dd6c9feb7493 1310 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1311 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1312 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1313 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1314 adjAnable = true;
M_souta 27:dd6c9feb7493 1315 break;
M_souta 27:dd6c9feb7493 1316 case -3:
M_souta 27:dd6c9feb7493 1317 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1318 tirePWM[TIRE_BL] = -10;
M_souta 27:dd6c9feb7493 1319 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1320 tirePWM[TIRE_FR] = 10;
M_souta 27:dd6c9feb7493 1321 adjAnable = true;
M_souta 27:dd6c9feb7493 1322 break;
M_souta 27:dd6c9feb7493 1323 case -1:
M_souta 27:dd6c9feb7493 1324 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1325 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1326 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1327 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1328 adjAnable = true;
M_souta 27:dd6c9feb7493 1329 break;
M_souta 27:dd6c9feb7493 1330 case 0:
M_souta 27:dd6c9feb7493 1331 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1332 tirePWM[TIRE_BL] = -30;
M_souta 27:dd6c9feb7493 1333 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1334 tirePWM[TIRE_FR] = 30;
M_souta 27:dd6c9feb7493 1335 adjAnable = true;
M_souta 27:dd6c9feb7493 1336 break;
M_souta 27:dd6c9feb7493 1337 case 1:
M_souta 27:dd6c9feb7493 1338 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1339 tirePWM[TIRE_BL] = -30;
M_souta 27:dd6c9feb7493 1340 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1341 tirePWM[TIRE_FR] = 30;
M_souta 27:dd6c9feb7493 1342 adjAnable = true;
M_souta 27:dd6c9feb7493 1343 break;
M_souta 27:dd6c9feb7493 1344 case 3:
M_souta 27:dd6c9feb7493 1345 tirePWM[TIRE_FL] = 10;
M_souta 27:dd6c9feb7493 1346 tirePWM[TIRE_BL] = -30;
M_souta 27:dd6c9feb7493 1347 tirePWM[TIRE_BR] = -10;
M_souta 27:dd6c9feb7493 1348 tirePWM[TIRE_FR] = 30;
M_souta 27:dd6c9feb7493 1349 adjAnable = true;
M_souta 27:dd6c9feb7493 1350 break;
M_souta 27:dd6c9feb7493 1351 case 2:
M_souta 27:dd6c9feb7493 1352 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1353 tirePWM[TIRE_BL] = -30;
M_souta 27:dd6c9feb7493 1354 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1355 tirePWM[TIRE_FR] = 30;
M_souta 27:dd6c9feb7493 1356 adjAnable = true;
M_souta 27:dd6c9feb7493 1357 break;
M_souta 27:dd6c9feb7493 1358 case 'A':
M_souta 27:dd6c9feb7493 1359 if(lineCheck == false) {
M_souta 27:dd6c9feb7493 1360 lineCheck = true;
M_souta 27:dd6c9feb7493 1361 lineCount = 0;
M_souta 27:dd6c9feb7493 1362 countW++;
M_souta 27:dd6c9feb7493 1363 }
M_souta 27:dd6c9feb7493 1364 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1365 tirePWM[TIRE_BL] = -30;
M_souta 27:dd6c9feb7493 1366 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1367 tirePWM[TIRE_FR] = 30;
M_souta 27:dd6c9feb7493 1368 adjAnable = true;
M_souta 27:dd6c9feb7493 1369 break;
M_souta 27:dd6c9feb7493 1370 case 'N':
M_souta 27:dd6c9feb7493 1371 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1372 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1373 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1374 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1375 adjAnable = false;
M_souta 27:dd6c9feb7493 1376 break;
M_souta 27:dd6c9feb7493 1377 default:
M_souta 27:dd6c9feb7493 1378 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1379 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1380 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1381 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1382 adjAnable = false;
M_souta 27:dd6c9feb7493 1383 }
M_souta 27:dd6c9feb7493 1384
M_souta 27:dd6c9feb7493 1385 if(adjAnable){
M_souta 27:dd6c9feb7493 1386 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta 27:dd6c9feb7493 1387 } else {
M_souta 27:dd6c9feb7493 1388 adj = 0;
M_souta 27:dd6c9feb7493 1389 }
M_souta 27:dd6c9feb7493 1390
M_souta 27:dd6c9feb7493 1391 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 27:dd6c9feb7493 1392 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 27:dd6c9feb7493 1393 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1394 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1395 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 27:dd6c9feb7493 1396 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 27:dd6c9feb7493 1397 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1398 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1399
M_souta 27:dd6c9feb7493 1400 if(lineCheck == true) {
M_souta 27:dd6c9feb7493 1401 lineCount++;
M_souta 27:dd6c9feb7493 1402 if(lineCount > 20) lineCheck = false;
M_souta 27:dd6c9feb7493 1403 }
M_souta 27:dd6c9feb7493 1404 if(countW == 2) {
M_souta 27:dd6c9feb7493 1405 countW = 0;
M_souta 27:dd6c9feb7493 1406 lineFase = 12;
M_souta 27:dd6c9feb7493 1407 lineCount = 0;
M_souta 27:dd6c9feb7493 1408 lineCheck = false;
M_souta 27:dd6c9feb7493 1409 }
M_souta 27:dd6c9feb7493 1410 } else if(lineFase == 12) { // 右 低速
M_souta 27:dd6c9feb7493 1411 motor[TIRE_FL].dir = FOR;
M_souta 27:dd6c9feb7493 1412 motor[TIRE_BL].dir = BACK;
M_souta 27:dd6c9feb7493 1413 motor[TIRE_BR].dir = BACK;
M_souta 27:dd6c9feb7493 1414 motor[TIRE_FR].dir = FOR;
M_souta 27:dd6c9feb7493 1415 if(linePara[LINE_TOW_1] == 0) {
M_souta 27:dd6c9feb7493 1416 lineFase = 13;
M_souta 27:dd6c9feb7493 1417 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1418 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1419 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1420 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1421 }
M_souta 27:dd6c9feb7493 1422 motor[TIRE_FL].pwm = 16;
M_souta 27:dd6c9feb7493 1423 motor[TIRE_BL].pwm = 16;
M_souta 27:dd6c9feb7493 1424 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 1425 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 1426 } else if (lineFase == 13) {
M_souta 27:dd6c9feb7493 1427 lineFase = 14;
M_souta 27:dd6c9feb7493 1428 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1429 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1430 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1431 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1432 } else if(lineFase == 14) { // タオル2 竿検知
M_souta 27:dd6c9feb7493 1433 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta 27:dd6c9feb7493 1434 lineFase = 15;
M_souta 27:dd6c9feb7493 1435 motor[TIRE_FL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1436 motor[TIRE_FL].pwm = 50;
M_souta 27:dd6c9feb7493 1437 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 1438 motor[TIRE_BL].pwm = 50;
M_souta 27:dd6c9feb7493 1439 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1440 motor[TIRE_BR].pwm = 50;
M_souta 27:dd6c9feb7493 1441 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1442 motor[TIRE_FR].pwm = 50;
M_souta 27:dd6c9feb7493 1443 } else if(LimitSw::IsPressed(TOW_2L)) {
M_souta 27:dd6c9feb7493 1444 motor[TIRE_FL].dir = FOR;
M_souta 27:dd6c9feb7493 1445 motor[TIRE_FL].pwm = 20;
M_souta 27:dd6c9feb7493 1446 motor[TIRE_BL].dir = FOR;
M_souta 27:dd6c9feb7493 1447 motor[TIRE_BL].pwm = 20;
M_souta 27:dd6c9feb7493 1448 motor[TIRE_BR].dir = FOR;
M_souta 27:dd6c9feb7493 1449 motor[TIRE_BR].pwm = 20;
M_souta 27:dd6c9feb7493 1450 motor[TIRE_FR].dir = FOR;
M_souta 27:dd6c9feb7493 1451 motor[TIRE_FR].pwm = 20;
M_souta 27:dd6c9feb7493 1452 } else if(LimitSw::IsPressed(TOW_2R)) {
M_souta 25:b3a9f34b201d 1453 motor[TIRE_FL].dir = BACK;
M_souta 27:dd6c9feb7493 1454 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 1455 motor[TIRE_BL].dir = BACK;
M_souta 27:dd6c9feb7493 1456 motor[TIRE_BL].pwm = 20;
M_souta 27:dd6c9feb7493 1457 motor[TIRE_BR].dir = BACK;
M_souta 27:dd6c9feb7493 1458 motor[TIRE_BR].pwm = 20;
M_souta 27:dd6c9feb7493 1459 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 1460 motor[TIRE_FR].pwm = 20;
M_souta 27:dd6c9feb7493 1461 } else {
M_souta 27:dd6c9feb7493 1462 if(LimitSw::IsPressed(QF_SW)) {
M_souta 27:dd6c9feb7493 1463 switch(linePara[LINE_TOW_1]) {
M_souta 27:dd6c9feb7493 1464 case -2:
M_souta 27:dd6c9feb7493 1465 tirePWM[TIRE_FL] = -10;
M_souta 27:dd6c9feb7493 1466 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1467 tirePWM[TIRE_BR] = 10;
M_souta 27:dd6c9feb7493 1468 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1469 adjAnable = true;
M_souta 27:dd6c9feb7493 1470 break;
M_souta 27:dd6c9feb7493 1471 case -3:
M_souta 27:dd6c9feb7493 1472 tirePWM[TIRE_FL] = -14;
M_souta 27:dd6c9feb7493 1473 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1474 tirePWM[TIRE_BR] = 14;
M_souta 27:dd6c9feb7493 1475 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1476 adjAnable = true;
M_souta 27:dd6c9feb7493 1477 break;
M_souta 27:dd6c9feb7493 1478 case -1:
M_souta 27:dd6c9feb7493 1479 tirePWM[TIRE_FL] = -17;
M_souta 27:dd6c9feb7493 1480 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1481 tirePWM[TIRE_BR] = 17;
M_souta 27:dd6c9feb7493 1482 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1483 adjAnable = true;
M_souta 27:dd6c9feb7493 1484 break;
M_souta 27:dd6c9feb7493 1485 case 0:
M_souta 27:dd6c9feb7493 1486 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1487 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1488 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1489 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1490 adjAnable = true;
M_souta 27:dd6c9feb7493 1491 break;
M_souta 27:dd6c9feb7493 1492 case 1:
M_souta 27:dd6c9feb7493 1493 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1494 tirePWM[TIRE_BL] = -17;
M_souta 27:dd6c9feb7493 1495 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1496 tirePWM[TIRE_FR] = 17;
M_souta 27:dd6c9feb7493 1497 adjAnable = true;
M_souta 27:dd6c9feb7493 1498 break;
M_souta 27:dd6c9feb7493 1499 case 3:
M_souta 27:dd6c9feb7493 1500 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1501 tirePWM[TIRE_BL] = -14;
M_souta 27:dd6c9feb7493 1502 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1503 tirePWM[TIRE_FR] = 14;
M_souta 27:dd6c9feb7493 1504 adjAnable = true;
M_souta 27:dd6c9feb7493 1505 break;
M_souta 27:dd6c9feb7493 1506 case 2:
M_souta 27:dd6c9feb7493 1507 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1508 tirePWM[TIRE_BL] = -10;
M_souta 27:dd6c9feb7493 1509 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1510 tirePWM[TIRE_FR] = 10;
M_souta 27:dd6c9feb7493 1511 adjAnable = true;
M_souta 27:dd6c9feb7493 1512 break;
M_souta 27:dd6c9feb7493 1513 case 'A':
M_souta 27:dd6c9feb7493 1514 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1515 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1516 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1517 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1518 adjAnable = true;
M_souta 27:dd6c9feb7493 1519 break;
M_souta 27:dd6c9feb7493 1520 case 'N':
M_souta 27:dd6c9feb7493 1521 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 1522 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 1523 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 1524 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 27:dd6c9feb7493 1525 adjAnable = false;
M_souta 27:dd6c9feb7493 1526 break;
M_souta 27:dd6c9feb7493 1527 default:
M_souta 27:dd6c9feb7493 1528 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1529 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1530 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1531 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1532 adjAnable = false;
M_souta 27:dd6c9feb7493 1533 }
M_souta 27:dd6c9feb7493 1534 } else {
M_souta 27:dd6c9feb7493 1535 switch(linePara[LINE_TOW_1]) {
M_souta 27:dd6c9feb7493 1536 case 2:
M_souta 27:dd6c9feb7493 1537 tirePWM[TIRE_FL] = -10;
M_souta 27:dd6c9feb7493 1538 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1539 tirePWM[TIRE_BR] = 10;
M_souta 27:dd6c9feb7493 1540 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1541 adjAnable = true;
M_souta 27:dd6c9feb7493 1542 break;
M_souta 27:dd6c9feb7493 1543 case 3:
M_souta 27:dd6c9feb7493 1544 tirePWM[TIRE_FL] = -14;
M_souta 27:dd6c9feb7493 1545 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1546 tirePWM[TIRE_BR] = 14;
M_souta 27:dd6c9feb7493 1547 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1548 adjAnable = true;
M_souta 27:dd6c9feb7493 1549 break;
M_souta 27:dd6c9feb7493 1550 case 1:
M_souta 27:dd6c9feb7493 1551 tirePWM[TIRE_FL] = -17;
M_souta 27:dd6c9feb7493 1552 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1553 tirePWM[TIRE_BR] = 17;
M_souta 27:dd6c9feb7493 1554 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1555 adjAnable = true;
M_souta 27:dd6c9feb7493 1556 break;
M_souta 27:dd6c9feb7493 1557 case 0:
M_souta 27:dd6c9feb7493 1558 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1559 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1560 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1561 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1562 adjAnable = true;
M_souta 27:dd6c9feb7493 1563 break;
M_souta 27:dd6c9feb7493 1564 case -1:
M_souta 27:dd6c9feb7493 1565 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1566 tirePWM[TIRE_BL] = -17;
M_souta 27:dd6c9feb7493 1567 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1568 tirePWM[TIRE_FR] = 17;
M_souta 27:dd6c9feb7493 1569 adjAnable = true;
M_souta 27:dd6c9feb7493 1570 break;
M_souta 27:dd6c9feb7493 1571 case -3:
M_souta 27:dd6c9feb7493 1572 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1573 tirePWM[TIRE_BL] = -14;
M_souta 27:dd6c9feb7493 1574 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1575 tirePWM[TIRE_FR] = 14;
M_souta 27:dd6c9feb7493 1576 adjAnable = true;
M_souta 27:dd6c9feb7493 1577 break;
M_souta 27:dd6c9feb7493 1578 case -2:
M_souta 27:dd6c9feb7493 1579 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1580 tirePWM[TIRE_BL] = -10;
M_souta 27:dd6c9feb7493 1581 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1582 tirePWM[TIRE_FR] = 10;
M_souta 27:dd6c9feb7493 1583 adjAnable = true;
M_souta 27:dd6c9feb7493 1584 break;
M_souta 27:dd6c9feb7493 1585 case 'A':
M_souta 27:dd6c9feb7493 1586 tirePWM[TIRE_FL] = -20;
M_souta 27:dd6c9feb7493 1587 tirePWM[TIRE_BL] = -20;
M_souta 27:dd6c9feb7493 1588 tirePWM[TIRE_BR] = 20;
M_souta 27:dd6c9feb7493 1589 tirePWM[TIRE_FR] = 20;
M_souta 27:dd6c9feb7493 1590 adjAnable = true;
M_souta 27:dd6c9feb7493 1591 break;
M_souta 27:dd6c9feb7493 1592 case 'N':
M_souta 27:dd6c9feb7493 1593 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 1594 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 1595 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 1596 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 27:dd6c9feb7493 1597 adjAnable = false;
M_souta 27:dd6c9feb7493 1598 break;
M_souta 27:dd6c9feb7493 1599 default:
M_souta 27:dd6c9feb7493 1600 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1601 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1602 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1603 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1604 adjAnable = false;
M_souta 27:dd6c9feb7493 1605 }
M_souta 27:dd6c9feb7493 1606 }
M_souta 27:dd6c9feb7493 1607
M_souta 27:dd6c9feb7493 1608 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1609 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1610 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1611 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 27:dd6c9feb7493 1612 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 27:dd6c9feb7493 1613 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 27:dd6c9feb7493 1614 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 27:dd6c9feb7493 1615 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1616 }
M_souta 27:dd6c9feb7493 1617 } else if(lineFase == 15 ){ // ライン 修正
M_souta 25:b3a9f34b201d 1618 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 1619 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1620 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1621 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1622 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1623 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1624 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1625 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1626 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1627 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 1628 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1629 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1630 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1631 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1632 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1633 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1634 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1635 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1636 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 1637 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1638 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1639 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1640 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1641 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1642 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1643 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1644 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1645 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 27:dd6c9feb7493 1646 lineFase = 16;
M_souta 25:b3a9f34b201d 1647 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1648 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1649 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1650 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1651 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1652 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1653 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1654 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1655 } else {
M_souta 25:b3a9f34b201d 1656 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1657 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1658 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1659 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1660 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1661 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1662 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1663 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1664 }
M_souta 27:dd6c9feb7493 1665 } else if(lineFase == 16) { // タオル2 解放
M_souta 27:dd6c9feb7493 1666 Air[1] = SOLENOID_ON;
M_souta 25:b3a9f34b201d 1667 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1668 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1669 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1670 motor[TIRE_FR].dir = BRAKE;
M_souta 27:dd6c9feb7493 1671 lineFase = 17;
M_souta 27:dd6c9feb7493 1672 } else if(lineFase == 17) { // 前 ライントレース
M_souta 27:dd6c9feb7493 1673 if(LimitSw::IsPressed(QF_SW)) {
M_souta 27:dd6c9feb7493 1674 switch(linePara[LINE_TOW_1]) {
M_souta 27:dd6c9feb7493 1675 case -2:
M_souta 27:dd6c9feb7493 1676 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1677 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1678 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1679 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1680 adjAnable = true;
M_souta 27:dd6c9feb7493 1681 break;
M_souta 27:dd6c9feb7493 1682 case -3:
M_souta 27:dd6c9feb7493 1683 tirePWM[TIRE_FL] = 10;
M_souta 27:dd6c9feb7493 1684 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1685 tirePWM[TIRE_BR] = -10;
M_souta 27:dd6c9feb7493 1686 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1687 adjAnable = true;
M_souta 27:dd6c9feb7493 1688 break;
M_souta 27:dd6c9feb7493 1689 case -1:
M_souta 27:dd6c9feb7493 1690 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1691 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1692 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1693 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1694 adjAnable = true;
M_souta 27:dd6c9feb7493 1695 break;
M_souta 27:dd6c9feb7493 1696 case 0:
M_souta 27:dd6c9feb7493 1697 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1698 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1699 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1700 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1701 adjAnable = true;
M_souta 27:dd6c9feb7493 1702 break;
M_souta 27:dd6c9feb7493 1703 case 1:
M_souta 27:dd6c9feb7493 1704 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1705 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1706 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1707 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1708 adjAnable = true;
M_souta 27:dd6c9feb7493 1709 break;
M_souta 27:dd6c9feb7493 1710 case 3:
M_souta 27:dd6c9feb7493 1711 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1712 tirePWM[TIRE_BL] = 10;
M_souta 27:dd6c9feb7493 1713 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1714 tirePWM[TIRE_FR] = -10;
M_souta 27:dd6c9feb7493 1715 adjAnable = true;
M_souta 27:dd6c9feb7493 1716 break;
M_souta 27:dd6c9feb7493 1717 case 2:
M_souta 27:dd6c9feb7493 1718 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1719 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1720 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1721 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1722 adjAnable = true;
M_souta 27:dd6c9feb7493 1723 break;
M_souta 27:dd6c9feb7493 1724 case 'A':
M_souta 27:dd6c9feb7493 1725 if(lineCheck == false) {
M_souta 27:dd6c9feb7493 1726 lineCheck = true;
M_souta 27:dd6c9feb7493 1727 lineCount = 0;
M_souta 27:dd6c9feb7493 1728 countW++;
M_souta 27:dd6c9feb7493 1729 }
M_souta 27:dd6c9feb7493 1730 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1731 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1732 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1733 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1734 adjAnable = true;
M_souta 27:dd6c9feb7493 1735 break;
M_souta 27:dd6c9feb7493 1736 case 'N':
M_souta 27:dd6c9feb7493 1737 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1738 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1739 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1740 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1741 adjAnable = false;
M_souta 27:dd6c9feb7493 1742 break;
M_souta 27:dd6c9feb7493 1743 default:
M_souta 27:dd6c9feb7493 1744 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1745 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1746 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1747 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1748 adjAnable = false;
M_souta 27:dd6c9feb7493 1749 }
M_souta 27:dd6c9feb7493 1750 } else {
M_souta 27:dd6c9feb7493 1751 switch(linePara[LINE_TOW_1]) {
M_souta 27:dd6c9feb7493 1752 case 2:
M_souta 27:dd6c9feb7493 1753 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1754 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1755 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1756 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1757 adjAnable = true;
M_souta 27:dd6c9feb7493 1758 break;
M_souta 27:dd6c9feb7493 1759 case 3:
M_souta 27:dd6c9feb7493 1760 tirePWM[TIRE_FL] = 10;
M_souta 27:dd6c9feb7493 1761 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1762 tirePWM[TIRE_BR] = -10;
M_souta 27:dd6c9feb7493 1763 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1764 adjAnable = true;
M_souta 27:dd6c9feb7493 1765 break;
M_souta 27:dd6c9feb7493 1766 case 1:
M_souta 27:dd6c9feb7493 1767 tirePWM[TIRE_FL] = 20;
M_souta 27:dd6c9feb7493 1768 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1769 tirePWM[TIRE_BR] = -20;
M_souta 27:dd6c9feb7493 1770 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1771 adjAnable = true;
M_souta 27:dd6c9feb7493 1772 break;
M_souta 27:dd6c9feb7493 1773 case 0:
M_souta 27:dd6c9feb7493 1774 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1775 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1776 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1777 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1778 adjAnable = true;
M_souta 27:dd6c9feb7493 1779 break;
M_souta 27:dd6c9feb7493 1780 case -1:
M_souta 27:dd6c9feb7493 1781 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1782 tirePWM[TIRE_BL] = 20;
M_souta 27:dd6c9feb7493 1783 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1784 tirePWM[TIRE_FR] = -20;
M_souta 27:dd6c9feb7493 1785 adjAnable = true;
M_souta 27:dd6c9feb7493 1786 break;
M_souta 27:dd6c9feb7493 1787 case -3:
M_souta 27:dd6c9feb7493 1788 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1789 tirePWM[TIRE_BL] = 10;
M_souta 27:dd6c9feb7493 1790 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1791 tirePWM[TIRE_FR] = -10;
M_souta 27:dd6c9feb7493 1792 adjAnable = true;
M_souta 27:dd6c9feb7493 1793 break;
M_souta 27:dd6c9feb7493 1794 case -2:
M_souta 27:dd6c9feb7493 1795 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1796 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1797 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1798 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1799 adjAnable = true;
M_souta 27:dd6c9feb7493 1800 break;
M_souta 27:dd6c9feb7493 1801 case 'A':
M_souta 27:dd6c9feb7493 1802 if(lineCheck == false) {
M_souta 27:dd6c9feb7493 1803 lineCheck = true;
M_souta 27:dd6c9feb7493 1804 lineCount = 0;
M_souta 27:dd6c9feb7493 1805 countW++;
M_souta 27:dd6c9feb7493 1806 }
M_souta 27:dd6c9feb7493 1807 tirePWM[TIRE_FL] = 30;
M_souta 27:dd6c9feb7493 1808 tirePWM[TIRE_BL] = 30;
M_souta 27:dd6c9feb7493 1809 tirePWM[TIRE_BR] = -30;
M_souta 27:dd6c9feb7493 1810 tirePWM[TIRE_FR] = -30;
M_souta 27:dd6c9feb7493 1811 adjAnable = true;
M_souta 27:dd6c9feb7493 1812 break;
M_souta 27:dd6c9feb7493 1813 case 'N':
M_souta 27:dd6c9feb7493 1814 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1815 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1816 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1817 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1818 adjAnable = false;
M_souta 27:dd6c9feb7493 1819 break;
M_souta 27:dd6c9feb7493 1820 default:
M_souta 27:dd6c9feb7493 1821 tirePWM[TIRE_FL] = 0;
M_souta 27:dd6c9feb7493 1822 tirePWM[TIRE_BL] = 0;
M_souta 27:dd6c9feb7493 1823 tirePWM[TIRE_BR] = 0;
M_souta 27:dd6c9feb7493 1824 tirePWM[TIRE_FR] = 0;
M_souta 27:dd6c9feb7493 1825 adjAnable = false;
M_souta 27:dd6c9feb7493 1826 }
M_souta 25:b3a9f34b201d 1827 }
M_souta 25:b3a9f34b201d 1828
M_souta 25:b3a9f34b201d 1829 if(adjAnable){
M_souta 25:b3a9f34b201d 1830 adj = 0;
M_souta 25:b3a9f34b201d 1831 } else {
M_souta 25:b3a9f34b201d 1832 adj = 0;
M_souta 25:b3a9f34b201d 1833 }
M_souta 25:b3a9f34b201d 1834
M_souta 25:b3a9f34b201d 1835 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1836 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1837 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1838 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1839 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1840 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1841 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1842 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1843
M_souta 25:b3a9f34b201d 1844 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1845 lineCount++;
M_souta 25:b3a9f34b201d 1846 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1847 }
M_souta 26:4c0ce2f05688 1848 if(countW == 1) {
M_souta 26:4c0ce2f05688 1849 countW = 0;
M_souta 27:dd6c9feb7493 1850 lineFase = 18;
M_souta 25:b3a9f34b201d 1851 lineCount = 0;
M_souta 25:b3a9f34b201d 1852 lineCheck = false;
M_souta 25:b3a9f34b201d 1853 }
M_souta 27:dd6c9feb7493 1854 } else if(lineFase == 18) { // 前 低速
M_souta 25:b3a9f34b201d 1855 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1856 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1857 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1858 motor[TIRE_FR].dir = BACK;
M_souta 27:dd6c9feb7493 1859 if(linePara[4] != 'N') {
M_souta 27:dd6c9feb7493 1860 lineFase = 19;
M_souta 27:dd6c9feb7493 1861 Air[0] = SOLENOID_OFF;
M_souta 27:dd6c9feb7493 1862 Air[1] = SOLENOID_OFF;
M_souta 25:b3a9f34b201d 1863 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1864 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1865 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1866 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1867 }
M_souta 25:b3a9f34b201d 1868 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1869 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1870 motor[TIRE_BR].pwm = 16;
M_souta 27:dd6c9feb7493 1871 motor[TIRE_FR].pwm = 16;
M_souta 27:dd6c9feb7493 1872 } else if(lineFase == 19) { // 左
M_souta 27:dd6c9feb7493 1873 switch(linePara[3]) {
M_souta 25:b3a9f34b201d 1874 case -2:
M_souta 26:4c0ce2f05688 1875 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1876 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 1877 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1878 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1879 adjAnable = true;
M_souta 22:7d93f79a3686 1880 break;
M_souta 25:b3a9f34b201d 1881 case -3:
M_souta 26:4c0ce2f05688 1882 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 1883 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 1884 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 1885 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 1886 adjAnable = true;
M_souta 25:b3a9f34b201d 1887 break;
M_souta 25:b3a9f34b201d 1888 case -1:
M_souta 26:4c0ce2f05688 1889 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 1890 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 1891 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 1892 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 1893 adjAnable = true;
M_souta 25:b3a9f34b201d 1894 break;
M_souta 25:b3a9f34b201d 1895 case 0:
M_souta 26:4c0ce2f05688 1896 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 1897 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 1898 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 1899 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1900 adjAnable = true;
M_souta 25:b3a9f34b201d 1901 break;
M_souta 25:b3a9f34b201d 1902 case 1:
M_souta 26:4c0ce2f05688 1903 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 1904 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 1905 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 1906 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1907 adjAnable = true;
M_souta 25:b3a9f34b201d 1908 break;
M_souta 25:b3a9f34b201d 1909 case 3:
M_souta 26:4c0ce2f05688 1910 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 1911 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 1912 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 1913 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1914 adjAnable = true;
M_souta 25:b3a9f34b201d 1915 break;
M_souta 25:b3a9f34b201d 1916 case 2:
M_souta 25:b3a9f34b201d 1917 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 1918 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1919 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 1920 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1921 adjAnable = true;
M_souta 25:b3a9f34b201d 1922 break;
M_souta 25:b3a9f34b201d 1923 case 'A':
M_souta 25:b3a9f34b201d 1924 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1925 lineCheck = true;
M_souta 25:b3a9f34b201d 1926 lineCount = 0;
M_souta 26:4c0ce2f05688 1927 countW++;
M_souta 25:b3a9f34b201d 1928 }
M_souta 26:4c0ce2f05688 1929 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 1930 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 1931 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 1932 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1933 adjAnable = true;
M_souta 25:b3a9f34b201d 1934 break;
M_souta 25:b3a9f34b201d 1935 case 'N':
M_souta 25:b3a9f34b201d 1936 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1937 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1938 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1939 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1940 adjAnable = false;
M_souta 25:b3a9f34b201d 1941 break;
M_souta 22:7d93f79a3686 1942 default:
M_souta 25:b3a9f34b201d 1943 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1944 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1945 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1946 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1947 adjAnable = false;
M_souta 22:7d93f79a3686 1948 }
M_souta 25:b3a9f34b201d 1949
M_souta 25:b3a9f34b201d 1950 if(adjAnable){
M_souta 25:b3a9f34b201d 1951 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta 25:b3a9f34b201d 1952 } else {
M_souta 25:b3a9f34b201d 1953 adj = 0;
M_souta 25:b3a9f34b201d 1954 }
M_souta 25:b3a9f34b201d 1955
M_souta 25:b3a9f34b201d 1956 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1957 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1958 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1959 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1960 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1961 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1962 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1963 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1964
M_souta 25:b3a9f34b201d 1965 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1966 lineCount++;
M_souta 25:b3a9f34b201d 1967 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1968 }
M_souta 25:b3a9f34b201d 1969
M_souta 25:b3a9f34b201d 1970 targetCount = 3;
M_souta 25:b3a9f34b201d 1971
M_souta 26:4c0ce2f05688 1972 if(countW == targetCount) {
M_souta 26:4c0ce2f05688 1973 countW = 0;
M_souta 25:b3a9f34b201d 1974 lineFase = 20;
M_souta 25:b3a9f34b201d 1975 lineCount = 0;
M_souta 25:b3a9f34b201d 1976 lineCheck = false;
M_souta 25:b3a9f34b201d 1977 }
M_souta 25:b3a9f34b201d 1978 } else if(lineFase == 20) { // 左 低速
M_souta 25:b3a9f34b201d 1979 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1980 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1981 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1982 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1983 if (linePara[2] == 'N') {
M_souta 25:b3a9f34b201d 1984 lineFase = 21;
M_souta 25:b3a9f34b201d 1985 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1986 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1987 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1988 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1989 }
M_souta 27:dd6c9feb7493 1990 motor[TIRE_FL].pwm = 20;
M_souta 27:dd6c9feb7493 1991 motor[TIRE_BL].pwm = 20;
M_souta 27:dd6c9feb7493 1992 motor[TIRE_BR].pwm = 20;
M_souta 27:dd6c9feb7493 1993 motor[TIRE_FR].pwm = 20;
M_souta 27:dd6c9feb7493 1994 } else if(lineFase == 21) { // 後 ライントレース
M_souta 25:b3a9f34b201d 1995 switch(linePara[2]) {
M_souta 25:b3a9f34b201d 1996 case -2:
M_souta 25:b3a9f34b201d 1997 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1998 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1999 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2000 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2001 adjAnable = true;
M_souta 25:b3a9f34b201d 2002 break;
M_souta 25:b3a9f34b201d 2003 case -3:
M_souta 25:b3a9f34b201d 2004 tirePWM[TIRE_FL] = -10;
M_souta 25:b3a9f34b201d 2005 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 2006 tirePWM[TIRE_BR] = 10;
M_souta 25:b3a9f34b201d 2007 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2008 adjAnable = true;
M_souta 25:b3a9f34b201d 2009 break;
M_souta 25:b3a9f34b201d 2010 case -1:
M_souta 25:b3a9f34b201d 2011 tirePWM[TIRE_FL] = -20;
M_souta 25:b3a9f34b201d 2012 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 2013 tirePWM[TIRE_BR] = 20;
M_souta 25:b3a9f34b201d 2014 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2015 adjAnable = true;
M_souta 25:b3a9f34b201d 2016 break;
M_souta 25:b3a9f34b201d 2017 case 0:
M_souta 25:b3a9f34b201d 2018 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2019 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 2020 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2021 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2022 adjAnable = true;
M_souta 25:b3a9f34b201d 2023 break;
M_souta 25:b3a9f34b201d 2024 case 1:
M_souta 25:b3a9f34b201d 2025 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2026 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 2027 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2028 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 2029 adjAnable = true;
M_souta 25:b3a9f34b201d 2030 break;
M_souta 25:b3a9f34b201d 2031 case 3:
M_souta 25:b3a9f34b201d 2032 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2033 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 2034 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2035 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 2036 adjAnable = true;
M_souta 25:b3a9f34b201d 2037 break;
M_souta 25:b3a9f34b201d 2038 case 2:
M_souta 25:b3a9f34b201d 2039 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2040 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2041 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2042 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2043 adjAnable = true;
M_souta 25:b3a9f34b201d 2044 break;
M_souta 25:b3a9f34b201d 2045 case 'A':
M_souta 25:b3a9f34b201d 2046 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2047 lineCheck = true;
M_souta 25:b3a9f34b201d 2048 lineCount = 0;
M_souta 26:4c0ce2f05688 2049 countW++;
M_souta 25:b3a9f34b201d 2050 }
M_souta 25:b3a9f34b201d 2051 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2052 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 2053 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2054 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2055 adjAnable = true;
M_souta 25:b3a9f34b201d 2056 break;
M_souta 25:b3a9f34b201d 2057 case 'N':
M_souta 25:b3a9f34b201d 2058 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2059 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2060 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2061 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2062 adjAnable = false;
M_souta 25:b3a9f34b201d 2063 break;
M_souta 25:b3a9f34b201d 2064 default:
M_souta 25:b3a9f34b201d 2065 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2066 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2067 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2068 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2069 adjAnable = false;
M_souta 25:b3a9f34b201d 2070 }
M_souta 25:b3a9f34b201d 2071
M_souta 25:b3a9f34b201d 2072 if(adjAnable){
M_souta 25:b3a9f34b201d 2073 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta 25:b3a9f34b201d 2074 } else {
M_souta 25:b3a9f34b201d 2075 adj = 0;
M_souta 25:b3a9f34b201d 2076 }
M_souta 25:b3a9f34b201d 2077
M_souta 25:b3a9f34b201d 2078 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 2079 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 2080 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 2081 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 2082 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 2083 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 2084 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 2085 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 2086
M_souta 25:b3a9f34b201d 2087 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2088 lineCount++;
M_souta 25:b3a9f34b201d 2089 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2090 }
M_souta 26:4c0ce2f05688 2091 if(countW == 3) {
M_souta 26:4c0ce2f05688 2092 countW = 0;
M_souta 25:b3a9f34b201d 2093 lineFase = 22;
M_souta 25:b3a9f34b201d 2094 lineCount = 0;
M_souta 25:b3a9f34b201d 2095 lineCheck = false;
M_souta 25:b3a9f34b201d 2096 }
M_souta 25:b3a9f34b201d 2097 } else if(lineFase == 22) {
M_souta 25:b3a9f34b201d 2098 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 2099 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 2100 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 2101 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2102 if (linePara[2] == 'N') {
M_souta 25:b3a9f34b201d 2103 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2104 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2105 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2106 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2107 }
M_souta 25:b3a9f34b201d 2108 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 2109 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2110 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2111 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2112 } else{
M_souta 25:b3a9f34b201d 2113 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2114 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2115 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2116 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 2117 }
M_souta 22:7d93f79a3686 2118
kishibekairohan 13:b6e02d6261d7 2119 }
kishibekairohan 13:b6e02d6261d7 2120 #endif
kishibekairohan 13:b6e02d6261d7 2121
kishibekairohan 13:b6e02d6261d7 2122 #if USE_PROCESS_NUM>5
kishibekairohan 16:3f2c2d89372b 2123 static void Process5()
kishibekairohan 16:3f2c2d89372b 2124 {
M_souta 25:b3a9f34b201d 2125
M_souta 25:b3a9f34b201d 2126 /* ************************************** //
M_souta 25:b3a9f34b201d 2127
M_souta 25:b3a9f34b201d 2128 青ゾーン 青ゾーン 青ゾーン
M_souta 25:b3a9f34b201d 2129
M_souta 25:b3a9f34b201d 2130 // ************************************** */
M_souta 25:b3a9f34b201d 2131
M_souta 25:b3a9f34b201d 2132 for(int i = 0; i < 8; i++) {
M_souta 25:b3a9f34b201d 2133 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 25:b3a9f34b201d 2134 }
M_souta 25:b3a9f34b201d 2135
M_souta 25:b3a9f34b201d 2136 if(lineFase == 0) {
M_souta 26:4c0ce2f05688 2137 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 2138 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 2139 motor[TIRE_BR].dir = BACK;
M_souta 26:4c0ce2f05688 2140 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2141 if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta 25:b3a9f34b201d 2142 lineFase = 1;
M_souta 25:b3a9f34b201d 2143 }
M_souta 26:4c0ce2f05688 2144 motor[TIRE_FL].pwm = 30;
M_souta 26:4c0ce2f05688 2145 motor[TIRE_BL].pwm = 30;
M_souta 26:4c0ce2f05688 2146 motor[TIRE_BR].pwm = 30;
M_souta 26:4c0ce2f05688 2147 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2148 }else if(lineFase == 1) { // 前 ライントレース
M_souta 25:b3a9f34b201d 2149 switch(linePara[0]) {
M_souta 25:b3a9f34b201d 2150 case -2:
M_souta 25:b3a9f34b201d 2151 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 2152 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 2153 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 2154 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2155 adjAnable = true;
M_souta 25:b3a9f34b201d 2156 break;
M_souta 25:b3a9f34b201d 2157 case -3:
M_souta 26:4c0ce2f05688 2158 tirePWM[TIRE_FL] = 10;
M_souta 26:4c0ce2f05688 2159 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2160 tirePWM[TIRE_BR] = -10;
M_souta 26:4c0ce2f05688 2161 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2162 adjAnable = true;
M_souta 25:b3a9f34b201d 2163 break;
M_souta 25:b3a9f34b201d 2164 case -1:
M_souta 25:b3a9f34b201d 2165 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2166 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 2167 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2168 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2169 adjAnable = true;
M_souta 25:b3a9f34b201d 2170 break;
M_souta 25:b3a9f34b201d 2171 case 0:
M_souta 26:4c0ce2f05688 2172 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 2173 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2174 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 2175 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2176 adjAnable = true;
M_souta 25:b3a9f34b201d 2177 break;
M_souta 25:b3a9f34b201d 2178 case 1:
M_souta 26:4c0ce2f05688 2179 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 2180 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2181 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 2182 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2183 adjAnable = true;
M_souta 25:b3a9f34b201d 2184 break;
M_souta 25:b3a9f34b201d 2185 case 3:
M_souta 26:4c0ce2f05688 2186 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 2187 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 2188 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 2189 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 2190 adjAnable = true;
M_souta 25:b3a9f34b201d 2191 break;
M_souta 25:b3a9f34b201d 2192 case 2:
M_souta 26:4c0ce2f05688 2193 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 2194 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2195 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 2196 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2197 adjAnable = true;
M_souta 25:b3a9f34b201d 2198 break;
M_souta 25:b3a9f34b201d 2199 case 'A':
M_souta 25:b3a9f34b201d 2200 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2201 lineCheck = true;
M_souta 25:b3a9f34b201d 2202 lineCount = 0;
M_souta 26:4c0ce2f05688 2203 countW++;
M_souta 25:b3a9f34b201d 2204 }
M_souta 26:4c0ce2f05688 2205 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 2206 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2207 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 2208 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2209 adjAnable = true;
M_souta 25:b3a9f34b201d 2210 break;
M_souta 25:b3a9f34b201d 2211 case 'N':
M_souta 25:b3a9f34b201d 2212 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2213 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2214 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2215 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2216 adjAnable = false;
M_souta 25:b3a9f34b201d 2217 break;
M_souta 25:b3a9f34b201d 2218 default:
M_souta 25:b3a9f34b201d 2219 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2220 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2221 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2222 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2223 adjAnable = false;
M_souta 25:b3a9f34b201d 2224 }
M_souta 25:b3a9f34b201d 2225
M_souta 25:b3a9f34b201d 2226 if(adjAnable){
M_souta 25:b3a9f34b201d 2227 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta 25:b3a9f34b201d 2228 } else {
M_souta 25:b3a9f34b201d 2229 adj = 0;
M_souta 25:b3a9f34b201d 2230 }
M_souta 25:b3a9f34b201d 2231
M_souta 25:b3a9f34b201d 2232 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2233 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2234 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2235 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 2236 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2237 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 26:4c0ce2f05688 2238 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 26:4c0ce2f05688 2239 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2240
M_souta 25:b3a9f34b201d 2241 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2242 lineCount++;
M_souta 25:b3a9f34b201d 2243 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2244 }
M_souta 26:4c0ce2f05688 2245 if(countW == 3) {
M_souta 26:4c0ce2f05688 2246 countW = 0;
M_souta 25:b3a9f34b201d 2247 lineFase = 2;
M_souta 25:b3a9f34b201d 2248 lineCount = 0;
M_souta 25:b3a9f34b201d 2249 lineCheck = false;
M_souta 25:b3a9f34b201d 2250 }
M_souta 25:b3a9f34b201d 2251 } else if(lineFase == 2) { // 前 低速
M_souta 26:4c0ce2f05688 2252 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 2253 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 2254 motor[TIRE_BR].dir = BACK;
M_souta 26:4c0ce2f05688 2255 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 2256 if(linePara[3] == 0) {
M_souta 25:b3a9f34b201d 2257 lineFase = 3;
M_souta 25:b3a9f34b201d 2258 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2259 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2260 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2261 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2262 }
M_souta 26:4c0ce2f05688 2263 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 2264 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 2265 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 2266 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2267 } else if(lineFase == 3) { // 左 ライントレース
M_souta 26:4c0ce2f05688 2268 switch(linePara[3]) {
M_souta 25:b3a9f34b201d 2269 case -2:
M_souta 26:4c0ce2f05688 2270 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2271 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2272 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2273 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2274 adjAnable = true;
M_souta 25:b3a9f34b201d 2275 break;
M_souta 25:b3a9f34b201d 2276 case -3:
M_souta 26:4c0ce2f05688 2277 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2278 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 2279 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2280 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 2281 adjAnable = true;
M_souta 25:b3a9f34b201d 2282 break;
M_souta 25:b3a9f34b201d 2283 case -1:
M_souta 26:4c0ce2f05688 2284 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2285 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2286 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2287 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2288 adjAnable = true;
M_souta 25:b3a9f34b201d 2289 break;
M_souta 25:b3a9f34b201d 2290 case 0:
M_souta 26:4c0ce2f05688 2291 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2292 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2293 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2294 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2295 adjAnable = true;
M_souta 25:b3a9f34b201d 2296 break;
M_souta 25:b3a9f34b201d 2297 case 1:
M_souta 26:4c0ce2f05688 2298 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2299 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2300 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2301 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2302 adjAnable = true;
M_souta 25:b3a9f34b201d 2303 break;
M_souta 25:b3a9f34b201d 2304 case 3:
M_souta 26:4c0ce2f05688 2305 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 2306 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2307 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 2308 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2309 adjAnable = true;
M_souta 25:b3a9f34b201d 2310 break;
M_souta 25:b3a9f34b201d 2311 case 2:
M_souta 25:b3a9f34b201d 2312 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 2313 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 2314 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 2315 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2316 adjAnable = true;
M_souta 25:b3a9f34b201d 2317 break;
M_souta 25:b3a9f34b201d 2318 case 'A':
M_souta 25:b3a9f34b201d 2319 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2320 lineCheck = true;
M_souta 25:b3a9f34b201d 2321 lineCount = 0;
M_souta 26:4c0ce2f05688 2322 countW++;
M_souta 25:b3a9f34b201d 2323 }
M_souta 26:4c0ce2f05688 2324 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2325 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2326 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2327 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2328 adjAnable = true;
M_souta 25:b3a9f34b201d 2329 break;
M_souta 25:b3a9f34b201d 2330 case 'N':
M_souta 25:b3a9f34b201d 2331 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2332 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2333 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2334 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2335 adjAnable = false;
M_souta 25:b3a9f34b201d 2336 break;
M_souta 25:b3a9f34b201d 2337 default:
M_souta 25:b3a9f34b201d 2338 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2339 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2340 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2341 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2342 adjAnable = false;
M_souta 25:b3a9f34b201d 2343 }
M_souta 25:b3a9f34b201d 2344
M_souta 25:b3a9f34b201d 2345 if(adjAnable){
M_souta 25:b3a9f34b201d 2346 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta 25:b3a9f34b201d 2347 } else {
M_souta 25:b3a9f34b201d 2348 adj = 0;
M_souta 25:b3a9f34b201d 2349 }
M_souta 25:b3a9f34b201d 2350
M_souta 25:b3a9f34b201d 2351 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2352 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 2353 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2354 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 2355 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2356 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 2357 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2358 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 2359
M_souta 25:b3a9f34b201d 2360 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2361 lineCount++;
M_souta 25:b3a9f34b201d 2362 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2363 }
M_souta 26:4c0ce2f05688 2364 if(countW == 1) {
M_souta 26:4c0ce2f05688 2365 countW = 0;
M_souta 25:b3a9f34b201d 2366 lineFase = 4;
M_souta 25:b3a9f34b201d 2367 lineCount = 0;
M_souta 25:b3a9f34b201d 2368 lineCheck = false;
M_souta 25:b3a9f34b201d 2369 }
M_souta 25:b3a9f34b201d 2370 } else if(lineFase == 4) { // 右 低速
M_souta 26:4c0ce2f05688 2371 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 2372 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 2373 motor[TIRE_BR].dir = FOR;
M_souta 26:4c0ce2f05688 2374 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2375 if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 2376 lineFase = 5;
M_souta 25:b3a9f34b201d 2377 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2378 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2379 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2380 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2381 }
M_souta 26:4c0ce2f05688 2382 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 2383 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 2384 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 2385 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2386 } else if (lineFase == 5) {
M_souta 25:b3a9f34b201d 2387 lineFase = 6;
M_souta 25:b3a9f34b201d 2388 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2389 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2390 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2391 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2392 } else if(lineFase == 6) { // タオル1 竿検知
M_souta 25:b3a9f34b201d 2393 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 2394 lineFase = 7;
M_souta 25:b3a9f34b201d 2395 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2396 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 2397 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2398 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2399 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2400 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2401 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2402 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 2403 } else if(LimitSw::IsPressed(TOW_1L)) {
M_souta 25:b3a9f34b201d 2404 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 2405 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 2406 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 2407 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 2408 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 2409 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 2410 motor[TIRE_FR].dir = FOR;
M_souta 26:4c0ce2f05688 2411 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 2412 } else if(LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 2413 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 2414 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 2415 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 2416 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 2417 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 2418 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 2419 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 2420 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 2421 } else {
M_souta 26:4c0ce2f05688 2422 switch(linePara[LINE_TOW_1]) {
M_souta 26:4c0ce2f05688 2423 case -2:
M_souta 26:4c0ce2f05688 2424 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 2425 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2426 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 2427 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2428 adjAnable = true;
M_souta 26:4c0ce2f05688 2429 break;
M_souta 26:4c0ce2f05688 2430 case -3:
M_souta 26:4c0ce2f05688 2431 tirePWM[TIRE_FL] = -14;
M_souta 26:4c0ce2f05688 2432 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2433 tirePWM[TIRE_BR] = 14;
M_souta 26:4c0ce2f05688 2434 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2435 adjAnable = true;
M_souta 26:4c0ce2f05688 2436 break;
M_souta 26:4c0ce2f05688 2437 case -1:
M_souta 26:4c0ce2f05688 2438 tirePWM[TIRE_FL] = -17;
M_souta 26:4c0ce2f05688 2439 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2440 tirePWM[TIRE_BR] = 17;
M_souta 26:4c0ce2f05688 2441 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2442 adjAnable = true;
M_souta 26:4c0ce2f05688 2443 break;
M_souta 26:4c0ce2f05688 2444 case 0:
M_souta 26:4c0ce2f05688 2445 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2446 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2447 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2448 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2449 adjAnable = true;
M_souta 26:4c0ce2f05688 2450 break;
M_souta 26:4c0ce2f05688 2451 case 1:
M_souta 26:4c0ce2f05688 2452 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2453 tirePWM[TIRE_BL] = -17;
M_souta 26:4c0ce2f05688 2454 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2455 tirePWM[TIRE_FR] = 17;
M_souta 26:4c0ce2f05688 2456 adjAnable = true;
M_souta 26:4c0ce2f05688 2457 break;
M_souta 26:4c0ce2f05688 2458 case 3:
M_souta 26:4c0ce2f05688 2459 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2460 tirePWM[TIRE_BL] = -14;
M_souta 26:4c0ce2f05688 2461 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2462 tirePWM[TIRE_FR] = 14;
M_souta 26:4c0ce2f05688 2463 adjAnable = true;
M_souta 26:4c0ce2f05688 2464 break;
M_souta 26:4c0ce2f05688 2465 case 2:
M_souta 26:4c0ce2f05688 2466 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2467 tirePWM[TIRE_BL] = -10;
M_souta 26:4c0ce2f05688 2468 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2469 tirePWM[TIRE_FR] = 10;
M_souta 26:4c0ce2f05688 2470 adjAnable = true;
M_souta 26:4c0ce2f05688 2471 break;
M_souta 26:4c0ce2f05688 2472 case 'A':
M_souta 26:4c0ce2f05688 2473 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2474 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2475 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2476 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2477 adjAnable = true;
M_souta 26:4c0ce2f05688 2478 break;
M_souta 26:4c0ce2f05688 2479 case 'N':
M_souta 27:dd6c9feb7493 2480 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 2481 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 2482 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 2483 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 26:4c0ce2f05688 2484 adjAnable = false;
M_souta 26:4c0ce2f05688 2485 break;
M_souta 26:4c0ce2f05688 2486 default:
M_souta 26:4c0ce2f05688 2487 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 2488 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2489 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 2490 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 2491 adjAnable = false;
M_souta 26:4c0ce2f05688 2492 }
M_souta 26:4c0ce2f05688 2493
M_souta 26:4c0ce2f05688 2494 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2495 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2496 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 2497 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 2498 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2499 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2500 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 2501 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2502 }
M_souta 25:b3a9f34b201d 2503 } else if(lineFase == 7) { // ライン 修正
M_souta 25:b3a9f34b201d 2504 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 2505 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2506 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 2507 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2508 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2509 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2510 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2511 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2512 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 2513 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 2514 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 2515 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 2516 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 2517 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 2518 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 2519 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 2520 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2521 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2522 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 2523 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2524 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 2525 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 2526 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 2527 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 2528 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 2529 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 2530 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2531 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 26:4c0ce2f05688 2532 //tow_stop.reset();
M_souta 26:4c0ce2f05688 2533 //tow_stop.start();
M_souta 25:b3a9f34b201d 2534 lineFase = 8;
M_souta 25:b3a9f34b201d 2535 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2536 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 2537 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2538 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2539 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2540 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2541 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2542 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 2543 } else {
M_souta 25:b3a9f34b201d 2544 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2545 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 2546 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2547 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2548 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2549 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2550 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2551 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 2552 }
M_souta 25:b3a9f34b201d 2553 } else if(lineFase == 8) { // タオル1 解放
M_souta 27:dd6c9feb7493 2554 Air[0] = SOLENOID_ON;
M_souta 25:b3a9f34b201d 2555 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2556 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2557 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2558 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2559 lineFase = 9;
M_souta 25:b3a9f34b201d 2560 } else if(lineFase == 9) { // 前
M_souta 25:b3a9f34b201d 2561 switch(linePara[LINE_TOW_1]) {
M_souta 25:b3a9f34b201d 2562 case 2:
M_souta 26:4c0ce2f05688 2563 tirePWM[TIRE_FL] = 10;
M_souta 26:4c0ce2f05688 2564 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2565 tirePWM[TIRE_BR] = -10;
M_souta 26:4c0ce2f05688 2566 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2567 adjAnable = true;
M_souta 25:b3a9f34b201d 2568 break;
M_souta 25:b3a9f34b201d 2569 case 3:
M_souta 26:4c0ce2f05688 2570 tirePWM[TIRE_FL] = 14;
M_souta 26:4c0ce2f05688 2571 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2572 tirePWM[TIRE_BR] = -14;
M_souta 26:4c0ce2f05688 2573 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2574 adjAnable = true;
M_souta 25:b3a9f34b201d 2575 break;
M_souta 25:b3a9f34b201d 2576 case 1:
M_souta 26:4c0ce2f05688 2577 tirePWM[TIRE_FL] = 17;
M_souta 26:4c0ce2f05688 2578 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2579 tirePWM[TIRE_BR] = -17;
M_souta 26:4c0ce2f05688 2580 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2581 adjAnable = true;
M_souta 25:b3a9f34b201d 2582 break;
M_souta 25:b3a9f34b201d 2583 case 0:
M_souta 26:4c0ce2f05688 2584 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2585 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2586 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2587 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2588 adjAnable = true;
M_souta 25:b3a9f34b201d 2589 break;
M_souta 25:b3a9f34b201d 2590 case -1:
M_souta 26:4c0ce2f05688 2591 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2592 tirePWM[TIRE_BL] = 17;
M_souta 26:4c0ce2f05688 2593 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2594 tirePWM[TIRE_FR] = -17;
M_souta 25:b3a9f34b201d 2595 adjAnable = true;
M_souta 25:b3a9f34b201d 2596 break;
M_souta 25:b3a9f34b201d 2597 case -3:
M_souta 26:4c0ce2f05688 2598 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2599 tirePWM[TIRE_BL] = 14;
M_souta 26:4c0ce2f05688 2600 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2601 tirePWM[TIRE_FR] = -14;
M_souta 25:b3a9f34b201d 2602 adjAnable = true;
M_souta 25:b3a9f34b201d 2603 break;
M_souta 25:b3a9f34b201d 2604 case -2:
M_souta 26:4c0ce2f05688 2605 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2606 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 2607 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2608 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 2609 adjAnable = true;
M_souta 25:b3a9f34b201d 2610 break;
M_souta 25:b3a9f34b201d 2611 case 'A':
M_souta 25:b3a9f34b201d 2612 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2613 lineCheck = true;
M_souta 25:b3a9f34b201d 2614 lineCount = 0;
M_souta 26:4c0ce2f05688 2615 countW++;
M_souta 25:b3a9f34b201d 2616 }
M_souta 26:4c0ce2f05688 2617 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2618 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2619 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2620 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2621 adjAnable = true;
M_souta 25:b3a9f34b201d 2622 break;
M_souta 25:b3a9f34b201d 2623 case 'N':
M_souta 25:b3a9f34b201d 2624 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2625 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2626 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2627 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2628 adjAnable = false;
M_souta 25:b3a9f34b201d 2629 break;
M_souta 25:b3a9f34b201d 2630 default:
M_souta 25:b3a9f34b201d 2631 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2632 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2633 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2634 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2635 adjAnable = false;
M_souta 25:b3a9f34b201d 2636 }
M_souta 25:b3a9f34b201d 2637
M_souta 25:b3a9f34b201d 2638 if(adjAnable){
M_souta 25:b3a9f34b201d 2639 adj = 0;
M_souta 25:b3a9f34b201d 2640 } else {
M_souta 25:b3a9f34b201d 2641 adj = 0;
M_souta 25:b3a9f34b201d 2642 }
M_souta 25:b3a9f34b201d 2643
M_souta 25:b3a9f34b201d 2644 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2645 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2646 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2647 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2648 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2649 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2650 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2651 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2652
M_souta 25:b3a9f34b201d 2653 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2654 lineCount++;
M_souta 25:b3a9f34b201d 2655 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2656 }
M_souta 26:4c0ce2f05688 2657 if(countW == 1) {
M_souta 26:4c0ce2f05688 2658 countW = 0;
M_souta 25:b3a9f34b201d 2659 lineFase = 10;
M_souta 25:b3a9f34b201d 2660 lineCount = 0;
M_souta 25:b3a9f34b201d 2661 lineCheck = false;
M_souta 25:b3a9f34b201d 2662 }
M_souta 26:4c0ce2f05688 2663 } else if(lineFase == 10) { // 前 低速
M_souta 25:b3a9f34b201d 2664 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2665 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 2666 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 2667 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 2668 if(linePara[3] == 0) {
M_souta 25:b3a9f34b201d 2669 lineFase = 11;
M_souta 25:b3a9f34b201d 2670 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2671 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2672 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2673 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2674 }
M_souta 26:4c0ce2f05688 2675 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 2676 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 2677 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 2678 motor[TIRE_FR].pwm = 16;
M_souta 26:4c0ce2f05688 2679 } else if(lineFase == 11) { // 左
M_souta 26:4c0ce2f05688 2680 switch(linePara[3]) {
M_souta 25:b3a9f34b201d 2681 case -2:
M_souta 26:4c0ce2f05688 2682 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 2683 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2684 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 2685 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2686 adjAnable = true;
M_souta 25:b3a9f34b201d 2687 break;
M_souta 25:b3a9f34b201d 2688 case -3:
M_souta 26:4c0ce2f05688 2689 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2690 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 2691 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2692 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 2693 adjAnable = true;
M_souta 25:b3a9f34b201d 2694 break;
M_souta 25:b3a9f34b201d 2695 case -1:
M_souta 26:4c0ce2f05688 2696 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2697 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2698 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2699 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2700 adjAnable = true;
M_souta 25:b3a9f34b201d 2701 break;
M_souta 25:b3a9f34b201d 2702 case 0:
M_souta 26:4c0ce2f05688 2703 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2704 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2705 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2706 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2707 adjAnable = true;
M_souta 25:b3a9f34b201d 2708 break;
M_souta 25:b3a9f34b201d 2709 case 1:
M_souta 26:4c0ce2f05688 2710 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2711 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2712 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2713 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2714 adjAnable = true;
M_souta 25:b3a9f34b201d 2715 break;
M_souta 25:b3a9f34b201d 2716 case 3:
M_souta 26:4c0ce2f05688 2717 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 2718 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2719 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 2720 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2721 adjAnable = true;
M_souta 25:b3a9f34b201d 2722 break;
M_souta 25:b3a9f34b201d 2723 case 2:
M_souta 25:b3a9f34b201d 2724 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 2725 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 2726 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 2727 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2728 adjAnable = true;
M_souta 25:b3a9f34b201d 2729 break;
M_souta 25:b3a9f34b201d 2730 case 'A':
M_souta 25:b3a9f34b201d 2731 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2732 lineCheck = true;
M_souta 25:b3a9f34b201d 2733 lineCount = 0;
M_souta 26:4c0ce2f05688 2734 countW++;
M_souta 25:b3a9f34b201d 2735 }
M_souta 26:4c0ce2f05688 2736 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 2737 tirePWM[TIRE_BL] = 30;
M_souta 26:4c0ce2f05688 2738 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 2739 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 2740 adjAnable = true;
M_souta 25:b3a9f34b201d 2741 break;
M_souta 25:b3a9f34b201d 2742 case 'N':
M_souta 25:b3a9f34b201d 2743 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2744 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2745 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2746 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2747 adjAnable = false;
M_souta 25:b3a9f34b201d 2748 break;
M_souta 25:b3a9f34b201d 2749 default:
M_souta 25:b3a9f34b201d 2750 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2751 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2752 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2753 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2754 adjAnable = false;
M_souta 25:b3a9f34b201d 2755 }
M_souta 25:b3a9f34b201d 2756
M_souta 25:b3a9f34b201d 2757 if(adjAnable){
M_souta 26:4c0ce2f05688 2758 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta 25:b3a9f34b201d 2759 } else {
M_souta 25:b3a9f34b201d 2760 adj = 0;
M_souta 25:b3a9f34b201d 2761 }
M_souta 25:b3a9f34b201d 2762
M_souta 26:4c0ce2f05688 2763 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2764 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2765 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 26:4c0ce2f05688 2766 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 26:4c0ce2f05688 2767 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2768 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2769 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 26:4c0ce2f05688 2770 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 2771 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2772 lineCount++;
M_souta 25:b3a9f34b201d 2773 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2774 }
M_souta 26:4c0ce2f05688 2775 if(countW == 2) {
M_souta 26:4c0ce2f05688 2776 countW = 0;
M_souta 25:b3a9f34b201d 2777 lineFase = 12;
M_souta 25:b3a9f34b201d 2778 lineCount = 0;
M_souta 25:b3a9f34b201d 2779 lineCheck = false;
M_souta 25:b3a9f34b201d 2780 }
M_souta 26:4c0ce2f05688 2781 } else if(lineFase == 12) { // 左 低速
M_souta 26:4c0ce2f05688 2782 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 2783 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 2784 motor[TIRE_BR].dir = FOR;
M_souta 26:4c0ce2f05688 2785 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2786 if(linePara[LINE_TOW_2] == 0) {
M_souta 25:b3a9f34b201d 2787 lineFase = 13;
M_souta 25:b3a9f34b201d 2788 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2789 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2790 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2791 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2792 }
M_souta 25:b3a9f34b201d 2793 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 2794 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 2795 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 2796 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2797 } else if (lineFase == 13) {
M_souta 25:b3a9f34b201d 2798 lineFase = 14;
M_souta 25:b3a9f34b201d 2799 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2800 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2801 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2802 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2803 } else if(lineFase == 14) { // タオル2 竿検知
M_souta 26:4c0ce2f05688 2804 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta 26:4c0ce2f05688 2805 lineFase = 15;
M_souta 26:4c0ce2f05688 2806 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2807 motor[TIRE_FL].pwm = 50;
M_souta 26:4c0ce2f05688 2808 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2809 motor[TIRE_BL].pwm = 50;
M_souta 26:4c0ce2f05688 2810 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2811 motor[TIRE_BR].pwm = 50;
M_souta 26:4c0ce2f05688 2812 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2813 motor[TIRE_FR].pwm = 50;
M_souta 26:4c0ce2f05688 2814 } else if(LimitSw::IsPressed(TOW_2L)) {
M_souta 26:4c0ce2f05688 2815 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 2816 motor[TIRE_FL].pwm = 20;
M_souta 26:4c0ce2f05688 2817 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 2818 motor[TIRE_BL].pwm = 20;
M_souta 26:4c0ce2f05688 2819 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 2820 motor[TIRE_BR].pwm = 20;
M_souta 26:4c0ce2f05688 2821 motor[TIRE_FR].dir = FOR;
M_souta 26:4c0ce2f05688 2822 motor[TIRE_FR].pwm = 20;
M_souta 26:4c0ce2f05688 2823 } else if(LimitSw::IsPressed(TOW_2R)) {
M_souta 26:4c0ce2f05688 2824 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 2825 motor[TIRE_FL].pwm = 20;
M_souta 26:4c0ce2f05688 2826 motor[TIRE_BL].dir = BRAKE;
M_souta 27:dd6c9feb7493 2827 motor[TIRE_BL].pwm = 20;
M_souta 26:4c0ce2f05688 2828 motor[TIRE_BR].dir = BRAKE;
M_souta 27:dd6c9feb7493 2829 motor[TIRE_BR].pwm = 20;
M_souta 26:4c0ce2f05688 2830 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 2831 motor[TIRE_FR].pwm = 20;
M_souta 26:4c0ce2f05688 2832 } else {
M_souta 26:4c0ce2f05688 2833 switch(linePara[LINE_TOW_2]) {
M_souta 26:4c0ce2f05688 2834 case -2:
M_souta 26:4c0ce2f05688 2835 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 2836 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2837 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 2838 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2839 break;
M_souta 26:4c0ce2f05688 2840 case -3:
M_souta 26:4c0ce2f05688 2841 tirePWM[TIRE_FL] = -14;
M_souta 26:4c0ce2f05688 2842 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2843 tirePWM[TIRE_BR] = 14;
M_souta 26:4c0ce2f05688 2844 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2845 break;
M_souta 26:4c0ce2f05688 2846 case -1:
M_souta 26:4c0ce2f05688 2847 tirePWM[TIRE_FL] = -17;
M_souta 26:4c0ce2f05688 2848 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2849 tirePWM[TIRE_BR] = 17;
M_souta 26:4c0ce2f05688 2850 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2851 break;
M_souta 26:4c0ce2f05688 2852 case 0:
M_souta 26:4c0ce2f05688 2853 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2854 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2855 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2856 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2857 break;
M_souta 26:4c0ce2f05688 2858 case 1:
M_souta 26:4c0ce2f05688 2859 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2860 tirePWM[TIRE_BL] = -17;
M_souta 26:4c0ce2f05688 2861 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2862 tirePWM[TIRE_FR] = 17;
M_souta 26:4c0ce2f05688 2863 break;
M_souta 26:4c0ce2f05688 2864 case 3:
M_souta 26:4c0ce2f05688 2865 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2866 tirePWM[TIRE_BL] = -14;
M_souta 26:4c0ce2f05688 2867 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2868 tirePWM[TIRE_FR] = 14;
M_souta 26:4c0ce2f05688 2869 break;
M_souta 26:4c0ce2f05688 2870 case 2:
M_souta 26:4c0ce2f05688 2871 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2872 tirePWM[TIRE_BL] = -10;
M_souta 26:4c0ce2f05688 2873 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2874 tirePWM[TIRE_FR] = 10;
M_souta 26:4c0ce2f05688 2875 break;
M_souta 26:4c0ce2f05688 2876 case 'A':
M_souta 26:4c0ce2f05688 2877 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 2878 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 2879 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 2880 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 2881 break;
M_souta 26:4c0ce2f05688 2882 case 'N':
M_souta 27:dd6c9feb7493 2883 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 27:dd6c9feb7493 2884 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 27:dd6c9feb7493 2885 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 27:dd6c9feb7493 2886 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 26:4c0ce2f05688 2887 break;
M_souta 26:4c0ce2f05688 2888 default:
M_souta 26:4c0ce2f05688 2889 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 2890 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 2891 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 2892 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 2893 }
M_souta 26:4c0ce2f05688 2894
M_souta 26:4c0ce2f05688 2895 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2896 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2897 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 2898 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 2899 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 2900 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 2901 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 2902 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 2903 }
M_souta 26:4c0ce2f05688 2904 } else if(lineFase == 15 ){ // ライン 修正
M_souta 26:4c0ce2f05688 2905 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
M_souta 26:4c0ce2f05688 2906 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2907 motor[TIRE_FL].pwm = 50;
M_souta 26:4c0ce2f05688 2908 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2909 motor[TIRE_BL].pwm = 50;
M_souta 26:4c0ce2f05688 2910 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2911 motor[TIRE_BR].pwm = 50;
M_souta 26:4c0ce2f05688 2912 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2913 motor[TIRE_FR].pwm = 50;
M_souta 26:4c0ce2f05688 2914 } else if(linePara[LINE_TOW_2] > 0) {
M_souta 26:4c0ce2f05688 2915 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 2916 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 2917 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 2918 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 2919 motor[TIRE_BR].dir = FOR;
M_souta 26:4c0ce2f05688 2920 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 2921 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 2922 motor[TIRE_FR].pwm = 16;
M_souta 26:4c0ce2f05688 2923 } else if(linePara[LINE_TOW_2] < 0) {
M_souta 26:4c0ce2f05688 2924 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 2925 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 2926 motor[TIRE_BL].dir = BACK;
M_souta 26:4c0ce2f05688 2927 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 2928 motor[TIRE_BR].dir = BACK;
M_souta 26:4c0ce2f05688 2929 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 2930 motor[TIRE_FR].dir = FOR;
M_souta 26:4c0ce2f05688 2931 motor[TIRE_FR].pwm = 16;
M_souta 26:4c0ce2f05688 2932 } else if(linePara[LINE_TOW_2] == 0) {
M_souta 26:4c0ce2f05688 2933 lineFase = 16;
M_souta 26:4c0ce2f05688 2934 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2935 motor[TIRE_FL].pwm = 50;
M_souta 26:4c0ce2f05688 2936 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2937 motor[TIRE_BL].pwm = 50;
M_souta 26:4c0ce2f05688 2938 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2939 motor[TIRE_BR].pwm = 50;
M_souta 26:4c0ce2f05688 2940 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2941 motor[TIRE_FR].pwm = 50;
M_souta 26:4c0ce2f05688 2942 } else {
M_souta 26:4c0ce2f05688 2943 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2944 motor[TIRE_FL].pwm = 50;
M_souta 26:4c0ce2f05688 2945 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2946 motor[TIRE_BL].pwm = 50;
M_souta 26:4c0ce2f05688 2947 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2948 motor[TIRE_BR].pwm = 50;
M_souta 26:4c0ce2f05688 2949 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2950 motor[TIRE_FR].pwm = 50;
M_souta 26:4c0ce2f05688 2951 }
M_souta 26:4c0ce2f05688 2952 } else if(lineFase == 16) { // タオル2 解放
M_souta 27:dd6c9feb7493 2953 Air[1] = SOLENOID_ON;
M_souta 26:4c0ce2f05688 2954 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2955 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 2956 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2957 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 2958 lineFase = 17;
M_souta 26:4c0ce2f05688 2959 } else if(lineFase == 17) { // 前
M_souta 26:4c0ce2f05688 2960 switch(linePara[LINE_TOW_2]) {
M_souta 26:4c0ce2f05688 2961 case 2:
M_souta 26:4c0ce2f05688 2962 tirePWM[TIRE_FL] = 10;
M_souta 26:4c0ce2f05688 2963 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2964 tirePWM[TIRE_BR] = -10;
M_souta 26:4c0ce2f05688 2965 tirePWM[TIRE_FR] = -20;
M_souta 26:4c0ce2f05688 2966 adjAnable = true;
M_souta 26:4c0ce2f05688 2967 break;
M_souta 26:4c0ce2f05688 2968 case 3:
M_souta 26:4c0ce2f05688 2969 tirePWM[TIRE_FL] = 14;
M_souta 26:4c0ce2f05688 2970 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2971 tirePWM[TIRE_BR] = -14;
M_souta 26:4c0ce2f05688 2972 tirePWM[TIRE_FR] = -20;
M_souta 26:4c0ce2f05688 2973 adjAnable = true;
M_souta 26:4c0ce2f05688 2974 break;
M_souta 26:4c0ce2f05688 2975 case 1:
M_souta 26:4c0ce2f05688 2976 tirePWM[TIRE_FL] = 17;
M_souta 26:4c0ce2f05688 2977 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2978 tirePWM[TIRE_BR] = -17;
M_souta 26:4c0ce2f05688 2979 tirePWM[TIRE_FR] = -20;
M_souta 26:4c0ce2f05688 2980 adjAnable = true;
M_souta 26:4c0ce2f05688 2981 break;
M_souta 26:4c0ce2f05688 2982 case 0:
M_souta 26:4c0ce2f05688 2983 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2984 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 2985 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2986 tirePWM[TIRE_FR] = -20;
M_souta 26:4c0ce2f05688 2987 adjAnable = true;
M_souta 26:4c0ce2f05688 2988 break;
M_souta 26:4c0ce2f05688 2989 case -1:
M_souta 26:4c0ce2f05688 2990 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2991 tirePWM[TIRE_BL] = 17;
M_souta 26:4c0ce2f05688 2992 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 2993 tirePWM[TIRE_FR] = -17;
M_souta 26:4c0ce2f05688 2994 adjAnable = true;
M_souta 26:4c0ce2f05688 2995 break;
M_souta 26:4c0ce2f05688 2996 case -3:
M_souta 26:4c0ce2f05688 2997 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 2998 tirePWM[TIRE_BL] = 14;
M_souta 26:4c0ce2f05688 2999 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 3000 tirePWM[TIRE_FR] = -14;
M_souta 26:4c0ce2f05688 3001 adjAnable = true;
M_souta 26:4c0ce2f05688 3002 break;
M_souta 26:4c0ce2f05688 3003 case -2:
M_souta 26:4c0ce2f05688 3004 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 3005 tirePWM[TIRE_BL] = 10;
M_souta 26:4c0ce2f05688 3006 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 3007 tirePWM[TIRE_FR] = 10;
M_souta 26:4c0ce2f05688 3008 adjAnable = true;
M_souta 26:4c0ce2f05688 3009 break;
M_souta 26:4c0ce2f05688 3010 case 'A':
M_souta 26:4c0ce2f05688 3011 if(lineCheck == false) {
M_souta 26:4c0ce2f05688 3012 lineCheck = true;
M_souta 26:4c0ce2f05688 3013 lineCount = 0;
M_souta 26:4c0ce2f05688 3014 countW++;
M_souta 26:4c0ce2f05688 3015 }
M_souta 26:4c0ce2f05688 3016 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 3017 tirePWM[TIRE_BL] = 20;
M_souta 26:4c0ce2f05688 3018 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 3019 tirePWM[TIRE_FR] = -20;
M_souta 26:4c0ce2f05688 3020 adjAnable = true;
M_souta 26:4c0ce2f05688 3021 break;
M_souta 26:4c0ce2f05688 3022 case 'N':
M_souta 26:4c0ce2f05688 3023 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3024 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3025 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3026 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3027 adjAnable = false;
M_souta 26:4c0ce2f05688 3028 break;
M_souta 26:4c0ce2f05688 3029 default:
M_souta 26:4c0ce2f05688 3030 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3031 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3032 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3033 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3034 adjAnable = false;
M_souta 26:4c0ce2f05688 3035 }
M_souta 26:4c0ce2f05688 3036
M_souta 26:4c0ce2f05688 3037 if(adjAnable){
M_souta 26:4c0ce2f05688 3038 adj = 0;
M_souta 26:4c0ce2f05688 3039 } else {
M_souta 26:4c0ce2f05688 3040 adj = 0;
M_souta 26:4c0ce2f05688 3041 }
M_souta 26:4c0ce2f05688 3042
M_souta 26:4c0ce2f05688 3043 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 3044 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 26:4c0ce2f05688 3045 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 26:4c0ce2f05688 3046 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 3047 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 26:4c0ce2f05688 3048 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 26:4c0ce2f05688 3049 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 26:4c0ce2f05688 3050 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 3051
M_souta 26:4c0ce2f05688 3052 if(lineCheck == true) {
M_souta 26:4c0ce2f05688 3053 lineCount++;
M_souta 26:4c0ce2f05688 3054 if(lineCount > 20) lineCheck = false;
M_souta 26:4c0ce2f05688 3055 }
M_souta 26:4c0ce2f05688 3056 if(countW == 1) {
M_souta 26:4c0ce2f05688 3057 countW = 0;
M_souta 26:4c0ce2f05688 3058 lineFase = 18;
M_souta 26:4c0ce2f05688 3059 lineCount = 0;
M_souta 26:4c0ce2f05688 3060 lineCheck = false;
M_souta 26:4c0ce2f05688 3061 }
M_souta 26:4c0ce2f05688 3062 } else if(lineFase == 18) { // 前 低速
M_souta 26:4c0ce2f05688 3063 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 3064 motor[TIRE_BL].dir = FOR;
M_souta 26:4c0ce2f05688 3065 motor[TIRE_BR].dir = BACK;
M_souta 26:4c0ce2f05688 3066 motor[TIRE_FR].dir = BACK;
M_souta 26:4c0ce2f05688 3067 if(linePara[4] == 0) {
M_souta 26:4c0ce2f05688 3068 lineFase = 19;
M_souta 27:dd6c9feb7493 3069 Air[0] = SOLENOID_OFF;
M_souta 27:dd6c9feb7493 3070 Air[1] = SOLENOID_OFF;
M_souta 26:4c0ce2f05688 3071 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3072 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3073 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3074 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3075 }
M_souta 26:4c0ce2f05688 3076 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 3077 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 3078 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 3079 motor[TIRE_FR].pwm = 16;
M_souta 26:4c0ce2f05688 3080 } else if(lineFase == 19) { // 右
M_souta 26:4c0ce2f05688 3081 switch(linePara[4]) {
M_souta 26:4c0ce2f05688 3082 case -2:
M_souta 26:4c0ce2f05688 3083 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 3084 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3085 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 3086 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3087 adjAnable = true;
M_souta 26:4c0ce2f05688 3088 break;
M_souta 26:4c0ce2f05688 3089 case -3:
M_souta 26:4c0ce2f05688 3090 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 3091 tirePWM[TIRE_BL] = -10;
M_souta 26:4c0ce2f05688 3092 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 3093 tirePWM[TIRE_FR] = 10;
M_souta 26:4c0ce2f05688 3094 adjAnable = true;
M_souta 26:4c0ce2f05688 3095 break;
M_souta 26:4c0ce2f05688 3096 case -1:
M_souta 26:4c0ce2f05688 3097 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 3098 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 3099 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 3100 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 3101 adjAnable = true;
M_souta 26:4c0ce2f05688 3102 break;
M_souta 26:4c0ce2f05688 3103 case 0:
M_souta 26:4c0ce2f05688 3104 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 3105 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3106 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 3107 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3108 adjAnable = true;
M_souta 26:4c0ce2f05688 3109 break;
M_souta 26:4c0ce2f05688 3110 case 1:
M_souta 26:4c0ce2f05688 3111 tirePWM[TIRE_FL] = 20;
M_souta 26:4c0ce2f05688 3112 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3113 tirePWM[TIRE_BR] = -20;
M_souta 26:4c0ce2f05688 3114 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3115 adjAnable = true;
M_souta 26:4c0ce2f05688 3116 break;
M_souta 26:4c0ce2f05688 3117 case 3:
M_souta 26:4c0ce2f05688 3118 tirePWM[TIRE_FL] = 10;
M_souta 26:4c0ce2f05688 3119 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3120 tirePWM[TIRE_BR] = -10;
M_souta 26:4c0ce2f05688 3121 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3122 adjAnable = true;
M_souta 26:4c0ce2f05688 3123 break;
M_souta 26:4c0ce2f05688 3124 case 2:
M_souta 26:4c0ce2f05688 3125 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3126 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3127 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3128 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3129 adjAnable = true;
M_souta 26:4c0ce2f05688 3130 break;
M_souta 26:4c0ce2f05688 3131 case 'A':
M_souta 26:4c0ce2f05688 3132 if(lineCheck == false) {
M_souta 26:4c0ce2f05688 3133 lineCheck = true;
M_souta 26:4c0ce2f05688 3134 lineCount = 0;
M_souta 26:4c0ce2f05688 3135 countW++;
M_souta 26:4c0ce2f05688 3136 }
M_souta 26:4c0ce2f05688 3137 tirePWM[TIRE_FL] = 30;
M_souta 26:4c0ce2f05688 3138 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3139 tirePWM[TIRE_BR] = -30;
M_souta 26:4c0ce2f05688 3140 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3141 adjAnable = true;
M_souta 26:4c0ce2f05688 3142 break;
M_souta 26:4c0ce2f05688 3143 case 'N':
M_souta 26:4c0ce2f05688 3144 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3145 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3146 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3147 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3148 adjAnable = false;
M_souta 26:4c0ce2f05688 3149 break;
M_souta 26:4c0ce2f05688 3150 default:
M_souta 26:4c0ce2f05688 3151 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3152 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3153 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3154 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3155 adjAnable = false;
M_souta 26:4c0ce2f05688 3156 }
M_souta 26:4c0ce2f05688 3157
M_souta 26:4c0ce2f05688 3158 if(adjAnable){
M_souta 26:4c0ce2f05688 3159 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta 26:4c0ce2f05688 3160 } else {
M_souta 26:4c0ce2f05688 3161 adj = 0;
M_souta 26:4c0ce2f05688 3162 }
M_souta 26:4c0ce2f05688 3163
M_souta 26:4c0ce2f05688 3164 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 26:4c0ce2f05688 3165 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 26:4c0ce2f05688 3166 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 3167 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 3168 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 26:4c0ce2f05688 3169 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 26:4c0ce2f05688 3170 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 3171 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 26:4c0ce2f05688 3172
M_souta 26:4c0ce2f05688 3173 if(lineCheck == true) {
M_souta 26:4c0ce2f05688 3174 lineCount++;
M_souta 26:4c0ce2f05688 3175 if(lineCount > 20) lineCheck = false;
M_souta 26:4c0ce2f05688 3176 }
M_souta 26:4c0ce2f05688 3177
M_souta 26:4c0ce2f05688 3178 /*
M_souta 26:4c0ce2f05688 3179 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta 26:4c0ce2f05688 3180 targetCount = 3;
M_souta 26:4c0ce2f05688 3181 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta 26:4c0ce2f05688 3182 targetCount = 3;
M_souta 26:4c0ce2f05688 3183 } else {
M_souta 26:4c0ce2f05688 3184 targetCount = 2;
M_souta 26:4c0ce2f05688 3185 }
M_souta 26:4c0ce2f05688 3186 */
M_souta 26:4c0ce2f05688 3187
M_souta 26:4c0ce2f05688 3188 targetCount = 3;
M_souta 26:4c0ce2f05688 3189
M_souta 26:4c0ce2f05688 3190 if(countW == targetCount) {
M_souta 26:4c0ce2f05688 3191 countW = 0;
M_souta 26:4c0ce2f05688 3192 lineFase = 20;
M_souta 26:4c0ce2f05688 3193 lineCount = 0;
M_souta 26:4c0ce2f05688 3194 lineCheck = false;
M_souta 26:4c0ce2f05688 3195 }
M_souta 26:4c0ce2f05688 3196 } else if(lineFase == 20) { // 右 低速
M_souta 26:4c0ce2f05688 3197 motor[TIRE_FL].dir = FOR;
M_souta 26:4c0ce2f05688 3198 motor[TIRE_BL].dir = BACK;
M_souta 26:4c0ce2f05688 3199 motor[TIRE_BR].dir = BACK;
M_souta 26:4c0ce2f05688 3200 motor[TIRE_FR].dir = FOR;
M_souta 26:4c0ce2f05688 3201 if (linePara[2] == 0) {
M_souta 26:4c0ce2f05688 3202 //(LimitSw::IsPressed(SHEETS_SW)) {
M_souta 26:4c0ce2f05688 3203 //ineFase = 20;
M_souta 26:4c0ce2f05688 3204 //else if(LimitSw::IsPressed(TOWEL2_SW) {
M_souta 26:4c0ce2f05688 3205 //ineFase = 14;
M_souta 26:4c0ce2f05688 3206 //else {
M_souta 26:4c0ce2f05688 3207 lineFase = 21;
M_souta 26:4c0ce2f05688 3208 //
M_souta 26:4c0ce2f05688 3209 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3210 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3211 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3212 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3213 }
M_souta 26:4c0ce2f05688 3214 motor[TIRE_FL].pwm = 16;
M_souta 26:4c0ce2f05688 3215 motor[TIRE_BL].pwm = 16;
M_souta 26:4c0ce2f05688 3216 motor[TIRE_BR].pwm = 16;
M_souta 26:4c0ce2f05688 3217 motor[TIRE_FR].pwm = 16;
M_souta 26:4c0ce2f05688 3218 } else if(lineFase == 21) {
M_souta 26:4c0ce2f05688 3219 switch(linePara[2]) {
M_souta 26:4c0ce2f05688 3220 case -2:
M_souta 26:4c0ce2f05688 3221 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3222 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3223 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3224 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3225 adjAnable = true;
M_souta 26:4c0ce2f05688 3226 break;
M_souta 26:4c0ce2f05688 3227 case -3:
M_souta 26:4c0ce2f05688 3228 tirePWM[TIRE_FL] = -10;
M_souta 26:4c0ce2f05688 3229 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3230 tirePWM[TIRE_BR] = 10;
M_souta 26:4c0ce2f05688 3231 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3232 adjAnable = true;
M_souta 26:4c0ce2f05688 3233 break;
M_souta 26:4c0ce2f05688 3234 case -1:
M_souta 26:4c0ce2f05688 3235 tirePWM[TIRE_FL] = -20;
M_souta 26:4c0ce2f05688 3236 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3237 tirePWM[TIRE_BR] = 20;
M_souta 26:4c0ce2f05688 3238 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3239 adjAnable = true;
M_souta 26:4c0ce2f05688 3240 break;
M_souta 26:4c0ce2f05688 3241 case 0:
M_souta 26:4c0ce2f05688 3242 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 3243 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3244 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 3245 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3246 adjAnable = true;
M_souta 26:4c0ce2f05688 3247 break;
M_souta 26:4c0ce2f05688 3248 case 1:
M_souta 26:4c0ce2f05688 3249 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 3250 tirePWM[TIRE_BL] = -20;
M_souta 26:4c0ce2f05688 3251 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 3252 tirePWM[TIRE_FR] = 20;
M_souta 26:4c0ce2f05688 3253 adjAnable = true;
M_souta 26:4c0ce2f05688 3254 break;
M_souta 26:4c0ce2f05688 3255 case 3:
M_souta 26:4c0ce2f05688 3256 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 3257 tirePWM[TIRE_BL] = -10;
M_souta 26:4c0ce2f05688 3258 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 3259 tirePWM[TIRE_FR] = 10;
M_souta 26:4c0ce2f05688 3260 adjAnable = true;
M_souta 26:4c0ce2f05688 3261 break;
M_souta 26:4c0ce2f05688 3262 case 2:
M_souta 26:4c0ce2f05688 3263 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 3264 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3265 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 3266 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3267 adjAnable = true;
M_souta 26:4c0ce2f05688 3268 break;
M_souta 26:4c0ce2f05688 3269 case 'A':
M_souta 26:4c0ce2f05688 3270 if(lineCheck == false) {
M_souta 26:4c0ce2f05688 3271 lineCheck = true;
M_souta 26:4c0ce2f05688 3272 lineCount = 0;
M_souta 26:4c0ce2f05688 3273 countW++;
M_souta 26:4c0ce2f05688 3274 }
M_souta 26:4c0ce2f05688 3275 tirePWM[TIRE_FL] = -30;
M_souta 26:4c0ce2f05688 3276 tirePWM[TIRE_BL] = -30;
M_souta 26:4c0ce2f05688 3277 tirePWM[TIRE_BR] = 30;
M_souta 26:4c0ce2f05688 3278 tirePWM[TIRE_FR] = 30;
M_souta 26:4c0ce2f05688 3279 adjAnable = true;
M_souta 26:4c0ce2f05688 3280 break;
M_souta 26:4c0ce2f05688 3281 case 'N':
M_souta 26:4c0ce2f05688 3282 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3283 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3284 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3285 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3286 adjAnable = false;
M_souta 26:4c0ce2f05688 3287 break;
M_souta 26:4c0ce2f05688 3288 default:
M_souta 26:4c0ce2f05688 3289 tirePWM[TIRE_FL] = 0;
M_souta 26:4c0ce2f05688 3290 tirePWM[TIRE_BL] = 0;
M_souta 26:4c0ce2f05688 3291 tirePWM[TIRE_BR] = 0;
M_souta 26:4c0ce2f05688 3292 tirePWM[TIRE_FR] = 0;
M_souta 26:4c0ce2f05688 3293 adjAnable = false;
M_souta 26:4c0ce2f05688 3294 }
M_souta 26:4c0ce2f05688 3295
M_souta 26:4c0ce2f05688 3296 if(adjAnable){
M_souta 26:4c0ce2f05688 3297 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta 26:4c0ce2f05688 3298 } else {
M_souta 26:4c0ce2f05688 3299 adj = 0;
M_souta 26:4c0ce2f05688 3300 }
M_souta 26:4c0ce2f05688 3301
M_souta 26:4c0ce2f05688 3302 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 26:4c0ce2f05688 3303 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 3304 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 3305 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 26:4c0ce2f05688 3306 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 26:4c0ce2f05688 3307 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 26:4c0ce2f05688 3308 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 26:4c0ce2f05688 3309 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 26:4c0ce2f05688 3310
M_souta 26:4c0ce2f05688 3311 if(lineCheck == true) {
M_souta 26:4c0ce2f05688 3312 lineCount++;
M_souta 26:4c0ce2f05688 3313 if(lineCount > 20) lineCheck = false;
M_souta 26:4c0ce2f05688 3314 }
M_souta 26:4c0ce2f05688 3315 if(countW == 2) {
M_souta 26:4c0ce2f05688 3316 countW = 0;
M_souta 26:4c0ce2f05688 3317 lineFase = 22;
M_souta 26:4c0ce2f05688 3318 lineCount = 0;
M_souta 26:4c0ce2f05688 3319 lineCheck = false;
M_souta 26:4c0ce2f05688 3320 }
M_souta 26:4c0ce2f05688 3321 } else if(lineFase == 22) {
M_souta 26:4c0ce2f05688 3322 motor[TIRE_FL].dir = BACK;
M_souta 26:4c0ce2f05688 3323 motor[TIRE_BL].dir = BACK;
M_souta 26:4c0ce2f05688 3324 motor[TIRE_BR].dir = FOR;
M_souta 26:4c0ce2f05688 3325 motor[TIRE_FR].dir = FOR;
M_souta 26:4c0ce2f05688 3326 if (linePara[0] == 'N') {
M_souta 26:4c0ce2f05688 3327 motor[TIRE_FL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3328 motor[TIRE_BL].dir = BRAKE;
M_souta 26:4c0ce2f05688 3329 motor[TIRE_BR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3330 motor[TIRE_FR].dir = BRAKE;
M_souta 26:4c0ce2f05688 3331 }
M_souta 26:4c0ce2f05688 3332 motor[TIRE_FL].pwm = 30;
M_souta 26:4c0ce2f05688 3333 motor[TIRE_BL].pwm = 30;
M_souta 26:4c0ce2f05688 3334 motor[TIRE_BR].pwm = 30;
M_souta 26:4c0ce2f05688 3335 motor[TIRE_FR].pwm = 30;
M_souta 26:4c0ce2f05688 3336 } else{
M_souta 25:b3a9f34b201d 3337 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 3338 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 3339 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 3340 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 3341 }
M_souta 25:b3a9f34b201d 3342
kishibekairohan 13:b6e02d6261d7 3343 }
kishibekairohan 13:b6e02d6261d7 3344 #endif
kishibekairohan 13:b6e02d6261d7 3345
kishibekairohan 13:b6e02d6261d7 3346 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 3347 static void Process6()
kishibekairohan 13:b6e02d6261d7 3348 {
M_souta 27:dd6c9feb7493 3349 /*
M_souta 27:dd6c9feb7493 3350 static bool stop_flag_L, stop_flag_R;
M_souta 27:dd6c9feb7493 3351 if() {
M_souta 27:dd6c9feb7493 3352 if(LimitSw::IsPressed(LSW_LB) {
M_souta 27:dd6c9feb7493 3353 if() {
M_souta 27:dd6c9feb7493 3354 } else {
M_souta 22:7d93f79a3686 3355
M_souta 27:dd6c9feb7493 3356 */
M_souta 25:b3a9f34b201d 3357
kishibekairohan 13:b6e02d6261d7 3358 }
kishibekairohan 13:b6e02d6261d7 3359 #endif
kishibekairohan 13:b6e02d6261d7 3360
kishibekairohan 13:b6e02d6261d7 3361 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 3362 static void Process7()
kishibekairohan 13:b6e02d6261d7 3363 {
M_souta 24:370616a56815 3364
kishibekairohan 12:c09b3e08a316 3365 }
kishibekairohan 12:c09b3e08a316 3366 #endif
kishibekairohan 12:c09b3e08a316 3367
kishibekairohan 16:3f2c2d89372b 3368 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 3369 static void Process8()
kishibekairohan 12:c09b3e08a316 3370 {
M_souta 27:dd6c9feb7493 3371 /*
M_souta 22:7d93f79a3686 3372 if(controller->Button.A) {
M_souta 22:7d93f79a3686 3373 rotaconSampling.start();
M_souta 22:7d93f79a3686 3374 PIDflag = true;
M_souta 22:7d93f79a3686 3375
M_souta 22:7d93f79a3686 3376 //linePara_U = LineHub::GetPara(0);
M_souta 22:7d93f79a3686 3377 //linePara_B = LineHub::GetPara(3);
M_souta 22:7d93f79a3686 3378
M_souta 22:7d93f79a3686 3379
M_souta 22:7d93f79a3686 3380 pulsePV[FL] = encoder[FL].getPulses();
M_souta 22:7d93f79a3686 3381 pulsePV[BL] = encoder[BL].getPulses();
M_souta 22:7d93f79a3686 3382 pulsePV[BR] = encoder[BR].getPulses();
M_souta 22:7d93f79a3686 3383 pulsePV[FR] = encoder[FR].getPulses();
M_souta 22:7d93f79a3686 3384
M_souta 22:7d93f79a3686 3385
M_souta 22:7d93f79a3686 3386 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 3387 timeCV[i] = timePV[i];
M_souta 22:7d93f79a3686 3388 timePV[i] = rotaconSampling.read();
M_souta 22:7d93f79a3686 3389 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta 22:7d93f79a3686 3390 pulseCV[i] = pulsePV[i];
M_souta 22:7d93f79a3686 3391 }
M_souta 22:7d93f79a3686 3392
M_souta 22:7d93f79a3686 3393 move.Vx = 0.5;
M_souta 22:7d93f79a3686 3394 move.Vy = 0.5;
M_souta 22:7d93f79a3686 3395 move.Va = 0;
M_souta 22:7d93f79a3686 3396
M_souta 22:7d93f79a3686 3397 correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta 22:7d93f79a3686 3398 correction_LT.Vy = 0;
M_souta 22:7d93f79a3686 3399 correction_LT.Va = 0;
M_souta 22:7d93f79a3686 3400
M_souta 22:7d93f79a3686 3401 synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta 22:7d93f79a3686 3402 synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta 22:7d93f79a3686 3403 synthetic.Va = move.Va + correction_LT.Va;
M_souta 22:7d93f79a3686 3404
M_souta 22:7d93f79a3686 3405 sita = 0;
M_souta 22:7d93f79a3686 3406
M_souta 22:7d93f79a3686 3407 //タイヤの目標速度算出
M_souta 22:7d93f79a3686 3408 float sinR = 0.7071 * (float)sin(sita);
M_souta 22:7d93f79a3686 3409 float cosR = 0.7071 * (float)cos(sita);
M_souta 22:7d93f79a3686 3410 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta 22:7d93f79a3686 3411 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 3412 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 3413 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 3414 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 3415
M_souta 22:7d93f79a3686 3416 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta 22:7d93f79a3686 3417
M_souta 22:7d93f79a3686 3418 //PIDによるPWM算出
M_souta 22:7d93f79a3686 3419
M_souta 22:7d93f79a3686 3420 //モータの駆動
M_souta 22:7d93f79a3686 3421 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 3422 if (tirePWM[i] > 255){
M_souta 22:7d93f79a3686 3423 tirePWM[i] = 255;
M_souta 22:7d93f79a3686 3424 } else if (tirePWM[i] < -255) {
M_souta 22:7d93f79a3686 3425 tirePWM[i] = -255;
M_souta 22:7d93f79a3686 3426 }
M_souta 22:7d93f79a3686 3427 }
M_souta 22:7d93f79a3686 3428
M_souta 22:7d93f79a3686 3429 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 3430 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 3431 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 3432 }
M_souta 22:7d93f79a3686 3433 } else {
M_souta 22:7d93f79a3686 3434 PIDflag = false;
M_souta 22:7d93f79a3686 3435 rotaconSampling.stop();
M_souta 22:7d93f79a3686 3436 rotaconSampling.reset();
M_souta 22:7d93f79a3686 3437 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 3438 encoder[i].reset();
M_souta 22:7d93f79a3686 3439 pulsePV[i] = 0;
M_souta 22:7d93f79a3686 3440 pulseCV[i] = 0;
M_souta 22:7d93f79a3686 3441 timePV[i] = 0;
M_souta 22:7d93f79a3686 3442 timeCV[i] = 0;
M_souta 22:7d93f79a3686 3443 tirePWM[i] = 0;
M_souta 22:7d93f79a3686 3444 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 3445 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 3446 }
M_souta 22:7d93f79a3686 3447 }
M_souta 27:dd6c9feb7493 3448 */
kishibekairohan 12:c09b3e08a316 3449 }
kishibekairohan 12:c09b3e08a316 3450 #endif
kishibekairohan 12:c09b3e08a316 3451
kishibekairohan 13:b6e02d6261d7 3452 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 3453 static void Process9()
kishibekairohan 12:c09b3e08a316 3454 {
kishibekairohan 2:c015739085d3 3455
t_yamamoto 0:669ef71cba68 3456 }
t_yamamoto 0:669ef71cba68 3457 #endif
t_yamamoto 0:669ef71cba68 3458 #endif
t_yamamoto 0:669ef71cba68 3459 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 3460
t_yamamoto 0:669ef71cba68 3461 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 3462 {
t_yamamoto 0:669ef71cba68 3463
t_yamamoto 0:669ef71cba68 3464 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 3465 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 3466 #endif
t_yamamoto 0:669ef71cba68 3467
t_yamamoto 0:669ef71cba68 3468 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 3469 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 3470 {
t_yamamoto 0:669ef71cba68 3471 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 3472 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 3473 }
t_yamamoto 0:669ef71cba68 3474 #endif
t_yamamoto 0:669ef71cba68 3475 }
t_yamamoto 0:669ef71cba68 3476
kishibekairohan 16:3f2c2d89372b 3477 void BuzzerTimer_func(){
kishibekairohan 16:3f2c2d89372b 3478 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 3479 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 3480 }
kishibekairohan 11:028a150943b5 3481
kishibekairohan 11:028a150943b5 3482 void TapeLedEms_func() {
kishibekairohan 16:3f2c2d89372b 3483 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 3484 }
kishibekairohan 16:3f2c2d89372b 3485
kishibekairohan 16:3f2c2d89372b 3486 #pragma region USER-DEFINED-FUNCTIONS
M_souta 22:7d93f79a3686 3487 void tirePID() {
M_souta 22:7d93f79a3686 3488 if(PIDflag == true) {
M_souta 22:7d93f79a3686 3489 //加算するPID値の算出
M_souta 22:7d93f79a3686 3490 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta 22:7d93f79a3686 3491 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta 22:7d93f79a3686 3492 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta 22:7d93f79a3686 3493 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta 22:7d93f79a3686 3494 //PID値の加算
M_souta 22:7d93f79a3686 3495 tirePWM[FL] += rotaconPID[0].GetMV();
M_souta 22:7d93f79a3686 3496 tirePWM[BL] += rotaconPID[1].GetMV();
M_souta 22:7d93f79a3686 3497 tirePWM[FR] += rotaconPID[2].GetMV();
M_souta 22:7d93f79a3686 3498 tirePWM[BR] += rotaconPID[3].GetMV();
M_souta 22:7d93f79a3686 3499 }
M_souta 22:7d93f79a3686 3500 }
kishibekairohan 16:3f2c2d89372b 3501
M_souta 22:7d93f79a3686 3502 int lineCast(char k) {
M_souta 22:7d93f79a3686 3503 int l;
M_souta 22:7d93f79a3686 3504 switch(k) {
M_souta 22:7d93f79a3686 3505 case 255:
M_souta 22:7d93f79a3686 3506 l = -1;
M_souta 22:7d93f79a3686 3507 break;
M_souta 22:7d93f79a3686 3508 case 254:
M_souta 22:7d93f79a3686 3509 l = -2;
M_souta 22:7d93f79a3686 3510 break;
M_souta 22:7d93f79a3686 3511 case 253:
M_souta 22:7d93f79a3686 3512 l = -3;
M_souta 22:7d93f79a3686 3513 break;
M_souta 22:7d93f79a3686 3514 default:
M_souta 22:7d93f79a3686 3515 l = k;
M_souta 22:7d93f79a3686 3516 }
M_souta 22:7d93f79a3686 3517 return l;
M_souta 22:7d93f79a3686 3518 }
M_souta 27:dd6c9feb7493 3519
M_souta 27:dd6c9feb7493 3520
kishibekairohan 16:3f2c2d89372b 3521
t_yamamoto 0:669ef71cba68 3522 #pragma endregion