Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@25:b3a9f34b201d, 2019-09-27 (annotated)

Committer:
M_souta
Date:
Fri Sep 27 07:51:32 2019 +0000
Revision:
25:b3a9f34b201d
Parent:
24:370616a56815
Child:
26:4c0ce2f05688
ii

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 22:7d93f79a3686 140 float pw = 0;
M_souta 22:7d93f79a3686 141 int lineFase = 0;
M_souta 22:7d93f79a3686 142 bool lineCheck = false;
M_souta 25:b3a9f34b201d 143 bool lineFlag = false;
M_souta 25:b3a9f34b201d 144 bool adjAnable = false;
M_souta 25:b3a9f34b201d 145 int adj = 0;
M_souta 25:b3a9f34b201d 146
M_souta 25:b3a9f34b201d 147 int count = 0;
M_souta 25:b3a9f34b201d 148 int lineCount = 0;
M_souta 25:b3a9f34b201d 149 int targetCount = 0;
M_souta 25:b3a9f34b201d 150
M_souta 25:b3a9f34b201d 151 bool startFlag = true;
M_souta 25:b3a9f34b201d 152
M_souta 22:7d93f79a3686 153 int linePWM;
M_souta 22:7d93f79a3686 154 int adj_F;
M_souta 22:7d93f79a3686 155 int adj_B;
M_souta 22:7d93f79a3686 156
M_souta 22:7d93f79a3686 157 int mode = 0;
M_souta 22:7d93f79a3686 158
M_souta 22:7d93f79a3686 159 // ************* Line ************** //
M_souta 22:7d93f79a3686 160
M_souta 21:e3b58d675c1c 161 const int omni[15][15] =
M_souta 20:eae8c84f318c 162 {
M_souta 21:e3b58d675c1c 163 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 164 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 165 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta 21:e3b58d675c1c 166 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta 21:e3b58d675c1c 167 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta 21:e3b58d675c1c 168 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta 21:e3b58d675c1c 169 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta 21:e3b58d675c1c 170 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta 21:e3b58d675c1c 171 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta 21:e3b58d675c1c 172 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta 21:e3b58d675c1c 173 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta 21:e3b58d675c1c 174 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta 21:e3b58d675c1c 175 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta 21:e3b58d675c1c 176 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta 21:e3b58d675c1c 177 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 178 };
M_souta 21:e3b58d675c1c 179
M_souta 21:e3b58d675c1c 180 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 181
M_souta 21:e3b58d675c1c 182 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 183 uint8_t SetStatus(int pwmVal) {
M_souta 21:e3b58d675c1c 184 if (pwmVal < 0) return BACK;
M_souta 21:e3b58d675c1c 185 else if (pwmVal > 0) return FOR;
M_souta 21:e3b58d675c1c 186 else if (pwmVal == 0) return BRAKE;
M_souta 21:e3b58d675c1c 187 else return BRAKE;
M_souta 20:eae8c84f318c 188 }
M_souta 21:e3b58d675c1c 189 uint8_t SetPWM(int);
M_souta 21:e3b58d675c1c 190 uint8_t SetPWM(int pwmVal) {
M_souta 21:e3b58d675c1c 191 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 21:e3b58d675c1c 192 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 193 }
M_souta 18:c694bae76e51 194
t_yamamoto 0:669ef71cba68 195 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 196
t_yamamoto 0:669ef71cba68 197 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 198 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 199 static void Process0(void);
t_yamamoto 0:669ef71cba68 200 #endif
t_yamamoto 0:669ef71cba68 201 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 202 static void Process1(void);
t_yamamoto 0:669ef71cba68 203 #endif
t_yamamoto 0:669ef71cba68 204 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 205 static void Process2(void);
t_yamamoto 0:669ef71cba68 206 #endif
t_yamamoto 0:669ef71cba68 207 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 208 static void Process3(void);
t_yamamoto 0:669ef71cba68 209 #endif
t_yamamoto 0:669ef71cba68 210 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 211 static void Process4(void);
t_yamamoto 0:669ef71cba68 212 #endif
t_yamamoto 0:669ef71cba68 213 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 214 static void Process5(void);
t_yamamoto 0:669ef71cba68 215 #endif
t_yamamoto 0:669ef71cba68 216 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 217 static void Process6(void);
t_yamamoto 0:669ef71cba68 218 #endif
t_yamamoto 0:669ef71cba68 219 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 220 static void Process7(void);
t_yamamoto 0:669ef71cba68 221 #endif
t_yamamoto 0:669ef71cba68 222 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 223 static void Process8(void);
t_yamamoto 0:669ef71cba68 224 #endif
t_yamamoto 0:669ef71cba68 225 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 226 static void Process9(void);
t_yamamoto 0:669ef71cba68 227 #endif
t_yamamoto 0:669ef71cba68 228 #endif
t_yamamoto 0:669ef71cba68 229
t_yamamoto 0:669ef71cba68 230 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 231 {
t_yamamoto 0:669ef71cba68 232 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 16:3f2c2d89372b 233 /*Replace here with the initialization code of your variables.*/
M_souta 25:b3a9f34b201d 234 //rotaconPIDtimer.attach(tirePID,0.1);
M_souta 25:b3a9f34b201d 235
M_souta 25:b3a9f34b201d 236 //DigitalOut Air_16(LS_16);
M_souta 25:b3a9f34b201d 237 //DigitalOut Air_17(LS_17);
M_souta 25:b3a9f34b201d 238 //DigitalOut Air_18(LS_18);
M_souta 25:b3a9f34b201d 239 //DigitalOut Air_19(LS_19);
M_souta 21:e3b58d675c1c 240
t_yamamoto 0:669ef71cba68 241 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 242
t_yamamoto 0:669ef71cba68 243 lock = true;
t_yamamoto 0:669ef71cba68 244 processChangeComp = true;
t_yamamoto 0:669ef71cba68 245 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 246
t_yamamoto 0:669ef71cba68 247 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 248 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 249 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 250 #endif
t_yamamoto 0:669ef71cba68 251 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 252 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 253 #endif
t_yamamoto 0:669ef71cba68 254 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 255 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 256 #endif
t_yamamoto 0:669ef71cba68 257 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 258 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 259 #endif
t_yamamoto 0:669ef71cba68 260 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 261 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 262 #endif
t_yamamoto 0:669ef71cba68 263 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 264 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 265 #endif
t_yamamoto 0:669ef71cba68 266 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 267 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 268 #endif
t_yamamoto 0:669ef71cba68 269 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 270 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 271 #endif
t_yamamoto 0:669ef71cba68 272 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 273 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 274 #endif
t_yamamoto 0:669ef71cba68 275 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 276 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 277 #endif
t_yamamoto 0:669ef71cba68 278 #endif
t_yamamoto 0:669ef71cba68 279 }
t_yamamoto 0:669ef71cba68 280
t_yamamoto 0:669ef71cba68 281 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 282 {
t_yamamoto 0:669ef71cba68 283 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 284 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 285
t_yamamoto 0:669ef71cba68 286 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 287 {
t_yamamoto 0:669ef71cba68 288 current++;
t_yamamoto 0:669ef71cba68 289 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 290 processChangeComp = false;
t_yamamoto 0:669ef71cba68 291 }
t_yamamoto 0:669ef71cba68 292 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 293 {
t_yamamoto 0:669ef71cba68 294 current--;
t_yamamoto 0:669ef71cba68 295 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 296 processChangeComp = false;
t_yamamoto 0:669ef71cba68 297 }
t_yamamoto 0:669ef71cba68 298 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 299 #endif
t_yamamoto 0:669ef71cba68 300
t_yamamoto 0:669ef71cba68 301 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 302 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 303 #endif
t_yamamoto 0:669ef71cba68 304
t_yamamoto 0:669ef71cba68 305 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 306 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 307 #endif
t_yamamoto 0:669ef71cba68 308
t_yamamoto 0:669ef71cba68 309 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 310 ACTUATORHUB::ActuatorHub::Update();
M_souta 21:e3b58d675c1c 311 //LINEHUB::LineHub::Update();
t_yamamoto 0:669ef71cba68 312 #endif
t_yamamoto 0:669ef71cba68 313
t_yamamoto 0:669ef71cba68 314 }
t_yamamoto 0:669ef71cba68 315
kishibekairohan 2:c015739085d3 316
kishibekairohan 2:c015739085d3 317
t_yamamoto 0:669ef71cba68 318 void SystemProcess()
t_yamamoto 0:669ef71cba68 319 {
t_yamamoto 0:669ef71cba68 320 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 321
t_yamamoto 0:669ef71cba68 322 while(1)
kishibekairohan 15:dfcec98f5aa9 323 {
M_souta 21:e3b58d675c1c 324 int g[8];
M_souta 21:e3b58d675c1c 325 for(int i = 0; i < 8; i++){
M_souta 22:7d93f79a3686 326 g[i] = lineCast(LineHub::GetPara(i));
M_souta 21:e3b58d675c1c 327 }
M_souta 22:7d93f79a3686 328
M_souta 22:7d93f79a3686 329 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 330
M_souta 25:b3a9f34b201d 331
M_souta 25:b3a9f34b201d 332 if(LimitSw::IsPressed(START_SW) && startFlag == true) {
M_souta 25:b3a9f34b201d 333 startFlag == false;
M_souta 25:b3a9f34b201d 334 lock = false;
M_souta 25:b3a9f34b201d 335 lineFase = 0;
M_souta 25:b3a9f34b201d 336 lineCount = 0;
M_souta 25:b3a9f34b201d 337 lineCheck = false;
M_souta 25:b3a9f34b201d 338 count = 0;
M_souta 25:b3a9f34b201d 339 /*
M_souta 25:b3a9f34b201d 340 if(LimitSw::IsPressed(ISREDBLUE_SW) {
M_souta 25:b3a9f34b201d 341 current = 4;
M_souta 25:b3a9f34b201d 342 } else {
M_souta 25:b3a9f34b201d 343 current = 5;
M_souta 25:b3a9f34b201d 344 }
M_souta 25:b3a9f34b201d 345 */
M_souta 25:b3a9f34b201d 346 current = 4;
M_souta 25:b3a9f34b201d 347 }
M_souta 20:eae8c84f318c 348
kishibekairohan 17:50dc4b449e69 349 buzzer.period(1.0/800);
kishibekairohan 15:dfcec98f5aa9 350
t_yamamoto 0:669ef71cba68 351 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 352 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 353 #endif
t_yamamoto 0:669ef71cba68 354
t_yamamoto 0:669ef71cba68 355 #ifdef USE_ERRORCHECK
M_souta 25:b3a9f34b201d 356 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
t_yamamoto 0:669ef71cba68 357 {
t_yamamoto 0:669ef71cba68 358 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 359 AllActuatorReset();
t_yamamoto 0:669ef71cba68 360 lock = true;
t_yamamoto 0:669ef71cba68 361 }
t_yamamoto 0:669ef71cba68 362 else
t_yamamoto 0:669ef71cba68 363 #endif
t_yamamoto 0:669ef71cba68 364 {
t_yamamoto 0:669ef71cba68 365
t_yamamoto 0:669ef71cba68 366 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 367 if(!lock)
t_yamamoto 0:669ef71cba68 368 {
t_yamamoto 0:669ef71cba68 369 Process[current]();
t_yamamoto 0:669ef71cba68 370 }
t_yamamoto 0:669ef71cba68 371 else
t_yamamoto 0:669ef71cba68 372 #endif
t_yamamoto 0:669ef71cba68 373 {
t_yamamoto 0:669ef71cba68 374 //ロック時の処理
t_yamamoto 0:669ef71cba68 375 }
t_yamamoto 0:669ef71cba68 376 }
t_yamamoto 0:669ef71cba68 377
M_souta 22:7d93f79a3686 378
kishibekairohan 16:3f2c2d89372b 379 //Emergency!
M_souta 22:7d93f79a3686 380 /*
kishibekairohan 17:50dc4b449e69 381 if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan 17:50dc4b449e69 382 buzzer = 0;
kishibekairohan 17:50dc4b449e69 383 BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan 17:50dc4b449e69 384 EMGflag = true;
kishibekairohan 17:50dc4b449e69 385 LED_DEBUG0 = 1;
kishibekairohan 9:f93fc79a49ea 386 }
kishibekairohan 17:50dc4b449e69 387 if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan 17:50dc4b449e69 388 buzzer = 1;
kishibekairohan 9:f93fc79a49ea 389 BuzzerTimer.detach();
kishibekairohan 17:50dc4b449e69 390 EMGflag = false;
kishibekairohan 9:f93fc79a49ea 391 }
M_souta 22:7d93f79a3686 392 */
t_yamamoto 0:669ef71cba68 393 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 394 }
t_yamamoto 0:669ef71cba68 395 }
t_yamamoto 0:669ef71cba68 396
kishibekairohan 2:c015739085d3 397
kishibekairohan 2:c015739085d3 398
kishibekairohan 2:c015739085d3 399
t_yamamoto 0:669ef71cba68 400 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 401 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 402 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 403 static void Process0()
kishibekairohan 10:1295d39fec3a 404 {
M_souta 21:e3b58d675c1c 405 AllActuatorReset();
t_yamamoto 0:669ef71cba68 406 }
t_yamamoto 0:669ef71cba68 407 #endif
t_yamamoto 0:669ef71cba68 408
t_yamamoto 0:669ef71cba68 409 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 410 static void Process1()
t_yamamoto 0:669ef71cba68 411 {
M_souta 22:7d93f79a3686 412
M_souta 22:7d93f79a3686 413 PIDflag = false;
M_souta 22:7d93f79a3686 414
M_souta 21:e3b58d675c1c 415 if(controller->Button.UP) {
M_souta 21:e3b58d675c1c 416 motor[LIFT_LB].dir = FOR;
M_souta 23:c853372cf626 417 motor[LIFT_LB].pwm = 190;
M_souta 21:e3b58d675c1c 418 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 419 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 420 } else if(controller->Button.DOWN) {
M_souta 21:e3b58d675c1c 421 motor[LIFT_LB].dir = BACK;
M_souta 21:e3b58d675c1c 422 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 423 motor[LIFT_RB].dir = FOR;
M_souta 23:c853372cf626 424 motor[LIFT_RB].pwm = 190;
M_souta 23:c853372cf626 425 } else if(controller->Button.LEFT) {
M_souta 23:c853372cf626 426 motor[LIFT_LB].dir = FOR;
M_souta 23:c853372cf626 427 motor[LIFT_LB].pwm = 180;
M_souta 23:c853372cf626 428 } else if(controller->Button.RIGHT) {
M_souta 23:c853372cf626 429 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 430 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 431 } else {
M_souta 25:b3a9f34b201d 432 motor[LIFT_LB].dir = BRAKE;
M_souta 23:c853372cf626 433 motor[LIFT_LB].pwm = 255;
M_souta 23:c853372cf626 434 motor[LIFT_RB].dir = BACK;
M_souta 25:b3a9f34b201d 435 motor[LIFT_RB].pwm = 20;
M_souta 21:e3b58d675c1c 436 }
M_souta 22:7d93f79a3686 437
7ka884 4:ba9df71868df 438
M_souta 21:e3b58d675c1c 439 if(controller->Button.X) {
M_souta 21:e3b58d675c1c 440 motor[LIFT_U].dir = FOR;
M_souta 21:e3b58d675c1c 441 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 442 } else if(controller->Button.Y) {
M_souta 21:e3b58d675c1c 443 motor[LIFT_U].dir = BACK;
M_souta 25:b3a9f34b201d 444 motor[LIFT_U].pwm = 230;
M_souta 21:e3b58d675c1c 445 } else {
M_souta 21:e3b58d675c1c 446 motor[LIFT_U].dir = BRAKE;
M_souta 25:b3a9f34b201d 447 motor[LIFT_U].pwm = 255;
M_souta 21:e3b58d675c1c 448 }
M_souta 21:e3b58d675c1c 449
M_souta 21:e3b58d675c1c 450 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
M_souta 25:b3a9f34b201d 451 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 452 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 453 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 25:b3a9f34b201d 454 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 455
M_souta 25:b3a9f34b201d 456 motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 457 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 458 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 25:b3a9f34b201d 459 motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 21:e3b58d675c1c 460 } else {
M_souta 25:b3a9f34b201d 461 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 462 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 463 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 25:b3a9f34b201d 464 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 465
M_souta 21:e3b58d675c1c 466 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 467 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 468 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 469 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 470 }
M_souta 25:b3a9f34b201d 471
M_souta 25:b3a9f34b201d 472 if(controller->Button.ZR) {
M_souta 25:b3a9f34b201d 473 solenoid.solenoid4 = SOLENOID_ON;
M_souta 25:b3a9f34b201d 474 } else {
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 22:7d93f79a3686 489 if(LimiSw::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 22:7d93f79a3686 495 seitchFlag_LB = true;
M_souta 22:7d93f79a3686 496 }
M_souta 22:7d93f79a3686 497 }
M_souta 22:7d93f79a3686 498 if(LimiSw::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 22:7d93f79a3686 504 seitchFlag_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 22:7d93f79a3686 510 if(!(state == UPPER)) {
M_souta 22:7d93f79a3686 511 state++;
M_souta 22:7d93f79a3686 512 motor[LIFT_LB].dir = BACK;
M_souta 22:7d93f79a3686 513 motor[LIFT_RB].dir = FOR;
M_souta 22:7d93f79a3686 514 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 515 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 516 }
M_souta 22:7d93f79a3686 517 } else if(controller->Button.DOWN) {
M_souta 22:7d93f79a3686 518 if(!(state == LOWER)) {
M_souta 22:7d93f79a3686 519 state--;
M_souta 22:7d93f79a3686 520 moving = true;
M_souta 22:7d93f79a3686 521 motor[LIFT_LB].dir = FOR;
M_souta 22:7d93f79a3686 522 motor[LIFT_RB].dir = BACK;
M_souta 22:7d93f79a3686 523 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 524 motor[LIFT_RB].pwm = 200;
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 */
kishibekairohan 13:b6e02d6261d7 534 }
kishibekairohan 13:b6e02d6261d7 535 #endif
kishibekairohan 13:b6e02d6261d7 536
kishibekairohan 13:b6e02d6261d7 537 #if USE_PROCESS_NUM>3
kishibekairohan 16:3f2c2d89372b 538 static void Process3()
kishibekairohan 13:b6e02d6261d7 539 {
M_souta 25:b3a9f34b201d 540 startFlag = true;
M_souta 22:7d93f79a3686 541 AllActuatorReset();
M_souta 23:c853372cf626 542 lineFase = 0;
M_souta 25:b3a9f34b201d 543 lineCheck = false;
M_souta 25:b3a9f34b201d 544 lineCount = 0;
M_souta 25:b3a9f34b201d 545 count = 0;
kishibekairohan 13:b6e02d6261d7 546 }
kishibekairohan 13:b6e02d6261d7 547 #endif
kishibekairohan 13:b6e02d6261d7 548
kishibekairohan 13:b6e02d6261d7 549 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 550 static void Process4()
M_souta 22:7d93f79a3686 551 {
M_souta 25:b3a9f34b201d 552 LED_DEBUG0 = LED_ON;
M_souta 22:7d93f79a3686 553
M_souta 25:b3a9f34b201d 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 for(int i = 0; i < 8; i++) {
M_souta 25:b3a9f34b201d 562 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 25:b3a9f34b201d 563 }
M_souta 22:7d93f79a3686 564
M_souta 25:b3a9f34b201d 565 if(lineFase == 0) {
M_souta 25:b3a9f34b201d 566 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 567 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 568 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 569 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 570 if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta 25:b3a9f34b201d 571 lineFase = 1;
M_souta 25:b3a9f34b201d 572 }
M_souta 25:b3a9f34b201d 573 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 574 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 575 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 576 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 577
M_souta 25:b3a9f34b201d 578 } else if(lineFase == 1) { // 前 ライントレース
M_souta 25:b3a9f34b201d 579 switch(linePara[0]) {
M_souta 22:7d93f79a3686 580 case -2:
M_souta 25:b3a9f34b201d 581 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 582 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 583 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 584 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 585 adjAnable = true;
M_souta 25:b3a9f34b201d 586 break;
M_souta 25:b3a9f34b201d 587 case -3:
M_souta 25:b3a9f34b201d 588 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 589 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 590 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 591 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 592 adjAnable = true;
M_souta 25:b3a9f34b201d 593 break;
M_souta 25:b3a9f34b201d 594 case -1:
M_souta 25:b3a9f34b201d 595 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 596 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 597 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 598 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 599 adjAnable = true;
M_souta 25:b3a9f34b201d 600 break;
M_souta 25:b3a9f34b201d 601 case 0:
M_souta 25:b3a9f34b201d 602 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 603 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 604 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 605 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 606 adjAnable = true;
M_souta 25:b3a9f34b201d 607 break;
M_souta 25:b3a9f34b201d 608 case 1:
M_souta 25:b3a9f34b201d 609 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 610 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 611 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 612 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 613 adjAnable = true;
M_souta 25:b3a9f34b201d 614 break;
M_souta 25:b3a9f34b201d 615 case 3:
M_souta 25:b3a9f34b201d 616 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 617 tirePWM[TIRE_BL] = 10;
M_souta 25:b3a9f34b201d 618 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 619 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 620 adjAnable = true;
M_souta 25:b3a9f34b201d 621 break;
M_souta 25:b3a9f34b201d 622 case 2:
M_souta 25:b3a9f34b201d 623 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 624 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 625 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 626 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 627 adjAnable = true;
M_souta 25:b3a9f34b201d 628 break;
M_souta 25:b3a9f34b201d 629 case 'A':
M_souta 25:b3a9f34b201d 630 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 631 lineCheck = true;
M_souta 25:b3a9f34b201d 632 lineCount = 0;
M_souta 25:b3a9f34b201d 633 count++;
M_souta 25:b3a9f34b201d 634 }
M_souta 25:b3a9f34b201d 635 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 636 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 637 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 638 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 639 adjAnable = true;
M_souta 25:b3a9f34b201d 640 break;
M_souta 25:b3a9f34b201d 641 case 'N':
M_souta 25:b3a9f34b201d 642 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta 25:b3a9f34b201d 643 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta 25:b3a9f34b201d 644 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta 25:b3a9f34b201d 645 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta 25:b3a9f34b201d 646 adjAnable = false;
M_souta 25:b3a9f34b201d 647 break;
M_souta 25:b3a9f34b201d 648 default:
M_souta 25:b3a9f34b201d 649 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 650 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 651 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 652 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 653 adjAnable = false;
M_souta 25:b3a9f34b201d 654 }
M_souta 25:b3a9f34b201d 655
M_souta 25:b3a9f34b201d 656 if(adjAnable){
M_souta 25:b3a9f34b201d 657 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta 25:b3a9f34b201d 658 } else {
M_souta 25:b3a9f34b201d 659 adj = 0;
M_souta 25:b3a9f34b201d 660 }
M_souta 25:b3a9f34b201d 661
M_souta 25:b3a9f34b201d 662 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 663 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 664 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 665 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 666 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 667 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 668 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 669 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 670
M_souta 25:b3a9f34b201d 671 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 672 lineCount++;
M_souta 25:b3a9f34b201d 673 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 674 }
M_souta 25:b3a9f34b201d 675 if(count == 3) {
M_souta 25:b3a9f34b201d 676 count = 0;
M_souta 25:b3a9f34b201d 677 lineFase = 2;
M_souta 25:b3a9f34b201d 678 lineCount = 0;
M_souta 25:b3a9f34b201d 679 lineCheck = false;
M_souta 25:b3a9f34b201d 680 }
M_souta 25:b3a9f34b201d 681 } else if(lineFase == 2) { // 前 低速
M_souta 25:b3a9f34b201d 682 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 683 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 684 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 685 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 686 if(linePara[4] == 0) {
M_souta 25:b3a9f34b201d 687 lineFase = 3;
M_souta 25:b3a9f34b201d 688 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 689 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 690 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 691 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 692 }
M_souta 25:b3a9f34b201d 693 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 694 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 695 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 696 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 697 } else if(lineFase == 3) { // 右 ライントレース
M_souta 25:b3a9f34b201d 698 switch(linePara[4]) {
M_souta 25:b3a9f34b201d 699 case -2:
M_souta 25:b3a9f34b201d 700 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 701 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 702 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 703 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 704 adjAnable = true;
M_souta 22:7d93f79a3686 705 break;
M_souta 22:7d93f79a3686 706 case -3:
M_souta 25:b3a9f34b201d 707 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 708 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 709 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 710 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 711 adjAnable = true;
M_souta 22:7d93f79a3686 712 break;
M_souta 22:7d93f79a3686 713 case -1:
M_souta 25:b3a9f34b201d 714 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 715 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 716 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 717 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 718 adjAnable = true;
M_souta 22:7d93f79a3686 719 break;
M_souta 22:7d93f79a3686 720 case 0:
M_souta 25:b3a9f34b201d 721 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 722 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 723 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 724 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 725 adjAnable = true;
M_souta 22:7d93f79a3686 726 break;
M_souta 22:7d93f79a3686 727 case 1:
M_souta 25:b3a9f34b201d 728 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 729 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 730 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 731 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 732 adjAnable = true;
M_souta 22:7d93f79a3686 733 break;
M_souta 22:7d93f79a3686 734 case 3:
M_souta 25:b3a9f34b201d 735 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 736 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 737 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 738 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 739 adjAnable = true;
M_souta 22:7d93f79a3686 740 break;
M_souta 22:7d93f79a3686 741 case 2:
M_souta 25:b3a9f34b201d 742 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 743 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 744 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 745 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 746 adjAnable = true;
M_souta 22:7d93f79a3686 747 break;
M_souta 22:7d93f79a3686 748 case 'A':
M_souta 25:b3a9f34b201d 749 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 750 lineCheck = true;
M_souta 25:b3a9f34b201d 751 lineCount = 0;
M_souta 25:b3a9f34b201d 752 count++;
M_souta 25:b3a9f34b201d 753 }
M_souta 25:b3a9f34b201d 754 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 755 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 756 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 757 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 758 adjAnable = true;
M_souta 25:b3a9f34b201d 759 break;
M_souta 25:b3a9f34b201d 760 case 'N':
M_souta 25:b3a9f34b201d 761 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 762 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 763 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 764 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 765 adjAnable = false;
M_souta 25:b3a9f34b201d 766 break;
M_souta 25:b3a9f34b201d 767 default:
M_souta 25:b3a9f34b201d 768 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 769 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 770 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 771 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 772 adjAnable = false;
M_souta 25:b3a9f34b201d 773 }
M_souta 25:b3a9f34b201d 774
M_souta 25:b3a9f34b201d 775 if(adjAnable){
M_souta 25:b3a9f34b201d 776 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta 25:b3a9f34b201d 777 } else {
M_souta 25:b3a9f34b201d 778 adj = 0;
M_souta 25:b3a9f34b201d 779 }
M_souta 25:b3a9f34b201d 780
M_souta 25:b3a9f34b201d 781 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 782 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 783 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 784 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 785 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 786 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 787 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 788 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 789
M_souta 25:b3a9f34b201d 790 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 791 lineCount++;
M_souta 25:b3a9f34b201d 792 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 793 }
M_souta 25:b3a9f34b201d 794
M_souta 25:b3a9f34b201d 795 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta 25:b3a9f34b201d 796 targetCount = 3;
M_souta 25:b3a9f34b201d 797 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta 25:b3a9f34b201d 798 targetCount = 3;
M_souta 25:b3a9f34b201d 799 } else {
M_souta 25:b3a9f34b201d 800 targetCount = 2;
M_souta 25:b3a9f34b201d 801 }
M_souta 25:b3a9f34b201d 802
M_souta 25:b3a9f34b201d 803 if(count == targetCount) {
M_souta 25:b3a9f34b201d 804 count = 0;
M_souta 25:b3a9f34b201d 805 lineFase = 4;
M_souta 25:b3a9f34b201d 806 lineCount = 0;
M_souta 25:b3a9f34b201d 807 lineCheck = false;
M_souta 25:b3a9f34b201d 808 }
M_souta 25:b3a9f34b201d 809 } else if(lineFase == 4) { // 右 低速
M_souta 25:b3a9f34b201d 810 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 811 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 812 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 813 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 814 if (linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 815 lineFase = 6;
M_souta 25:b3a9f34b201d 816 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 817 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 818 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 819 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 820 }
M_souta 25:b3a9f34b201d 821 motor[TIRE_FL].pwm = 14;
M_souta 25:b3a9f34b201d 822 motor[TIRE_BL].pwm = 14;
M_souta 25:b3a9f34b201d 823 motor[TIRE_BR].pwm = 14;
M_souta 25:b3a9f34b201d 824 motor[TIRE_FR].pwm = 14;
M_souta 25:b3a9f34b201d 825 } else if (lineFase == 5) {
M_souta 25:b3a9f34b201d 826 lineFase = 6;
M_souta 25:b3a9f34b201d 827 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 828 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 829 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 830 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 831 } else if(lineFase == 6) { // タオル1 検知
M_souta 25:b3a9f34b201d 832 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 833 lineFase = 7;
M_souta 25:b3a9f34b201d 834 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 835 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 836 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 837 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 838 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 839 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 840 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 841 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 842 } else if(LimitSw::IsPressed(TOW_1L)) {
M_souta 25:b3a9f34b201d 843 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 844 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 845 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 846 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 847 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 848 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 849 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 850 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 851 } else if(LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 852 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 853 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 854 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 855 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 856 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 857 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 858 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 859 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 860 } else {
M_souta 25:b3a9f34b201d 861 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 862 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 863 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 864 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 865 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 866 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 867 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 868 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 869 }
M_souta 25:b3a9f34b201d 870 } else if(lineFase == 7) { // ライン 修正
M_souta 25:b3a9f34b201d 871 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 872 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 873 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 874 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 875 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 876 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 877 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 878 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 879 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 880 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 881 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 882 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 883 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 884 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 885 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 886 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 887 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 888 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 889 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 890 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 891 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 892 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 893 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 894 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 895 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 896 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 897 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 898 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 899 lineFase = 8;
M_souta 25:b3a9f34b201d 900 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 901 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 902 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 903 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 904 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 905 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 906 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 907 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 908 } else {
M_souta 25:b3a9f34b201d 909 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 910 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 911 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 912 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 913 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 914 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 915 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 916 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 917 }
M_souta 25:b3a9f34b201d 918 } else if(lineFase == 8) { // タオル1 解放
M_souta 25:b3a9f34b201d 919 //solenoid.TOWEL1 = SOLENOID_ON;
M_souta 25:b3a9f34b201d 920 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 921 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 922 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 923 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 924 lineFase = 9;
M_souta 25:b3a9f34b201d 925 } else if(lineFase == 9) { // 前
M_souta 25:b3a9f34b201d 926 switch(linePara[LINE_TOW_1]) {
M_souta 25:b3a9f34b201d 927 case 2:
M_souta 25:b3a9f34b201d 928 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 929 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 930 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 931 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 932 adjAnable = true;
M_souta 25:b3a9f34b201d 933 break;
M_souta 25:b3a9f34b201d 934 case 3:
M_souta 25:b3a9f34b201d 935 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 936 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 937 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 938 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 939 adjAnable = true;
M_souta 25:b3a9f34b201d 940 break;
M_souta 25:b3a9f34b201d 941 case 1:
M_souta 25:b3a9f34b201d 942 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 943 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 944 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 945 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 946 adjAnable = true;
M_souta 25:b3a9f34b201d 947 break;
M_souta 25:b3a9f34b201d 948 case 0:
M_souta 25:b3a9f34b201d 949 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 950 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 951 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 952 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 953 adjAnable = true;
M_souta 25:b3a9f34b201d 954 break;
M_souta 25:b3a9f34b201d 955 case -1:
M_souta 25:b3a9f34b201d 956 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 957 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 958 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 959 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 960 adjAnable = true;
M_souta 25:b3a9f34b201d 961 break;
M_souta 25:b3a9f34b201d 962 case -3:
M_souta 25:b3a9f34b201d 963 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 964 tirePWM[TIRE_BL] = 10;
M_souta 25:b3a9f34b201d 965 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 966 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 967 adjAnable = true;
M_souta 25:b3a9f34b201d 968 break;
M_souta 25:b3a9f34b201d 969 case -2:
M_souta 25:b3a9f34b201d 970 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 971 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 972 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 973 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 974 adjAnable = true;
M_souta 25:b3a9f34b201d 975 break;
M_souta 25:b3a9f34b201d 976 case 'A':
M_souta 25:b3a9f34b201d 977 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 978 lineCheck = true;
M_souta 25:b3a9f34b201d 979 lineCount = 0;
M_souta 25:b3a9f34b201d 980 count++;
M_souta 25:b3a9f34b201d 981 }
M_souta 25:b3a9f34b201d 982 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 983 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 984 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 985 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 986 adjAnable = true;
M_souta 22:7d93f79a3686 987 break;
M_souta 22:7d93f79a3686 988 case 'N':
M_souta 25:b3a9f34b201d 989 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 990 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 991 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 992 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 993 adjAnable = false;
M_souta 25:b3a9f34b201d 994 break;
M_souta 25:b3a9f34b201d 995 default:
M_souta 25:b3a9f34b201d 996 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 997 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 998 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 999 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1000 adjAnable = false;
M_souta 25:b3a9f34b201d 1001 }
M_souta 25:b3a9f34b201d 1002
M_souta 25:b3a9f34b201d 1003 if(adjAnable){
M_souta 25:b3a9f34b201d 1004 adj = 0;
M_souta 25:b3a9f34b201d 1005 } else {
M_souta 25:b3a9f34b201d 1006 adj = 0;
M_souta 25:b3a9f34b201d 1007 }
M_souta 25:b3a9f34b201d 1008
M_souta 25:b3a9f34b201d 1009 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1010 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1011 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1012 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1013 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1014 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1015 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1016 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1017
M_souta 25:b3a9f34b201d 1018 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1019 lineCount++;
M_souta 25:b3a9f34b201d 1020 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1021 }
M_souta 25:b3a9f34b201d 1022 if(count == 1) {
M_souta 25:b3a9f34b201d 1023 count = 0;
M_souta 25:b3a9f34b201d 1024 lineFase = 10;
M_souta 25:b3a9f34b201d 1025 lineCount = 0;
M_souta 25:b3a9f34b201d 1026 lineCheck = false;
M_souta 25:b3a9f34b201d 1027 }
M_souta 25:b3a9f34b201d 1028 } else if(lineFase == 10) {
M_souta 25:b3a9f34b201d 1029 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1030 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1031 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1032 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1033 if(linePara[4] == 0) {
M_souta 25:b3a9f34b201d 1034 lineFase = 11;
M_souta 25:b3a9f34b201d 1035 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1036 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1037 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1038 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1039 }
M_souta 25:b3a9f34b201d 1040 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1041 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1042 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1043 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1044 } else if(lineFase == 11) {
M_souta 25:b3a9f34b201d 1045 switch(linePara[4]) {
M_souta 25:b3a9f34b201d 1046 case -2:
M_souta 25:b3a9f34b201d 1047 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1048 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1049 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1050 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1051 adjAnable = true;
M_souta 22:7d93f79a3686 1052 break;
M_souta 25:b3a9f34b201d 1053 case -3:
M_souta 25:b3a9f34b201d 1054 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1055 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 1056 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1057 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 1058 adjAnable = true;
M_souta 25:b3a9f34b201d 1059 break;
M_souta 25:b3a9f34b201d 1060 case -1:
M_souta 25:b3a9f34b201d 1061 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1062 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 1063 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1064 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 1065 adjAnable = true;
M_souta 25:b3a9f34b201d 1066 break;
M_souta 25:b3a9f34b201d 1067 case 0:
M_souta 25:b3a9f34b201d 1068 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1069 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1070 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1071 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1072 adjAnable = true;
M_souta 25:b3a9f34b201d 1073 break;
M_souta 25:b3a9f34b201d 1074 case 1:
M_souta 25:b3a9f34b201d 1075 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 1076 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1077 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 1078 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1079 adjAnable = true;
M_souta 25:b3a9f34b201d 1080 break;
M_souta 25:b3a9f34b201d 1081 case 3:
M_souta 25:b3a9f34b201d 1082 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 1083 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1084 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 1085 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1086 adjAnable = true;
M_souta 25:b3a9f34b201d 1087 break;
M_souta 25:b3a9f34b201d 1088 case 2:
M_souta 25:b3a9f34b201d 1089 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1090 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1091 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1092 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1093 adjAnable = true;
M_souta 25:b3a9f34b201d 1094 break;
M_souta 25:b3a9f34b201d 1095 case 'A':
M_souta 25:b3a9f34b201d 1096 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1097 lineCheck = true;
M_souta 25:b3a9f34b201d 1098 lineCount = 0;
M_souta 25:b3a9f34b201d 1099 count++;
M_souta 25:b3a9f34b201d 1100 }
M_souta 25:b3a9f34b201d 1101 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1102 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1103 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1104 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1105 adjAnable = true;
M_souta 25:b3a9f34b201d 1106 break;
M_souta 25:b3a9f34b201d 1107 case 'N':
M_souta 25:b3a9f34b201d 1108 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1109 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1110 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1111 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1112 adjAnable = false;
M_souta 25:b3a9f34b201d 1113 break;
M_souta 22:7d93f79a3686 1114 default:
M_souta 25:b3a9f34b201d 1115 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1116 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1117 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1118 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1119 adjAnable = false;
M_souta 22:7d93f79a3686 1120 }
M_souta 25:b3a9f34b201d 1121
M_souta 25:b3a9f34b201d 1122 if(adjAnable){
M_souta 25:b3a9f34b201d 1123 adj = 0;//linePara[3];
M_souta 25:b3a9f34b201d 1124 } else {
M_souta 25:b3a9f34b201d 1125 adj = 0;
M_souta 25:b3a9f34b201d 1126 }
M_souta 25:b3a9f34b201d 1127
M_souta 25:b3a9f34b201d 1128 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1129 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1130 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1131 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1132 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1133 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1134 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1135 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1136
M_souta 25:b3a9f34b201d 1137 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1138 lineCount++;
M_souta 25:b3a9f34b201d 1139 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1140 }
M_souta 25:b3a9f34b201d 1141 if(count == 1) {
M_souta 25:b3a9f34b201d 1142 count = 0;
M_souta 25:b3a9f34b201d 1143 lineFase = 12;
M_souta 25:b3a9f34b201d 1144 lineCount = 0;
M_souta 25:b3a9f34b201d 1145 lineCheck = false;
M_souta 25:b3a9f34b201d 1146 }
M_souta 25:b3a9f34b201d 1147 } else if(lineFase == 12) { // 右 低速
M_souta 25:b3a9f34b201d 1148 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1149 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1150 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1151 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1152 if(linePara[LINE_TOW_2] == 0) {
M_souta 25:b3a9f34b201d 1153 lineFase = 13;
M_souta 25:b3a9f34b201d 1154 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1155 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1156 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1157 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 1158 }
M_souta 25:b3a9f34b201d 1159 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1160 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1161 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1162 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1163 } else if (lineFase == 13) {
M_souta 25:b3a9f34b201d 1164 lineFase = 14;
M_souta 25:b3a9f34b201d 1165 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1166 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1167 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1168 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1169 } else if(lineFase == 14) { // タオル2 竿検知
M_souta 25:b3a9f34b201d 1170 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta 25:b3a9f34b201d 1171 lineFase = 15;
M_souta 25:b3a9f34b201d 1172 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1173 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1174 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1175 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1176 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1177 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1178 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1179 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1180 } else if(LimitSw::IsPressed(TOW_2L)) {
M_souta 25:b3a9f34b201d 1181 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1182 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1183 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1184 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1185 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1186 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1187 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1188 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1189 } else if(LimitSw::IsPressed(TOW_2R)) {
M_souta 25:b3a9f34b201d 1190 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1191 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1192 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1193 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1194 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1195 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1196 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1197 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1198 } else {
M_souta 25:b3a9f34b201d 1199 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1200 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1201 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1202 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1203 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1204 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1205 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1206 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1207 }
M_souta 25:b3a9f34b201d 1208 } else if(lineFase == 15 ){ // ライン 修正
M_souta 25:b3a9f34b201d 1209 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
M_souta 25:b3a9f34b201d 1210 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1211 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1212 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1213 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1214 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1215 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1216 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1217 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1218 } else if(linePara[LINE_TOW_2] > 0) {
M_souta 25:b3a9f34b201d 1219 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1220 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1221 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1222 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1223 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1224 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1225 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1226 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1227 } else if(linePara[LINE_TOW_2] < 0) {
M_souta 25:b3a9f34b201d 1228 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1229 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1230 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1231 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1232 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1233 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1234 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1235 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1236 } else if(linePara[LINE_TOW_2] == 0) {
M_souta 25:b3a9f34b201d 1237 lineFase = 16;
M_souta 25:b3a9f34b201d 1238 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1239 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1240 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1241 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1242 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1243 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1244 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1245 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1246 } else {
M_souta 25:b3a9f34b201d 1247 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1248 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1249 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1250 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1251 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1252 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1253 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1254 motor[TIRE_FR].pwm = 50;
M_souta 22:7d93f79a3686 1255 }
M_souta 25:b3a9f34b201d 1256 } else if(lineFase == 16) { // タオル2 解放
M_souta 25:b3a9f34b201d 1257 //solenoid.TOWEL2 = SOLENOID_ON;
M_souta 25:b3a9f34b201d 1258 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1259 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1260 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1261 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1262 lineFase = 17;
M_souta 25:b3a9f34b201d 1263 } else if(lineFase == 17) { // 前
M_souta 25:b3a9f34b201d 1264 switch(linePara[LINE_TOW_2]) {
M_souta 25:b3a9f34b201d 1265 case 2:
M_souta 25:b3a9f34b201d 1266 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1267 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1268 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1269 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1270 adjAnable = true;
M_souta 25:b3a9f34b201d 1271 break;
M_souta 25:b3a9f34b201d 1272 case 3:
M_souta 25:b3a9f34b201d 1273 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 1274 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1275 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 1276 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1277 adjAnable = true;
M_souta 25:b3a9f34b201d 1278 break;
M_souta 25:b3a9f34b201d 1279 case 1:
M_souta 25:b3a9f34b201d 1280 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 1281 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1282 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 1283 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1284 adjAnable = true;
M_souta 25:b3a9f34b201d 1285 break;
M_souta 25:b3a9f34b201d 1286 case 0:
M_souta 25:b3a9f34b201d 1287 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1288 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1289 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1290 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1291 adjAnable = true;
M_souta 25:b3a9f34b201d 1292 break;
M_souta 25:b3a9f34b201d 1293 case -1:
M_souta 25:b3a9f34b201d 1294 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1295 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 1296 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1297 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 1298 adjAnable = true;
M_souta 25:b3a9f34b201d 1299 break;
M_souta 25:b3a9f34b201d 1300 case -3:
M_souta 25:b3a9f34b201d 1301 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1302 tirePWM[TIRE_BL] = 10;
M_souta 25:b3a9f34b201d 1303 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1304 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 1305 adjAnable = true;
M_souta 25:b3a9f34b201d 1306 break;
M_souta 25:b3a9f34b201d 1307 case -2:
M_souta 25:b3a9f34b201d 1308 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1309 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1310 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1311 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1312 adjAnable = true;
M_souta 25:b3a9f34b201d 1313 break;
M_souta 25:b3a9f34b201d 1314 case 'A':
M_souta 25:b3a9f34b201d 1315 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1316 lineCheck = true;
M_souta 25:b3a9f34b201d 1317 lineCount = 0;
M_souta 25:b3a9f34b201d 1318 count++;
M_souta 25:b3a9f34b201d 1319 }
M_souta 25:b3a9f34b201d 1320 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1321 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1322 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1323 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1324 adjAnable = true;
M_souta 25:b3a9f34b201d 1325 break;
M_souta 25:b3a9f34b201d 1326 case 'N':
M_souta 25:b3a9f34b201d 1327 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1328 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1329 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1330 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1331 adjAnable = false;
M_souta 25:b3a9f34b201d 1332 break;
M_souta 25:b3a9f34b201d 1333 default:
M_souta 25:b3a9f34b201d 1334 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1335 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1336 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1337 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1338 adjAnable = false;
M_souta 25:b3a9f34b201d 1339 }
M_souta 25:b3a9f34b201d 1340
M_souta 25:b3a9f34b201d 1341 if(adjAnable){
M_souta 25:b3a9f34b201d 1342 adj = 0;
M_souta 25:b3a9f34b201d 1343 } else {
M_souta 25:b3a9f34b201d 1344 adj = 0;
M_souta 25:b3a9f34b201d 1345 }
M_souta 25:b3a9f34b201d 1346
M_souta 25:b3a9f34b201d 1347 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1348 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1349 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1350 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1351 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1352 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1353 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1354 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1355
M_souta 25:b3a9f34b201d 1356 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1357 lineCount++;
M_souta 25:b3a9f34b201d 1358 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1359 }
M_souta 25:b3a9f34b201d 1360 if(count == 1) {
M_souta 25:b3a9f34b201d 1361 count = 0;
M_souta 25:b3a9f34b201d 1362 lineFase = 18;
M_souta 25:b3a9f34b201d 1363 lineCount = 0;
M_souta 25:b3a9f34b201d 1364 lineCheck = false;
M_souta 25:b3a9f34b201d 1365 }
M_souta 25:b3a9f34b201d 1366 } else if(lineFase == 18) { // 前 低速
M_souta 25:b3a9f34b201d 1367 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1368 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1369 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1370 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1371 if(linePara[4] != 'N') {
M_souta 25:b3a9f34b201d 1372 lineFase = 19;
M_souta 25:b3a9f34b201d 1373 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1374 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1375 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1376 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1377 }
M_souta 25:b3a9f34b201d 1378 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1379 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1380 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1381 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1382 } else if(lineFase == 19) { // 左
M_souta 25:b3a9f34b201d 1383 switch(linePara[3]) {
M_souta 25:b3a9f34b201d 1384 case -2:
M_souta 25:b3a9f34b201d 1385 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1386 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1387 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1388 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1389 adjAnable = true;
M_souta 25:b3a9f34b201d 1390 break;
M_souta 25:b3a9f34b201d 1391 case -3:
M_souta 25:b3a9f34b201d 1392 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1393 tirePWM[TIRE_BL] = 10;
M_souta 25:b3a9f34b201d 1394 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1395 tirePWM[TIRE_FR] = -10;
M_souta 25:b3a9f34b201d 1396 adjAnable = true;
M_souta 25:b3a9f34b201d 1397 break;
M_souta 25:b3a9f34b201d 1398 case -1:
M_souta 25:b3a9f34b201d 1399 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1400 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 1401 tirePWM[TIRE_BR] = 30
M_souta 25:b3a9f34b201d 1402 tirePWM[TIRE_FR] = -2;
M_souta 25:b3a9f34b201d 1403 adjAnable = true;
M_souta 25:b3a9f34b201d 1404 break;
M_souta 25:b3a9f34b201d 1405 case 0:
M_souta 25:b3a9f34b201d 1406 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1407 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1408 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1409 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1410 adjAnable = true;
M_souta 25:b3a9f34b201d 1411 break;
M_souta 25:b3a9f34b201d 1412 case 1:
M_souta 25:b3a9f34b201d 1413 tirePWM[TIRE_FL] = -20;
M_souta 25:b3a9f34b201d 1414 tirePWM[TIRE_BL] = 40;
M_souta 25:b3a9f34b201d 1415 tirePWM[TIRE_BR] = 20;
M_souta 25:b3a9f34b201d 1416 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1417 adjAnable = true;
M_souta 25:b3a9f34b201d 1418 break;
M_souta 25:b3a9f34b201d 1419 case 3:
M_souta 25:b3a9f34b201d 1420 tirePWM[TIRE_FL] = -10;
M_souta 25:b3a9f34b201d 1421 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1422 tirePWM[TIRE_BR] = 10;
M_souta 25:b3a9f34b201d 1423 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1424 adjAnable = true;
M_souta 25:b3a9f34b201d 1425 break;
M_souta 25:b3a9f34b201d 1426 case 2:
M_souta 25:b3a9f34b201d 1427 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1428 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1429 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1430 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1431 adjAnable = true;
M_souta 25:b3a9f34b201d 1432 break;
M_souta 25:b3a9f34b201d 1433 case 'A':
M_souta 25:b3a9f34b201d 1434 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1435 lineCheck = true;
M_souta 25:b3a9f34b201d 1436 lineCount = 0;
M_souta 25:b3a9f34b201d 1437 count++;
M_souta 25:b3a9f34b201d 1438 }
M_souta 25:b3a9f34b201d 1439 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1440 tirePWM[TIRE_BL] = 30;
M_souta 25:b3a9f34b201d 1441 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1442 tirePWM[TIRE_FR] = -30;
M_souta 25:b3a9f34b201d 1443 adjAnable = true;
M_souta 25:b3a9f34b201d 1444 break;
M_souta 25:b3a9f34b201d 1445 case 'N':
M_souta 25:b3a9f34b201d 1446 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1447 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1448 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1449 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1450 adjAnable = false;
M_souta 25:b3a9f34b201d 1451 break;
M_souta 25:b3a9f34b201d 1452 default:
M_souta 25:b3a9f34b201d 1453 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1454 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1455 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1456 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1457 adjAnable = false;
M_souta 25:b3a9f34b201d 1458 }
M_souta 25:b3a9f34b201d 1459
M_souta 25:b3a9f34b201d 1460 if(adjAnable){
M_souta 25:b3a9f34b201d 1461 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta 25:b3a9f34b201d 1462 } else {
M_souta 25:b3a9f34b201d 1463 adj = 0;
M_souta 25:b3a9f34b201d 1464 }
M_souta 25:b3a9f34b201d 1465
M_souta 25:b3a9f34b201d 1466 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1467 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1468 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1469 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1470 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1471 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1472 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1473 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1474
M_souta 25:b3a9f34b201d 1475 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1476 lineCount++;
M_souta 25:b3a9f34b201d 1477 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1478 }
M_souta 25:b3a9f34b201d 1479
M_souta 25:b3a9f34b201d 1480 /*
M_souta 25:b3a9f34b201d 1481 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta 25:b3a9f34b201d 1482 targetCount = 3;
M_souta 25:b3a9f34b201d 1483 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta 25:b3a9f34b201d 1484 targetCount = 3;
M_souta 25:b3a9f34b201d 1485 } else {
M_souta 25:b3a9f34b201d 1486 targetCount = 2;
M_souta 25:b3a9f34b201d 1487 }
M_souta 25:b3a9f34b201d 1488 */
M_souta 25:b3a9f34b201d 1489
M_souta 25:b3a9f34b201d 1490 targetCount = 3;
M_souta 25:b3a9f34b201d 1491
M_souta 25:b3a9f34b201d 1492 if(count == targetCount) {
M_souta 25:b3a9f34b201d 1493 count = 0;
M_souta 25:b3a9f34b201d 1494 lineFase = 20;
M_souta 25:b3a9f34b201d 1495 lineCount = 0;
M_souta 25:b3a9f34b201d 1496 lineCheck = false;
M_souta 25:b3a9f34b201d 1497 }
M_souta 25:b3a9f34b201d 1498 } else if(lineFase == 20) { // 左 低速
M_souta 25:b3a9f34b201d 1499 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1500 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1501 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1502 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1503 if (linePara[2] == 'N') {
M_souta 25:b3a9f34b201d 1504 //(LimitSw::IsPressed(SHEETS_SW)) {
M_souta 25:b3a9f34b201d 1505 //ineFase = 20;
M_souta 25:b3a9f34b201d 1506 //else if(LimitSw::IsPressed(TOWEL2_SW) {
M_souta 25:b3a9f34b201d 1507 //ineFase = 14;
M_souta 25:b3a9f34b201d 1508 //else {
M_souta 25:b3a9f34b201d 1509 lineFase = 21;
M_souta 25:b3a9f34b201d 1510 //
M_souta 25:b3a9f34b201d 1511 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1512 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1513 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1514 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1515 }
M_souta 25:b3a9f34b201d 1516 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1517 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1518 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1519 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1520 } else if(lineFase == 21) {
M_souta 25:b3a9f34b201d 1521 switch(linePara[2]) {
M_souta 25:b3a9f34b201d 1522 case -2:
M_souta 25:b3a9f34b201d 1523 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1524 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1525 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1526 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1527 adjAnable = true;
M_souta 25:b3a9f34b201d 1528 break;
M_souta 25:b3a9f34b201d 1529 case -3:
M_souta 25:b3a9f34b201d 1530 tirePWM[TIRE_FL] = -10;
M_souta 25:b3a9f34b201d 1531 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1532 tirePWM[TIRE_BR] = 10;
M_souta 25:b3a9f34b201d 1533 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1534 adjAnable = true;
M_souta 25:b3a9f34b201d 1535 break;
M_souta 25:b3a9f34b201d 1536 case -1:
M_souta 25:b3a9f34b201d 1537 tirePWM[TIRE_FL] = -20;
M_souta 25:b3a9f34b201d 1538 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1539 tirePWM[TIRE_BR] = 20;
M_souta 25:b3a9f34b201d 1540 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1541 adjAnable = true;
M_souta 25:b3a9f34b201d 1542 break;
M_souta 25:b3a9f34b201d 1543 case 0:
M_souta 25:b3a9f34b201d 1544 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1545 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1546 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1547 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1548 adjAnable = true;
M_souta 25:b3a9f34b201d 1549 break;
M_souta 25:b3a9f34b201d 1550 case 1:
M_souta 25:b3a9f34b201d 1551 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1552 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 1553 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1554 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 1555 adjAnable = true;
M_souta 25:b3a9f34b201d 1556 break;
M_souta 25:b3a9f34b201d 1557 case 3:
M_souta 25:b3a9f34b201d 1558 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1559 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 1560 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1561 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 1562 adjAnable = true;
M_souta 25:b3a9f34b201d 1563 break;
M_souta 25:b3a9f34b201d 1564 case 2:
M_souta 25:b3a9f34b201d 1565 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1566 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1567 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1568 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1569 adjAnable = true;
M_souta 25:b3a9f34b201d 1570 break;
M_souta 25:b3a9f34b201d 1571 case 'A':
M_souta 25:b3a9f34b201d 1572 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1573 lineCheck = true;
M_souta 25:b3a9f34b201d 1574 lineCount = 0;
M_souta 25:b3a9f34b201d 1575 count++;
M_souta 25:b3a9f34b201d 1576 }
M_souta 25:b3a9f34b201d 1577 tirePWM[TIRE_FL] = -30;
M_souta 25:b3a9f34b201d 1578 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1579 tirePWM[TIRE_BR] = 30;
M_souta 25:b3a9f34b201d 1580 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1581 adjAnable = true;
M_souta 25:b3a9f34b201d 1582 break;
M_souta 25:b3a9f34b201d 1583 case 'N':
M_souta 25:b3a9f34b201d 1584 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1585 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1586 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1587 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1588 adjAnable = false;
M_souta 25:b3a9f34b201d 1589 break;
M_souta 25:b3a9f34b201d 1590 default:
M_souta 25:b3a9f34b201d 1591 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1592 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1593 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1594 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1595 adjAnable = false;
M_souta 25:b3a9f34b201d 1596 }
M_souta 25:b3a9f34b201d 1597
M_souta 25:b3a9f34b201d 1598 if(adjAnable){
M_souta 25:b3a9f34b201d 1599 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta 25:b3a9f34b201d 1600 } else {
M_souta 25:b3a9f34b201d 1601 adj = 0;
M_souta 25:b3a9f34b201d 1602 }
M_souta 25:b3a9f34b201d 1603
M_souta 25:b3a9f34b201d 1604 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1605 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1606 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1607 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1608 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1609 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1610 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1611 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1612
M_souta 25:b3a9f34b201d 1613 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1614 lineCount++;
M_souta 25:b3a9f34b201d 1615 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1616 }
M_souta 25:b3a9f34b201d 1617 if(count == 3) {
M_souta 25:b3a9f34b201d 1618 count = 0;
M_souta 25:b3a9f34b201d 1619 lineFase = 22;
M_souta 25:b3a9f34b201d 1620 lineCount = 0;
M_souta 25:b3a9f34b201d 1621 lineCheck = false;
M_souta 25:b3a9f34b201d 1622 }
M_souta 25:b3a9f34b201d 1623 } else if(lineFase == 22) {
M_souta 25:b3a9f34b201d 1624 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1625 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1626 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1627 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1628 if (linePara[2] == 'N') {
M_souta 25:b3a9f34b201d 1629 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1630 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1631 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1632 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1633 }
M_souta 25:b3a9f34b201d 1634 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 1635 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 1636 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 1637 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 1638 } else{
M_souta 25:b3a9f34b201d 1639 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1640 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1641 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1642 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 1643 }
M_souta 22:7d93f79a3686 1644
kishibekairohan 13:b6e02d6261d7 1645 }
kishibekairohan 13:b6e02d6261d7 1646 #endif
kishibekairohan 13:b6e02d6261d7 1647
kishibekairohan 13:b6e02d6261d7 1648 #if USE_PROCESS_NUM>5
kishibekairohan 16:3f2c2d89372b 1649 static void Process5()
kishibekairohan 16:3f2c2d89372b 1650 {
M_souta 25:b3a9f34b201d 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 for(int i = 0; i < 8; i++) {
M_souta 25:b3a9f34b201d 1659 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 25:b3a9f34b201d 1660 }
M_souta 25:b3a9f34b201d 1661
M_souta 25:b3a9f34b201d 1662 if(lineFase == 0) {
M_souta 25:b3a9f34b201d 1663 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1664 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1665 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1666 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1667 if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta 25:b3a9f34b201d 1668 lineFase = 1;
M_souta 25:b3a9f34b201d 1669 }
M_souta 25:b3a9f34b201d 1670 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 1671 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 1672 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1673 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 1674 }else if(lineFase == 1) { // 前 ライントレース
M_souta 25:b3a9f34b201d 1675 switch(linePara[0]) {
M_souta 25:b3a9f34b201d 1676 case -2:
M_souta 25:b3a9f34b201d 1677 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1678 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1679 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1680 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1681 adjAnable = true;
M_souta 25:b3a9f34b201d 1682 break;
M_souta 25:b3a9f34b201d 1683 case -3:
M_souta 25:b3a9f34b201d 1684 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 1685 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1686 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1687 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1688 adjAnable = true;
M_souta 25:b3a9f34b201d 1689 break;
M_souta 25:b3a9f34b201d 1690 case -1:
M_souta 25:b3a9f34b201d 1691 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 1692 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1693 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 1694 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1695 adjAnable = true;
M_souta 25:b3a9f34b201d 1696 break;
M_souta 25:b3a9f34b201d 1697 case 0:
M_souta 25:b3a9f34b201d 1698 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 1699 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1700 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 1701 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1702 adjAnable = true;
M_souta 25:b3a9f34b201d 1703 break;
M_souta 25:b3a9f34b201d 1704 case 1:
M_souta 25:b3a9f34b201d 1705 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 1706 tirePWM[TIRE_BL] = 40;
M_souta 25:b3a9f34b201d 1707 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 1708 tirePWM[TIRE_FR] = -40;
M_souta 25:b3a9f34b201d 1709 adjAnable = true;
M_souta 25:b3a9f34b201d 1710 break;
M_souta 25:b3a9f34b201d 1711 case 3:
M_souta 25:b3a9f34b201d 1712 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 1713 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 1714 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 1715 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 1716 adjAnable = true;
M_souta 25:b3a9f34b201d 1717 break;
M_souta 25:b3a9f34b201d 1718 case 2:
M_souta 25:b3a9f34b201d 1719 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 1720 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1721 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 1722 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1723 adjAnable = true;
M_souta 25:b3a9f34b201d 1724 break;
M_souta 25:b3a9f34b201d 1725 case 'A':
M_souta 25:b3a9f34b201d 1726 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1727 lineCheck = true;
M_souta 25:b3a9f34b201d 1728 lineCount = 0;
M_souta 25:b3a9f34b201d 1729 count++;
M_souta 25:b3a9f34b201d 1730 }
M_souta 25:b3a9f34b201d 1731 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 1732 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1733 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 1734 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1735 adjAnable = true;
M_souta 25:b3a9f34b201d 1736 break;
M_souta 25:b3a9f34b201d 1737 case 'N':
M_souta 25:b3a9f34b201d 1738 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1739 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1740 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1741 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1742 adjAnable = false;
M_souta 25:b3a9f34b201d 1743 break;
M_souta 25:b3a9f34b201d 1744 default:
M_souta 25:b3a9f34b201d 1745 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1746 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1747 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1748 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1749 adjAnable = false;
M_souta 25:b3a9f34b201d 1750 }
M_souta 25:b3a9f34b201d 1751
M_souta 25:b3a9f34b201d 1752 if(adjAnable){
M_souta 25:b3a9f34b201d 1753 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta 25:b3a9f34b201d 1754 } else {
M_souta 25:b3a9f34b201d 1755 adj = 0;
M_souta 25:b3a9f34b201d 1756 }
M_souta 25:b3a9f34b201d 1757
M_souta 25:b3a9f34b201d 1758 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1759 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1760 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1761 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1762 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]) * 0.8;
M_souta 25:b3a9f34b201d 1763 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj) * 0.8;
M_souta 25:b3a9f34b201d 1764 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj) * 0.8;
M_souta 25:b3a9f34b201d 1765 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]) * 0.8;
M_souta 25:b3a9f34b201d 1766
M_souta 25:b3a9f34b201d 1767 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1768 lineCount++;
M_souta 25:b3a9f34b201d 1769 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1770 }
M_souta 25:b3a9f34b201d 1771 if(count == 3) {
M_souta 25:b3a9f34b201d 1772 count = 0;
M_souta 25:b3a9f34b201d 1773 lineFase = 2;
M_souta 25:b3a9f34b201d 1774 lineCount = 0;
M_souta 25:b3a9f34b201d 1775 lineCheck = false;
M_souta 25:b3a9f34b201d 1776 }
M_souta 25:b3a9f34b201d 1777 } else if(lineFase == 2) { // 前 低速
M_souta 25:b3a9f34b201d 1778 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1779 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1780 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1781 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1782 if(linePara[3] != 'N') {
M_souta 25:b3a9f34b201d 1783 lineFase = 3;
M_souta 25:b3a9f34b201d 1784 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1785 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1786 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1787 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1788 }
M_souta 25:b3a9f34b201d 1789 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 1790 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 1791 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1792 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 1793 } else if(lineFase == 3) { // 左 ライントレース
M_souta 25:b3a9f34b201d 1794 switch(linePara[4]) {
M_souta 25:b3a9f34b201d 1795 case -2:
M_souta 25:b3a9f34b201d 1796 tirePWM[TIRE_FL] = -50;
M_souta 25:b3a9f34b201d 1797 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1798 tirePWM[TIRE_BR] = 50;
M_souta 25:b3a9f34b201d 1799 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1800 adjAnable = true;
M_souta 25:b3a9f34b201d 1801 break;
M_souta 25:b3a9f34b201d 1802 case -3:
M_souta 25:b3a9f34b201d 1803 tirePWM[TIRE_FL] = -50;
M_souta 25:b3a9f34b201d 1804 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 1805 tirePWM[TIRE_BR] = 50;
M_souta 25:b3a9f34b201d 1806 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 1807 adjAnable = true;
M_souta 25:b3a9f34b201d 1808 break;
M_souta 25:b3a9f34b201d 1809 case -1:
M_souta 25:b3a9f34b201d 1810 tirePWM[TIRE_FL] = -50;
M_souta 25:b3a9f34b201d 1811 tirePWM[TIRE_BL] = 40;
M_souta 25:b3a9f34b201d 1812 tirePWM[TIRE_BR] = 50;
M_souta 25:b3a9f34b201d 1813 tirePWM[TIRE_FR] = -40;
M_souta 25:b3a9f34b201d 1814 adjAnable = true;
M_souta 25:b3a9f34b201d 1815 break;
M_souta 25:b3a9f34b201d 1816 case 0:
M_souta 25:b3a9f34b201d 1817 tirePWM[TIRE_FL] = -50;
M_souta 25:b3a9f34b201d 1818 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1819 tirePWM[TIRE_BR] = 50;
M_souta 25:b3a9f34b201d 1820 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1821 adjAnable = true;
M_souta 25:b3a9f34b201d 1822 break;
M_souta 25:b3a9f34b201d 1823 case 1:
M_souta 25:b3a9f34b201d 1824 tirePWM[TIRE_FL] = -40;
M_souta 25:b3a9f34b201d 1825 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1826 tirePWM[TIRE_BR] = 40;
M_souta 25:b3a9f34b201d 1827 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1828 adjAnable = true;
M_souta 25:b3a9f34b201d 1829 break;
M_souta 25:b3a9f34b201d 1830 case 3:
M_souta 25:b3a9f34b201d 1831 tirePWM[TIRE_FL] = -20;
M_souta 25:b3a9f34b201d 1832 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1833 tirePWM[TIRE_BR] = 20;
M_souta 25:b3a9f34b201d 1834 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1835 adjAnable = true;
M_souta 25:b3a9f34b201d 1836 break;
M_souta 25:b3a9f34b201d 1837 case 2:
M_souta 25:b3a9f34b201d 1838 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1839 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1840 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1841 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1842 adjAnable = true;
M_souta 25:b3a9f34b201d 1843 break;
M_souta 25:b3a9f34b201d 1844 case 'A':
M_souta 25:b3a9f34b201d 1845 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1846 lineCheck = true;
M_souta 25:b3a9f34b201d 1847 lineCount = 0;
M_souta 25:b3a9f34b201d 1848 count++;
M_souta 25:b3a9f34b201d 1849 }
M_souta 25:b3a9f34b201d 1850 tirePWM[TIRE_FL] = -50;
M_souta 25:b3a9f34b201d 1851 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 1852 tirePWM[TIRE_BR] = 50;
M_souta 25:b3a9f34b201d 1853 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 1854 adjAnable = true;
M_souta 25:b3a9f34b201d 1855 break;
M_souta 25:b3a9f34b201d 1856 case 'N':
M_souta 25:b3a9f34b201d 1857 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1858 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1859 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1860 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1861 adjAnable = false;
M_souta 25:b3a9f34b201d 1862 break;
M_souta 25:b3a9f34b201d 1863 default:
M_souta 25:b3a9f34b201d 1864 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1865 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1866 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1867 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1868 adjAnable = false;
M_souta 25:b3a9f34b201d 1869 }
M_souta 25:b3a9f34b201d 1870
M_souta 25:b3a9f34b201d 1871 if(adjAnable){
M_souta 25:b3a9f34b201d 1872 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta 25:b3a9f34b201d 1873 } else {
M_souta 25:b3a9f34b201d 1874 adj = 0;
M_souta 25:b3a9f34b201d 1875 }
M_souta 25:b3a9f34b201d 1876
M_souta 25:b3a9f34b201d 1877 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1878 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1879 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1880 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1881 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 1882 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta 25:b3a9f34b201d 1883 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 1884 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta 25:b3a9f34b201d 1885
M_souta 25:b3a9f34b201d 1886 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1887 lineCount++;
M_souta 25:b3a9f34b201d 1888 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1889 }
M_souta 25:b3a9f34b201d 1890 if(count == 2) {
M_souta 25:b3a9f34b201d 1891 count = 0;
M_souta 25:b3a9f34b201d 1892 lineFase = 4;
M_souta 25:b3a9f34b201d 1893 lineCount = 0;
M_souta 25:b3a9f34b201d 1894 lineCheck = false;
M_souta 25:b3a9f34b201d 1895 }
M_souta 25:b3a9f34b201d 1896 } else if(lineFase == 4) { // 右 低速
M_souta 25:b3a9f34b201d 1897 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1898 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1899 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1900 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1901 if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 1902 lineFase = 5;
M_souta 25:b3a9f34b201d 1903 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1904 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1905 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1906 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1907 }
M_souta 25:b3a9f34b201d 1908 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 1909 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 1910 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1911 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 1912 } else if (lineFase == 5) {
M_souta 25:b3a9f34b201d 1913 lineFase = 6;
M_souta 25:b3a9f34b201d 1914 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1915 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1916 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1917 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1918 } else if(lineFase == 6) { // タオル 竿検知
M_souta 25:b3a9f34b201d 1919 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 1920 lineFase = 7;
M_souta 25:b3a9f34b201d 1921 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1922 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1923 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1924 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1925 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1926 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1927 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1928 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1929 } else if(LimitSw::IsPressed(TOW_1L)) {
M_souta 25:b3a9f34b201d 1930 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1931 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1932 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1933 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1934 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1935 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1936 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1937 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1938 } else if(LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 1939 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1940 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1941 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1942 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1943 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1944 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1945 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1946 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1947 } else {
M_souta 25:b3a9f34b201d 1948 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1949 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 1950 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1951 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 1952 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1953 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1954 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1955 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 1956 }
M_souta 25:b3a9f34b201d 1957 } else if(lineFase == 7) { // ライン 修正
M_souta 25:b3a9f34b201d 1958 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 1959 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1960 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1961 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1962 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1963 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1964 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1965 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1966 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1967 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 1968 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1969 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1970 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1971 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1972 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1973 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1974 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1975 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1976 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 1977 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1978 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1979 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1980 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1981 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1982 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1983 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1984 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1985 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 1986 lineFase = 8;
M_souta 25:b3a9f34b201d 1987 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1988 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1989 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1990 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1991 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1992 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1993 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1994 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1995 } else {
M_souta 25:b3a9f34b201d 1996 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1997 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 1998 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1999 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2000 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2001 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2002 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2003 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2004 }
M_souta 25:b3a9f34b201d 2005 } else if(lineFase == 8) { // タオル1 解放
M_souta 25:b3a9f34b201d 2006 //solenoid.TOWEL1 = SOLENOID_ON;
M_souta 25:b3a9f34b201d 2007 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2008 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2009 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2010 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2011 lineFase = 9;
M_souta 25:b3a9f34b201d 2012 } else if(lineFase == 9) { // 前
M_souta 25:b3a9f34b201d 2013 switch(linePara[LINE_TOW_1]) {
M_souta 25:b3a9f34b201d 2014 case 2:
M_souta 25:b3a9f34b201d 2015 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2016 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 2017 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2018 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 2019 adjAnable = true;
M_souta 25:b3a9f34b201d 2020 break;
M_souta 25:b3a9f34b201d 2021 case 3:
M_souta 25:b3a9f34b201d 2022 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 2023 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 2024 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 2025 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 2026 adjAnable = true;
M_souta 25:b3a9f34b201d 2027 break;
M_souta 25:b3a9f34b201d 2028 case 1:
M_souta 25:b3a9f34b201d 2029 tirePWM[TIRE_FL] = 40;
M_souta 25:b3a9f34b201d 2030 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 2031 tirePWM[TIRE_BR] = -40;
M_souta 25:b3a9f34b201d 2032 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 2033 adjAnable = true;
M_souta 25:b3a9f34b201d 2034 break;
M_souta 25:b3a9f34b201d 2035 case 0:
M_souta 25:b3a9f34b201d 2036 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2037 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 2038 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2039 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 2040 adjAnable = true;
M_souta 25:b3a9f34b201d 2041 break;
M_souta 25:b3a9f34b201d 2042 case -1:
M_souta 25:b3a9f34b201d 2043 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2044 tirePWM[TIRE_BL] = 40;
M_souta 25:b3a9f34b201d 2045 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2046 tirePWM[TIRE_FR] = -40;
M_souta 25:b3a9f34b201d 2047 adjAnable = true;
M_souta 25:b3a9f34b201d 2048 break;
M_souta 25:b3a9f34b201d 2049 case -3:
M_souta 25:b3a9f34b201d 2050 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2051 tirePWM[TIRE_BL] = 20;
M_souta 25:b3a9f34b201d 2052 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2053 tirePWM[TIRE_FR] = -20;
M_souta 25:b3a9f34b201d 2054 adjAnable = true;
M_souta 25:b3a9f34b201d 2055 break;
M_souta 25:b3a9f34b201d 2056 case -2:
M_souta 25:b3a9f34b201d 2057 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2058 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2059 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2060 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2061 adjAnable = true;
M_souta 25:b3a9f34b201d 2062 break;
M_souta 25:b3a9f34b201d 2063 case 'A':
M_souta 25:b3a9f34b201d 2064 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2065 lineCheck = true;
M_souta 25:b3a9f34b201d 2066 lineCount = 0;
M_souta 25:b3a9f34b201d 2067 count++;
M_souta 25:b3a9f34b201d 2068 }
M_souta 25:b3a9f34b201d 2069 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2070 tirePWM[TIRE_BL] = 50;
M_souta 25:b3a9f34b201d 2071 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2072 tirePWM[TIRE_FR] = -50;
M_souta 25:b3a9f34b201d 2073 adjAnable = true;
M_souta 25:b3a9f34b201d 2074 break;
M_souta 25:b3a9f34b201d 2075 case 'N':
M_souta 25:b3a9f34b201d 2076 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2077 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2078 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2079 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2080 adjAnable = false;
M_souta 25:b3a9f34b201d 2081 break;
M_souta 25:b3a9f34b201d 2082 default:
M_souta 25:b3a9f34b201d 2083 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2084 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2085 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2086 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2087 adjAnable = false;
M_souta 25:b3a9f34b201d 2088 }
M_souta 25:b3a9f34b201d 2089
M_souta 25:b3a9f34b201d 2090 if(adjAnable){
M_souta 25:b3a9f34b201d 2091 adj = 0;
M_souta 25:b3a9f34b201d 2092 } else {
M_souta 25:b3a9f34b201d 2093 adj = 0;
M_souta 25:b3a9f34b201d 2094 }
M_souta 25:b3a9f34b201d 2095
M_souta 25:b3a9f34b201d 2096 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2097 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2098 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2099 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2100 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta 25:b3a9f34b201d 2101 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2102 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta 25:b3a9f34b201d 2103 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2104
M_souta 25:b3a9f34b201d 2105 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2106 lineCount++;
M_souta 25:b3a9f34b201d 2107 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2108 }
M_souta 25:b3a9f34b201d 2109 if(count == 1) {
M_souta 25:b3a9f34b201d 2110 count = 0;
M_souta 25:b3a9f34b201d 2111 lineFase = 10;
M_souta 25:b3a9f34b201d 2112 lineCount = 0;
M_souta 25:b3a9f34b201d 2113 lineCheck = false;
M_souta 25:b3a9f34b201d 2114 }
M_souta 25:b3a9f34b201d 2115 } else if(lineFase == 10) {
M_souta 25:b3a9f34b201d 2116 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2117 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 2118 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 2119 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2120 if(linePara[4] == 0) {
M_souta 25:b3a9f34b201d 2121 lineFase = 11;
M_souta 25:b3a9f34b201d 2122 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2123 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2124 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2125 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2126 }
M_souta 25:b3a9f34b201d 2127 motor[TIRE_FL].pwm = 20;
M_souta 25:b3a9f34b201d 2128 motor[TIRE_BL].pwm = 20;
M_souta 25:b3a9f34b201d 2129 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 2130 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 2131 } else if(lineFase == 11) {
M_souta 25:b3a9f34b201d 2132 switch(linePara[4]) {
M_souta 25:b3a9f34b201d 2133 case -2:
M_souta 25:b3a9f34b201d 2134 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2135 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2136 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2137 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2138 adjAnable = true;
M_souta 25:b3a9f34b201d 2139 break;
M_souta 25:b3a9f34b201d 2140 case -3:
M_souta 25:b3a9f34b201d 2141 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2142 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 2143 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2144 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 2145 adjAnable = true;
M_souta 25:b3a9f34b201d 2146 break;
M_souta 25:b3a9f34b201d 2147 case -1:
M_souta 25:b3a9f34b201d 2148 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2149 tirePWM[TIRE_BL] = -40;
M_souta 25:b3a9f34b201d 2150 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2151 tirePWM[TIRE_FR] = 40;
M_souta 25:b3a9f34b201d 2152 adjAnable = true;
M_souta 25:b3a9f34b201d 2153 break;
M_souta 25:b3a9f34b201d 2154 case 0:
M_souta 25:b3a9f34b201d 2155 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2156 tirePWM[TIRE_BL] = -50;
M_souta 25:b3a9f34b201d 2157 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2158 tirePWM[TIRE_FR] = 50;
M_souta 25:b3a9f34b201d 2159 adjAnable = true;
M_souta 25:b3a9f34b201d 2160 break;
M_souta 25:b3a9f34b201d 2161 case 1:
M_souta 25:b3a9f34b201d 2162 tirePWM[TIRE_FL] = 40;
M_souta 25:b3a9f34b201d 2163 tirePWM[TIRE_BL] = -50;
M_souta 25:b3a9f34b201d 2164 tirePWM[TIRE_BR] = -40;
M_souta 25:b3a9f34b201d 2165 tirePWM[TIRE_FR] = 50;
M_souta 25:b3a9f34b201d 2166 adjAnable = true;
M_souta 25:b3a9f34b201d 2167 break;
M_souta 25:b3a9f34b201d 2168 case 3:
M_souta 25:b3a9f34b201d 2169 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 2170 tirePWM[TIRE_BL] = -50;
M_souta 25:b3a9f34b201d 2171 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 2172 tirePWM[TIRE_FR] = 50;
M_souta 25:b3a9f34b201d 2173 adjAnable = true;
M_souta 25:b3a9f34b201d 2174 break;
M_souta 25:b3a9f34b201d 2175 case 2:
M_souta 25:b3a9f34b201d 2176 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2177 tirePWM[TIRE_BL] = -50;
M_souta 25:b3a9f34b201d 2178 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2179 tirePWM[TIRE_FR] = 50;
M_souta 25:b3a9f34b201d 2180 adjAnable = true;
M_souta 25:b3a9f34b201d 2181 break;
M_souta 25:b3a9f34b201d 2182 case 'A':
M_souta 25:b3a9f34b201d 2183 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 2184 lineCheck = true;
M_souta 25:b3a9f34b201d 2185 lineCount = 0;
M_souta 25:b3a9f34b201d 2186 count++;
M_souta 25:b3a9f34b201d 2187 }
M_souta 25:b3a9f34b201d 2188 tirePWM[TIRE_FL] = 50;
M_souta 25:b3a9f34b201d 2189 tirePWM[TIRE_BL] = -50;
M_souta 25:b3a9f34b201d 2190 tirePWM[TIRE_BR] = -50;
M_souta 25:b3a9f34b201d 2191 tirePWM[TIRE_FR] = 50;
M_souta 25:b3a9f34b201d 2192 adjAnable = true;
M_souta 25:b3a9f34b201d 2193 break;
M_souta 25:b3a9f34b201d 2194 case 'N':
M_souta 25:b3a9f34b201d 2195 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2196 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2197 tirePWM[TIRE_BR] = 0.;
M_souta 25:b3a9f34b201d 2198 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2199 adjAnable = false;
M_souta 25:b3a9f34b201d 2200 break;
M_souta 25:b3a9f34b201d 2201 default:
M_souta 25:b3a9f34b201d 2202 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 2203 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 2204 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 2205 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 2206 adjAnable = false;
M_souta 25:b3a9f34b201d 2207 }
M_souta 25:b3a9f34b201d 2208
M_souta 25:b3a9f34b201d 2209 if(adjAnable){
M_souta 25:b3a9f34b201d 2210 adj = 0;//linePara[3];
M_souta 25:b3a9f34b201d 2211 } else {
M_souta 25:b3a9f34b201d 2212 adj = 0;
M_souta 25:b3a9f34b201d 2213 }
M_souta 25:b3a9f34b201d 2214
M_souta 25:b3a9f34b201d 2215 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 2216 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2217 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 2218 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2219 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 2220 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 2221 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 2222 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 2223
M_souta 25:b3a9f34b201d 2224 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 2225 lineCount++;
M_souta 25:b3a9f34b201d 2226 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 2227 }
M_souta 25:b3a9f34b201d 2228 if(count == 3) {
M_souta 25:b3a9f34b201d 2229 count = 0;
M_souta 25:b3a9f34b201d 2230 lineFase = 12;
M_souta 25:b3a9f34b201d 2231 lineCount = 0;
M_souta 25:b3a9f34b201d 2232 lineCheck = false;
M_souta 25:b3a9f34b201d 2233 }
M_souta 25:b3a9f34b201d 2234 } else if(lineFase == 12) { // 右 低速
M_souta 25:b3a9f34b201d 2235 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2236 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 2237 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 2238 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 2239 if(linePara[LINE_TOW_2] == 0) {
M_souta 25:b3a9f34b201d 2240 lineFase = 13;
M_souta 25:b3a9f34b201d 2241 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2242 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2243 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2244 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2245 }
M_souta 25:b3a9f34b201d 2246 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 2247 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 2248 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 2249 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 2250 } else if (lineFase == 13) {
M_souta 25:b3a9f34b201d 2251 lineFase = 14;
M_souta 25:b3a9f34b201d 2252 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2253 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2254 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2255 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2256 } else {
M_souta 25:b3a9f34b201d 2257 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2258 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2259 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2260 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2261 }
M_souta 25:b3a9f34b201d 2262
kishibekairohan 13:b6e02d6261d7 2263 }
kishibekairohan 13:b6e02d6261d7 2264 #endif
kishibekairohan 13:b6e02d6261d7 2265
kishibekairohan 13:b6e02d6261d7 2266 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 2267 static void Process6()
kishibekairohan 13:b6e02d6261d7 2268 {
M_souta 25:b3a9f34b201d 2269 for(int i = 0; i < 8; i++) {
M_souta 22:7d93f79a3686 2270 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 22:7d93f79a3686 2271 }
M_souta 22:7d93f79a3686 2272
M_souta 25:b3a9f34b201d 2273 if(lineFase == 0) {
M_souta 25:b3a9f34b201d 2274 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 2275 lineFase = 1;
M_souta 25:b3a9f34b201d 2276 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2277 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 2278 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2279 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2280 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2281 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2282 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2283 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2284 } else if(LimitSw::IsPressed(TOW_1L)) {
M_souta 25:b3a9f34b201d 2285 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2286 motor[TIRE_FL].pwm = 12;
M_souta 25:b3a9f34b201d 2287 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2288 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2289 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2290 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2291 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 2292 motor[TIRE_FR].pwm = 12;
M_souta 25:b3a9f34b201d 2293 } else if(LimitSw::IsPressed(TOW_1R)) {
M_souta 25:b3a9f34b201d 2294 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 2295 motor[TIRE_FL].pwm = 12;
M_souta 25:b3a9f34b201d 2296 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2297 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 2298 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2299 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 2300 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2301 motor[TIRE_FR].pwm = 12;
M_souta 25:b3a9f34b201d 2302 } else {
M_souta 25:b3a9f34b201d 2303 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 2304 motor[TIRE_FL].pwm = 18;
M_souta 25:b3a9f34b201d 2305 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 2306 motor[TIRE_BL].pwm = 18;
M_souta 25:b3a9f34b201d 2307 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 2308 motor[TIRE_BR].pwm = 18;
M_souta 25:b3a9f34b201d 2309 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 2310 motor[TIRE_FR].pwm = 18;
M_souta 25:b3a9f34b201d 2311 }
M_souta 25:b3a9f34b201d 2312 } else if(lineFase == 1) {
M_souta 25:b3a9f34b201d 2313 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 2314 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2315 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 2316 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2317 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2318 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2319 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2320 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2321 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2322 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 2323 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 2324 motor[TIRE_FL].pwm = 15;
M_souta 25:b3a9f34b201d 2325 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 2326 motor[TIRE_BL].pwm = 15;
M_souta 25:b3a9f34b201d 2327 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 2328 motor[TIRE_BR].pwm = 15;
M_souta 25:b3a9f34b201d 2329 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 2330 motor[TIRE_FR].pwm = 15;
M_souta 25:b3a9f34b201d 2331 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 2332 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 2333 motor[TIRE_FL].pwm = 15;
M_souta 25:b3a9f34b201d 2334 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 2335 motor[TIRE_BL].pwm = 15;
M_souta 25:b3a9f34b201d 2336 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 2337 motor[TIRE_BR].pwm = 15;
M_souta 25:b3a9f34b201d 2338 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 2339 motor[TIRE_FR].pwm = 15;
M_souta 25:b3a9f34b201d 2340 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 2341 lineFase = 2;
M_souta 25:b3a9f34b201d 2342 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2343 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 2344 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2345 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2346 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2347 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2348 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2349 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2350 } else {
M_souta 25:b3a9f34b201d 2351 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2352 motor[TIRE_FL].pwm = 30;
M_souta 25:b3a9f34b201d 2353 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 2354 motor[TIRE_BL].pwm = 30;
M_souta 25:b3a9f34b201d 2355 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2356 motor[TIRE_BR].pwm = 30;
M_souta 25:b3a9f34b201d 2357 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 2358 motor[TIRE_FR].pwm = 30;
M_souta 25:b3a9f34b201d 2359 }
M_souta 25:b3a9f34b201d 2360 } else if(lineFase == 2) {
M_souta 25:b3a9f34b201d 2361 //solenoid::solenoid1 = SOLENOID_ON
M_souta 25:b3a9f34b201d 2362
M_souta 22:7d93f79a3686 2363 }
kishibekairohan 13:b6e02d6261d7 2364 }
kishibekairohan 13:b6e02d6261d7 2365 #endif
kishibekairohan 13:b6e02d6261d7 2366
kishibekairohan 13:b6e02d6261d7 2367 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 2368 static void Process7()
kishibekairohan 13:b6e02d6261d7 2369 {
M_souta 24:370616a56815 2370
kishibekairohan 12:c09b3e08a316 2371 }
kishibekairohan 12:c09b3e08a316 2372 #endif
kishibekairohan 12:c09b3e08a316 2373
kishibekairohan 16:3f2c2d89372b 2374 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 2375 static void Process8()
kishibekairohan 12:c09b3e08a316 2376 {
M_souta 22:7d93f79a3686 2377 if(controller->Button.A) {
M_souta 22:7d93f79a3686 2378 rotaconSampling.start();
M_souta 22:7d93f79a3686 2379 PIDflag = true;
M_souta 22:7d93f79a3686 2380
M_souta 22:7d93f79a3686 2381 //linePara_U = LineHub::GetPara(0);
M_souta 22:7d93f79a3686 2382 //linePara_B = LineHub::GetPara(3);
M_souta 22:7d93f79a3686 2383
M_souta 22:7d93f79a3686 2384
M_souta 22:7d93f79a3686 2385 pulsePV[FL] = encoder[FL].getPulses();
M_souta 22:7d93f79a3686 2386 pulsePV[BL] = encoder[BL].getPulses();
M_souta 22:7d93f79a3686 2387 pulsePV[BR] = encoder[BR].getPulses();
M_souta 22:7d93f79a3686 2388 pulsePV[FR] = encoder[FR].getPulses();
M_souta 22:7d93f79a3686 2389
M_souta 22:7d93f79a3686 2390
M_souta 22:7d93f79a3686 2391 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 2392 timeCV[i] = timePV[i];
M_souta 22:7d93f79a3686 2393 timePV[i] = rotaconSampling.read();
M_souta 22:7d93f79a3686 2394 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta 22:7d93f79a3686 2395 pulseCV[i] = pulsePV[i];
M_souta 22:7d93f79a3686 2396 }
M_souta 22:7d93f79a3686 2397
M_souta 22:7d93f79a3686 2398 move.Vx = 0.5;
M_souta 22:7d93f79a3686 2399 move.Vy = 0.5;
M_souta 22:7d93f79a3686 2400 move.Va = 0;
M_souta 22:7d93f79a3686 2401
M_souta 22:7d93f79a3686 2402 correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta 22:7d93f79a3686 2403 correction_LT.Vy = 0;
M_souta 22:7d93f79a3686 2404 correction_LT.Va = 0;
M_souta 22:7d93f79a3686 2405
M_souta 22:7d93f79a3686 2406 synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta 22:7d93f79a3686 2407 synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta 22:7d93f79a3686 2408 synthetic.Va = move.Va + correction_LT.Va;
M_souta 22:7d93f79a3686 2409
M_souta 22:7d93f79a3686 2410 sita = 0;
M_souta 22:7d93f79a3686 2411
M_souta 22:7d93f79a3686 2412 //タイヤの目標速度算出
M_souta 22:7d93f79a3686 2413 float sinR = 0.7071 * (float)sin(sita);
M_souta 22:7d93f79a3686 2414 float cosR = 0.7071 * (float)cos(sita);
M_souta 22:7d93f79a3686 2415 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta 22:7d93f79a3686 2416 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 2417 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 2418 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 2419 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 2420
M_souta 22:7d93f79a3686 2421 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta 22:7d93f79a3686 2422
M_souta 22:7d93f79a3686 2423 //PIDによるPWM算出
M_souta 22:7d93f79a3686 2424
M_souta 22:7d93f79a3686 2425 //モータの駆動
M_souta 22:7d93f79a3686 2426 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 2427 if (tirePWM[i] > 255){
M_souta 22:7d93f79a3686 2428 tirePWM[i] = 255;
M_souta 22:7d93f79a3686 2429 } else if (tirePWM[i] < -255) {
M_souta 22:7d93f79a3686 2430 tirePWM[i] = -255;
M_souta 22:7d93f79a3686 2431 }
M_souta 22:7d93f79a3686 2432 }
M_souta 22:7d93f79a3686 2433
M_souta 22:7d93f79a3686 2434 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 2435 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 2436 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 2437 }
M_souta 22:7d93f79a3686 2438 } else {
M_souta 22:7d93f79a3686 2439 PIDflag = false;
M_souta 22:7d93f79a3686 2440 rotaconSampling.stop();
M_souta 22:7d93f79a3686 2441 rotaconSampling.reset();
M_souta 22:7d93f79a3686 2442 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 2443 encoder[i].reset();
M_souta 22:7d93f79a3686 2444 pulsePV[i] = 0;
M_souta 22:7d93f79a3686 2445 pulseCV[i] = 0;
M_souta 22:7d93f79a3686 2446 timePV[i] = 0;
M_souta 22:7d93f79a3686 2447 timeCV[i] = 0;
M_souta 22:7d93f79a3686 2448 tirePWM[i] = 0;
M_souta 22:7d93f79a3686 2449 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 2450 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 2451 }
M_souta 22:7d93f79a3686 2452 }
kishibekairohan 12:c09b3e08a316 2453 }
kishibekairohan 12:c09b3e08a316 2454 #endif
kishibekairohan 12:c09b3e08a316 2455
kishibekairohan 13:b6e02d6261d7 2456 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 2457 static void Process9()
kishibekairohan 12:c09b3e08a316 2458 {
kishibekairohan 2:c015739085d3 2459
t_yamamoto 0:669ef71cba68 2460 }
t_yamamoto 0:669ef71cba68 2461 #endif
t_yamamoto 0:669ef71cba68 2462 #endif
t_yamamoto 0:669ef71cba68 2463 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 2464
t_yamamoto 0:669ef71cba68 2465 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 2466 {
t_yamamoto 0:669ef71cba68 2467
t_yamamoto 0:669ef71cba68 2468 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 2469 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 2470 #endif
t_yamamoto 0:669ef71cba68 2471
t_yamamoto 0:669ef71cba68 2472 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 2473 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 2474 {
t_yamamoto 0:669ef71cba68 2475 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 2476 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 2477 }
t_yamamoto 0:669ef71cba68 2478 #endif
t_yamamoto 0:669ef71cba68 2479 }
t_yamamoto 0:669ef71cba68 2480
kishibekairohan 16:3f2c2d89372b 2481 void BuzzerTimer_func(){
kishibekairohan 16:3f2c2d89372b 2482 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 2483 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 2484 }
kishibekairohan 11:028a150943b5 2485
kishibekairohan 11:028a150943b5 2486 void TapeLedEms_func() {
kishibekairohan 16:3f2c2d89372b 2487 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 2488 }
kishibekairohan 16:3f2c2d89372b 2489
kishibekairohan 16:3f2c2d89372b 2490 #pragma region USER-DEFINED-FUNCTIONS
M_souta 22:7d93f79a3686 2491 void tirePID() {
M_souta 22:7d93f79a3686 2492 if(PIDflag == true) {
M_souta 22:7d93f79a3686 2493 //加算するPID値の算出
M_souta 22:7d93f79a3686 2494 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta 22:7d93f79a3686 2495 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta 22:7d93f79a3686 2496 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta 22:7d93f79a3686 2497 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta 22:7d93f79a3686 2498 //PID値の加算
M_souta 22:7d93f79a3686 2499 tirePWM[FL] += rotaconPID[0].GetMV();
M_souta 22:7d93f79a3686 2500 tirePWM[BL] += rotaconPID[1].GetMV();
M_souta 22:7d93f79a3686 2501 tirePWM[FR] += rotaconPID[2].GetMV();
M_souta 22:7d93f79a3686 2502 tirePWM[BR] += rotaconPID[3].GetMV();
M_souta 22:7d93f79a3686 2503 }
M_souta 22:7d93f79a3686 2504 }
kishibekairohan 16:3f2c2d89372b 2505
M_souta 22:7d93f79a3686 2506 int lineCast(char k) {
M_souta 22:7d93f79a3686 2507 int l;
M_souta 22:7d93f79a3686 2508 switch(k) {
M_souta 22:7d93f79a3686 2509 case 255:
M_souta 22:7d93f79a3686 2510 l = -1;
M_souta 22:7d93f79a3686 2511 break;
M_souta 22:7d93f79a3686 2512 case 254:
M_souta 22:7d93f79a3686 2513 l = -2;
M_souta 22:7d93f79a3686 2514 break;
M_souta 22:7d93f79a3686 2515 case 253:
M_souta 22:7d93f79a3686 2516 l = -3;
M_souta 22:7d93f79a3686 2517 break;
M_souta 22:7d93f79a3686 2518 default:
M_souta 22:7d93f79a3686 2519 l = k;
M_souta 22:7d93f79a3686 2520 }
M_souta 22:7d93f79a3686 2521 return l;
M_souta 22:7d93f79a3686 2522 }
kishibekairohan 16:3f2c2d89372b 2523
t_yamamoto 0:669ef71cba68 2524 #pragma endregion