aa

Dependencies:   mbed

Committer:
M_souta
Date:
Sat Sep 28 23:02:17 2019 +0000
Revision:
26:4c0ce2f05688
Parent:
25:b3a9f34b201d
Child:
27:dd9f27fce7d1
lll; ;

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