大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_old

aa

Dependencies:   mbed

System/Process/Process.cpp@28:479631c2de29, 2019-10-03 (annotated)

Committer:
Ryosei
Date:
Thu Oct 03 05:33:48 2019 +0000
Revision:
28:479631c2de29
Parent:
27:dd9f27fce7d1
Child:
29:44d5454ce8fa
The Last version

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"
Ryosei 27:dd9f27fce7d1 3 #include "Pulse.h"
Ryosei 27:dd9f27fce7d1 4 #include <stdlib.h>
7ka884 4:ba9df71868df 5 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 6 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 7 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/Switch/Switch.h"
kishibekairohan 16:3f2c2d89372b 11 #include "../../Input/Encoder/Encoder.h"
Ryosei 27:dd9f27fce7d1 12
t_yamamoto 0:669ef71cba68 13 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 14 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 15 #include "../Using.h"
t_yamamoto 0:669ef71cba68 16
t_yamamoto 0:669ef71cba68 17 using namespace SWITCH;
7ka884 4:ba9df71868df 18 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 19 using namespace ENCODER;
Ryosei 27:dd9f27fce7d1 20
M_souta 21:e3b58d675c1c 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
M_souta 22:7d93f79a3686 23
t_yamamoto 0:669ef71cba68 24 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 27
t_yamamoto 0:669ef71cba68 28 static bool lock;
t_yamamoto 0:669ef71cba68 29 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 30 static int current;
t_yamamoto 0:669ef71cba68 31
t_yamamoto 0:669ef71cba68 32 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 35 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 36 #endif
t_yamamoto 0:669ef71cba68 37
t_yamamoto 0:669ef71cba68 38 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 41
kishibekairohan 2:c015739085d3 42
M_souta 22:7d93f79a3686 43 Serial pc(USBTX, USBRX);
kishibekairohan 7:e88c5d47a3be 44
kishibekairohan 16:3f2c2d89372b 45 //**************Buzzer****************
kishibekairohan 11:028a150943b5 46 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 47 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 48 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 49 bool EMGflag = false;
kishibekairohan 16:3f2c2d89372b 50 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 51 //**************Buzzer****************
kishibekairohan 11:028a150943b5 52
kishibekairohan 11:028a150943b5 53 //************TapeLed*****************
kishibekairohan 11:028a150943b5 54 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 55 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 56 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 57 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 58 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 59 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 60
M_souta 22:7d93f79a3686 61 //*************** lift ***************
M_souta 22:7d93f79a3686 62 #define LOWER 1
M_souta 22:7d93f79a3686 63 #define MIDDLRE 2
M_souta 22:7d93f79a3686 64 #define UPPER 3
M_souta 22:7d93f79a3686 65 uint8_t liftState = LOWER;
M_souta 22:7d93f79a3686 66 bool moving = false;
M_souta 22:7d93f79a3686 67 bool switchFlag_LB = false;
M_souta 22:7d93f79a3686 68 bool switchFlag_RB = false;
M_souta 22:7d93f79a3686 69
M_souta 22:7d93f79a3686 70 //*************** lift ***************
M_souta 22:7d93f79a3686 71
Ryosei 27:dd9f27fce7d1 72 //*****************Air********************
Ryosei 27:dd9f27fce7d1 73 DigitalOut air[]= {
Ryosei 27:dd9f27fce7d1 74 DigitalOut(ECD_A_0),
Ryosei 27:dd9f27fce7d1 75 DigitalOut(ECD_B_0),
Ryosei 27:dd9f27fce7d1 76 DigitalOut(ECD_A_1),
Ryosei 27:dd9f27fce7d1 77 DigitalOut(ECD_B_1),
Ryosei 27:dd9f27fce7d1 78 };
Ryosei 27:dd9f27fce7d1 79
Ryosei 27:dd9f27fce7d1 80 bool Air[4];
Ryosei 27:dd9f27fce7d1 81 void AirUpdate()
Ryosei 27:dd9f27fce7d1 82 {
Ryosei 27:dd9f27fce7d1 83 for(int i=0; i<=3; i++) {
Ryosei 27:dd9f27fce7d1 84 air[i]=Air[i];
Ryosei 27:dd9f27fce7d1 85 }
Ryosei 27:dd9f27fce7d1 86 }
Ryosei 27:dd9f27fce7d1 87 //*****************Air********************
Ryosei 27:dd9f27fce7d1 88
M_souta 22:7d93f79a3686 89 //*************tire*************
M_souta 22:7d93f79a3686 90 PID rotaconPID[] = {
Ryosei 27:dd9f27fce7d1 91 PID(0.0001,-1,1,0.05,0,0), //LF
Ryosei 27:dd9f27fce7d1 92 PID(0.0001,-1,1,0.05,0,0), //LB
Ryosei 27:dd9f27fce7d1 93 PID(0.0001,-1,1,0.05,0,0), //RB
Ryosei 27:dd9f27fce7d1 94 PID(0.0001,-1,1,0.05,0,0), //RF
M_souta 22:7d93f79a3686 95 };
M_souta 22:7d93f79a3686 96
M_souta 22:7d93f79a3686 97 #define FL 0
M_souta 22:7d93f79a3686 98 #define BL 1
M_souta 22:7d93f79a3686 99 #define BR 2
M_souta 22:7d93f79a3686 100 #define FR 3
M_souta 22:7d93f79a3686 101
M_souta 22:7d93f79a3686 102 #define PI 3.141592
M_souta 22:7d93f79a3686 103
M_souta 22:7d93f79a3686 104 const float tireR = 101.6; //タイヤの半径
M_souta 22:7d93f79a3686 105 const float ucR = 420.0; //中心からのタイヤの距離
M_souta 22:7d93f79a3686 106
M_souta 22:7d93f79a3686 107 typedef struct {
Ryosei 27:dd9f27fce7d1 108 float Vx; //X方向の速度
Ryosei 27:dd9f27fce7d1 109 float Vy; //Y方向の速度
Ryosei 27:dd9f27fce7d1 110 float Va; //角速度
M_souta 22:7d93f79a3686 111 } Vvector;
M_souta 22:7d93f79a3686 112
M_souta 22:7d93f79a3686 113 Vvector move; //進む速度
M_souta 22:7d93f79a3686 114 Vvector correction_LT; //ライントレースの補正速度
M_souta 22:7d93f79a3686 115 Vvector synthetic; //合成速度
M_souta 22:7d93f79a3686 116
M_souta 22:7d93f79a3686 117 float sita = 0;
M_souta 22:7d93f79a3686 118
M_souta 22:7d93f79a3686 119 bool PIDflag = false;
M_souta 22:7d93f79a3686 120
M_souta 22:7d93f79a3686 121 int linePara[8];
M_souta 22:7d93f79a3686 122 int linePara_U;
M_souta 22:7d93f79a3686 123 int linePara_B;
M_souta 22:7d93f79a3686 124 int linePara_L;
M_souta 22:7d93f79a3686 125 int linePara_R;
M_souta 22:7d93f79a3686 126
M_souta 22:7d93f79a3686 127 #define FL 0
M_souta 22:7d93f79a3686 128 #define BL 1
M_souta 22:7d93f79a3686 129 #define BR 2
M_souta 22:7d93f79a3686 130 #define FR 3
M_souta 22:7d93f79a3686 131
M_souta 22:7d93f79a3686 132 float tireProcessRPM[4];
M_souta 22:7d93f79a3686 133 float tireTargetMaxRPM[4];
M_souta 22:7d93f79a3686 134 float tireTargetRPM[4];
M_souta 22:7d93f79a3686 135
M_souta 22:7d93f79a3686 136 float tirePWM[4];
M_souta 22:7d93f79a3686 137
M_souta 22:7d93f79a3686 138 float timePV[4];
M_souta 22:7d93f79a3686 139 float timeCV[4];
M_souta 22:7d93f79a3686 140 float pulsePV[4];
M_souta 22:7d93f79a3686 141 float pulseCV[4];
M_souta 22:7d93f79a3686 142
M_souta 22:7d93f79a3686 143 void tirePID();
M_souta 22:7d93f79a3686 144 int lineCast(char k);
M_souta 22:7d93f79a3686 145
M_souta 22:7d93f79a3686 146 Timer rotaconSampling;
M_souta 22:7d93f79a3686 147 Ticker rotaconPIDtimer;
M_souta 22:7d93f79a3686 148
M_souta 22:7d93f79a3686 149 bool countFlag;
M_souta 22:7d93f79a3686 150 //*************tire**************//
M_souta 22:7d93f79a3686 151
Ryosei 27:dd9f27fce7d1 152 // ************* Ultra ************** //
Ryosei 27:dd9f27fce7d1 153 //double temp=( Temp.read()* 3.3 - 0.6) / 0.01;
Ryosei 27:dd9f27fce7d1 154 PulseInOut Echo0(ECHO_0);
Ryosei 27:dd9f27fce7d1 155 PulseInOut Echo1(ECHO_1);
Ryosei 27:dd9f27fce7d1 156 PulseInOut Trig0(TRIG_0);
Ryosei 27:dd9f27fce7d1 157 PulseInOut Trig1(TRIG_1);
Ryosei 27:dd9f27fce7d1 158 double UltraRead(int num)
Ryosei 27:dd9f27fce7d1 159 {
Ryosei 27:dd9f27fce7d1 160 double Distance=0;
Ryosei 27:dd9f27fce7d1 161 double Duration=0;
Ryosei 27:dd9f27fce7d1 162 double temp=28;////////////////////////////////////////////////温度
Ryosei 27:dd9f27fce7d1 163 if(num==0) {
Ryosei 27:dd9f27fce7d1 164 Trig0.write_us(1,10);
Ryosei 27:dd9f27fce7d1 165 Duration=Echo0.read_high_us(5000);
Ryosei 27:dd9f27fce7d1 166 } else if(num==1) {
Ryosei 27:dd9f27fce7d1 167 Trig1.write_us(1,10);
Ryosei 27:dd9f27fce7d1 168 Duration=Echo1.read_high_us(5000);
Ryosei 27:dd9f27fce7d1 169 }
Ryosei 27:dd9f27fce7d1 170 if(Duration>0) {
Ryosei 27:dd9f27fce7d1 171 Duration=Duration/2;
Ryosei 27:dd9f27fce7d1 172 double sspead=331.5+0.6*temp;
Ryosei 27:dd9f27fce7d1 173 Distance=Duration*sspead*100/1000000;
Ryosei 27:dd9f27fce7d1 174 } else {
Ryosei 27:dd9f27fce7d1 175 return 0;
Ryosei 27:dd9f27fce7d1 176 }
Ryosei 27:dd9f27fce7d1 177 return Distance;
Ryosei 27:dd9f27fce7d1 178 }
Ryosei 27:dd9f27fce7d1 179 //*********Ultra***************
Ryosei 27:dd9f27fce7d1 180
Ryosei 27:dd9f27fce7d1 181 //*********Tape LED**************************
Ryosei 27:dd9f27fce7d1 182 DigitalOut select1(SELECT_1);
Ryosei 27:dd9f27fce7d1 183 DigitalOut select2(SELECT_2);
Ryosei 27:dd9f27fce7d1 184 DigitalOut select3(SELECT_3);
Ryosei 27:dd9f27fce7d1 185 void LedOut(int num)
Ryosei 27:dd9f27fce7d1 186 {
Ryosei 27:dd9f27fce7d1 187 int selectnum[8][3]= {
Ryosei 27:dd9f27fce7d1 188 {0,0,0},
Ryosei 27:dd9f27fce7d1 189 {0,0,1},
Ryosei 27:dd9f27fce7d1 190 {0,1,0},
Ryosei 27:dd9f27fce7d1 191 {0,1,1},
Ryosei 27:dd9f27fce7d1 192 {1,0,0},
Ryosei 27:dd9f27fce7d1 193 {1,0,1},
Ryosei 27:dd9f27fce7d1 194 {1,1,0},
Ryosei 27:dd9f27fce7d1 195 {1,1,1}
Ryosei 27:dd9f27fce7d1 196 };
Ryosei 27:dd9f27fce7d1 197 select1=selectnum[num][0];
Ryosei 27:dd9f27fce7d1 198 select2=selectnum[num][1];
Ryosei 27:dd9f27fce7d1 199 select3=selectnum[num][2];
Ryosei 27:dd9f27fce7d1 200 }
Ryosei 27:dd9f27fce7d1 201 //*********Tape LED**************************
Ryosei 27:dd9f27fce7d1 202
M_souta 22:7d93f79a3686 203 // ************* Line ************** //
M_souta 22:7d93f79a3686 204
M_souta 26:4c0ce2f05688 205 Timer tow_stop;
M_souta 26:4c0ce2f05688 206
M_souta 22:7d93f79a3686 207 float pw = 0;
M_souta 22:7d93f79a3686 208 int lineFase = 0;
M_souta 22:7d93f79a3686 209 bool lineCheck = false;
M_souta 25:b3a9f34b201d 210 bool lineFlag = false;
M_souta 25:b3a9f34b201d 211 bool adjAnable = false;
M_souta 25:b3a9f34b201d 212 int adj = 0;
M_souta 25:b3a9f34b201d 213
M_souta 26:4c0ce2f05688 214 int countW = 0;
M_souta 25:b3a9f34b201d 215 int lineCount = 0;
M_souta 25:b3a9f34b201d 216 int targetCount = 0;
M_souta 25:b3a9f34b201d 217
M_souta 25:b3a9f34b201d 218 bool startFlag = true;
M_souta 25:b3a9f34b201d 219
M_souta 22:7d93f79a3686 220 int linePWM;
M_souta 22:7d93f79a3686 221 int adj_F;
M_souta 22:7d93f79a3686 222 int adj_B;
M_souta 22:7d93f79a3686 223
M_souta 22:7d93f79a3686 224 int mode = 0;
M_souta 22:7d93f79a3686 225
M_souta 22:7d93f79a3686 226 // ************* Line ************** //
M_souta 22:7d93f79a3686 227
Ryosei 27:dd9f27fce7d1 228 const int omni[15][15] = {
Ryosei 27:dd9f27fce7d1 229 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 230 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 231 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 232 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei 27:dd9f27fce7d1 233 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei 27:dd9f27fce7d1 234 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei 27:dd9f27fce7d1 235 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei 27:dd9f27fce7d1 236 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei 27:dd9f27fce7d1 237 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei 27:dd9f27fce7d1 238 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei 27:dd9f27fce7d1 239 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei 27:dd9f27fce7d1 240 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei 27:dd9f27fce7d1 241 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei 27:dd9f27fce7d1 242 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei 27:dd9f27fce7d1 243 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 244 };
M_souta 21:e3b58d675c1c 245
M_souta 21:e3b58d675c1c 246 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 247
M_souta 21:e3b58d675c1c 248 uint8_t SetStatus(int);
Ryosei 27:dd9f27fce7d1 249 uint8_t SetStatus(int pwmVal)
Ryosei 27:dd9f27fce7d1 250 {
Ryosei 27:dd9f27fce7d1 251 if (pwmVal < 0) return BACK;
Ryosei 27:dd9f27fce7d1 252 else if (pwmVal > 0) return FOR;
Ryosei 27:dd9f27fce7d1 253 else if (pwmVal == 0) return BRAKE;
Ryosei 27:dd9f27fce7d1 254 else return BRAKE;
M_souta 20:eae8c84f318c 255 }
M_souta 21:e3b58d675c1c 256 uint8_t SetPWM(int);
Ryosei 27:dd9f27fce7d1 257 uint8_t SetPWM(int pwmVal)
Ryosei 27:dd9f27fce7d1 258 {
Ryosei 27:dd9f27fce7d1 259 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
Ryosei 27:dd9f27fce7d1 260 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 261 }
M_souta 18:c694bae76e51 262
t_yamamoto 0:669ef71cba68 263 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 264
t_yamamoto 0:669ef71cba68 265 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 266 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 267 static void Process0(void);
t_yamamoto 0:669ef71cba68 268 #endif
t_yamamoto 0:669ef71cba68 269 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 270 static void Process1(void);
t_yamamoto 0:669ef71cba68 271 #endif
t_yamamoto 0:669ef71cba68 272 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 273 static void Process2(void);
t_yamamoto 0:669ef71cba68 274 #endif
t_yamamoto 0:669ef71cba68 275 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 276 static void Process3(void);
t_yamamoto 0:669ef71cba68 277 #endif
t_yamamoto 0:669ef71cba68 278 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 279 static void Process4(void);
t_yamamoto 0:669ef71cba68 280 #endif
t_yamamoto 0:669ef71cba68 281 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 282 static void Process5(void);
t_yamamoto 0:669ef71cba68 283 #endif
t_yamamoto 0:669ef71cba68 284 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 285 static void Process6(void);
t_yamamoto 0:669ef71cba68 286 #endif
t_yamamoto 0:669ef71cba68 287 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 288 static void Process7(void);
t_yamamoto 0:669ef71cba68 289 #endif
t_yamamoto 0:669ef71cba68 290 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 291 static void Process8(void);
t_yamamoto 0:669ef71cba68 292 #endif
t_yamamoto 0:669ef71cba68 293 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 294 static void Process9(void);
t_yamamoto 0:669ef71cba68 295 #endif
t_yamamoto 0:669ef71cba68 296 #endif
t_yamamoto 0:669ef71cba68 297
t_yamamoto 0:669ef71cba68 298 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 299 {
Ryosei 27:dd9f27fce7d1 300 #pragma region USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 301 /*Replace here with the initialization code of your variables.*/
Ryosei 27:dd9f27fce7d1 302 //rotaconPIDtimer.attach(tirePID,0.1);
t_yamamoto 0:669ef71cba68 303
Ryosei 27:dd9f27fce7d1 304 //DigitalOut Air_16(LS_16);
Ryosei 27:dd9f27fce7d1 305 //DigitalOut Air_17(LS_17);
Ryosei 27:dd9f27fce7d1 306 //DigitalOut Air_18(LS_18);
Ryosei 27:dd9f27fce7d1 307 //DigitalOut Air_19(LS_19);
Ryosei 27:dd9f27fce7d1 308
Ryosei 27:dd9f27fce7d1 309 #pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 310
Ryosei 27:dd9f27fce7d1 311 lock = true;
Ryosei 27:dd9f27fce7d1 312 processChangeComp = true;
Ryosei 27:dd9f27fce7d1 313 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 314
Ryosei 27:dd9f27fce7d1 315 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 316 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 317 Process[0] = Process0;
Ryosei 27:dd9f27fce7d1 318 #endif
Ryosei 27:dd9f27fce7d1 319 #if USE_PROCESS_NUM>1
Ryosei 27:dd9f27fce7d1 320 Process[1] = Process1;
Ryosei 27:dd9f27fce7d1 321 #endif
Ryosei 27:dd9f27fce7d1 322 #if USE_PROCESS_NUM>2
Ryosei 27:dd9f27fce7d1 323 Process[2] = Process2;
Ryosei 27:dd9f27fce7d1 324 #endif
Ryosei 27:dd9f27fce7d1 325 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 326 Process[3] = Process3;
Ryosei 27:dd9f27fce7d1 327 #endif
Ryosei 27:dd9f27fce7d1 328 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 329 Process[4] = Process4;
Ryosei 27:dd9f27fce7d1 330 #endif
Ryosei 27:dd9f27fce7d1 331 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 332 Process[5] = Process5;
Ryosei 27:dd9f27fce7d1 333 #endif
Ryosei 27:dd9f27fce7d1 334 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 335 Process[6] = Process6;
Ryosei 27:dd9f27fce7d1 336 #endif
Ryosei 27:dd9f27fce7d1 337 #if USE_PROCESS_NUM>7
Ryosei 27:dd9f27fce7d1 338 Process[7] = Process7;
Ryosei 27:dd9f27fce7d1 339 #endif
Ryosei 27:dd9f27fce7d1 340 #if USE_PROCESS_NUM>8
Ryosei 27:dd9f27fce7d1 341 Process[8] = Process8;
Ryosei 27:dd9f27fce7d1 342 #endif
Ryosei 27:dd9f27fce7d1 343 #if USE_PROCESS_NUM>9
Ryosei 27:dd9f27fce7d1 344 Process[9] = Process9;
Ryosei 27:dd9f27fce7d1 345 #endif
Ryosei 27:dd9f27fce7d1 346 #endif
t_yamamoto 0:669ef71cba68 347 }
t_yamamoto 0:669ef71cba68 348
t_yamamoto 0:669ef71cba68 349 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 350 {
Ryosei 27:dd9f27fce7d1 351 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 352 if(controller->Button.HOME) lock = false;
Ryosei 27:dd9f27fce7d1 353
Ryosei 27:dd9f27fce7d1 354 if(controller->Button.START && processChangeComp) {
Ryosei 27:dd9f27fce7d1 355 current++;
Ryosei 27:dd9f27fce7d1 356 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei 27:dd9f27fce7d1 357 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 358 } else if(controller->Button.SELECT && processChangeComp) {
Ryosei 27:dd9f27fce7d1 359 current--;
Ryosei 27:dd9f27fce7d1 360 if (current < 0) current = 0;
Ryosei 27:dd9f27fce7d1 361 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 362 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei 27:dd9f27fce7d1 363 #endif
Ryosei 27:dd9f27fce7d1 364
Ryosei 27:dd9f27fce7d1 365 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 366 ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei 27:dd9f27fce7d1 367 #endif
Ryosei 27:dd9f27fce7d1 368
Ryosei 27:dd9f27fce7d1 369 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 370 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei 27:dd9f27fce7d1 371 #endif
Ryosei 27:dd9f27fce7d1 372
Ryosei 27:dd9f27fce7d1 373 #ifdef USE_RS485
Ryosei 27:dd9f27fce7d1 374 ACTUATORHUB::ActuatorHub::Update();
Ryosei 27:dd9f27fce7d1 375 //LINEHUB::LineHub::Update();
Ryosei 27:dd9f27fce7d1 376 #endif
Ryosei 27:dd9f27fce7d1 377
Ryosei 27:dd9f27fce7d1 378 }
Ryosei 27:dd9f27fce7d1 379
Ryosei 27:dd9f27fce7d1 380
Ryosei 27:dd9f27fce7d1 381 double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor
Ryosei 27:dd9f27fce7d1 382 double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
Ryosei 27:dd9f27fce7d1 383 static int Limitphase=0;
Ryosei 27:dd9f27fce7d1 384 void SystemProcess()
Ryosei 27:dd9f27fce7d1 385 {
Ryosei 27:dd9f27fce7d1 386 SystemProcessInitialize();
Ryosei 27:dd9f27fce7d1 387 while(1) {
Ryosei 27:dd9f27fce7d1 388 AirUpdate();
Ryosei 27:dd9f27fce7d1 389 int g[8];
Ryosei 27:dd9f27fce7d1 390 for(int i = 0; i < 8; i++) {
Ryosei 27:dd9f27fce7d1 391 g[i] = lineCast(LineHub::GetPara(i));
Ryosei 27:dd9f27fce7d1 392 }
Ryosei 27:dd9f27fce7d1 393 Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
Ryosei 27:dd9f27fce7d1 394 Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
Ryosei 27:dd9f27fce7d1 395 pc.printf("%lf,%lf",UltraRead(0),UltraRead(1));
Ryosei 27:dd9f27fce7d1 396 // 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]);
t_yamamoto 0:669ef71cba68 397
Ryosei 27:dd9f27fce7d1 398 pc.printf("%d\n\r",current);
Ryosei 27:dd9f27fce7d1 399 /*上
Ryosei 27:dd9f27fce7d1 400 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 401 motor[LIFT_LB].pwm = 180;
Ryosei 27:dd9f27fce7d1 402 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 403 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 404 */
Ryosei 27:dd9f27fce7d1 405 if(!(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW))&&startFlag==true) {
Ryosei 27:dd9f27fce7d1 406 startFlag=false;
Ryosei 27:dd9f27fce7d1 407 lock=false;
Ryosei 27:dd9f27fce7d1 408 current=7;
Ryosei 27:dd9f27fce7d1 409 } else {
Ryosei 27:dd9f27fce7d1 410 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 411 motor[LIFT_LB].pwm = 180;
Ryosei 27:dd9f27fce7d1 412 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 413 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 414 }
Ryosei 27:dd9f27fce7d1 415 if(LimitSw::IsPressed(START_SW) && startFlag == true) {
Ryosei 27:dd9f27fce7d1 416 LedOut(6);
Ryosei 27:dd9f27fce7d1 417 startFlag = false;
Ryosei 27:dd9f27fce7d1 418 lock = false;
Ryosei 27:dd9f27fce7d1 419 lineFase = 0;
Ryosei 27:dd9f27fce7d1 420 lineCount = 0;
Ryosei 27:dd9f27fce7d1 421 lineCheck = false;
Ryosei 27:dd9f27fce7d1 422 countW = 0;
Ryosei 27:dd9f27fce7d1 423 if(LimitSw::IsPressed(REDBLUE_SW)) {
Ryosei 27:dd9f27fce7d1 424 current = 4;
Ryosei 27:dd9f27fce7d1 425 } else {
Ryosei 27:dd9f27fce7d1 426 current = 5;
Ryosei 27:dd9f27fce7d1 427 }
Ryosei 27:dd9f27fce7d1 428 }
Ryosei 27:dd9f27fce7d1 429 buzzer.period(1.0/800);
Ryosei 27:dd9f27fce7d1 430
Ryosei 27:dd9f27fce7d1 431 #ifdef USE_MU
Ryosei 27:dd9f27fce7d1 432 controller = CONTROLLER::Controller::GetData();
Ryosei 27:dd9f27fce7d1 433 #endif
Ryosei 27:dd9f27fce7d1 434
Ryosei 27:dd9f27fce7d1 435 #ifdef USE_ERRORCHECK
Ryosei 27:dd9f27fce7d1 436 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag) {
Ryosei 27:dd9f27fce7d1 437 CONTROLLER::Controller::DataReset();
Ryosei 27:dd9f27fce7d1 438 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 439 lock = true;
Ryosei 27:dd9f27fce7d1 440 } else
Ryosei 27:dd9f27fce7d1 441 #endif
Ryosei 27:dd9f27fce7d1 442 {
Ryosei 27:dd9f27fce7d1 443
Ryosei 27:dd9f27fce7d1 444 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 445 if(!lock) {
Ryosei 27:dd9f27fce7d1 446 Process[current]();
Ryosei 27:dd9f27fce7d1 447 } else
Ryosei 27:dd9f27fce7d1 448 #endif
Ryosei 27:dd9f27fce7d1 449 {
Ryosei 27:dd9f27fce7d1 450 //ロック時の処理
Ryosei 27:dd9f27fce7d1 451 }
Ryosei 27:dd9f27fce7d1 452 }
Ryosei 27:dd9f27fce7d1 453
Ryosei 27:dd9f27fce7d1 454
Ryosei 27:dd9f27fce7d1 455 //Emergency!
Ryosei 27:dd9f27fce7d1 456 /*
Ryosei 27:dd9f27fce7d1 457 if(!EMG_0 && !EMG_1 && !EMGflag){
Ryosei 27:dd9f27fce7d1 458 buzzer = 0;
Ryosei 27:dd9f27fce7d1 459 BuzzerTimer.attach(BuzzerTimer_func, 1);
Ryosei 27:dd9f27fce7d1 460 EMGflag = true;
Ryosei 27:dd9f27fce7d1 461 LED_DEBUG0 = 1;
Ryosei 27:dd9f27fce7d1 462 }
Ryosei 27:dd9f27fce7d1 463 if(EMG_0 && EMG_1 && EMGflag){
Ryosei 27:dd9f27fce7d1 464 buzzer = 1;
Ryosei 27:dd9f27fce7d1 465 BuzzerTimer.detach();
Ryosei 27:dd9f27fce7d1 466 EMGflag = false;
Ryosei 27:dd9f27fce7d1 467 }
Ryosei 27:dd9f27fce7d1 468 */
Ryosei 27:dd9f27fce7d1 469 SystemProcessUpdate();
Ryosei 27:dd9f27fce7d1 470 }
t_yamamoto 0:669ef71cba68 471 }
t_yamamoto 0:669ef71cba68 472
kishibekairohan 2:c015739085d3 473
kishibekairohan 2:c015739085d3 474
kishibekairohan 2:c015739085d3 475
t_yamamoto 0:669ef71cba68 476 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 477 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 478 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 479 static void Process0()
Ryosei 27:dd9f27fce7d1 480 {
Ryosei 27:dd9f27fce7d1 481 AllActuatorReset();
t_yamamoto 0:669ef71cba68 482 }
t_yamamoto 0:669ef71cba68 483 #endif
t_yamamoto 0:669ef71cba68 484
t_yamamoto 0:669ef71cba68 485 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 486 static void Process1()
t_yamamoto 0:669ef71cba68 487 {
Ryosei 27:dd9f27fce7d1 488
M_souta 22:7d93f79a3686 489 PIDflag = false;
Ryosei 27:dd9f27fce7d1 490 LedOut(0);
M_souta 21:e3b58d675c1c 491 if(controller->Button.UP) {
Ryosei 27:dd9f27fce7d1 492 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 493 motor[LIFT_LB].pwm = 180;
Ryosei 27:dd9f27fce7d1 494 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 495 motor[LIFT_RB].pwm = 200;
M_souta 21:e3b58d675c1c 496 } else if(controller->Button.DOWN) {
Ryosei 27:dd9f27fce7d1 497 motor[LIFT_LB].dir = BACK;
Ryosei 27:dd9f27fce7d1 498 motor[LIFT_LB].pwm = 180;
Ryosei 27:dd9f27fce7d1 499 motor[LIFT_RB].dir = FOR;
Ryosei 27:dd9f27fce7d1 500 motor[LIFT_RB].pwm = 200;
M_souta 23:c853372cf626 501 } else if(controller->Button.LEFT) {
Ryosei 27:dd9f27fce7d1 502 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 503 motor[LIFT_LB].pwm = 180;
M_souta 23:c853372cf626 504 } else if(controller->Button.RIGHT) {
Ryosei 27:dd9f27fce7d1 505 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 506 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 507 } else {
Ryosei 27:dd9f27fce7d1 508 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 509 motor[LIFT_LB].pwm = 255;
Ryosei 27:dd9f27fce7d1 510 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 511 motor[LIFT_RB].pwm = 255;
M_souta 21:e3b58d675c1c 512 }
M_souta 21:e3b58d675c1c 513
Ryosei 27:dd9f27fce7d1 514
Ryosei 27:dd9f27fce7d1 515 if(controller->Button.X) {
Ryosei 27:dd9f27fce7d1 516 motor[LIFT_U].dir = FOR;
Ryosei 27:dd9f27fce7d1 517 motor[LIFT_U].pwm = 180;
Ryosei 27:dd9f27fce7d1 518 } else if(controller->Button.Y) {
Ryosei 27:dd9f27fce7d1 519 motor[LIFT_U].dir = BACK;
Ryosei 27:dd9f27fce7d1 520 motor[LIFT_U].pwm = 230;
Ryosei 27:dd9f27fce7d1 521 } else {
Ryosei 27:dd9f27fce7d1 522 motor[LIFT_U].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 523 motor[LIFT_U].pwm = 255;
Ryosei 27:dd9f27fce7d1 524 }
Ryosei 27:dd9f27fce7d1 525
Ryosei 27:dd9f27fce7d1 526 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
Ryosei 27:dd9f27fce7d1 527 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
Ryosei 27:dd9f27fce7d1 528 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
Ryosei 27:dd9f27fce7d1 529 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
Ryosei 27:dd9f27fce7d1 530 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
Ryosei 27:dd9f27fce7d1 531
Ryosei 27:dd9f27fce7d1 532 motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
Ryosei 27:dd9f27fce7d1 533 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
Ryosei 27:dd9f27fce7d1 534 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
Ryosei 27:dd9f27fce7d1 535 motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
Ryosei 27:dd9f27fce7d1 536 } else {
Ryosei 27:dd9f27fce7d1 537 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 538 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 539 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 540 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 541
Ryosei 27:dd9f27fce7d1 542 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 543 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 544 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 545 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei 27:dd9f27fce7d1 546 }
Ryosei 27:dd9f27fce7d1 547
Ryosei 27:dd9f27fce7d1 548 if(controller->Button.ZR) {
Ryosei 27:dd9f27fce7d1 549 Air[CLOTHESPIN] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 550 }
Ryosei 27:dd9f27fce7d1 551 if(controller->Button.ZL) {
Ryosei 27:dd9f27fce7d1 552 Air[CLOTHESPIN] = SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 553 }
t_yamamoto 0:669ef71cba68 554 }
t_yamamoto 0:669ef71cba68 555 #endif
t_yamamoto 0:669ef71cba68 556
t_yamamoto 0:669ef71cba68 557 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 558 static void Process2()
Ryosei 27:dd9f27fce7d1 559 {
M_souta 26:4c0ce2f05688 560
kishibekairohan 13:b6e02d6261d7 561 }
kishibekairohan 13:b6e02d6261d7 562 #endif
kishibekairohan 13:b6e02d6261d7 563
kishibekairohan 13:b6e02d6261d7 564 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 565 static void Process3()
kishibekairohan 13:b6e02d6261d7 566 {
Ryosei 27:dd9f27fce7d1 567 startFlag = true;
Ryosei 27:dd9f27fce7d1 568 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 569 lineFase = 0;
Ryosei 27:dd9f27fce7d1 570 lineCheck = false;
Ryosei 27:dd9f27fce7d1 571 lineCount = 0;
Ryosei 27:dd9f27fce7d1 572 countW = 0;
kishibekairohan 13:b6e02d6261d7 573 }
kishibekairohan 13:b6e02d6261d7 574 #endif
kishibekairohan 13:b6e02d6261d7 575
kishibekairohan 13:b6e02d6261d7 576 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 577 static void Process4()
M_souta 22:7d93f79a3686 578 {
Ryosei 27:dd9f27fce7d1 579 LED_DEBUG0 = LED_ON;
Ryosei 27:dd9f27fce7d1 580
Ryosei 27:dd9f27fce7d1 581 /* ************************************** //
Ryosei 27:dd9f27fce7d1 582
Ryosei 27:dd9f27fce7d1 583 赤ゾーン 赤ゾーン 赤ゾーン
Ryosei 27:dd9f27fce7d1 584
Ryosei 27:dd9f27fce7d1 585 // ************************************** */
Ryosei 27:dd9f27fce7d1 586
Ryosei 27:dd9f27fce7d1 587
Ryosei 27:dd9f27fce7d1 588 for(int i = 0; i < 8; i++) {
Ryosei 27:dd9f27fce7d1 589 linePara[i] = lineCast(LineHub::GetPara(i));
Ryosei 27:dd9f27fce7d1 590 }
Ryosei 27:dd9f27fce7d1 591
Ryosei 27:dd9f27fce7d1 592 if(lineFase == 0) {
Ryosei 27:dd9f27fce7d1 593 LedOut(2);
Ryosei 27:dd9f27fce7d1 594 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 595 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 596 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 597 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 598 if(linePara[0] != 'N' && linePara[0] != 'A') {
Ryosei 27:dd9f27fce7d1 599 lineFase = 1;
Ryosei 27:dd9f27fce7d1 600 }
Ryosei 27:dd9f27fce7d1 601 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 602 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 603 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 604 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 605
Ryosei 27:dd9f27fce7d1 606 } else if(lineFase == 1) { // 前 ライントレース
Ryosei 27:dd9f27fce7d1 607 switch(linePara[0]) {
Ryosei 27:dd9f27fce7d1 608 case -2:
Ryosei 27:dd9f27fce7d1 609 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 610 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 611 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 612 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 613 adjAnable = true;
Ryosei 27:dd9f27fce7d1 614 break;
Ryosei 27:dd9f27fce7d1 615 case -3:
Ryosei 27:dd9f27fce7d1 616 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 617 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 618 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 619 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 620 adjAnable = true;
Ryosei 27:dd9f27fce7d1 621 break;
Ryosei 27:dd9f27fce7d1 622 case -1:
Ryosei 27:dd9f27fce7d1 623 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 624 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 625 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 626 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 627 adjAnable = true;
Ryosei 27:dd9f27fce7d1 628 break;
Ryosei 27:dd9f27fce7d1 629 case 0:
Ryosei 27:dd9f27fce7d1 630 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 631 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 632 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 633 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 634 adjAnable = true;
Ryosei 27:dd9f27fce7d1 635 break;
Ryosei 27:dd9f27fce7d1 636 case 1:
Ryosei 27:dd9f27fce7d1 637 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 638 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 639 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 640 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 641 adjAnable = true;
Ryosei 27:dd9f27fce7d1 642 break;
Ryosei 27:dd9f27fce7d1 643 case 3:
Ryosei 27:dd9f27fce7d1 644 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 645 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 646 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 647 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 648 adjAnable = true;
Ryosei 27:dd9f27fce7d1 649 break;
Ryosei 27:dd9f27fce7d1 650 case 2:
Ryosei 27:dd9f27fce7d1 651 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 652 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 653 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 654 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 655 adjAnable = true;
Ryosei 27:dd9f27fce7d1 656 break;
Ryosei 27:dd9f27fce7d1 657 case 'A':
Ryosei 27:dd9f27fce7d1 658 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 659 lineCheck = true;
Ryosei 27:dd9f27fce7d1 660 lineCount = 0;
Ryosei 27:dd9f27fce7d1 661 countW++;
Ryosei 27:dd9f27fce7d1 662 }
Ryosei 27:dd9f27fce7d1 663 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 664 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 665 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 666 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 667 adjAnable = true;
Ryosei 27:dd9f27fce7d1 668 break;
Ryosei 27:dd9f27fce7d1 669 case 'N':
Ryosei 27:dd9f27fce7d1 670 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei 27:dd9f27fce7d1 671 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei 27:dd9f27fce7d1 672 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei 27:dd9f27fce7d1 673 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei 27:dd9f27fce7d1 674 adjAnable = false;
Ryosei 27:dd9f27fce7d1 675 break;
Ryosei 27:dd9f27fce7d1 676 default:
Ryosei 27:dd9f27fce7d1 677 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 678 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 679 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 680 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 681 adjAnable = false;
Ryosei 27:dd9f27fce7d1 682 }
Ryosei 27:dd9f27fce7d1 683
Ryosei 27:dd9f27fce7d1 684 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 685 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
Ryosei 27:dd9f27fce7d1 686 } else {
Ryosei 27:dd9f27fce7d1 687 adj = 0;
Ryosei 27:dd9f27fce7d1 688 }
Ryosei 27:dd9f27fce7d1 689
Ryosei 27:dd9f27fce7d1 690 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 691 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 692 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 693 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 694 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 695 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 696 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 697 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 698
Ryosei 27:dd9f27fce7d1 699 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 700 lineCount++;
Ryosei 27:dd9f27fce7d1 701 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 702 }
Ryosei 27:dd9f27fce7d1 703 if(countW == 3) {
Ryosei 27:dd9f27fce7d1 704 countW = 0;
Ryosei 27:dd9f27fce7d1 705 lineFase = 2;
Ryosei 27:dd9f27fce7d1 706 lineCount = 0;
Ryosei 27:dd9f27fce7d1 707 lineCheck = false;
Ryosei 27:dd9f27fce7d1 708 }
Ryosei 27:dd9f27fce7d1 709 } else if(lineFase == 2) { // 前 低速
Ryosei 27:dd9f27fce7d1 710 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 711 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 712 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 713 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 714 if(linePara[4] >= -1 && linePara[4] <= 1) {
Ryosei 27:dd9f27fce7d1 715 lineFase = 3;
Ryosei 27:dd9f27fce7d1 716 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 717 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 718 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 719 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 720 }
Ryosei 27:dd9f27fce7d1 721 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 722 motor[TIRE_BL].pwm = 20;
Ryosei 27:dd9f27fce7d1 723 motor[TIRE_BR].pwm = 20;
Ryosei 27:dd9f27fce7d1 724 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 725 } else if(lineFase == 3) { // 右 ライントレース
Ryosei 27:dd9f27fce7d1 726 switch(linePara[4]) {
Ryosei 27:dd9f27fce7d1 727 case -2:
Ryosei 27:dd9f27fce7d1 728 LedOut(2);
Ryosei 27:dd9f27fce7d1 729 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 730 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 731 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 732 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 733 adjAnable = true;
Ryosei 27:dd9f27fce7d1 734 break;
Ryosei 27:dd9f27fce7d1 735 case -3:
Ryosei 27:dd9f27fce7d1 736 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 737 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 738 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 739 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 740 adjAnable = true;
Ryosei 27:dd9f27fce7d1 741 break;
Ryosei 27:dd9f27fce7d1 742 case -1:
Ryosei 27:dd9f27fce7d1 743 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 744 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 745 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 746 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 747 adjAnable = true;
Ryosei 27:dd9f27fce7d1 748 break;
Ryosei 27:dd9f27fce7d1 749 case 0:
Ryosei 27:dd9f27fce7d1 750 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 751 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 752 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 753 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 754 adjAnable = true;
Ryosei 27:dd9f27fce7d1 755 break;
Ryosei 27:dd9f27fce7d1 756 case 1:
Ryosei 27:dd9f27fce7d1 757 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 758 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 759 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 760 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 761 adjAnable = true;
Ryosei 27:dd9f27fce7d1 762 break;
Ryosei 27:dd9f27fce7d1 763 case 3:
Ryosei 27:dd9f27fce7d1 764 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 765 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 766 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 767 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 768 adjAnable = true;
Ryosei 27:dd9f27fce7d1 769 break;
Ryosei 27:dd9f27fce7d1 770 case 2:
Ryosei 27:dd9f27fce7d1 771 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 772 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 773 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 774 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 775 adjAnable = true;
Ryosei 27:dd9f27fce7d1 776 break;
Ryosei 27:dd9f27fce7d1 777 case 'A':
Ryosei 27:dd9f27fce7d1 778 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 779 lineCheck = true;
Ryosei 27:dd9f27fce7d1 780 lineCount = 0;
Ryosei 27:dd9f27fce7d1 781 countW++;
Ryosei 27:dd9f27fce7d1 782 }
Ryosei 27:dd9f27fce7d1 783 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 784 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 785 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 786 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 787 adjAnable = true;
Ryosei 27:dd9f27fce7d1 788 break;
Ryosei 27:dd9f27fce7d1 789 case 'N':
Ryosei 27:dd9f27fce7d1 790 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 791 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 792 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 793 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 794 adjAnable = false;
Ryosei 27:dd9f27fce7d1 795 break;
Ryosei 27:dd9f27fce7d1 796 default:
Ryosei 27:dd9f27fce7d1 797 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 798 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 799 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 800 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 801 adjAnable = false;
Ryosei 27:dd9f27fce7d1 802 }
Ryosei 27:dd9f27fce7d1 803
Ryosei 27:dd9f27fce7d1 804 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 805 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
Ryosei 27:dd9f27fce7d1 806 } else {
Ryosei 27:dd9f27fce7d1 807 adj = 0;
Ryosei 27:dd9f27fce7d1 808 }
Ryosei 27:dd9f27fce7d1 809
Ryosei 27:dd9f27fce7d1 810 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 811 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 812 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 813 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 814 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 815 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 816 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 817 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 818
Ryosei 27:dd9f27fce7d1 819 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 820 lineCount++;
Ryosei 27:dd9f27fce7d1 821 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 822 }
Ryosei 27:dd9f27fce7d1 823
Ryosei 27:dd9f27fce7d1 824 targetCount = 1;
Ryosei 27:dd9f27fce7d1 825
Ryosei 27:dd9f27fce7d1 826 if(countW == targetCount) {
Ryosei 27:dd9f27fce7d1 827 countW = 0;
Ryosei 27:dd9f27fce7d1 828 lineFase = 4;
Ryosei 27:dd9f27fce7d1 829 lineCount = 0;
Ryosei 27:dd9f27fce7d1 830 lineCheck = false;
Ryosei 27:dd9f27fce7d1 831 }
Ryosei 27:dd9f27fce7d1 832 } else if(lineFase == 4) { // 右 低速
Ryosei 27:dd9f27fce7d1 833 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 834 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 835 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 836 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 837 if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
Ryosei 27:dd9f27fce7d1 838 lineFase = 6;
Ryosei 27:dd9f27fce7d1 839 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 840 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 841 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 842 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 843 }
Ryosei 27:dd9f27fce7d1 844 if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 845 if(!LimitSw::IsPressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 846 lineFase=100;
Ryosei 27:dd9f27fce7d1 847 } else {
Ryosei 27:dd9f27fce7d1 848 lineFase = 6;
Ryosei 27:dd9f27fce7d1 849 }
Ryosei 27:dd9f27fce7d1 850 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 851 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 852 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 853 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 854 }
Ryosei 27:dd9f27fce7d1 855 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 856 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 857 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 858 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 859 } else if (lineFase == 5) {
Ryosei 27:dd9f27fce7d1 860 lineFase = 6;
Ryosei 27:dd9f27fce7d1 861 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 862 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 863 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 864 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 865 } else if(lineFase == 6) { // タオル1 検知
Ryosei 27:dd9f27fce7d1 866 LedOut(4);
Ryosei 27:dd9f27fce7d1 867 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 868 lineFase = 7;
Ryosei 27:dd9f27fce7d1 869 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 870 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 871 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 872 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 873 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 874 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 875 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 876 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 877 } else if(LimitSw::IsPressed(TOW_1L)) {
Ryosei 27:dd9f27fce7d1 878 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 879 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 880 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 881 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 882 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 883 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 884 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 885 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 886 } else if(LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 887 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 888 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 889 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 890 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 891 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 892 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 893 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 894 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 895 } else {
Ryosei 27:dd9f27fce7d1 896 if(LimitSw::IsPressed(QF_SW)) {
Ryosei 27:dd9f27fce7d1 897 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 898 case -2:
Ryosei 27:dd9f27fce7d1 899 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 900 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 901 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 902 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 903 adjAnable = true;
Ryosei 27:dd9f27fce7d1 904 break;
Ryosei 27:dd9f27fce7d1 905 case -3:
Ryosei 27:dd9f27fce7d1 906 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 907 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 908 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 909 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 910 adjAnable = true;
Ryosei 27:dd9f27fce7d1 911 break;
Ryosei 27:dd9f27fce7d1 912 case -1:
Ryosei 27:dd9f27fce7d1 913 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 914 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 915 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 916 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 917 adjAnable = true;
Ryosei 27:dd9f27fce7d1 918 break;
Ryosei 27:dd9f27fce7d1 919 case 0:
Ryosei 27:dd9f27fce7d1 920 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 921 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 922 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 923 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 924 adjAnable = true;
Ryosei 27:dd9f27fce7d1 925 break;
Ryosei 27:dd9f27fce7d1 926 case 1:
Ryosei 27:dd9f27fce7d1 927 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 928 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 929 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 930 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 931 adjAnable = true;
Ryosei 27:dd9f27fce7d1 932 break;
Ryosei 27:dd9f27fce7d1 933 case 3:
Ryosei 27:dd9f27fce7d1 934 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 935 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 936 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 937 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 938 adjAnable = true;
Ryosei 27:dd9f27fce7d1 939 break;
Ryosei 27:dd9f27fce7d1 940 case 2:
Ryosei 27:dd9f27fce7d1 941 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 942 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 943 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 944 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 945 adjAnable = true;
Ryosei 27:dd9f27fce7d1 946 break;
Ryosei 27:dd9f27fce7d1 947 case 'A':
Ryosei 27:dd9f27fce7d1 948 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 949 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 950 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 951 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 952 adjAnable = true;
Ryosei 27:dd9f27fce7d1 953 break;
Ryosei 27:dd9f27fce7d1 954 case 'N':
Ryosei 27:dd9f27fce7d1 955 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei 27:dd9f27fce7d1 956 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei 27:dd9f27fce7d1 957 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei 27:dd9f27fce7d1 958 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei 27:dd9f27fce7d1 959 adjAnable = false;
Ryosei 27:dd9f27fce7d1 960 break;
Ryosei 27:dd9f27fce7d1 961 default:
Ryosei 27:dd9f27fce7d1 962 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 963 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 964 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 965 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 966 adjAnable = false;
Ryosei 27:dd9f27fce7d1 967 }
Ryosei 27:dd9f27fce7d1 968 } else {
Ryosei 27:dd9f27fce7d1 969 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 970 case 2:
Ryosei 27:dd9f27fce7d1 971 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 972 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 973 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 974 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 975 adjAnable = true;
Ryosei 27:dd9f27fce7d1 976 break;
Ryosei 27:dd9f27fce7d1 977 case 3:
Ryosei 27:dd9f27fce7d1 978 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 979 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 980 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 981 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 982 adjAnable = true;
Ryosei 27:dd9f27fce7d1 983 break;
Ryosei 27:dd9f27fce7d1 984 case 1:
Ryosei 27:dd9f27fce7d1 985 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 986 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 987 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 988 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 989 adjAnable = true;
Ryosei 27:dd9f27fce7d1 990 break;
Ryosei 27:dd9f27fce7d1 991 case 0:
Ryosei 27:dd9f27fce7d1 992 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 993 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 994 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 995 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 996 adjAnable = true;
Ryosei 27:dd9f27fce7d1 997 break;
Ryosei 27:dd9f27fce7d1 998 case -1:
Ryosei 27:dd9f27fce7d1 999 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1000 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 1001 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1002 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 1003 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1004 break;
Ryosei 27:dd9f27fce7d1 1005 case -3:
Ryosei 27:dd9f27fce7d1 1006 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1007 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 1008 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1009 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 1010 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1011 break;
Ryosei 27:dd9f27fce7d1 1012 case -2:
Ryosei 27:dd9f27fce7d1 1013 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1014 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 1015 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1016 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 1017 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1018 break;
Ryosei 27:dd9f27fce7d1 1019 case 'A':
Ryosei 27:dd9f27fce7d1 1020 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1021 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1022 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1023 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1024 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1025 break;
Ryosei 27:dd9f27fce7d1 1026 case 'N':
Ryosei 27:dd9f27fce7d1 1027 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei 27:dd9f27fce7d1 1028 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei 27:dd9f27fce7d1 1029 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei 27:dd9f27fce7d1 1030 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei 27:dd9f27fce7d1 1031 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1032 break;
Ryosei 27:dd9f27fce7d1 1033 default:
Ryosei 27:dd9f27fce7d1 1034 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1035 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1036 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1037 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1038 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1039 }
Ryosei 27:dd9f27fce7d1 1040 }
Ryosei 27:dd9f27fce7d1 1041
Ryosei 27:dd9f27fce7d1 1042 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1043 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1044 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 1045 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 1046 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1047 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1048 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 1049 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1050 }
Ryosei 27:dd9f27fce7d1 1051 } else if(lineFase == 7) { // ライン 修正
Ryosei 27:dd9f27fce7d1 1052 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
Ryosei 27:dd9f27fce7d1 1053 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1054 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1055 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1056 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1057 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1058 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1059 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1060 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1061 } else if(linePara[LINE_TOW_2] > 0) {
Ryosei 27:dd9f27fce7d1 1062 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 1063 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1064 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1065 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1066 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 1067 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1068 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1069 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1070 } else if(linePara[LINE_TOW_2] < 0) {
Ryosei 27:dd9f27fce7d1 1071 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1072 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1073 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 1074 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1075 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1076 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1077 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 1078 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1079 } else if(linePara[LINE_TOW_2] == 0) {
Ryosei 27:dd9f27fce7d1 1080 lineFase = 8;
Ryosei 27:dd9f27fce7d1 1081 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1082 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1083 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1084 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1085 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1086 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1087 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1088 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1089 } else {
Ryosei 27:dd9f27fce7d1 1090 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1091 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1092 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1093 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 1094 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1095 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1096 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1097 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1098 }
Ryosei 27:dd9f27fce7d1 1099 } else if(lineFase == 8) { // タオル1 解放
Ryosei 27:dd9f27fce7d1 1100 Air[0] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 1101 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1102 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1103 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1104 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1105 lineFase = 9;
Ryosei 27:dd9f27fce7d1 1106 } else if(lineFase == 9) { // 前 ライントレース
Ryosei 27:dd9f27fce7d1 1107 LedOut(2);
Ryosei 27:dd9f27fce7d1 1108 if(LimitSw::IsPressed(QF_SW)) {
Ryosei 27:dd9f27fce7d1 1109 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 1110 case -2:
Ryosei 27:dd9f27fce7d1 1111 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 1112 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1113 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 1114 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1115 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1116 break;
Ryosei 27:dd9f27fce7d1 1117 case -3:
Ryosei 27:dd9f27fce7d1 1118 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 1119 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1120 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 1121 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1122 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1123 break;
Ryosei 27:dd9f27fce7d1 1124 case -1:
Ryosei 27:dd9f27fce7d1 1125 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 1126 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1127 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 1128 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1129 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1130 break;
Ryosei 27:dd9f27fce7d1 1131 case 0:
Ryosei 27:dd9f27fce7d1 1132 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1133 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1134 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1135 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1136 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1137 break;
Ryosei 27:dd9f27fce7d1 1138 case 1:
Ryosei 27:dd9f27fce7d1 1139 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1140 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 1141 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1142 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 1143 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1144 break;
Ryosei 27:dd9f27fce7d1 1145 case 3:
Ryosei 27:dd9f27fce7d1 1146 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1147 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 1148 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1149 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 1150 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1151 break;
Ryosei 27:dd9f27fce7d1 1152 case 2:
Ryosei 27:dd9f27fce7d1 1153 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1154 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 1155 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1156 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 1157 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1158 break;
Ryosei 27:dd9f27fce7d1 1159 case 'A':
Ryosei 27:dd9f27fce7d1 1160 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 1161 lineCheck = true;
Ryosei 27:dd9f27fce7d1 1162 lineCount = 0;
Ryosei 27:dd9f27fce7d1 1163 countW++;
Ryosei 27:dd9f27fce7d1 1164 }
Ryosei 27:dd9f27fce7d1 1165 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1166 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1167 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1168 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1169 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1170 break;
Ryosei 27:dd9f27fce7d1 1171 case 'N':
Ryosei 27:dd9f27fce7d1 1172 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1173 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1174 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1175 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1176 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1177 break;
Ryosei 27:dd9f27fce7d1 1178 default:
Ryosei 27:dd9f27fce7d1 1179 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1180 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1181 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1182 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1183 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1184 }
M_souta 25:b3a9f34b201d 1185 } else {
Ryosei 27:dd9f27fce7d1 1186 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 1187 case 2:
Ryosei 27:dd9f27fce7d1 1188 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 1189 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1190 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 1191 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1192 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1193 break;
Ryosei 27:dd9f27fce7d1 1194 case 3:
Ryosei 27:dd9f27fce7d1 1195 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 1196 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1197 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 1198 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1199 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1200 break;
Ryosei 27:dd9f27fce7d1 1201 case 1:
Ryosei 27:dd9f27fce7d1 1202 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 1203 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1204 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 1205 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1206 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1207 break;
Ryosei 27:dd9f27fce7d1 1208 case 0:
Ryosei 27:dd9f27fce7d1 1209 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1210 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1211 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1212 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1213 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1214 break;
Ryosei 27:dd9f27fce7d1 1215 case -1:
Ryosei 27:dd9f27fce7d1 1216 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1217 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 1218 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1219 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 1220 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1221 break;
Ryosei 27:dd9f27fce7d1 1222 case -3:
Ryosei 27:dd9f27fce7d1 1223 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1224 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 1225 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1226 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 1227 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1228 break;
Ryosei 27:dd9f27fce7d1 1229 case -2:
Ryosei 27:dd9f27fce7d1 1230 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1231 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 1232 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1233 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 1234 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1235 break;
Ryosei 27:dd9f27fce7d1 1236 case 'A':
Ryosei 27:dd9f27fce7d1 1237 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 1238 lineCheck = true;
Ryosei 27:dd9f27fce7d1 1239 lineCount = 0;
Ryosei 27:dd9f27fce7d1 1240 countW++;
Ryosei 27:dd9f27fce7d1 1241 }
Ryosei 27:dd9f27fce7d1 1242 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1243 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1244 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1245 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1246 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1247 break;
Ryosei 27:dd9f27fce7d1 1248 case 'N':
Ryosei 27:dd9f27fce7d1 1249 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1250 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1251 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1252 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1253 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1254 break;
Ryosei 27:dd9f27fce7d1 1255 default:
Ryosei 27:dd9f27fce7d1 1256 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1257 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1258 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1259 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1260 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1261 }
M_souta 25:b3a9f34b201d 1262 }
M_souta 25:b3a9f34b201d 1263
M_souta 25:b3a9f34b201d 1264 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1265 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1266 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1267 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1268 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1269 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1270 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1271 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1272
M_souta 25:b3a9f34b201d 1273 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1274 lineCount++;
M_souta 25:b3a9f34b201d 1275 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1276 }
Ryosei 27:dd9f27fce7d1 1277 if(countW == 1) {
M_souta 26:4c0ce2f05688 1278 countW = 0;
Ryosei 27:dd9f27fce7d1 1279 lineFase = 10;
M_souta 25:b3a9f34b201d 1280 lineCount = 0;
M_souta 25:b3a9f34b201d 1281 lineCheck = false;
M_souta 25:b3a9f34b201d 1282 }
Ryosei 27:dd9f27fce7d1 1283 } else if(lineFase == 10) { // 前 低速
M_souta 25:b3a9f34b201d 1284 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1285 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1286 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1287 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1288 if(linePara[4] == 0) {
Ryosei 27:dd9f27fce7d1 1289 lineFase = 11;
M_souta 25:b3a9f34b201d 1290 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1291 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1292 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1293 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1294 }
M_souta 25:b3a9f34b201d 1295 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1296 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1297 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1298 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1299 } else if(lineFase == 11) {
Ryosei 27:dd9f27fce7d1 1300 switch(linePara[4]) { // 右 ライントレース
M_souta 25:b3a9f34b201d 1301 case -2:
M_souta 25:b3a9f34b201d 1302 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1303 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1304 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1305 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1306 adjAnable = true;
M_souta 22:7d93f79a3686 1307 break;
M_souta 22:7d93f79a3686 1308 case -3:
M_souta 25:b3a9f34b201d 1309 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1310 tirePWM[TIRE_BL] = -10;
M_souta 25:b3a9f34b201d 1311 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1312 tirePWM[TIRE_FR] = 10;
M_souta 25:b3a9f34b201d 1313 adjAnable = true;
M_souta 22:7d93f79a3686 1314 break;
M_souta 22:7d93f79a3686 1315 case -1:
M_souta 25:b3a9f34b201d 1316 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1317 tirePWM[TIRE_BL] = -20;
M_souta 25:b3a9f34b201d 1318 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1319 tirePWM[TIRE_FR] = 20;
M_souta 25:b3a9f34b201d 1320 adjAnable = true;
M_souta 22:7d93f79a3686 1321 break;
M_souta 22:7d93f79a3686 1322 case 0:
M_souta 25:b3a9f34b201d 1323 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1324 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1325 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1326 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1327 adjAnable = true;
M_souta 22:7d93f79a3686 1328 break;
M_souta 22:7d93f79a3686 1329 case 1:
M_souta 25:b3a9f34b201d 1330 tirePWM[TIRE_FL] = 20;
M_souta 25:b3a9f34b201d 1331 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1332 tirePWM[TIRE_BR] = -20;
M_souta 25:b3a9f34b201d 1333 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1334 adjAnable = true;
M_souta 22:7d93f79a3686 1335 break;
M_souta 22:7d93f79a3686 1336 case 3:
M_souta 25:b3a9f34b201d 1337 tirePWM[TIRE_FL] = 10;
M_souta 25:b3a9f34b201d 1338 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1339 tirePWM[TIRE_BR] = -10;
M_souta 25:b3a9f34b201d 1340 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1341 adjAnable = true;
M_souta 22:7d93f79a3686 1342 break;
M_souta 22:7d93f79a3686 1343 case 2:
M_souta 25:b3a9f34b201d 1344 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1345 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1346 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1347 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1348 adjAnable = true;
M_souta 22:7d93f79a3686 1349 break;
M_souta 22:7d93f79a3686 1350 case 'A':
M_souta 25:b3a9f34b201d 1351 if(lineCheck == false) {
M_souta 25:b3a9f34b201d 1352 lineCheck = true;
M_souta 25:b3a9f34b201d 1353 lineCount = 0;
M_souta 26:4c0ce2f05688 1354 countW++;
M_souta 25:b3a9f34b201d 1355 }
M_souta 25:b3a9f34b201d 1356 tirePWM[TIRE_FL] = 30;
M_souta 25:b3a9f34b201d 1357 tirePWM[TIRE_BL] = -30;
M_souta 25:b3a9f34b201d 1358 tirePWM[TIRE_BR] = -30;
M_souta 25:b3a9f34b201d 1359 tirePWM[TIRE_FR] = 30;
M_souta 25:b3a9f34b201d 1360 adjAnable = true;
M_souta 25:b3a9f34b201d 1361 break;
M_souta 25:b3a9f34b201d 1362 case 'N':
M_souta 25:b3a9f34b201d 1363 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1364 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1365 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1366 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1367 adjAnable = false;
M_souta 25:b3a9f34b201d 1368 break;
M_souta 25:b3a9f34b201d 1369 default:
M_souta 25:b3a9f34b201d 1370 tirePWM[TIRE_FL] = 0;
M_souta 25:b3a9f34b201d 1371 tirePWM[TIRE_BL] = 0;
M_souta 25:b3a9f34b201d 1372 tirePWM[TIRE_BR] = 0;
M_souta 25:b3a9f34b201d 1373 tirePWM[TIRE_FR] = 0;
M_souta 25:b3a9f34b201d 1374 adjAnable = false;
M_souta 25:b3a9f34b201d 1375 }
M_souta 25:b3a9f34b201d 1376
M_souta 25:b3a9f34b201d 1377 if(adjAnable){
M_souta 25:b3a9f34b201d 1378 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta 25:b3a9f34b201d 1379 } else {
M_souta 25:b3a9f34b201d 1380 adj = 0;
M_souta 25:b3a9f34b201d 1381 }
M_souta 25:b3a9f34b201d 1382
M_souta 25:b3a9f34b201d 1383 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1384 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1385 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1386 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1387 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta 25:b3a9f34b201d 1388 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta 25:b3a9f34b201d 1389 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta 25:b3a9f34b201d 1390 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1391
M_souta 25:b3a9f34b201d 1392 if(lineCheck == true) {
M_souta 25:b3a9f34b201d 1393 lineCount++;
M_souta 25:b3a9f34b201d 1394 if(lineCount > 20) lineCheck = false;
M_souta 25:b3a9f34b201d 1395 }
Ryosei 27:dd9f27fce7d1 1396 if(countW == 2) {
M_souta 26:4c0ce2f05688 1397 countW = 0;
Ryosei 27:dd9f27fce7d1 1398 lineFase = 12;
M_souta 25:b3a9f34b201d 1399 lineCount = 0;
M_souta 25:b3a9f34b201d 1400 lineCheck = false;
M_souta 25:b3a9f34b201d 1401 }
Ryosei 27:dd9f27fce7d1 1402 } else if(lineFase == 12) { // 右 低速
M_souta 25:b3a9f34b201d 1403 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1404 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1405 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1406 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 1407 if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 1408 lineFase = 13;
M_souta 25:b3a9f34b201d 1409 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1410 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1411 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1412 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1413 }
Ryosei 27:dd9f27fce7d1 1414 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1415 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 1416 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1417 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 1418 } else if (lineFase == 13) {
Ryosei 27:dd9f27fce7d1 1419 lineFase = 14;
M_souta 25:b3a9f34b201d 1420 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1421 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1422 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1423 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 1424 } else if(lineFase == 14) { // タオル2 竿検知
Ryosei 27:dd9f27fce7d1 1425 LedOut(4);
Ryosei 27:dd9f27fce7d1 1426 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
Ryosei 27:dd9f27fce7d1 1427 lineFase = 15;
M_souta 25:b3a9f34b201d 1428 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1429 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1430 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1431 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1432 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1433 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1434 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1435 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 1436 } else if(LimitSw::IsPressed(TOW_2L)) {
Ryosei 27:dd9f27fce7d1 1437 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1438 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 1439 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1440 motor[TIRE_BL].pwm = 20;
Ryosei 27:dd9f27fce7d1 1441 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 1442 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1443 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 1444 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 1445 } else if(LimitSw::IsPressed(TOW_2R)) {
M_souta 25:b3a9f34b201d 1446 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 1447 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 1448 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 1449 motor[TIRE_BL].pwm = 20;
Ryosei 27:dd9f27fce7d1 1450 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1451 motor[TIRE_BR].pwm = 20;
M_souta 25:b3a9f34b201d 1452 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1453 motor[TIRE_FR].pwm = 20;
M_souta 25:b3a9f34b201d 1454 } else {
Ryosei 27:dd9f27fce7d1 1455 if(LimitSw::IsPressed(QF_SW)) {
Ryosei 27:dd9f27fce7d1 1456 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 1457 case -2:
Ryosei 27:dd9f27fce7d1 1458 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 1459 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1460 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 1461 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1462 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1463 break;
Ryosei 27:dd9f27fce7d1 1464 case -3:
Ryosei 27:dd9f27fce7d1 1465 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 1466 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1467 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 1468 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1469 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1470 break;
Ryosei 27:dd9f27fce7d1 1471 case -1:
Ryosei 27:dd9f27fce7d1 1472 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 1473 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1474 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 1475 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1476 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1477 break;
Ryosei 27:dd9f27fce7d1 1478 case 0:
Ryosei 27:dd9f27fce7d1 1479 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1480 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1481 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1482 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1483 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1484 break;
Ryosei 27:dd9f27fce7d1 1485 case 1:
Ryosei 27:dd9f27fce7d1 1486 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1487 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 1488 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1489 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 1490 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1491 break;
Ryosei 27:dd9f27fce7d1 1492 case 3:
Ryosei 27:dd9f27fce7d1 1493 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1494 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 1495 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1496 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 1497 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1498 break;
Ryosei 27:dd9f27fce7d1 1499 case 2:
Ryosei 27:dd9f27fce7d1 1500 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1501 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 1502 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1503 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 1504 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1505 break;
Ryosei 27:dd9f27fce7d1 1506 case 'A':
Ryosei 27:dd9f27fce7d1 1507 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1508 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1509 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1510 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1511 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1512 break;
Ryosei 27:dd9f27fce7d1 1513 case 'N':
Ryosei 27:dd9f27fce7d1 1514 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei 27:dd9f27fce7d1 1515 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei 27:dd9f27fce7d1 1516 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei 27:dd9f27fce7d1 1517 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei 27:dd9f27fce7d1 1518 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1519 break;
Ryosei 27:dd9f27fce7d1 1520 default:
Ryosei 27:dd9f27fce7d1 1521 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1522 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1523 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1524 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1525 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1526 }
Ryosei 27:dd9f27fce7d1 1527 } else {
Ryosei 27:dd9f27fce7d1 1528 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 1529 case 2:
Ryosei 27:dd9f27fce7d1 1530 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 1531 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1532 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 1533 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1534 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1535 break;
Ryosei 27:dd9f27fce7d1 1536 case 3:
Ryosei 27:dd9f27fce7d1 1537 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 1538 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1539 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 1540 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1541 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1542 break;
Ryosei 27:dd9f27fce7d1 1543 case 1:
Ryosei 27:dd9f27fce7d1 1544 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 1545 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1546 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 1547 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1548 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1549 break;
Ryosei 27:dd9f27fce7d1 1550 case 0:
Ryosei 27:dd9f27fce7d1 1551 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1552 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1553 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1554 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1555 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1556 break;
Ryosei 27:dd9f27fce7d1 1557 case -1:
Ryosei 27:dd9f27fce7d1 1558 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1559 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 1560 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1561 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 1562 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1563 break;
Ryosei 27:dd9f27fce7d1 1564 case -3:
Ryosei 27:dd9f27fce7d1 1565 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1566 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 1567 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1568 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 1569 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1570 break;
Ryosei 27:dd9f27fce7d1 1571 case -2:
Ryosei 27:dd9f27fce7d1 1572 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1573 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 1574 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1575 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 1576 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1577 break;
Ryosei 27:dd9f27fce7d1 1578 case 'A':
Ryosei 27:dd9f27fce7d1 1579 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 1580 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 1581 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 1582 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 1583 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1584 break;
Ryosei 27:dd9f27fce7d1 1585 case 'N':
Ryosei 27:dd9f27fce7d1 1586 tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei 27:dd9f27fce7d1 1587 tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei 27:dd9f27fce7d1 1588 tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei 27:dd9f27fce7d1 1589 tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei 27:dd9f27fce7d1 1590 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1591 break;
Ryosei 27:dd9f27fce7d1 1592 default:
Ryosei 27:dd9f27fce7d1 1593 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1594 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1595 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1596 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1597 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1598 }
Ryosei 27:dd9f27fce7d1 1599 }
Ryosei 27:dd9f27fce7d1 1600
Ryosei 27:dd9f27fce7d1 1601 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1602 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1603 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 1604 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 1605 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1606 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 1607 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 1608 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta 25:b3a9f34b201d 1609 }
Ryosei 27:dd9f27fce7d1 1610 } else if(lineFase == 15 ){ // ライン 修正
M_souta 25:b3a9f34b201d 1611 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
M_souta 25:b3a9f34b201d 1612 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1613 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1614 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1615 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1616 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1617 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1618 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1619 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1620 } else if(linePara[LINE_TOW_1] > 0) {
M_souta 25:b3a9f34b201d 1621 motor[TIRE_FL].dir = BACK;
M_souta 25:b3a9f34b201d 1622 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1623 motor[TIRE_BL].dir = FOR;
M_souta 25:b3a9f34b201d 1624 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1625 motor[TIRE_BR].dir = FOR;
M_souta 25:b3a9f34b201d 1626 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1627 motor[TIRE_FR].dir = BACK;
M_souta 25:b3a9f34b201d 1628 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1629 } else if(linePara[LINE_TOW_1] < 0) {
M_souta 25:b3a9f34b201d 1630 motor[TIRE_FL].dir = FOR;
M_souta 25:b3a9f34b201d 1631 motor[TIRE_FL].pwm = 16;
M_souta 25:b3a9f34b201d 1632 motor[TIRE_BL].dir = BACK;
M_souta 25:b3a9f34b201d 1633 motor[TIRE_BL].pwm = 16;
M_souta 25:b3a9f34b201d 1634 motor[TIRE_BR].dir = BACK;
M_souta 25:b3a9f34b201d 1635 motor[TIRE_BR].pwm = 16;
M_souta 25:b3a9f34b201d 1636 motor[TIRE_FR].dir = FOR;
M_souta 25:b3a9f34b201d 1637 motor[TIRE_FR].pwm = 16;
M_souta 25:b3a9f34b201d 1638 } else if(linePara[LINE_TOW_1] == 0) {
M_souta 25:b3a9f34b201d 1639 lineFase = 16;
M_souta 25:b3a9f34b201d 1640 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1641 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1642 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1643 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1644 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1645 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1646 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1647 motor[TIRE_FR].pwm = 50;
M_souta 25:b3a9f34b201d 1648 } else {
M_souta 25:b3a9f34b201d 1649 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1650 motor[TIRE_FL].pwm = 50;
M_souta 25:b3a9f34b201d 1651 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1652 motor[TIRE_BL].pwm = 50;
M_souta 25:b3a9f34b201d 1653 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1654 motor[TIRE_BR].pwm = 50;
M_souta 25:b3a9f34b201d 1655 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1656 motor[TIRE_FR].pwm = 50;
M_souta 22:7d93f79a3686 1657 }
M_souta 25:b3a9f34b201d 1658 } else if(lineFase == 16) { // タオル2 解放
Ryosei 27:dd9f27fce7d1 1659 Air[1] = SOLENOID_ON;
M_souta 25:b3a9f34b201d 1660 motor[TIRE_FL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1661 motor[TIRE_BL].dir = BRAKE;
M_souta 25:b3a9f34b201d 1662 motor[TIRE_BR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1663 motor[TIRE_FR].dir = BRAKE;
M_souta 25:b3a9f34b201d 1664 lineFase = 17;
M_souta 25:b3a9f34b201d 1665 } else if(lineFase == 17) { // 前
Ryosei 27:dd9f27fce7d1 1666 if(LimitSw::IsPressed(LSW_UU)) {
Ryosei 27:dd9f27fce7d1 1667 motor[LIFT_U].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1668 motor[LIFT_U].pwm=100;
Ryosei 27:dd9f27fce7d1 1669 } else {
Ryosei 27:dd9f27fce7d1 1670 motor[LIFT_U].dir=BACK;
Ryosei 27:dd9f27fce7d1 1671 motor[LIFT_U].pwm=180;
Ryosei 27:dd9f27fce7d1 1672 }
Ryosei 27:dd9f27fce7d1 1673 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 1674 case 2:
Ryosei 27:dd9f27fce7d1 1675 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1676 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 1677 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1678 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 1679 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1680 break;
Ryosei 27:dd9f27fce7d1 1681 case 3:
Ryosei 27:dd9f27fce7d1 1682 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 1683 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 1684 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 1685 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 1686 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1687 break;
Ryosei 27:dd9f27fce7d1 1688 case 1:
Ryosei 27:dd9f27fce7d1 1689 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 1690 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 1691 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 1692 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 1693 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1694 break;
Ryosei 27:dd9f27fce7d1 1695 case 0:
Ryosei 27:dd9f27fce7d1 1696 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 1697 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 1698 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 1699 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 1700 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1701 break;
Ryosei 27:dd9f27fce7d1 1702 case -1:
Ryosei 27:dd9f27fce7d1 1703 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 1704 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 1705 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 1706 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 1707 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1708 break;
Ryosei 27:dd9f27fce7d1 1709 case -3:
Ryosei 27:dd9f27fce7d1 1710 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 1711 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 1712 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 1713 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 1714 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1715 break;
Ryosei 27:dd9f27fce7d1 1716 case -2:
Ryosei 27:dd9f27fce7d1 1717 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 1718 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1719 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 1720 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1721 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1722 break;
Ryosei 27:dd9f27fce7d1 1723 case 'A':
Ryosei 27:dd9f27fce7d1 1724 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 1725 lineCheck = true;
Ryosei 27:dd9f27fce7d1 1726 lineCount = 0;
Ryosei 27:dd9f27fce7d1 1727 countW++;
Ryosei 27:dd9f27fce7d1 1728 }
Ryosei 27:dd9f27fce7d1 1729 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 1730 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 1731 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 1732 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 1733 adjAnable = true;
Ryosei 27:dd9f27fce7d1 1734 break;
Ryosei 27:dd9f27fce7d1 1735 case 'N':
Ryosei 27:dd9f27fce7d1 1736 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1737 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1738 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1739 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1740 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1741 break;
Ryosei 27:dd9f27fce7d1 1742 default:
Ryosei 27:dd9f27fce7d1 1743 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 1744 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 1745 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 1746 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 1747 adjAnable = false;
Ryosei 27:dd9f27fce7d1 1748 }
Ryosei 27:dd9f27fce7d1 1749
Ryosei 27:dd9f27fce7d1 1750 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 1751 adj = 0;
Ryosei 27:dd9f27fce7d1 1752 } else {
Ryosei 27:dd9f27fce7d1 1753 adj = 0;
Ryosei 27:dd9f27fce7d1 1754 }
Ryosei 27:dd9f27fce7d1 1755
Ryosei 27:dd9f27fce7d1 1756 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1757 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 1758 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 1759 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 1760 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 1761 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 1762 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 1763 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 1764
Ryosei 27:dd9f27fce7d1 1765 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 1766 lineCount++;
Ryosei 27:dd9f27fce7d1 1767 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 1768 }
Ryosei 27:dd9f27fce7d1 1769 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 1770 countW = 0;
Ryosei 27:dd9f27fce7d1 1771 lineFase = 100;
Ryosei 27:dd9f27fce7d1 1772 lineCount = 0;
Ryosei 27:dd9f27fce7d1 1773 lineCheck = false;
Ryosei 27:dd9f27fce7d1 1774 }
Ryosei 27:dd9f27fce7d1 1775 } else if(lineFase==100) {
Ryosei 27:dd9f27fce7d1 1776 LedOut(6);
Ryosei 27:dd9f27fce7d1 1777 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1778 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 1779 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1780 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 1781 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1782 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1783 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1784 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1785 if(LimitSw::IsPressed(LSW_UU)) {
Ryosei 27:dd9f27fce7d1 1786 motor[LIFT_U].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1787 motor[LIFT_U].pwm=100;
Ryosei 27:dd9f27fce7d1 1788 } else {
Ryosei 27:dd9f27fce7d1 1789 motor[LIFT_U].dir=BACK;
Ryosei 27:dd9f27fce7d1 1790 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 1791 }
Ryosei 27:dd9f27fce7d1 1792 if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
Ryosei 27:dd9f27fce7d1 1793 lineFase=101;
Ryosei 27:dd9f27fce7d1 1794 }
Ryosei 27:dd9f27fce7d1 1795 } else if(lineFase==101) {
Ryosei 27:dd9f27fce7d1 1796 //位置調整
Ryosei 27:dd9f27fce7d1 1797 //(P制御)
Ryosei 27:dd9f27fce7d1 1798 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 1799 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 1800 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1801 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1802 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1803 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1804 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 1805 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 1806 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 1807 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 1808 lineFase=102;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 1809 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 1810 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 1811 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1812 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1813 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1814 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1815 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 1816 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1817 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 1818 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1819 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 1820 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1821 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1822 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1823 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1824 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 1825 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1826 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 1827 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1828 }
Ryosei 27:dd9f27fce7d1 1829 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 1830 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 1831 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1832 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1833 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1834 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1835 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1836 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 1837 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1838 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 1839 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 1840 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1841 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1842 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1843 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1844 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1845 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 1846 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1847 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 1848 }
Ryosei 27:dd9f27fce7d1 1849 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 1850 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 1851 if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 1852 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 1853 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1854 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1855 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1856 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1857 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1858 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1859 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1860 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1861 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 1862 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1863 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1864 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1865 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1866 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1867 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1868 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1869 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1870 }
Ryosei 27:dd9f27fce7d1 1871 } else { //傾きが大きくなくて離れているとき
Ryosei 27:dd9f27fce7d1 1872 if((Ult_right+Ult_left)<=25) { //近すぎるとき
Ryosei 27:dd9f27fce7d1 1873 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1874 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1875 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1876 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1877 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 1878 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 1879 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 1880 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 1881 } else if((Ult_right+Ult_left)>=35) { //離れているとき
Ryosei 27:dd9f27fce7d1 1882 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1883 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1884 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1885 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1886 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 1887 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 1888 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 1889 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 1890 }
Ryosei 27:dd9f27fce7d1 1891 }
Ryosei 27:dd9f27fce7d1 1892
Ryosei 27:dd9f27fce7d1 1893 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 1894 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 1895 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1896 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1897 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1898 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1899 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1900 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1901 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1902 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1903 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 1904 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1905 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1906 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1907 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1908 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 1909 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 1910 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 1911 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 1912 }
Ryosei 27:dd9f27fce7d1 1913 }
Ryosei 27:dd9f27fce7d1 1914 }
Ryosei 27:dd9f27fce7d1 1915
Ryosei 27:dd9f27fce7d1 1916 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 1917 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1918 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1919 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1920 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 1921 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 1922 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 1923 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 1924 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 1925 }
Ryosei 27:dd9f27fce7d1 1926
Ryosei 27:dd9f27fce7d1 1927 } else if(lineFase==102) {
Ryosei 27:dd9f27fce7d1 1928 Air[CLOTHESPIN]=SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 1929 LedOut(1);
Ryosei 27:dd9f27fce7d1 1930 //リミットスイッチに当てる
Ryosei 27:dd9f27fce7d1 1931 static int count2=0;
Ryosei 27:dd9f27fce7d1 1932 if(count2==0) {
Ryosei 27:dd9f27fce7d1 1933 if(LimitSw::IsPressed(RIGHTlim)) {
Ryosei 27:dd9f27fce7d1 1934 count2=1;
Ryosei 27:dd9f27fce7d1 1935 } else if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 1936 if( ((Ult_left+Ult_right)<=25)||((Ult_left+Ult_right)>=35) ) {
Ryosei 27:dd9f27fce7d1 1937 if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
Ryosei 27:dd9f27fce7d1 1938 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 1939 //右
Ryosei 27:dd9f27fce7d1 1940 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1941 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1942 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1943 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1944
Ryosei 27:dd9f27fce7d1 1945 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 1946 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 1947
Ryosei 27:dd9f27fce7d1 1948 motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1949 motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1950 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 1951 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1952 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1953 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1954 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1955
Ryosei 27:dd9f27fce7d1 1956 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 1957 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 1958 motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1959 motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1960 }
Ryosei 27:dd9f27fce7d1 1961 } else if((Ult_left+Ult_right)<=25) {
Ryosei 27:dd9f27fce7d1 1962 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1963 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1964 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1965 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1966
Ryosei 27:dd9f27fce7d1 1967 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 1968 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 1969 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 1970 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 1971 } else if((Ult_left+Ult_right)>=35) {
Ryosei 27:dd9f27fce7d1 1972 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1973 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1974 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1975 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1976
Ryosei 27:dd9f27fce7d1 1977 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 1978 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 1979 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 1980 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 1981 }
Ryosei 27:dd9f27fce7d1 1982 } else {
Ryosei 27:dd9f27fce7d1 1983 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 1984 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1985 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1986 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1987 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1988
Ryosei 27:dd9f27fce7d1 1989 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 1990 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 1991
Ryosei 27:dd9f27fce7d1 1992 motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1993 motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 1994 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 1995 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 1996 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 1997 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 1998 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 1999
Ryosei 27:dd9f27fce7d1 2000 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 2001 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 2002 motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 2003 motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 2004 }
Ryosei 27:dd9f27fce7d1 2005 }
Ryosei 27:dd9f27fce7d1 2006
Ryosei 27:dd9f27fce7d1 2007 } else {
Ryosei 27:dd9f27fce7d1 2008 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2009 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2010 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2011 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2012 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 2013 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 2014 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 2015 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 2016
Ryosei 27:dd9f27fce7d1 2017 }
Ryosei 27:dd9f27fce7d1 2018 } else if(count2==1) {
Ryosei 27:dd9f27fce7d1 2019 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2020 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2021 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2022 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2023 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 2024 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 2025 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 2026 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 2027 lineFase=103;
Ryosei 27:dd9f27fce7d1 2028 }
Ryosei 27:dd9f27fce7d1 2029 } else if(lineFase==103) {
Ryosei 27:dd9f27fce7d1 2030 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 2031 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 2032 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2033 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2034 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2035 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2036 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 2037 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 2038 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 2039 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 2040 lineFase=104;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 2041 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 2042 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 2043 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2044 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2045 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2046 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2047 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 2048 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 2049 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 2050 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 2051 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 2052 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2053 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2054 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2055 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2056 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 2057 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 2058 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 2059 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 2060 }
Ryosei 27:dd9f27fce7d1 2061 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 2062 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 2063 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2064 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2065 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2066 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2067 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 2068 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 2069 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 2070 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 2071 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 2072 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2073 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2074 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2075 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2076 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 2077 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 2078 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 2079 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 2080 }
Ryosei 27:dd9f27fce7d1 2081 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 2082 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 2083 if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 2084 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 2085 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2086 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2087 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2088 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2089 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 2090 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 2091 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 2092 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 2093 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 2094 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2095 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2096 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2097 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2098 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 2099 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 2100 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 2101 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 2102 }
Ryosei 27:dd9f27fce7d1 2103 }
Ryosei 27:dd9f27fce7d1 2104 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 2105 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 2106 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2107 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2108 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2109 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2110 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 2111 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 2112 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 2113 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 2114 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 2115 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2116 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2117 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2118 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2119 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 2120 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 2121 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 2122 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 2123 }
Ryosei 27:dd9f27fce7d1 2124 }
Ryosei 27:dd9f27fce7d1 2125 }
Ryosei 27:dd9f27fce7d1 2126
Ryosei 27:dd9f27fce7d1 2127 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 2128 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2129 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2130 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2131 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2132 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 2133 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 2134 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 2135 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 2136 }
Ryosei 27:dd9f27fce7d1 2137 } else if(lineFase==104) {
Ryosei 27:dd9f27fce7d1 2138 static int count3=0;
Ryosei 27:dd9f27fce7d1 2139 static int loop=0;
Ryosei 27:dd9f27fce7d1 2140 pc.printf("%d\r\n",loop);
Ryosei 27:dd9f27fce7d1 2141 Air[CLOTHESPIN]=1;
Ryosei 27:dd9f27fce7d1 2142 if(count3==0) {
Ryosei 27:dd9f27fce7d1 2143 loop++;
Ryosei 27:dd9f27fce7d1 2144 if(loop==60) {
Ryosei 27:dd9f27fce7d1 2145 count3=1;
Ryosei 27:dd9f27fce7d1 2146 }
Ryosei 27:dd9f27fce7d1 2147 } else if(count3==1) {
Ryosei 27:dd9f27fce7d1 2148 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 2149 lineFase=105;
Ryosei 27:dd9f27fce7d1 2150
Ryosei 27:dd9f27fce7d1 2151 }
Ryosei 27:dd9f27fce7d1 2152 } else if(lineFase==105) {
Ryosei 27:dd9f27fce7d1 2153 LedOut(6);
Ryosei 27:dd9f27fce7d1 2154 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 2155 if(!(LimitSw::IsPressed(LEFTlim))) {
Ryosei 27:dd9f27fce7d1 2156
Ryosei 27:dd9f27fce7d1 2157 //超音波
Ryosei 27:dd9f27fce7d1 2158 if(Ult_right>0) {
Ryosei 27:dd9f27fce7d1 2159 if(Ult_right<15) {
Ryosei 27:dd9f27fce7d1 2160 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2161 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2162 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2163 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2164 motor[TIRE_FL].pwm=25+(15-Ult_left);
Ryosei 27:dd9f27fce7d1 2165 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 2166 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 2167 motor[TIRE_BR].pwm=25+(15-Ult_left);
Ryosei 27:dd9f27fce7d1 2168 } else if(Ult_right>=15) {
Ryosei 27:dd9f27fce7d1 2169 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2170 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2171 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2172 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2173 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 2174 motor[TIRE_FR].pwm=25+(15-Ult_left);
Ryosei 27:dd9f27fce7d1 2175 motor[TIRE_BL].pwm=25+(15-Ult_left);
Ryosei 27:dd9f27fce7d1 2176 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 2177 }
Ryosei 27:dd9f27fce7d1 2178 } else {
Ryosei 27:dd9f27fce7d1 2179 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2180 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 2181 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 2182 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2183 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 2184 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 2185 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 2186 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 2187 }
Ryosei 27:dd9f27fce7d1 2188
Ryosei 27:dd9f27fce7d1 2189
Ryosei 27:dd9f27fce7d1 2190 } else {
Ryosei 27:dd9f27fce7d1 2191 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2192 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2193 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2194 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 2195 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 2196 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 2197 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 2198 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 2199 lineFase=106;
Ryosei 27:dd9f27fce7d1 2200 }
Ryosei 27:dd9f27fce7d1 2201
Ryosei 27:dd9f27fce7d1 2202 } else if(lineFase==106) {
Ryosei 27:dd9f27fce7d1 2203 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2204 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2205 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 2206 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 2207 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 2208 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 2209 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 2210 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 2211 if(linePara[4]!='N') {
Ryosei 27:dd9f27fce7d1 2212 lineFase=19;
Ryosei 27:dd9f27fce7d1 2213 }
Ryosei 27:dd9f27fce7d1 2214 } else if(lineFase == 18) { // 前 低速
Ryosei 27:dd9f27fce7d1 2215 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2216 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2217 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2218 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2219 if(linePara[4] != 'N') {
Ryosei 27:dd9f27fce7d1 2220 lineFase = 19;
Ryosei 27:dd9f27fce7d1 2221 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2222 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2223 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2224 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2225 }
Ryosei 27:dd9f27fce7d1 2226 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2227 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2228 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2229 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2230 } else if(lineFase == 19) { // 左
Ryosei 27:dd9f27fce7d1 2231 if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei 27:dd9f27fce7d1 2232 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2233 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2234 } else {
Ryosei 27:dd9f27fce7d1 2235 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2236 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2237 }
Ryosei 27:dd9f27fce7d1 2238 switch(linePara[3]) {
Ryosei 27:dd9f27fce7d1 2239 case -2:
Ryosei 27:dd9f27fce7d1 2240 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2241 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2242 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2243 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2244 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2245 break;
Ryosei 27:dd9f27fce7d1 2246 case -3:
Ryosei 27:dd9f27fce7d1 2247 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2248 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 2249 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2250 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 2251 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2252 break;
Ryosei 27:dd9f27fce7d1 2253 case -1:
Ryosei 27:dd9f27fce7d1 2254 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2255 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2256 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2257 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2258 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2259 break;
Ryosei 27:dd9f27fce7d1 2260 case 0:
Ryosei 27:dd9f27fce7d1 2261 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2262 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2263 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2264 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2265 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2266 break;
Ryosei 27:dd9f27fce7d1 2267 case 1:
Ryosei 27:dd9f27fce7d1 2268 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2269 tirePWM[TIRE_BL] = 40;
Ryosei 27:dd9f27fce7d1 2270 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2271 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2272 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2273 break;
Ryosei 27:dd9f27fce7d1 2274 case 3:
Ryosei 27:dd9f27fce7d1 2275 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2276 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2277 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2278 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2279 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2280 break;
Ryosei 27:dd9f27fce7d1 2281 case 2:
Ryosei 27:dd9f27fce7d1 2282 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2283 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2284 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2285 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2286 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2287 break;
Ryosei 27:dd9f27fce7d1 2288 case 'A':
Ryosei 27:dd9f27fce7d1 2289 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2290 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2291 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2292 countW++;
Ryosei 27:dd9f27fce7d1 2293 }
Ryosei 27:dd9f27fce7d1 2294 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2295 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2296 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2297 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2298 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2299 break;
Ryosei 27:dd9f27fce7d1 2300 case 'N':
Ryosei 27:dd9f27fce7d1 2301 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2302 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2303 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2304 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2305 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2306 break;
Ryosei 27:dd9f27fce7d1 2307 default:
Ryosei 27:dd9f27fce7d1 2308 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2309 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2310 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2311 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2312 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2313 }
Ryosei 27:dd9f27fce7d1 2314
Ryosei 27:dd9f27fce7d1 2315 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2316 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei 27:dd9f27fce7d1 2317 } else {
Ryosei 27:dd9f27fce7d1 2318 adj = 0;
Ryosei 27:dd9f27fce7d1 2319 }
Ryosei 27:dd9f27fce7d1 2320
Ryosei 27:dd9f27fce7d1 2321 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2322 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2323 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2324 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2325 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2326 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2327 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2328 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2329
Ryosei 27:dd9f27fce7d1 2330 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2331 lineCount++;
Ryosei 27:dd9f27fce7d1 2332 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2333 }
Ryosei 27:dd9f27fce7d1 2334
Ryosei 27:dd9f27fce7d1 2335 /*
Ryosei 27:dd9f27fce7d1 2336 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
Ryosei 27:dd9f27fce7d1 2337 targetCount = 3;
Ryosei 27:dd9f27fce7d1 2338 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
Ryosei 27:dd9f27fce7d1 2339 targetCount = 3;
Ryosei 27:dd9f27fce7d1 2340 } else {
Ryosei 27:dd9f27fce7d1 2341 targetCount = 2;
Ryosei 27:dd9f27fce7d1 2342 }
Ryosei 27:dd9f27fce7d1 2343 */
Ryosei 27:dd9f27fce7d1 2344
Ryosei 27:dd9f27fce7d1 2345 targetCount = 3;
Ryosei 27:dd9f27fce7d1 2346
Ryosei 27:dd9f27fce7d1 2347 if(countW == targetCount) {
Ryosei 27:dd9f27fce7d1 2348 countW = 0;
Ryosei 27:dd9f27fce7d1 2349 lineFase = 20;
Ryosei 27:dd9f27fce7d1 2350 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2351 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2352 }
Ryosei 27:dd9f27fce7d1 2353 } else if(lineFase == 20) { // 左 低速
Ryosei 27:dd9f27fce7d1 2354 if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei 27:dd9f27fce7d1 2355 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2356 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2357 } else {
Ryosei 27:dd9f27fce7d1 2358 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2359 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2360 }
Ryosei 27:dd9f27fce7d1 2361 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2362 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2363 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2364 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2365 if (linePara[2] == 'N') {
Ryosei 27:dd9f27fce7d1 2366 //(!!LimitSw::Ispressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 2367 //ineFase = 20;
Ryosei 27:dd9f27fce7d1 2368 //else if(LimitSw::IsPressed(TOWEL2_SW) {
Ryosei 27:dd9f27fce7d1 2369 //ineFase = 14;
Ryosei 27:dd9f27fce7d1 2370 //else {
Ryosei 27:dd9f27fce7d1 2371 lineFase = 21;
Ryosei 27:dd9f27fce7d1 2372 //
Ryosei 27:dd9f27fce7d1 2373 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2374 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2375 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2376 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2377 }
Ryosei 27:dd9f27fce7d1 2378 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2379 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2380 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2381 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2382 } else if(lineFase == 21) {
Ryosei 27:dd9f27fce7d1 2383 if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei 27:dd9f27fce7d1 2384 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2385 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2386 } else {
Ryosei 27:dd9f27fce7d1 2387 motor[LIFT_U].dir=FOR;
Ryosei 27:dd9f27fce7d1 2388 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 2389 }
Ryosei 27:dd9f27fce7d1 2390 switch(linePara[2]) {
Ryosei 27:dd9f27fce7d1 2391 case -2:
Ryosei 27:dd9f27fce7d1 2392 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2393 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 2394 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2395 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 2396 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2397 break;
Ryosei 27:dd9f27fce7d1 2398 case -3:
Ryosei 27:dd9f27fce7d1 2399 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2400 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 2401 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2402 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 2403 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2404 break;
Ryosei 27:dd9f27fce7d1 2405 case -1:
Ryosei 27:dd9f27fce7d1 2406 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2407 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 2408 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2409 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 2410 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2411 break;
Ryosei 27:dd9f27fce7d1 2412 case 0:
Ryosei 27:dd9f27fce7d1 2413 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2414 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 2415 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2416 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 2417 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2418 break;
Ryosei 27:dd9f27fce7d1 2419 case 1:
Ryosei 27:dd9f27fce7d1 2420 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2421 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2422 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2423 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2424 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2425 break;
Ryosei 27:dd9f27fce7d1 2426 case 3:
Ryosei 27:dd9f27fce7d1 2427 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2428 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 2429 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2430 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 2431 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2432 break;
Ryosei 27:dd9f27fce7d1 2433 case 2:
Ryosei 27:dd9f27fce7d1 2434 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2435 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2436 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2437 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2438 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2439 break;
Ryosei 27:dd9f27fce7d1 2440 case 'A':
Ryosei 27:dd9f27fce7d1 2441 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2442 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2443 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2444 countW++;
Ryosei 27:dd9f27fce7d1 2445 }
Ryosei 27:dd9f27fce7d1 2446 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2447 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 2448 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2449 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 2450 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2451 break;
Ryosei 27:dd9f27fce7d1 2452 case 'N':
Ryosei 27:dd9f27fce7d1 2453 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2454 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2455 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2456 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2457 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2458 break;
Ryosei 27:dd9f27fce7d1 2459 default:
Ryosei 27:dd9f27fce7d1 2460 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2461 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2462 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2463 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2464 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2465 }
Ryosei 27:dd9f27fce7d1 2466
Ryosei 27:dd9f27fce7d1 2467 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2468 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
Ryosei 27:dd9f27fce7d1 2469 } else {
Ryosei 27:dd9f27fce7d1 2470 adj = 0;
Ryosei 27:dd9f27fce7d1 2471 }
Ryosei 27:dd9f27fce7d1 2472
Ryosei 27:dd9f27fce7d1 2473 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 2474 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2475 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2476 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2477 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 2478 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2479 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2480 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2481
Ryosei 27:dd9f27fce7d1 2482 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2483 lineCount++;
Ryosei 27:dd9f27fce7d1 2484 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2485 }
Ryosei 27:dd9f27fce7d1 2486 if(countW == 3) {
Ryosei 27:dd9f27fce7d1 2487 countW = 0;
Ryosei 27:dd9f27fce7d1 2488 lineFase = 22;
Ryosei 27:dd9f27fce7d1 2489 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2490 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2491 }
Ryosei 27:dd9f27fce7d1 2492 } else if(lineFase == 22) {
Ryosei 27:dd9f27fce7d1 2493 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2494 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2495 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2496 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2497 if (linePara[2] == 'N') {
Ryosei 27:dd9f27fce7d1 2498 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2499 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2500 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2501 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2502 }
Ryosei 27:dd9f27fce7d1 2503 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2504 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2505 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2506 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2507 } else {
Ryosei 27:dd9f27fce7d1 2508 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2509 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2510 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2511 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2512 }
Ryosei 27:dd9f27fce7d1 2513
kishibekairohan 13:b6e02d6261d7 2514 }
kishibekairohan 13:b6e02d6261d7 2515 #endif
kishibekairohan 13:b6e02d6261d7 2516
kishibekairohan 13:b6e02d6261d7 2517 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 2518 static void Process5()
Ryosei 27:dd9f27fce7d1 2519 {
Ryosei 27:dd9f27fce7d1 2520
Ryosei 27:dd9f27fce7d1 2521 /* ************************************** //
Ryosei 27:dd9f27fce7d1 2522
Ryosei 27:dd9f27fce7d1 2523 青ゾーン 青ゾーン 青ゾーン
Ryosei 27:dd9f27fce7d1 2524
Ryosei 27:dd9f27fce7d1 2525 // ************************************** */
Ryosei 27:dd9f27fce7d1 2526
Ryosei 27:dd9f27fce7d1 2527 for(int i = 0; i < 8; i++) {
Ryosei 27:dd9f27fce7d1 2528 linePara[i] = lineCast(LineHub::GetPara(i));
Ryosei 27:dd9f27fce7d1 2529 }
Ryosei 27:dd9f27fce7d1 2530
Ryosei 27:dd9f27fce7d1 2531 if(lineFase == 0) {
Ryosei 28:479631c2de29 2532 LedOut(1);
Ryosei 27:dd9f27fce7d1 2533 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2534 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2535 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2536 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2537 if(linePara[0] != 'N' && linePara[0] != 'A') {
Ryosei 27:dd9f27fce7d1 2538 lineFase = 1;
Ryosei 27:dd9f27fce7d1 2539 }
Ryosei 27:dd9f27fce7d1 2540 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2541 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2542 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2543 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2544 } else if(lineFase == 1) { // 前 ライントレース
Ryosei 27:dd9f27fce7d1 2545 switch(linePara[0]) {
Ryosei 27:dd9f27fce7d1 2546 case -2:
Ryosei 27:dd9f27fce7d1 2547 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2548 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2549 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2550 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2551 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2552 break;
Ryosei 27:dd9f27fce7d1 2553 case -3:
Ryosei 27:dd9f27fce7d1 2554 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 2555 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2556 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 2557 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2558 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2559 break;
Ryosei 27:dd9f27fce7d1 2560 case -1:
Ryosei 27:dd9f27fce7d1 2561 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2562 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2563 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2564 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2565 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2566 break;
Ryosei 27:dd9f27fce7d1 2567 case 0:
Ryosei 27:dd9f27fce7d1 2568 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2569 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2570 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2571 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2572 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2573 break;
Ryosei 27:dd9f27fce7d1 2574 case 1:
Ryosei 27:dd9f27fce7d1 2575 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2576 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2577 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2578 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2579 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2580 break;
Ryosei 27:dd9f27fce7d1 2581 case 3:
Ryosei 27:dd9f27fce7d1 2582 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2583 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 2584 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2585 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 2586 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2587 break;
Ryosei 27:dd9f27fce7d1 2588 case 2:
Ryosei 27:dd9f27fce7d1 2589 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2590 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2591 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2592 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2593 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2594 break;
Ryosei 27:dd9f27fce7d1 2595 case 'A':
Ryosei 27:dd9f27fce7d1 2596 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2597 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2598 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2599 countW++;
Ryosei 27:dd9f27fce7d1 2600 }
Ryosei 27:dd9f27fce7d1 2601 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2602 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2603 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2604 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2605 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2606 break;
Ryosei 27:dd9f27fce7d1 2607 case 'N':
Ryosei 27:dd9f27fce7d1 2608 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2609 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2610 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2611 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2612 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2613 break;
Ryosei 27:dd9f27fce7d1 2614 default:
Ryosei 27:dd9f27fce7d1 2615 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2616 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2617 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2618 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2619 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2620 }
Ryosei 27:dd9f27fce7d1 2621
Ryosei 27:dd9f27fce7d1 2622 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2623 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
Ryosei 27:dd9f27fce7d1 2624 } else {
Ryosei 27:dd9f27fce7d1 2625 adj = 0;
Ryosei 27:dd9f27fce7d1 2626 }
Ryosei 27:dd9f27fce7d1 2627
Ryosei 27:dd9f27fce7d1 2628 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2629 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 2630 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2631 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2632 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2633 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 2634 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2635 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2636
Ryosei 27:dd9f27fce7d1 2637 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2638 lineCount++;
Ryosei 27:dd9f27fce7d1 2639 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2640 }
Ryosei 27:dd9f27fce7d1 2641 if(countW == 3) {
Ryosei 27:dd9f27fce7d1 2642 countW = 0;
Ryosei 27:dd9f27fce7d1 2643 lineFase = 2;
Ryosei 27:dd9f27fce7d1 2644 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2645 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2646 }
Ryosei 27:dd9f27fce7d1 2647 } else if(lineFase == 2) { // 前 低速
Ryosei 27:dd9f27fce7d1 2648 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2649 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2650 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2651 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2652 if(linePara[3] == 0) {
Ryosei 27:dd9f27fce7d1 2653 lineFase = 3;
Ryosei 27:dd9f27fce7d1 2654 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2655 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2656 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2657 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2658 }
Ryosei 27:dd9f27fce7d1 2659 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2660 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2661 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2662 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2663 } else if(lineFase == 3) { // 左 ライントレース
Ryosei 27:dd9f27fce7d1 2664 switch(linePara[3]) {
Ryosei 27:dd9f27fce7d1 2665 case -2:
Ryosei 27:dd9f27fce7d1 2666 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2667 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2668 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2669 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2670 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2671 break;
Ryosei 27:dd9f27fce7d1 2672 case -3:
Ryosei 27:dd9f27fce7d1 2673 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2674 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 2675 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2676 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 2677 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2678 break;
Ryosei 27:dd9f27fce7d1 2679 case -1:
Ryosei 27:dd9f27fce7d1 2680 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2681 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2682 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2683 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2684 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2685 break;
Ryosei 27:dd9f27fce7d1 2686 case 0:
Ryosei 27:dd9f27fce7d1 2687 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2688 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2689 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2690 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2691 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2692 break;
Ryosei 27:dd9f27fce7d1 2693 case 1:
Ryosei 27:dd9f27fce7d1 2694 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2695 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2696 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2697 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2698 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2699 break;
Ryosei 27:dd9f27fce7d1 2700 case 3:
Ryosei 27:dd9f27fce7d1 2701 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2702 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2703 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2704 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2705 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2706 break;
Ryosei 27:dd9f27fce7d1 2707 case 2:
Ryosei 27:dd9f27fce7d1 2708 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2709 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2710 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2711 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2712 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2713 break;
Ryosei 27:dd9f27fce7d1 2714 case 'A':
Ryosei 27:dd9f27fce7d1 2715 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2716 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2717 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2718 countW++;
Ryosei 27:dd9f27fce7d1 2719 }
Ryosei 27:dd9f27fce7d1 2720 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2721 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2722 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2723 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2724 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2725 break;
Ryosei 27:dd9f27fce7d1 2726 case 'N':
Ryosei 27:dd9f27fce7d1 2727 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2728 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2729 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2730 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2731 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2732 break;
Ryosei 27:dd9f27fce7d1 2733 default:
Ryosei 27:dd9f27fce7d1 2734 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2735 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2736 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2737 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2738 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2739 }
Ryosei 27:dd9f27fce7d1 2740
Ryosei 27:dd9f27fce7d1 2741 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2742 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei 27:dd9f27fce7d1 2743 } else {
Ryosei 27:dd9f27fce7d1 2744 adj = 0;
Ryosei 27:dd9f27fce7d1 2745 }
Ryosei 27:dd9f27fce7d1 2746
Ryosei 27:dd9f27fce7d1 2747 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2748 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2749 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2750 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2751 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2752 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2753 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2754 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2755
Ryosei 27:dd9f27fce7d1 2756 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2757 lineCount++;
Ryosei 27:dd9f27fce7d1 2758 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2759 }
Ryosei 27:dd9f27fce7d1 2760 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 2761 countW = 0;
Ryosei 27:dd9f27fce7d1 2762 lineFase = 4;
Ryosei 27:dd9f27fce7d1 2763 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2764 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2765 }
Ryosei 27:dd9f27fce7d1 2766 } else if(lineFase == 4) { // 右 低速
Ryosei 27:dd9f27fce7d1 2767 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2768 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2769 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2770 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2771 if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 2772 if(!LimitSw::IsPressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 2773 lineFase=100;
Ryosei 27:dd9f27fce7d1 2774 } else {
Ryosei 27:dd9f27fce7d1 2775 lineFase = 6;
Ryosei 27:dd9f27fce7d1 2776 }
Ryosei 27:dd9f27fce7d1 2777 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2778 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2779 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2780 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2781 }
Ryosei 27:dd9f27fce7d1 2782 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2783 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2784 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2785 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2786 } else if (lineFase == 5) {
Ryosei 27:dd9f27fce7d1 2787 lineFase = 6;
Ryosei 27:dd9f27fce7d1 2788 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2789 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2790 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2791 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2792 } else if(lineFase == 6) { // タオル1 竿検知
Ryosei 28:479631c2de29 2793 LedOut(4);
Ryosei 27:dd9f27fce7d1 2794 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 2795 lineFase = 7;
Ryosei 27:dd9f27fce7d1 2796 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2797 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2798 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2799 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2800 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2801 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2802 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2803 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2804 } else if(LimitSw::IsPressed(TOW_1L)) {
Ryosei 27:dd9f27fce7d1 2805 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2806 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 2807 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2808 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2809 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2810 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2811 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2812 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 2813 } else if(LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 2814 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2815 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 2816 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2817 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2818 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2819 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2820 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2821 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 2822 } else {
Ryosei 27:dd9f27fce7d1 2823 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 2824 case -2:
Ryosei 27:dd9f27fce7d1 2825 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2826 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2827 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2828 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2829 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2830 break;
Ryosei 27:dd9f27fce7d1 2831 case -3:
Ryosei 27:dd9f27fce7d1 2832 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 2833 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2834 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 2835 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2836 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2837 break;
Ryosei 27:dd9f27fce7d1 2838 case -1:
Ryosei 27:dd9f27fce7d1 2839 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 2840 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2841 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 2842 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2843 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2844 break;
Ryosei 27:dd9f27fce7d1 2845 case 0:
Ryosei 27:dd9f27fce7d1 2846 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2847 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2848 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2849 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2850 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2851 break;
Ryosei 27:dd9f27fce7d1 2852 case 1:
Ryosei 27:dd9f27fce7d1 2853 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2854 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 2855 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2856 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 2857 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2858 break;
Ryosei 27:dd9f27fce7d1 2859 case 3:
Ryosei 27:dd9f27fce7d1 2860 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2861 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 2862 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2863 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 2864 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2865 break;
Ryosei 27:dd9f27fce7d1 2866 case 2:
Ryosei 27:dd9f27fce7d1 2867 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2868 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 2869 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2870 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 2871 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2872 break;
Ryosei 27:dd9f27fce7d1 2873 case 'A':
Ryosei 27:dd9f27fce7d1 2874 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2875 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2876 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2877 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2878 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2879 break;
Ryosei 27:dd9f27fce7d1 2880 case 'N':
Ryosei 27:dd9f27fce7d1 2881 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2882 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2883 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2884 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2885 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2886 break;
Ryosei 27:dd9f27fce7d1 2887 default:
Ryosei 27:dd9f27fce7d1 2888 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2889 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2890 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2891 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2892 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2893 }
Ryosei 27:dd9f27fce7d1 2894
Ryosei 27:dd9f27fce7d1 2895 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2896 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2897 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2898 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2899 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2900 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2901 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2902 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2903 }
Ryosei 27:dd9f27fce7d1 2904 } else if(lineFase == 7) { // ライン 修正
Ryosei 27:dd9f27fce7d1 2905 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
Ryosei 27:dd9f27fce7d1 2906 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2907 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2908 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2909 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2910 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2911 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2912 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2913 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2914 } else if(linePara[LINE_TOW_1] > 0) {
Ryosei 27:dd9f27fce7d1 2915 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2916 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2917 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2918 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2919 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2920 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2921 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2922 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2923 } else if(linePara[LINE_TOW_1] < 0) {
Ryosei 27:dd9f27fce7d1 2924 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2925 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2926 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2927 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2928 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2929 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2930 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2931 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2932 } else if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 2933 //tow_stop.reset();
Ryosei 27:dd9f27fce7d1 2934 //tow_stop.start();
Ryosei 27:dd9f27fce7d1 2935 lineFase = 8;
Ryosei 27:dd9f27fce7d1 2936 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2937 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2938 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2939 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2940 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2941 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2942 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2943 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2944 } else {
Ryosei 27:dd9f27fce7d1 2945 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2946 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2947 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2948 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2949 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2950 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2951 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2952 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2953 }
Ryosei 27:dd9f27fce7d1 2954 } else if(lineFase == 8) { // タオル1 解放
Ryosei 27:dd9f27fce7d1 2955 Air[TOWEL1] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 2956 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2957 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2958 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2959 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2960 lineFase = 9;
Ryosei 27:dd9f27fce7d1 2961 } else if(lineFase == 9) { // 前
Ryosei 28:479631c2de29 2962 LedOut(2);
Ryosei 27:dd9f27fce7d1 2963 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 2964 case 2:
Ryosei 27:dd9f27fce7d1 2965 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 2966 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2967 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 2968 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2969 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2970 break;
Ryosei 27:dd9f27fce7d1 2971 case 3:
Ryosei 27:dd9f27fce7d1 2972 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 2973 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2974 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 2975 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2976 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2977 break;
Ryosei 27:dd9f27fce7d1 2978 case 1:
Ryosei 27:dd9f27fce7d1 2979 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 2980 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2981 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 2982 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2983 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2984 break;
Ryosei 27:dd9f27fce7d1 2985 case 0:
Ryosei 27:dd9f27fce7d1 2986 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2987 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2988 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2989 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2990 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2991 break;
Ryosei 27:dd9f27fce7d1 2992 case -1:
Ryosei 27:dd9f27fce7d1 2993 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2994 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 2995 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2996 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 2997 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2998 break;
Ryosei 27:dd9f27fce7d1 2999 case -3:
Ryosei 27:dd9f27fce7d1 3000 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3001 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 3002 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3003 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 3004 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3005 break;
Ryosei 27:dd9f27fce7d1 3006 case -2:
Ryosei 27:dd9f27fce7d1 3007 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3008 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3009 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3010 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 3011 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3012 break;
Ryosei 27:dd9f27fce7d1 3013 case 'A':
Ryosei 27:dd9f27fce7d1 3014 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3015 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3016 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3017 countW++;
Ryosei 27:dd9f27fce7d1 3018 }
Ryosei 27:dd9f27fce7d1 3019 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3020 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3021 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3022 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3023 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3024 break;
Ryosei 27:dd9f27fce7d1 3025 case 'N':
Ryosei 27:dd9f27fce7d1 3026 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3027 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3028 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3029 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3030 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3031 break;
Ryosei 27:dd9f27fce7d1 3032 default:
Ryosei 27:dd9f27fce7d1 3033 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3034 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3035 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3036 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3037 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3038 }
Ryosei 27:dd9f27fce7d1 3039
Ryosei 27:dd9f27fce7d1 3040 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3041 adj = 0;
Ryosei 27:dd9f27fce7d1 3042 } else {
Ryosei 27:dd9f27fce7d1 3043 adj = 0;
Ryosei 27:dd9f27fce7d1 3044 }
Ryosei 27:dd9f27fce7d1 3045
Ryosei 27:dd9f27fce7d1 3046 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3047 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3048 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3049 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3050 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3051 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3052 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3053 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3054
Ryosei 27:dd9f27fce7d1 3055 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3056 lineCount++;
Ryosei 27:dd9f27fce7d1 3057 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3058 }
Ryosei 27:dd9f27fce7d1 3059 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 3060 countW = 0;
Ryosei 27:dd9f27fce7d1 3061 lineFase = 10;
Ryosei 27:dd9f27fce7d1 3062 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3063 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3064 }
Ryosei 27:dd9f27fce7d1 3065 } else if(lineFase == 10) { // 前 低速
Ryosei 27:dd9f27fce7d1 3066 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3067 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3068 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3069 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3070 if(linePara[3] == 0) {
Ryosei 27:dd9f27fce7d1 3071 lineFase = 11;
Ryosei 27:dd9f27fce7d1 3072 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3073 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3074 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3075 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3076 }
Ryosei 27:dd9f27fce7d1 3077 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3078 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3079 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3080 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3081 } else if(lineFase == 11) { // 左
Ryosei 27:dd9f27fce7d1 3082 switch(linePara[3]) {
Ryosei 27:dd9f27fce7d1 3083 case -2:
Ryosei 27:dd9f27fce7d1 3084 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3085 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3086 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3087 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3088 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3089 break;
Ryosei 27:dd9f27fce7d1 3090 case -3:
Ryosei 27:dd9f27fce7d1 3091 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3092 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3093 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3094 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 3095 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3096 break;
Ryosei 27:dd9f27fce7d1 3097 case -1:
Ryosei 27:dd9f27fce7d1 3098 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3099 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3100 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3101 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3102 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3103 break;
Ryosei 27:dd9f27fce7d1 3104 case 0:
Ryosei 27:dd9f27fce7d1 3105 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3106 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3107 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3108 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3109 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3110 break;
Ryosei 27:dd9f27fce7d1 3111 case 1:
Ryosei 27:dd9f27fce7d1 3112 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3113 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3114 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3115 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3116 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3117 break;
Ryosei 27:dd9f27fce7d1 3118 case 3:
Ryosei 27:dd9f27fce7d1 3119 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 3120 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3121 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 3122 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3123 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3124 break;
Ryosei 27:dd9f27fce7d1 3125 case 2:
Ryosei 27:dd9f27fce7d1 3126 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3127 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3128 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3129 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3130 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3131 break;
Ryosei 27:dd9f27fce7d1 3132 case 'A':
Ryosei 27:dd9f27fce7d1 3133 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3134 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3135 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3136 countW++;
Ryosei 27:dd9f27fce7d1 3137 }
Ryosei 27:dd9f27fce7d1 3138 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3139 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3140 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3141 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3142 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3143 break;
Ryosei 27:dd9f27fce7d1 3144 case 'N':
Ryosei 27:dd9f27fce7d1 3145 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3146 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3147 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3148 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3149 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3150 break;
Ryosei 27:dd9f27fce7d1 3151 default:
Ryosei 27:dd9f27fce7d1 3152 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3153 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3154 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3155 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3156 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3157 }
Ryosei 27:dd9f27fce7d1 3158
Ryosei 27:dd9f27fce7d1 3159 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3160 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei 27:dd9f27fce7d1 3161 } else {
Ryosei 27:dd9f27fce7d1 3162 adj = 0;
Ryosei 27:dd9f27fce7d1 3163 }
Ryosei 27:dd9f27fce7d1 3164
Ryosei 27:dd9f27fce7d1 3165 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3166 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3167 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3168 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 3169 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3170 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3171 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3172 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 3173 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3174 lineCount++;
Ryosei 27:dd9f27fce7d1 3175 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3176 }
Ryosei 27:dd9f27fce7d1 3177 if(countW == 2) {
Ryosei 27:dd9f27fce7d1 3178 countW = 0;
Ryosei 27:dd9f27fce7d1 3179 lineFase = 12;
Ryosei 27:dd9f27fce7d1 3180 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3181 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3182 }
Ryosei 27:dd9f27fce7d1 3183 } else if(lineFase == 12) { // 左 低速
Ryosei 27:dd9f27fce7d1 3184 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3185 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3186 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3187 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3188 if(linePara[LINE_TOW_2] == 0) {
Ryosei 27:dd9f27fce7d1 3189 lineFase = 13;
Ryosei 27:dd9f27fce7d1 3190 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3191 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3192 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3193 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3194 }
Ryosei 27:dd9f27fce7d1 3195 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3196 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3197 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3198 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3199 } else if (lineFase == 13) {
Ryosei 27:dd9f27fce7d1 3200 lineFase = 14;
Ryosei 27:dd9f27fce7d1 3201 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3202 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3203 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3204 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3205 } else if(lineFase == 14) { // タオル2 竿検知
Ryosei 28:479631c2de29 3206 LedOut(4);
Ryosei 27:dd9f27fce7d1 3207 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
Ryosei 27:dd9f27fce7d1 3208 lineFase = 15;
Ryosei 27:dd9f27fce7d1 3209 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3210 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3211 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3212 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3213 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3214 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3215 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3216 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3217 } else if(LimitSw::IsPressed(TOW_2L)) {
Ryosei 27:dd9f27fce7d1 3218 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3219 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 3220 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3221 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3222 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3223 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3224 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3225 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 3226 } else if(LimitSw::IsPressed(TOW_2R)) {
Ryosei 27:dd9f27fce7d1 3227 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3228 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 3229 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3230 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3231 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3232 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3233 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3234 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 3235 } else {
Ryosei 27:dd9f27fce7d1 3236 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 3237 case -2:
Ryosei 27:dd9f27fce7d1 3238 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 3239 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3240 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 3241 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3242 break;
Ryosei 27:dd9f27fce7d1 3243 case -3:
Ryosei 27:dd9f27fce7d1 3244 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 3245 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3246 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 3247 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3248 break;
Ryosei 27:dd9f27fce7d1 3249 case -1:
Ryosei 27:dd9f27fce7d1 3250 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 3251 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3252 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 3253 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3254 break;
Ryosei 27:dd9f27fce7d1 3255 case 0:
Ryosei 27:dd9f27fce7d1 3256 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3257 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3258 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3259 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3260 break;
Ryosei 27:dd9f27fce7d1 3261 case 1:
Ryosei 27:dd9f27fce7d1 3262 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3263 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 3264 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3265 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 3266 break;
Ryosei 27:dd9f27fce7d1 3267 case 3:
Ryosei 27:dd9f27fce7d1 3268 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3269 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 3270 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3271 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 3272 break;
Ryosei 27:dd9f27fce7d1 3273 case 2:
Ryosei 27:dd9f27fce7d1 3274 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3275 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 3276 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3277 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3278 break;
Ryosei 27:dd9f27fce7d1 3279 case 'A':
Ryosei 27:dd9f27fce7d1 3280
Ryosei 27:dd9f27fce7d1 3281 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3282 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3283 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3284 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3285 break;
Ryosei 27:dd9f27fce7d1 3286 case 'N':
Ryosei 27:dd9f27fce7d1 3287 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3288 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3289 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3290 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3291 break;
Ryosei 27:dd9f27fce7d1 3292 default:
Ryosei 27:dd9f27fce7d1 3293 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3294 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3295 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3296 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3297 }
Ryosei 27:dd9f27fce7d1 3298
Ryosei 27:dd9f27fce7d1 3299 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3300 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3301 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 3302 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3303 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3304 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3305 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 3306 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3307 }
Ryosei 27:dd9f27fce7d1 3308 } else if(lineFase == 15 ) { // ライン 修正
Ryosei 27:dd9f27fce7d1 3309 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
Ryosei 27:dd9f27fce7d1 3310 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3311 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3312 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3313 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3314 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3315 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3316 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3317 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3318 } else if(linePara[LINE_TOW_2] > 0) {
Ryosei 27:dd9f27fce7d1 3319 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3320 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3321 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3322 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3323 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3324 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3325 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3326 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3327 } else if(linePara[LINE_TOW_2] < 0) {
Ryosei 27:dd9f27fce7d1 3328 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3329 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3330 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3331 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3332 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3333 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3334 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3335 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3336 } else if(linePara[LINE_TOW_2] == 0) {
Ryosei 27:dd9f27fce7d1 3337 lineFase = 16;
Ryosei 27:dd9f27fce7d1 3338 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3339 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3340 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3341 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3342 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3343 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3344 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3345 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3346 } else {
Ryosei 27:dd9f27fce7d1 3347 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3348 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3349 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3350 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3351 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3352 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3353 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3354 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3355 }
Ryosei 27:dd9f27fce7d1 3356 } else if(lineFase == 16) { // タオル2 解放
Ryosei 27:dd9f27fce7d1 3357 Air[TOWEL2] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 3358 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3359 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3360 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3361 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3362 lineFase = 17;
Ryosei 27:dd9f27fce7d1 3363 } else if(lineFase == 17) { // 前
Ryosei 27:dd9f27fce7d1 3364 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 3365 case 2:
Ryosei 27:dd9f27fce7d1 3366 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 3367 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3368 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 3369 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3370 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3371 break;
Ryosei 27:dd9f27fce7d1 3372 case 3:
Ryosei 27:dd9f27fce7d1 3373 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 3374 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3375 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 3376 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3377 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3378 break;
Ryosei 27:dd9f27fce7d1 3379 case 1:
Ryosei 27:dd9f27fce7d1 3380 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 3381 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3382 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 3383 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3384 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3385 break;
Ryosei 27:dd9f27fce7d1 3386 case 0:
Ryosei 27:dd9f27fce7d1 3387 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3388 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3389 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3390 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3391 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3392 break;
Ryosei 27:dd9f27fce7d1 3393 case -1:
Ryosei 27:dd9f27fce7d1 3394 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3395 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 3396 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3397 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 3398 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3399 break;
Ryosei 27:dd9f27fce7d1 3400 case -3:
Ryosei 27:dd9f27fce7d1 3401 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3402 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 3403 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3404 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 3405 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3406 break;
Ryosei 27:dd9f27fce7d1 3407 case -2:
Ryosei 27:dd9f27fce7d1 3408 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3409 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3410 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3411 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3412 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3413 break;
Ryosei 27:dd9f27fce7d1 3414 case 'A':
Ryosei 27:dd9f27fce7d1 3415 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3416 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3417 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3418 countW++;
Ryosei 27:dd9f27fce7d1 3419 }
Ryosei 27:dd9f27fce7d1 3420 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3421 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3422 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3423 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3424 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3425 break;
Ryosei 27:dd9f27fce7d1 3426 case 'N':
Ryosei 27:dd9f27fce7d1 3427 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3428 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3429 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3430 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3431 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3432 break;
Ryosei 27:dd9f27fce7d1 3433 default:
Ryosei 27:dd9f27fce7d1 3434 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3435 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3436 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3437 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3438 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3439 }
Ryosei 27:dd9f27fce7d1 3440
Ryosei 27:dd9f27fce7d1 3441 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3442 adj = 0;
Ryosei 27:dd9f27fce7d1 3443 } else {
Ryosei 27:dd9f27fce7d1 3444 adj = 0;
Ryosei 27:dd9f27fce7d1 3445 }
Ryosei 27:dd9f27fce7d1 3446
Ryosei 27:dd9f27fce7d1 3447 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3448 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3449 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3450 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3451 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3452 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3453 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3454 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3455
Ryosei 27:dd9f27fce7d1 3456 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3457 lineCount++;
Ryosei 27:dd9f27fce7d1 3458 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3459 }
Ryosei 27:dd9f27fce7d1 3460 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 3461 countW = 0;
Ryosei 27:dd9f27fce7d1 3462 lineFase = 100;
Ryosei 27:dd9f27fce7d1 3463 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3464 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3465 }
Ryosei 27:dd9f27fce7d1 3466 } else if(lineFase==100) {
Ryosei 28:479631c2de29 3467 LedOut(6);
Ryosei 27:dd9f27fce7d1 3468 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3469 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3470 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3471 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3472 if(LimitSw::IsPressed(LSW_UU)) {
Ryosei 27:dd9f27fce7d1 3473 motor[LIFT_U].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3474 motor[LIFT_U].pwm=100;
Ryosei 27:dd9f27fce7d1 3475 } else {
Ryosei 27:dd9f27fce7d1 3476 motor[LIFT_U].dir=BACK;
Ryosei 27:dd9f27fce7d1 3477 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 3478 }
Ryosei 27:dd9f27fce7d1 3479 if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
Ryosei 27:dd9f27fce7d1 3480 lineFase=101;
Ryosei 27:dd9f27fce7d1 3481 }
Ryosei 27:dd9f27fce7d1 3482 } else if(lineFase==101) {
Ryosei 27:dd9f27fce7d1 3483 //位置調整
Ryosei 27:dd9f27fce7d1 3484 //(P制御)
Ryosei 27:dd9f27fce7d1 3485 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3486 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 3487 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3488 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3489 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3490 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3491 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3492 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3493 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3494 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3495 lineFase=102;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 3496 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 3497 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 3498 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3499 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3500 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3501 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3502 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3503 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3504 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3505 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3506 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 3507 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3508 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3509 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3510 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3511 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3512 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3513 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3514 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3515 }
Ryosei 27:dd9f27fce7d1 3516 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 3517 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 3518 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3519 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3520 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3521 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3522 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3523 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3524 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3525 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3526 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 3527 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3528 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3529 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3530 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3531 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3532 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3533 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3534 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3535 }
Ryosei 27:dd9f27fce7d1 3536 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 3537 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 3538 if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3539 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3540 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3541 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3542 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3543 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3544 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3545 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3546 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3547 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3548 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3549 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3550 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3551 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3552 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3553 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3554 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3555 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3556 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3557 }
Ryosei 27:dd9f27fce7d1 3558 } else { //傾きが大きくなくて離れているとき
Ryosei 27:dd9f27fce7d1 3559 if((Ult_right+Ult_left)<=25) { //近すぎるとき
Ryosei 27:dd9f27fce7d1 3560 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3561 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3562 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3563 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3564 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 3565 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 3566 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 3567 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 3568 } else if((Ult_right+Ult_left)>=35) { //離れているとき
Ryosei 27:dd9f27fce7d1 3569 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3570 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3571 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3572 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3573 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 3574 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 3575 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 3576 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 3577 }
Ryosei 27:dd9f27fce7d1 3578 }
Ryosei 27:dd9f27fce7d1 3579
Ryosei 27:dd9f27fce7d1 3580 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3581 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3582 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3583 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3584 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3585 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3586 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3587 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3588 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3589 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3590 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3591 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3592 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3593 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3594 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3595 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3596 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3597 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3598 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3599 }
Ryosei 27:dd9f27fce7d1 3600 }
Ryosei 27:dd9f27fce7d1 3601 }
Ryosei 27:dd9f27fce7d1 3602
Ryosei 27:dd9f27fce7d1 3603 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 3604 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3605 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3606 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3607 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3608 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3609 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3610 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3611 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3612 }
Ryosei 27:dd9f27fce7d1 3613
Ryosei 27:dd9f27fce7d1 3614 } else if(lineFase==102) {
Ryosei 27:dd9f27fce7d1 3615 Air[CLOTHESPIN]=SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 3616 LedOut(1);
Ryosei 27:dd9f27fce7d1 3617 //リミットスイッチに当てる
Ryosei 27:dd9f27fce7d1 3618 static int count2=0;
Ryosei 27:dd9f27fce7d1 3619 if(count2==0) {
Ryosei 27:dd9f27fce7d1 3620 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3621 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {
Ryosei 27:dd9f27fce7d1 3622 if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
Ryosei 27:dd9f27fce7d1 3623 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 3624 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3625 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3626 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3627 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3628
Ryosei 27:dd9f27fce7d1 3629 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3630 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3631
Ryosei 27:dd9f27fce7d1 3632 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3633 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3634 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3635 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3636 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3637 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3638 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3639 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3640 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3641 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3642 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3643 }
Ryosei 27:dd9f27fce7d1 3644 } else if((Ult_left+Ult_right)<=25) {
Ryosei 27:dd9f27fce7d1 3645 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3646 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3647 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3648 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3649 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3650 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3651 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3652 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3653 } else if((Ult_left+Ult_right)>=35) {
Ryosei 27:dd9f27fce7d1 3654 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3655 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3656 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3657 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3658 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3659 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3660 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3661 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3662 }
Ryosei 27:dd9f27fce7d1 3663 } else {
Ryosei 27:dd9f27fce7d1 3664 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 3665 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3666 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3667 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3668 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3669
Ryosei 27:dd9f27fce7d1 3670 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3671 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3672
Ryosei 27:dd9f27fce7d1 3673 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3674 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3675 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3676 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3677 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3678 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3679 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3680 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3681 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3682 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3683 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3684 }
Ryosei 27:dd9f27fce7d1 3685 }
Ryosei 27:dd9f27fce7d1 3686
Ryosei 27:dd9f27fce7d1 3687 } else {
Ryosei 27:dd9f27fce7d1 3688 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3689 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3690 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3691 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3692 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3693 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3694 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3695 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3696
Ryosei 27:dd9f27fce7d1 3697 }
Ryosei 27:dd9f27fce7d1 3698 if(LimitSw::IsPressed(LEFTlim)) {
Ryosei 27:dd9f27fce7d1 3699 count2=1;
Ryosei 27:dd9f27fce7d1 3700 }
Ryosei 27:dd9f27fce7d1 3701
Ryosei 27:dd9f27fce7d1 3702 } else if(count2==1) {
Ryosei 27:dd9f27fce7d1 3703 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3704 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3705 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3706 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3707 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 3708 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 3709 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 3710 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 3711 lineFase=103;
Ryosei 27:dd9f27fce7d1 3712 }
Ryosei 27:dd9f27fce7d1 3713 } else if(lineFase==103) {
Ryosei 27:dd9f27fce7d1 3714 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3715 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 3716 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3717 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3718 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3719 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3720 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 3721 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 3722 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 3723 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 3724 lineFase=104;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 3725 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 3726 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 3727 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3728 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3729 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3730 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3731 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3732 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 3733 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3734 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 3735 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 3736 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3737 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3738 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3739 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3740 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3741 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 3742 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3743 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 3744 }
Ryosei 27:dd9f27fce7d1 3745 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 3746 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 3747 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3748 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3749 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3750 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3751 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 3752 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3753 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 3754 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3755 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 3756 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3757 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3758 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3759 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3760 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 3761 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3762 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 3763 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3764 }
Ryosei 27:dd9f27fce7d1 3765 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 3766 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 3767 if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3768 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3769 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3770 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3771 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3772 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3773 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3774 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3775 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3776 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3777 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3778 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3779 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3780 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3781 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3782 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3783 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3784 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3785 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3786 }
Ryosei 27:dd9f27fce7d1 3787 }
Ryosei 27:dd9f27fce7d1 3788 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3789 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3790 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3791 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3792 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3793 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3794 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3795 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3796 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3797 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3798 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3799 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3800 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3801 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3802 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3803 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3804 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3805 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3806 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3807 }
Ryosei 27:dd9f27fce7d1 3808 }
Ryosei 27:dd9f27fce7d1 3809 }
Ryosei 27:dd9f27fce7d1 3810
Ryosei 27:dd9f27fce7d1 3811 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 3812 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3813 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3814 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3815 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3816 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3817 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3818 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3819 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3820 }
Ryosei 27:dd9f27fce7d1 3821 } else if(lineFase==104) {
Ryosei 27:dd9f27fce7d1 3822 static int count3=0;
Ryosei 27:dd9f27fce7d1 3823 static int loop=0;
Ryosei 27:dd9f27fce7d1 3824 pc.printf("%d\r\n",loop);
Ryosei 27:dd9f27fce7d1 3825 Air[CLOTHESPIN]=1;
Ryosei 27:dd9f27fce7d1 3826 if(count3==0) {
Ryosei 27:dd9f27fce7d1 3827 loop++;
Ryosei 27:dd9f27fce7d1 3828 if(loop==60) {
Ryosei 27:dd9f27fce7d1 3829 count3=1;
Ryosei 27:dd9f27fce7d1 3830 }
Ryosei 27:dd9f27fce7d1 3831 } else if(count3==1) {
Ryosei 27:dd9f27fce7d1 3832 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 3833 lineFase=105;
Ryosei 27:dd9f27fce7d1 3834
Ryosei 27:dd9f27fce7d1 3835 }
Ryosei 27:dd9f27fce7d1 3836 } else if(lineFase==105) {
Ryosei 27:dd9f27fce7d1 3837 LedOut(6);
Ryosei 27:dd9f27fce7d1 3838 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 3839 if(!(LimitSw::IsPressed(RIGHTlim))) {
Ryosei 27:dd9f27fce7d1 3840
Ryosei 27:dd9f27fce7d1 3841 //超音波
Ryosei 27:dd9f27fce7d1 3842 if(Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3843 if(Ult_right<15) {
Ryosei 27:dd9f27fce7d1 3844 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3845 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3846 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3847 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3848 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3849 motor[TIRE_FR].pwm=25+(15-Ult_right);
Ryosei 27:dd9f27fce7d1 3850 motor[TIRE_BL].pwm=25+(15-Ult_right);
Ryosei 27:dd9f27fce7d1 3851 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3852 } else if(Ult_right>=15) {
Ryosei 27:dd9f27fce7d1 3853 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3854 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3855 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3856 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3857 motor[TIRE_FL].pwm=25+(Ult_right-15);
Ryosei 27:dd9f27fce7d1 3858 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3859 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3860 motor[TIRE_BR].pwm=25+(Ult_right-15);
Ryosei 27:dd9f27fce7d1 3861 }
Ryosei 27:dd9f27fce7d1 3862 } else {
Ryosei 27:dd9f27fce7d1 3863 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3864 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3865 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3866 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3867 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3868 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3869 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3870 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3871 }
Ryosei 27:dd9f27fce7d1 3872
Ryosei 27:dd9f27fce7d1 3873
Ryosei 27:dd9f27fce7d1 3874 } else {
Ryosei 27:dd9f27fce7d1 3875 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3876 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3877 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3878 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3879 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3880 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3881 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3882 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3883 lineFase=106;
Ryosei 27:dd9f27fce7d1 3884 }
Ryosei 27:dd9f27fce7d1 3885
Ryosei 27:dd9f27fce7d1 3886 } else if(lineFase==106) {
Ryosei 27:dd9f27fce7d1 3887 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3888 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3889 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3890 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3891 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3892 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3893 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3894 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3895 if(linePara[4]!='N') {
Ryosei 27:dd9f27fce7d1 3896 lineFase=19;
Ryosei 27:dd9f27fce7d1 3897 }
Ryosei 27:dd9f27fce7d1 3898 } else if(lineFase == 18) { // 前 低速
Ryosei 27:dd9f27fce7d1 3899 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3900 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3901 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3902 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3903 if(linePara[4] == 0) {
Ryosei 27:dd9f27fce7d1 3904 lineFase = 19;
Ryosei 27:dd9f27fce7d1 3905 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3906 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3907 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3908 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3909 }
Ryosei 27:dd9f27fce7d1 3910 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3911 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3912 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3913 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3914 } else if(lineFase == 19) { // 右
Ryosei 27:dd9f27fce7d1 3915 switch(linePara[4]) {
Ryosei 27:dd9f27fce7d1 3916 case -2:
Ryosei 27:dd9f27fce7d1 3917 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3918 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3919 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3920 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3921 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3922 break;
Ryosei 27:dd9f27fce7d1 3923 case -3:
Ryosei 27:dd9f27fce7d1 3924 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3925 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 3926 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3927 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3928 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3929 break;
Ryosei 27:dd9f27fce7d1 3930 case -1:
Ryosei 27:dd9f27fce7d1 3931 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3932 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3933 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3934 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3935 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3936 break;
Ryosei 27:dd9f27fce7d1 3937 case 0:
Ryosei 27:dd9f27fce7d1 3938 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3939 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3940 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3941 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3942 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3943 break;
Ryosei 27:dd9f27fce7d1 3944 case 1:
Ryosei 27:dd9f27fce7d1 3945 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3946 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3947 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3948 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3949 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3950 break;
Ryosei 27:dd9f27fce7d1 3951 case 3:
Ryosei 27:dd9f27fce7d1 3952 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 3953 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3954 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 3955 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3956 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3957 break;
Ryosei 27:dd9f27fce7d1 3958 case 2:
Ryosei 27:dd9f27fce7d1 3959 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3960 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3961 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3962 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3963 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3964 break;
Ryosei 27:dd9f27fce7d1 3965 case 'A':
Ryosei 27:dd9f27fce7d1 3966 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3967 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3968 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3969 countW++;
Ryosei 27:dd9f27fce7d1 3970 }
Ryosei 27:dd9f27fce7d1 3971 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3972 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3973 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3974 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3975 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3976 break;
Ryosei 27:dd9f27fce7d1 3977 case 'N':
Ryosei 27:dd9f27fce7d1 3978 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3979 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3980 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3981 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3982 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3983 break;
Ryosei 27:dd9f27fce7d1 3984 default:
Ryosei 27:dd9f27fce7d1 3985 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3986 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3987 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3988 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3989 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3990 }
Ryosei 27:dd9f27fce7d1 3991
Ryosei 27:dd9f27fce7d1 3992 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3993 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
Ryosei 27:dd9f27fce7d1 3994 } else {
Ryosei 27:dd9f27fce7d1 3995 adj = 0;
Ryosei 27:dd9f27fce7d1 3996 }
Ryosei 27:dd9f27fce7d1 3997
Ryosei 27:dd9f27fce7d1 3998 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 3999 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 4000 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4001 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 4002 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 4003 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 4004 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4005 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 4006
Ryosei 27:dd9f27fce7d1 4007 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 4008 lineCount++;
Ryosei 27:dd9f27fce7d1 4009 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 4010 }
Ryosei 27:dd9f27fce7d1 4011
Ryosei 27:dd9f27fce7d1 4012 /*
Ryosei 27:dd9f27fce7d1 4013 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
Ryosei 27:dd9f27fce7d1 4014 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4015 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
Ryosei 27:dd9f27fce7d1 4016 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4017 } else {
Ryosei 27:dd9f27fce7d1 4018 targetCount = 2;
Ryosei 27:dd9f27fce7d1 4019 }
Ryosei 27:dd9f27fce7d1 4020 */
Ryosei 27:dd9f27fce7d1 4021
Ryosei 27:dd9f27fce7d1 4022 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4023
Ryosei 27:dd9f27fce7d1 4024 if(countW == targetCount) {
Ryosei 27:dd9f27fce7d1 4025 countW = 0;
Ryosei 27:dd9f27fce7d1 4026 lineFase = 20;
Ryosei 27:dd9f27fce7d1 4027 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4028 lineCheck = false;
Ryosei 27:dd9f27fce7d1 4029 }
Ryosei 27:dd9f27fce7d1 4030 } else if(lineFase == 20) { // 右 低速
Ryosei 27:dd9f27fce7d1 4031 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 4032 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4033 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 4034 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4035 if (linePara[2] == 0) {
Ryosei 27:dd9f27fce7d1 4036 //(!!LimitSw::Ispressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 4037 //ineFase = 20;
Ryosei 27:dd9f27fce7d1 4038 //else if(LimitSw::IsPressed(TOWEL2_SW) {
Ryosei 27:dd9f27fce7d1 4039 //ineFase = 14;
Ryosei 27:dd9f27fce7d1 4040 //else {
Ryosei 27:dd9f27fce7d1 4041 lineFase = 21;
Ryosei 27:dd9f27fce7d1 4042 //
Ryosei 27:dd9f27fce7d1 4043 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4044 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4045 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4046 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4047 }
Ryosei 27:dd9f27fce7d1 4048 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 4049 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 4050 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 4051 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 4052 } else if(lineFase == 21) {
Ryosei 27:dd9f27fce7d1 4053 switch(linePara[2]) {
Ryosei 27:dd9f27fce7d1 4054 case -2:
Ryosei 27:dd9f27fce7d1 4055 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4056 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4057 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4058 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4059 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4060 break;
Ryosei 27:dd9f27fce7d1 4061 case -3:
Ryosei 27:dd9f27fce7d1 4062 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 4063 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4064 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 4065 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4066 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4067 break;
Ryosei 27:dd9f27fce7d1 4068 case -1:
Ryosei 27:dd9f27fce7d1 4069 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 4070 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4071 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 4072 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4073 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4074 break;
Ryosei 27:dd9f27fce7d1 4075 case 0:
Ryosei 27:dd9f27fce7d1 4076 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4077 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4078 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4079 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4080 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4081 break;
Ryosei 27:dd9f27fce7d1 4082 case 1:
Ryosei 27:dd9f27fce7d1 4083 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4084 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 4085 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4086 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 4087 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4088 break;
Ryosei 27:dd9f27fce7d1 4089 case 3:
Ryosei 27:dd9f27fce7d1 4090 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4091 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 4092 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4093 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 4094 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4095 break;
Ryosei 27:dd9f27fce7d1 4096 case 2:
Ryosei 27:dd9f27fce7d1 4097 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4098 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4099 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4100 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4101 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4102 break;
Ryosei 27:dd9f27fce7d1 4103 case 'A':
Ryosei 27:dd9f27fce7d1 4104 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 4105 lineCheck = true;
Ryosei 27:dd9f27fce7d1 4106 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4107 countW++;
Ryosei 27:dd9f27fce7d1 4108 }
Ryosei 27:dd9f27fce7d1 4109 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4110 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4111 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4112 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4113 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4114 break;
Ryosei 27:dd9f27fce7d1 4115 case 'N':
Ryosei 27:dd9f27fce7d1 4116 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4117 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4118 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4119 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4120 adjAnable = false;
Ryosei 27:dd9f27fce7d1 4121 break;
Ryosei 27:dd9f27fce7d1 4122 default:
Ryosei 27:dd9f27fce7d1 4123 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4124 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4125 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4126 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4127 adjAnable = false;
Ryosei 27:dd9f27fce7d1 4128 }
Ryosei 27:dd9f27fce7d1 4129
Ryosei 27:dd9f27fce7d1 4130 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 4131 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
Ryosei 27:dd9f27fce7d1 4132 } else {
Ryosei 27:dd9f27fce7d1 4133 adj = 0;
Ryosei 27:dd9f27fce7d1 4134 }
Ryosei 27:dd9f27fce7d1 4135
Ryosei 27:dd9f27fce7d1 4136 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 4137 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 4138 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4139 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 4140 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 4141 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 4142 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4143 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 4144
Ryosei 27:dd9f27fce7d1 4145 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 4146 lineCount++;
Ryosei 27:dd9f27fce7d1 4147 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 4148 }
Ryosei 27:dd9f27fce7d1 4149 if(countW == 2) {
Ryosei 27:dd9f27fce7d1 4150 countW = 0;
Ryosei 27:dd9f27fce7d1 4151 lineFase = 22;
Ryosei 27:dd9f27fce7d1 4152 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4153 lineCheck = false;
Ryosei 27:dd9f27fce7d1 4154 }
Ryosei 27:dd9f27fce7d1 4155 } else if(lineFase == 22) {
Ryosei 27:dd9f27fce7d1 4156 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4157 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4158 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4159 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4160 if (linePara[0] == 'N') {
Ryosei 27:dd9f27fce7d1 4161 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4162 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4163 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4164 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4165 }
Ryosei 27:dd9f27fce7d1 4166 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 4167 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 4168 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 4169 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 4170 } else {
Ryosei 27:dd9f27fce7d1 4171 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4172 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4173 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4174 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4175 }
Ryosei 27:dd9f27fce7d1 4176
kishibekairohan 13:b6e02d6261d7 4177 }
kishibekairohan 13:b6e02d6261d7 4178 #endif
kishibekairohan 13:b6e02d6261d7 4179
kishibekairohan 13:b6e02d6261d7 4180 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 4181 static void Process6()
kishibekairohan 13:b6e02d6261d7 4182 {
Ryosei 27:dd9f27fce7d1 4183
kishibekairohan 13:b6e02d6261d7 4184 }
kishibekairohan 13:b6e02d6261d7 4185 #endif
kishibekairohan 13:b6e02d6261d7 4186
kishibekairohan 13:b6e02d6261d7 4187 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 4188 static void Process7()
kishibekairohan 13:b6e02d6261d7 4189 {
Ryosei 27:dd9f27fce7d1 4190 static int SW_flag=0;
Ryosei 27:dd9f27fce7d1 4191 static int Processflag=0;
Ryosei 27:dd9f27fce7d1 4192 if(Processflag==0) {
Ryosei 27:dd9f27fce7d1 4193 if(Limitphase==0) {
Ryosei 27:dd9f27fce7d1 4194 //下待機
Ryosei 27:dd9f27fce7d1 4195 Limitphase=2;
Ryosei 27:dd9f27fce7d1 4196 } else if(Limitphase==1) {
Ryosei 27:dd9f27fce7d1 4197 } else if(Limitphase==2) {
Ryosei 27:dd9f27fce7d1 4198 //下→上とタオル展開
Ryosei 27:dd9f27fce7d1 4199 pc.printf("%d\r\n",SW_flag);
Ryosei 27:dd9f27fce7d1 4200
Ryosei 27:dd9f27fce7d1 4201 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4202 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4203 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4204 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4205
Ryosei 27:dd9f27fce7d1 4206 if(SW_flag==0) {
Ryosei 27:dd9f27fce7d1 4207 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4208 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4209 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4210 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4211 if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4212 SW_flag=1;
Ryosei 27:dd9f27fce7d1 4213 }
Ryosei 27:dd9f27fce7d1 4214 } else if(SW_flag==1) {
Ryosei 27:dd9f27fce7d1 4215 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4216 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4217 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4218 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4219 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4220 SW_flag=2;
Ryosei 27:dd9f27fce7d1 4221 }
Ryosei 27:dd9f27fce7d1 4222 } else if(SW_flag==2) {
Ryosei 27:dd9f27fce7d1 4223 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4224 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4225 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4226 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4227 if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4228 SW_flag=3;
Ryosei 27:dd9f27fce7d1 4229 }
Ryosei 27:dd9f27fce7d1 4230 } else if(SW_flag==3) {
Ryosei 27:dd9f27fce7d1 4231 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4232 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4233 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4234 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4235 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4236 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4237 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4238 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4239 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4240 Limitphase=4;
Ryosei 27:dd9f27fce7d1 4241 }
Ryosei 27:dd9f27fce7d1 4242 }
Ryosei 27:dd9f27fce7d1 4243 } else if(Limitphase==3) {
Ryosei 27:dd9f27fce7d1 4244 } else if(Limitphase==4) {
Ryosei 27:dd9f27fce7d1 4245 //上段待機
Ryosei 27:dd9f27fce7d1 4246 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4247 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4248 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4249 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4250 current=0;
Ryosei 27:dd9f27fce7d1 4251 startFlag=true;
Ryosei 27:dd9f27fce7d1 4252 } else {
Ryosei 27:dd9f27fce7d1 4253 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4254 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4255 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4256 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4257 startFlag=true;
Ryosei 27:dd9f27fce7d1 4258 current=0;
Ryosei 27:dd9f27fce7d1 4259 }
Ryosei 27:dd9f27fce7d1 4260 if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)) {
Ryosei 27:dd9f27fce7d1 4261 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4262 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4263 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4264 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4265 Air[TOWEL0]=1;
Ryosei 27:dd9f27fce7d1 4266 SW_flag=0;
Ryosei 27:dd9f27fce7d1 4267 Limitphase=0;
Ryosei 27:dd9f27fce7d1 4268 current=0;
Ryosei 27:dd9f27fce7d1 4269 startFlag=true;
Ryosei 27:dd9f27fce7d1 4270 Processflag=1;
Ryosei 27:dd9f27fce7d1 4271 }
Ryosei 27:dd9f27fce7d1 4272 } else if(Processflag==1) {
Ryosei 27:dd9f27fce7d1 4273 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4274 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4275 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4276 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4277 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4278 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4279 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4280 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4281 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4282 current=0;
Ryosei 27:dd9f27fce7d1 4283 startFlag=true;
Ryosei 27:dd9f27fce7d1 4284 Processflag=1;
Ryosei 27:dd9f27fce7d1 4285 }
Ryosei 27:dd9f27fce7d1 4286 if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)){
Ryosei 27:dd9f27fce7d1 4287 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4288 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4289 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4290 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4291 Air[TOWEL0]=1;
Ryosei 27:dd9f27fce7d1 4292 SW_flag=0;
Ryosei 27:dd9f27fce7d1 4293 Limitphase=0;
Ryosei 27:dd9f27fce7d1 4294 current=0;
Ryosei 27:dd9f27fce7d1 4295 startFlag=true;
Ryosei 27:dd9f27fce7d1 4296 Processflag=1;
Ryosei 27:dd9f27fce7d1 4297 }
Ryosei 27:dd9f27fce7d1 4298 }
kishibekairohan 12:c09b3e08a316 4299 }
Ryosei 27:dd9f27fce7d1 4300
Ryosei 27:dd9f27fce7d1 4301
Ryosei 27:dd9f27fce7d1 4302
kishibekairohan 12:c09b3e08a316 4303 #endif
kishibekairohan 12:c09b3e08a316 4304
Ryosei 27:dd9f27fce7d1 4305 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 4306 static void Process8()
kishibekairohan 12:c09b3e08a316 4307 {
Ryosei 27:dd9f27fce7d1 4308 if(controller->Button.A) {
Ryosei 27:dd9f27fce7d1 4309 rotaconSampling.start();
Ryosei 27:dd9f27fce7d1 4310 PIDflag = true;
Ryosei 27:dd9f27fce7d1 4311
Ryosei 27:dd9f27fce7d1 4312 //linePara_U = LineHub::GetPara(0);
Ryosei 27:dd9f27fce7d1 4313 //linePara_B = LineHub::GetPara(3);
Ryosei 27:dd9f27fce7d1 4314
Ryosei 27:dd9f27fce7d1 4315
Ryosei 27:dd9f27fce7d1 4316 pulsePV[FL] = encoder[FL].getPulses();
Ryosei 27:dd9f27fce7d1 4317 pulsePV[BL] = encoder[BL].getPulses();
Ryosei 27:dd9f27fce7d1 4318 pulsePV[BR] = encoder[BR].getPulses();
Ryosei 27:dd9f27fce7d1 4319 pulsePV[FR] = encoder[FR].getPulses();
Ryosei 27:dd9f27fce7d1 4320
Ryosei 27:dd9f27fce7d1 4321
Ryosei 27:dd9f27fce7d1 4322 for (int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4323 timeCV[i] = timePV[i];
Ryosei 27:dd9f27fce7d1 4324 timePV[i] = rotaconSampling.read();
Ryosei 27:dd9f27fce7d1 4325 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
Ryosei 27:dd9f27fce7d1 4326 pulseCV[i] = pulsePV[i];
Ryosei 27:dd9f27fce7d1 4327 }
Ryosei 27:dd9f27fce7d1 4328
Ryosei 27:dd9f27fce7d1 4329 move.Vx = 0.5;
Ryosei 27:dd9f27fce7d1 4330 move.Vy = 0.5;
Ryosei 27:dd9f27fce7d1 4331 move.Va = 0;
Ryosei 27:dd9f27fce7d1 4332
Ryosei 27:dd9f27fce7d1 4333 correction_LT.Vx = 0; //0.1 * linePara_U;
Ryosei 27:dd9f27fce7d1 4334 correction_LT.Vy = 0;
Ryosei 27:dd9f27fce7d1 4335 correction_LT.Va = 0;
Ryosei 27:dd9f27fce7d1 4336
Ryosei 27:dd9f27fce7d1 4337 synthetic.Vx = move.Vx + correction_LT.Vx;
Ryosei 27:dd9f27fce7d1 4338 synthetic.Vy = move.Vy + correction_LT.Vy;
Ryosei 27:dd9f27fce7d1 4339 synthetic.Va = move.Va + correction_LT.Va;
Ryosei 27:dd9f27fce7d1 4340
Ryosei 27:dd9f27fce7d1 4341 sita = 0;
Ryosei 27:dd9f27fce7d1 4342
Ryosei 27:dd9f27fce7d1 4343 //タイヤの目標速度算出
Ryosei 27:dd9f27fce7d1 4344 float sinR = 0.7071 * (float)sin(sita);
Ryosei 27:dd9f27fce7d1 4345 float cosR = 0.7071 * (float)cos(sita);
Ryosei 27:dd9f27fce7d1 4346 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
Ryosei 27:dd9f27fce7d1 4347 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4348 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4349 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4350 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4351
Ryosei 27:dd9f27fce7d1 4352 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
Ryosei 27:dd9f27fce7d1 4353
Ryosei 27:dd9f27fce7d1 4354 //PIDによるPWM算出
Ryosei 27:dd9f27fce7d1 4355
Ryosei 27:dd9f27fce7d1 4356 //モータの駆動
Ryosei 27:dd9f27fce7d1 4357 for (int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4358 if (tirePWM[i] > 255) {
Ryosei 27:dd9f27fce7d1 4359 tirePWM[i] = 255;
Ryosei 27:dd9f27fce7d1 4360 } else if (tirePWM[i] < -255) {
Ryosei 27:dd9f27fce7d1 4361 tirePWM[i] = -255;
Ryosei 27:dd9f27fce7d1 4362 }
Ryosei 27:dd9f27fce7d1 4363 }
Ryosei 27:dd9f27fce7d1 4364
Ryosei 27:dd9f27fce7d1 4365 for(int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4366 motor[i].dir = SetStatus(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4367 motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4368 }
Ryosei 27:dd9f27fce7d1 4369 } else {
Ryosei 27:dd9f27fce7d1 4370 PIDflag = false;
Ryosei 27:dd9f27fce7d1 4371 rotaconSampling.stop();
Ryosei 27:dd9f27fce7d1 4372 rotaconSampling.reset();
Ryosei 27:dd9f27fce7d1 4373 for(int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4374 encoder[i].reset();
Ryosei 27:dd9f27fce7d1 4375 pulsePV[i] = 0;
Ryosei 27:dd9f27fce7d1 4376 pulseCV[i] = 0;
Ryosei 27:dd9f27fce7d1 4377 timePV[i] = 0;
Ryosei 27:dd9f27fce7d1 4378 timeCV[i] = 0;
Ryosei 27:dd9f27fce7d1 4379 tirePWM[i] = 0;
Ryosei 27:dd9f27fce7d1 4380 motor[i].dir = SetStatus(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4381 motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4382 }
Ryosei 27:dd9f27fce7d1 4383 }
kishibekairohan 12:c09b3e08a316 4384 }
kishibekairohan 12:c09b3e08a316 4385 #endif
kishibekairohan 12:c09b3e08a316 4386
kishibekairohan 13:b6e02d6261d7 4387 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 4388 static void Process9()
kishibekairohan 12:c09b3e08a316 4389 {
Ryosei 27:dd9f27fce7d1 4390
t_yamamoto 0:669ef71cba68 4391 }
t_yamamoto 0:669ef71cba68 4392 #endif
t_yamamoto 0:669ef71cba68 4393 #endif
t_yamamoto 0:669ef71cba68 4394 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 4395
t_yamamoto 0:669ef71cba68 4396 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 4397 {
t_yamamoto 0:669ef71cba68 4398
Ryosei 27:dd9f27fce7d1 4399 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 4400 solenoid.all = ALL_SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 4401 #endif
t_yamamoto 0:669ef71cba68 4402
Ryosei 27:dd9f27fce7d1 4403 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 4404 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
Ryosei 27:dd9f27fce7d1 4405 motor[i].dir = FREE;
Ryosei 27:dd9f27fce7d1 4406 motor[i].pwm = 0;
Ryosei 27:dd9f27fce7d1 4407 }
Ryosei 27:dd9f27fce7d1 4408 #endif
t_yamamoto 0:669ef71cba68 4409 }
t_yamamoto 0:669ef71cba68 4410
Ryosei 27:dd9f27fce7d1 4411 void BuzzerTimer_func()
Ryosei 27:dd9f27fce7d1 4412 {
kishibekairohan 16:3f2c2d89372b 4413 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 4414 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 4415 }
kishibekairohan 11:028a150943b5 4416
Ryosei 27:dd9f27fce7d1 4417 void TapeLedEms_func()
Ryosei 27:dd9f27fce7d1 4418 {
kishibekairohan 16:3f2c2d89372b 4419 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 4420 }
kishibekairohan 16:3f2c2d89372b 4421
kishibekairohan 16:3f2c2d89372b 4422 #pragma region USER-DEFINED-FUNCTIONS
Ryosei 27:dd9f27fce7d1 4423 void tirePID()
Ryosei 27:dd9f27fce7d1 4424 {
Ryosei 27:dd9f27fce7d1 4425 if(PIDflag == true) {
Ryosei 27:dd9f27fce7d1 4426 //加算するPID値の算出
Ryosei 27:dd9f27fce7d1 4427 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
Ryosei 27:dd9f27fce7d1 4428 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
Ryosei 27:dd9f27fce7d1 4429 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
Ryosei 27:dd9f27fce7d1 4430 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
Ryosei 27:dd9f27fce7d1 4431 //PID値の加算
Ryosei 27:dd9f27fce7d1 4432 tirePWM[FL] += rotaconPID[0].GetMV();
Ryosei 27:dd9f27fce7d1 4433 tirePWM[BL] += rotaconPID[1].GetMV();
Ryosei 27:dd9f27fce7d1 4434 tirePWM[FR] += rotaconPID[2].GetMV();
Ryosei 27:dd9f27fce7d1 4435 tirePWM[BR] += rotaconPID[3].GetMV();
Ryosei 27:dd9f27fce7d1 4436 }
M_souta 22:7d93f79a3686 4437 }
kishibekairohan 16:3f2c2d89372b 4438
Ryosei 27:dd9f27fce7d1 4439 int lineCast(char k)
Ryosei 27:dd9f27fce7d1 4440 {
Ryosei 27:dd9f27fce7d1 4441 int l;
Ryosei 27:dd9f27fce7d1 4442 switch(k) {
Ryosei 27:dd9f27fce7d1 4443 case 255:
Ryosei 27:dd9f27fce7d1 4444 l = -1;
Ryosei 27:dd9f27fce7d1 4445 break;
Ryosei 27:dd9f27fce7d1 4446 case 254:
Ryosei 27:dd9f27fce7d1 4447 l = -2;
Ryosei 27:dd9f27fce7d1 4448 break;
Ryosei 27:dd9f27fce7d1 4449 case 253:
Ryosei 27:dd9f27fce7d1 4450 l = -3;
Ryosei 27:dd9f27fce7d1 4451 break;
Ryosei 27:dd9f27fce7d1 4452 default:
Ryosei 27:dd9f27fce7d1 4453 l = k;
Ryosei 27:dd9f27fce7d1 4454 }
Ryosei 27:dd9f27fce7d1 4455 return l;
M_souta 22:7d93f79a3686 4456 }
kishibekairohan 16:3f2c2d89372b 4457
t_yamamoto 0:669ef71cba68 4458 #pragma endregion