大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_old

aa

Dependencies:   mbed

System/Process/Process.cpp@27:dd9f27fce7d1, 2019-10-03 (annotated)

Committer:
Ryosei
Date:
Thu Oct 03 05:25:43 2019 +0000
Revision:
27:dd9f27fce7d1
Parent:
26:4c0ce2f05688
Child:
28:479631c2de29
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 27:dd9f27fce7d1 2532 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2533 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2534 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2535 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2536 if(linePara[0] != 'N' && linePara[0] != 'A') {
Ryosei 27:dd9f27fce7d1 2537 lineFase = 1;
Ryosei 27:dd9f27fce7d1 2538 }
Ryosei 27:dd9f27fce7d1 2539 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2540 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 2541 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2542 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 2543 } else if(lineFase == 1) { // 前 ライントレース
Ryosei 27:dd9f27fce7d1 2544 switch(linePara[0]) {
Ryosei 27:dd9f27fce7d1 2545 case -2:
Ryosei 27:dd9f27fce7d1 2546 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2547 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2548 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2549 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2550 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2551 break;
Ryosei 27:dd9f27fce7d1 2552 case -3:
Ryosei 27:dd9f27fce7d1 2553 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 2554 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2555 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 2556 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2557 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2558 break;
Ryosei 27:dd9f27fce7d1 2559 case -1:
Ryosei 27:dd9f27fce7d1 2560 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2561 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2562 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2563 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2564 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2565 break;
Ryosei 27:dd9f27fce7d1 2566 case 0:
Ryosei 27:dd9f27fce7d1 2567 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2568 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2569 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2570 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2571 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2572 break;
Ryosei 27:dd9f27fce7d1 2573 case 1:
Ryosei 27:dd9f27fce7d1 2574 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2575 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2576 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2577 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2578 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2579 break;
Ryosei 27:dd9f27fce7d1 2580 case 3:
Ryosei 27:dd9f27fce7d1 2581 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2582 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 2583 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2584 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 2585 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2586 break;
Ryosei 27:dd9f27fce7d1 2587 case 2:
Ryosei 27:dd9f27fce7d1 2588 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2589 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2590 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2591 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2592 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2593 break;
Ryosei 27:dd9f27fce7d1 2594 case 'A':
Ryosei 27:dd9f27fce7d1 2595 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2596 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2597 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2598 countW++;
Ryosei 27:dd9f27fce7d1 2599 }
Ryosei 27:dd9f27fce7d1 2600 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 2601 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2602 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 2603 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2604 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2605 break;
Ryosei 27:dd9f27fce7d1 2606 case 'N':
Ryosei 27:dd9f27fce7d1 2607 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2608 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2609 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2610 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2611 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2612 break;
Ryosei 27:dd9f27fce7d1 2613 default:
Ryosei 27:dd9f27fce7d1 2614 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2615 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2616 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2617 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2618 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2619 }
Ryosei 27:dd9f27fce7d1 2620
Ryosei 27:dd9f27fce7d1 2621 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2622 if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
Ryosei 27:dd9f27fce7d1 2623 } else {
Ryosei 27:dd9f27fce7d1 2624 adj = 0;
Ryosei 27:dd9f27fce7d1 2625 }
Ryosei 27:dd9f27fce7d1 2626
Ryosei 27:dd9f27fce7d1 2627 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2628 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 2629 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2630 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2631 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2632 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 2633 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2634 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2635
Ryosei 27:dd9f27fce7d1 2636 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2637 lineCount++;
Ryosei 27:dd9f27fce7d1 2638 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2639 }
Ryosei 27:dd9f27fce7d1 2640 if(countW == 3) {
Ryosei 27:dd9f27fce7d1 2641 countW = 0;
Ryosei 27:dd9f27fce7d1 2642 lineFase = 2;
Ryosei 27:dd9f27fce7d1 2643 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2644 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2645 }
Ryosei 27:dd9f27fce7d1 2646 } else if(lineFase == 2) { // 前 低速
Ryosei 27:dd9f27fce7d1 2647 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2648 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2649 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2650 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2651 if(linePara[3] == 0) {
Ryosei 27:dd9f27fce7d1 2652 lineFase = 3;
Ryosei 27:dd9f27fce7d1 2653 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2654 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2655 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2656 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2657 }
Ryosei 27:dd9f27fce7d1 2658 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2659 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2660 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2661 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2662 } else if(lineFase == 3) { // 左 ライントレース
Ryosei 27:dd9f27fce7d1 2663 switch(linePara[3]) {
Ryosei 27:dd9f27fce7d1 2664 case -2:
Ryosei 27:dd9f27fce7d1 2665 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2666 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2667 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2668 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2669 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2670 break;
Ryosei 27:dd9f27fce7d1 2671 case -3:
Ryosei 27:dd9f27fce7d1 2672 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2673 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 2674 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2675 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 2676 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2677 break;
Ryosei 27:dd9f27fce7d1 2678 case -1:
Ryosei 27:dd9f27fce7d1 2679 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2680 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2681 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2682 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2683 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2684 break;
Ryosei 27:dd9f27fce7d1 2685 case 0:
Ryosei 27:dd9f27fce7d1 2686 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2687 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2688 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2689 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2690 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2691 break;
Ryosei 27:dd9f27fce7d1 2692 case 1:
Ryosei 27:dd9f27fce7d1 2693 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2694 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2695 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2696 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2697 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2698 break;
Ryosei 27:dd9f27fce7d1 2699 case 3:
Ryosei 27:dd9f27fce7d1 2700 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2701 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2702 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2703 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2704 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2705 break;
Ryosei 27:dd9f27fce7d1 2706 case 2:
Ryosei 27:dd9f27fce7d1 2707 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2708 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2709 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2710 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2711 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2712 break;
Ryosei 27:dd9f27fce7d1 2713 case 'A':
Ryosei 27:dd9f27fce7d1 2714 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 2715 lineCheck = true;
Ryosei 27:dd9f27fce7d1 2716 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2717 countW++;
Ryosei 27:dd9f27fce7d1 2718 }
Ryosei 27:dd9f27fce7d1 2719 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 2720 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 2721 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 2722 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 2723 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2724 break;
Ryosei 27:dd9f27fce7d1 2725 case 'N':
Ryosei 27:dd9f27fce7d1 2726 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2727 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2728 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2729 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2730 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2731 break;
Ryosei 27:dd9f27fce7d1 2732 default:
Ryosei 27:dd9f27fce7d1 2733 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2734 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2735 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2736 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2737 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2738 }
Ryosei 27:dd9f27fce7d1 2739
Ryosei 27:dd9f27fce7d1 2740 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 2741 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei 27:dd9f27fce7d1 2742 } else {
Ryosei 27:dd9f27fce7d1 2743 adj = 0;
Ryosei 27:dd9f27fce7d1 2744 }
Ryosei 27:dd9f27fce7d1 2745
Ryosei 27:dd9f27fce7d1 2746 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2747 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2748 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2749 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2750 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2751 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2752 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 2753 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 2754
Ryosei 27:dd9f27fce7d1 2755 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 2756 lineCount++;
Ryosei 27:dd9f27fce7d1 2757 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 2758 }
Ryosei 27:dd9f27fce7d1 2759 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 2760 countW = 0;
Ryosei 27:dd9f27fce7d1 2761 lineFase = 4;
Ryosei 27:dd9f27fce7d1 2762 lineCount = 0;
Ryosei 27:dd9f27fce7d1 2763 lineCheck = false;
Ryosei 27:dd9f27fce7d1 2764 }
Ryosei 27:dd9f27fce7d1 2765 } else if(lineFase == 4) { // 右 低速
Ryosei 27:dd9f27fce7d1 2766 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2767 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2768 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2769 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2770 if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 2771 if(!LimitSw::IsPressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 2772 lineFase=100;
Ryosei 27:dd9f27fce7d1 2773 } else {
Ryosei 27:dd9f27fce7d1 2774 lineFase = 6;
Ryosei 27:dd9f27fce7d1 2775 }
Ryosei 27:dd9f27fce7d1 2776 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2777 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2778 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2779 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2780 }
Ryosei 27:dd9f27fce7d1 2781 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2782 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2783 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2784 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2785 } else if (lineFase == 5) {
Ryosei 27:dd9f27fce7d1 2786 lineFase = 6;
Ryosei 27:dd9f27fce7d1 2787 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2788 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2789 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2790 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2791 } else if(lineFase == 6) { // タオル1 竿検知
Ryosei 27:dd9f27fce7d1 2792 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 2793 lineFase = 7;
Ryosei 27:dd9f27fce7d1 2794 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2795 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2796 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2797 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2798 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2799 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2800 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2801 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2802 } else if(LimitSw::IsPressed(TOW_1L)) {
Ryosei 27:dd9f27fce7d1 2803 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2804 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 2805 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2806 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2807 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2808 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2809 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2810 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 2811 } else if(LimitSw::IsPressed(TOW_1R)) {
Ryosei 27:dd9f27fce7d1 2812 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2813 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 2814 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2815 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2816 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2817 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2818 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2819 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 2820 } else {
Ryosei 27:dd9f27fce7d1 2821 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 2822 case -2:
Ryosei 27:dd9f27fce7d1 2823 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 2824 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2825 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 2826 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2827 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2828 break;
Ryosei 27:dd9f27fce7d1 2829 case -3:
Ryosei 27:dd9f27fce7d1 2830 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 2831 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2832 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 2833 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2834 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2835 break;
Ryosei 27:dd9f27fce7d1 2836 case -1:
Ryosei 27:dd9f27fce7d1 2837 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 2838 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2839 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 2840 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2841 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2842 break;
Ryosei 27:dd9f27fce7d1 2843 case 0:
Ryosei 27:dd9f27fce7d1 2844 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2845 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2846 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2847 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2848 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2849 break;
Ryosei 27:dd9f27fce7d1 2850 case 1:
Ryosei 27:dd9f27fce7d1 2851 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2852 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 2853 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2854 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 2855 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2856 break;
Ryosei 27:dd9f27fce7d1 2857 case 3:
Ryosei 27:dd9f27fce7d1 2858 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2859 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 2860 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2861 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 2862 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2863 break;
Ryosei 27:dd9f27fce7d1 2864 case 2:
Ryosei 27:dd9f27fce7d1 2865 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2866 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 2867 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2868 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 2869 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2870 break;
Ryosei 27:dd9f27fce7d1 2871 case 'A':
Ryosei 27:dd9f27fce7d1 2872 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 2873 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 2874 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 2875 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 2876 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2877 break;
Ryosei 27:dd9f27fce7d1 2878 case 'N':
Ryosei 27:dd9f27fce7d1 2879 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2880 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2881 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2882 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2883 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2884 break;
Ryosei 27:dd9f27fce7d1 2885 default:
Ryosei 27:dd9f27fce7d1 2886 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 2887 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 2888 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 2889 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 2890 adjAnable = false;
Ryosei 27:dd9f27fce7d1 2891 }
Ryosei 27:dd9f27fce7d1 2892
Ryosei 27:dd9f27fce7d1 2893 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2894 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2895 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2896 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2897 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 2898 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 2899 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 2900 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 2901 }
Ryosei 27:dd9f27fce7d1 2902 } else if(lineFase == 7) { // ライン 修正
Ryosei 27:dd9f27fce7d1 2903 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
Ryosei 27:dd9f27fce7d1 2904 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2905 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2906 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2907 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2908 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2909 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2910 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2911 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2912 } else if(linePara[LINE_TOW_1] > 0) {
Ryosei 27:dd9f27fce7d1 2913 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2914 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2915 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2916 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2917 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2918 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2919 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2920 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2921 } else if(linePara[LINE_TOW_1] < 0) {
Ryosei 27:dd9f27fce7d1 2922 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 2923 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2924 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 2925 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 2926 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 2927 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2928 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 2929 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 2930 } else if(linePara[LINE_TOW_1] == 0) {
Ryosei 27:dd9f27fce7d1 2931 //tow_stop.reset();
Ryosei 27:dd9f27fce7d1 2932 //tow_stop.start();
Ryosei 27:dd9f27fce7d1 2933 lineFase = 8;
Ryosei 27:dd9f27fce7d1 2934 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2935 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2936 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2937 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2938 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2939 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2940 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2941 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2942 } else {
Ryosei 27:dd9f27fce7d1 2943 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2944 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2945 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2946 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 2947 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2948 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2949 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2950 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 2951 }
Ryosei 27:dd9f27fce7d1 2952 } else if(lineFase == 8) { // タオル1 解放
Ryosei 27:dd9f27fce7d1 2953 Air[TOWEL1] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 2954 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2955 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2956 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2957 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 2958 lineFase = 9;
Ryosei 27:dd9f27fce7d1 2959 } else if(lineFase == 9) { // 前
Ryosei 27:dd9f27fce7d1 2960 switch(linePara[LINE_TOW_1]) {
Ryosei 27:dd9f27fce7d1 2961 case 2:
Ryosei 27:dd9f27fce7d1 2962 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 2963 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2964 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 2965 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2966 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2967 break;
Ryosei 27:dd9f27fce7d1 2968 case 3:
Ryosei 27:dd9f27fce7d1 2969 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 2970 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2971 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 2972 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2973 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2974 break;
Ryosei 27:dd9f27fce7d1 2975 case 1:
Ryosei 27:dd9f27fce7d1 2976 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 2977 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2978 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 2979 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2980 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2981 break;
Ryosei 27:dd9f27fce7d1 2982 case 0:
Ryosei 27:dd9f27fce7d1 2983 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2984 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 2985 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2986 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 2987 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2988 break;
Ryosei 27:dd9f27fce7d1 2989 case -1:
Ryosei 27:dd9f27fce7d1 2990 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2991 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 2992 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 2993 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 2994 adjAnable = true;
Ryosei 27:dd9f27fce7d1 2995 break;
Ryosei 27:dd9f27fce7d1 2996 case -3:
Ryosei 27:dd9f27fce7d1 2997 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 2998 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 2999 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3000 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 3001 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3002 break;
Ryosei 27:dd9f27fce7d1 3003 case -2:
Ryosei 27:dd9f27fce7d1 3004 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3005 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3006 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3007 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 3008 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3009 break;
Ryosei 27:dd9f27fce7d1 3010 case 'A':
Ryosei 27:dd9f27fce7d1 3011 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3012 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3013 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3014 countW++;
Ryosei 27:dd9f27fce7d1 3015 }
Ryosei 27:dd9f27fce7d1 3016 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3017 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3018 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3019 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3020 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3021 break;
Ryosei 27:dd9f27fce7d1 3022 case 'N':
Ryosei 27:dd9f27fce7d1 3023 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3024 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3025 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3026 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3027 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3028 break;
Ryosei 27:dd9f27fce7d1 3029 default:
Ryosei 27:dd9f27fce7d1 3030 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3031 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3032 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3033 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3034 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3035 }
Ryosei 27:dd9f27fce7d1 3036
Ryosei 27:dd9f27fce7d1 3037 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3038 adj = 0;
Ryosei 27:dd9f27fce7d1 3039 } else {
Ryosei 27:dd9f27fce7d1 3040 adj = 0;
Ryosei 27:dd9f27fce7d1 3041 }
Ryosei 27:dd9f27fce7d1 3042
Ryosei 27:dd9f27fce7d1 3043 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3044 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3045 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3046 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3047 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3048 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3049 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3050 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3051
Ryosei 27:dd9f27fce7d1 3052 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3053 lineCount++;
Ryosei 27:dd9f27fce7d1 3054 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3055 }
Ryosei 27:dd9f27fce7d1 3056 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 3057 countW = 0;
Ryosei 27:dd9f27fce7d1 3058 lineFase = 10;
Ryosei 27:dd9f27fce7d1 3059 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3060 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3061 }
Ryosei 27:dd9f27fce7d1 3062 } else if(lineFase == 10) { // 前 低速
Ryosei 27:dd9f27fce7d1 3063 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3064 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3065 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3066 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3067 if(linePara[3] == 0) {
Ryosei 27:dd9f27fce7d1 3068 lineFase = 11;
Ryosei 27:dd9f27fce7d1 3069 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3070 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3071 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3072 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3073 }
Ryosei 27:dd9f27fce7d1 3074 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3075 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3076 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3077 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3078 } else if(lineFase == 11) { // 左
Ryosei 27:dd9f27fce7d1 3079 switch(linePara[3]) {
Ryosei 27:dd9f27fce7d1 3080 case -2:
Ryosei 27:dd9f27fce7d1 3081 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3082 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3083 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3084 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3085 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3086 break;
Ryosei 27:dd9f27fce7d1 3087 case -3:
Ryosei 27:dd9f27fce7d1 3088 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3089 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3090 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3091 tirePWM[TIRE_FR] = -10;
Ryosei 27:dd9f27fce7d1 3092 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3093 break;
Ryosei 27:dd9f27fce7d1 3094 case -1:
Ryosei 27:dd9f27fce7d1 3095 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3096 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3097 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3098 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3099 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3100 break;
Ryosei 27:dd9f27fce7d1 3101 case 0:
Ryosei 27:dd9f27fce7d1 3102 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3103 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3104 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3105 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3106 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3107 break;
Ryosei 27:dd9f27fce7d1 3108 case 1:
Ryosei 27:dd9f27fce7d1 3109 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3110 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3111 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3112 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3113 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3114 break;
Ryosei 27:dd9f27fce7d1 3115 case 3:
Ryosei 27:dd9f27fce7d1 3116 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 3117 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3118 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 3119 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3120 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3121 break;
Ryosei 27:dd9f27fce7d1 3122 case 2:
Ryosei 27:dd9f27fce7d1 3123 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3124 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3125 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3126 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3127 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3128 break;
Ryosei 27:dd9f27fce7d1 3129 case 'A':
Ryosei 27:dd9f27fce7d1 3130 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3131 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3132 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3133 countW++;
Ryosei 27:dd9f27fce7d1 3134 }
Ryosei 27:dd9f27fce7d1 3135 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 3136 tirePWM[TIRE_BL] = 30;
Ryosei 27:dd9f27fce7d1 3137 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 3138 tirePWM[TIRE_FR] = -30;
Ryosei 27:dd9f27fce7d1 3139 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3140 break;
Ryosei 27:dd9f27fce7d1 3141 case 'N':
Ryosei 27:dd9f27fce7d1 3142 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3143 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3144 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3145 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3146 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3147 break;
Ryosei 27:dd9f27fce7d1 3148 default:
Ryosei 27:dd9f27fce7d1 3149 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3150 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3151 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3152 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3153 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3154 }
Ryosei 27:dd9f27fce7d1 3155
Ryosei 27:dd9f27fce7d1 3156 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3157 if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei 27:dd9f27fce7d1 3158 } else {
Ryosei 27:dd9f27fce7d1 3159 adj = 0;
Ryosei 27:dd9f27fce7d1 3160 }
Ryosei 27:dd9f27fce7d1 3161
Ryosei 27:dd9f27fce7d1 3162 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3163 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3164 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3165 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 3166 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3167 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3168 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3169 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 3170 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3171 lineCount++;
Ryosei 27:dd9f27fce7d1 3172 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3173 }
Ryosei 27:dd9f27fce7d1 3174 if(countW == 2) {
Ryosei 27:dd9f27fce7d1 3175 countW = 0;
Ryosei 27:dd9f27fce7d1 3176 lineFase = 12;
Ryosei 27:dd9f27fce7d1 3177 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3178 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3179 }
Ryosei 27:dd9f27fce7d1 3180 } else if(lineFase == 12) { // 左 低速
Ryosei 27:dd9f27fce7d1 3181 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3182 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3183 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3184 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3185 if(linePara[LINE_TOW_2] == 0) {
Ryosei 27:dd9f27fce7d1 3186 lineFase = 13;
Ryosei 27:dd9f27fce7d1 3187 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3188 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3189 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3190 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3191 }
Ryosei 27:dd9f27fce7d1 3192 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3193 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3194 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3195 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3196 } else if (lineFase == 13) {
Ryosei 27:dd9f27fce7d1 3197 lineFase = 14;
Ryosei 27:dd9f27fce7d1 3198 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3199 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3200 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3201 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3202 } else if(lineFase == 14) { // タオル2 竿検知
Ryosei 27:dd9f27fce7d1 3203 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
Ryosei 27:dd9f27fce7d1 3204 lineFase = 15;
Ryosei 27:dd9f27fce7d1 3205 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3206 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3207 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3208 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3209 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3210 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3211 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3212 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3213 } else if(LimitSw::IsPressed(TOW_2L)) {
Ryosei 27:dd9f27fce7d1 3214 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3215 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 3216 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3217 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3218 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3219 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3220 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3221 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 3222 } else if(LimitSw::IsPressed(TOW_2R)) {
Ryosei 27:dd9f27fce7d1 3223 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3224 motor[TIRE_FL].pwm = 20;
Ryosei 27:dd9f27fce7d1 3225 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3226 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3227 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3228 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3229 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3230 motor[TIRE_FR].pwm = 20;
Ryosei 27:dd9f27fce7d1 3231 } else {
Ryosei 27:dd9f27fce7d1 3232 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 3233 case -2:
Ryosei 27:dd9f27fce7d1 3234 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 3235 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3236 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 3237 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3238 break;
Ryosei 27:dd9f27fce7d1 3239 case -3:
Ryosei 27:dd9f27fce7d1 3240 tirePWM[TIRE_FL] = -14;
Ryosei 27:dd9f27fce7d1 3241 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3242 tirePWM[TIRE_BR] = 14;
Ryosei 27:dd9f27fce7d1 3243 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3244 break;
Ryosei 27:dd9f27fce7d1 3245 case -1:
Ryosei 27:dd9f27fce7d1 3246 tirePWM[TIRE_FL] = -17;
Ryosei 27:dd9f27fce7d1 3247 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3248 tirePWM[TIRE_BR] = 17;
Ryosei 27:dd9f27fce7d1 3249 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3250 break;
Ryosei 27:dd9f27fce7d1 3251 case 0:
Ryosei 27:dd9f27fce7d1 3252 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3253 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3254 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3255 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3256 break;
Ryosei 27:dd9f27fce7d1 3257 case 1:
Ryosei 27:dd9f27fce7d1 3258 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3259 tirePWM[TIRE_BL] = -17;
Ryosei 27:dd9f27fce7d1 3260 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3261 tirePWM[TIRE_FR] = 17;
Ryosei 27:dd9f27fce7d1 3262 break;
Ryosei 27:dd9f27fce7d1 3263 case 3:
Ryosei 27:dd9f27fce7d1 3264 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3265 tirePWM[TIRE_BL] = -14;
Ryosei 27:dd9f27fce7d1 3266 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3267 tirePWM[TIRE_FR] = 14;
Ryosei 27:dd9f27fce7d1 3268 break;
Ryosei 27:dd9f27fce7d1 3269 case 2:
Ryosei 27:dd9f27fce7d1 3270 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3271 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 3272 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3273 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3274 break;
Ryosei 27:dd9f27fce7d1 3275 case 'A':
Ryosei 27:dd9f27fce7d1 3276
Ryosei 27:dd9f27fce7d1 3277 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 3278 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3279 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 3280 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3281 break;
Ryosei 27:dd9f27fce7d1 3282 case 'N':
Ryosei 27:dd9f27fce7d1 3283 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3284 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3285 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3286 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3287 break;
Ryosei 27:dd9f27fce7d1 3288 default:
Ryosei 27:dd9f27fce7d1 3289 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3290 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3291 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3292 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3293 }
Ryosei 27:dd9f27fce7d1 3294
Ryosei 27:dd9f27fce7d1 3295 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3296 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3297 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 3298 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3299 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3300 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 3301 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 3302 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3303 }
Ryosei 27:dd9f27fce7d1 3304 } else if(lineFase == 15 ) { // ライン 修正
Ryosei 27:dd9f27fce7d1 3305 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
Ryosei 27:dd9f27fce7d1 3306 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3307 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3308 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3309 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3310 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3311 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3312 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3313 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3314 } else if(linePara[LINE_TOW_2] > 0) {
Ryosei 27:dd9f27fce7d1 3315 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3316 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3317 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3318 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3319 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3320 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3321 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3322 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3323 } else if(linePara[LINE_TOW_2] < 0) {
Ryosei 27:dd9f27fce7d1 3324 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3325 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3326 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 3327 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3328 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3329 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3330 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 3331 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3332 } else if(linePara[LINE_TOW_2] == 0) {
Ryosei 27:dd9f27fce7d1 3333 lineFase = 16;
Ryosei 27:dd9f27fce7d1 3334 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3335 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3336 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3337 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3338 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3339 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3340 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3341 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3342 } else {
Ryosei 27:dd9f27fce7d1 3343 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3344 motor[TIRE_FL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3345 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3346 motor[TIRE_BL].pwm = 50;
Ryosei 27:dd9f27fce7d1 3347 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3348 motor[TIRE_BR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3349 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3350 motor[TIRE_FR].pwm = 50;
Ryosei 27:dd9f27fce7d1 3351 }
Ryosei 27:dd9f27fce7d1 3352 } else if(lineFase == 16) { // タオル2 解放
Ryosei 27:dd9f27fce7d1 3353 Air[TOWEL2] = SOLENOID_ON;
Ryosei 27:dd9f27fce7d1 3354 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3355 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3356 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3357 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3358 lineFase = 17;
Ryosei 27:dd9f27fce7d1 3359 } else if(lineFase == 17) { // 前
Ryosei 27:dd9f27fce7d1 3360 switch(linePara[LINE_TOW_2]) {
Ryosei 27:dd9f27fce7d1 3361 case 2:
Ryosei 27:dd9f27fce7d1 3362 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 3363 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3364 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 3365 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3366 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3367 break;
Ryosei 27:dd9f27fce7d1 3368 case 3:
Ryosei 27:dd9f27fce7d1 3369 tirePWM[TIRE_FL] = 14;
Ryosei 27:dd9f27fce7d1 3370 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3371 tirePWM[TIRE_BR] = -14;
Ryosei 27:dd9f27fce7d1 3372 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3373 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3374 break;
Ryosei 27:dd9f27fce7d1 3375 case 1:
Ryosei 27:dd9f27fce7d1 3376 tirePWM[TIRE_FL] = 17;
Ryosei 27:dd9f27fce7d1 3377 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3378 tirePWM[TIRE_BR] = -17;
Ryosei 27:dd9f27fce7d1 3379 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3380 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3381 break;
Ryosei 27:dd9f27fce7d1 3382 case 0:
Ryosei 27:dd9f27fce7d1 3383 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3384 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3385 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3386 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3387 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3388 break;
Ryosei 27:dd9f27fce7d1 3389 case -1:
Ryosei 27:dd9f27fce7d1 3390 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3391 tirePWM[TIRE_BL] = 17;
Ryosei 27:dd9f27fce7d1 3392 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3393 tirePWM[TIRE_FR] = -17;
Ryosei 27:dd9f27fce7d1 3394 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3395 break;
Ryosei 27:dd9f27fce7d1 3396 case -3:
Ryosei 27:dd9f27fce7d1 3397 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3398 tirePWM[TIRE_BL] = 14;
Ryosei 27:dd9f27fce7d1 3399 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3400 tirePWM[TIRE_FR] = -14;
Ryosei 27:dd9f27fce7d1 3401 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3402 break;
Ryosei 27:dd9f27fce7d1 3403 case -2:
Ryosei 27:dd9f27fce7d1 3404 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3405 tirePWM[TIRE_BL] = 10;
Ryosei 27:dd9f27fce7d1 3406 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3407 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3408 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3409 break;
Ryosei 27:dd9f27fce7d1 3410 case 'A':
Ryosei 27:dd9f27fce7d1 3411 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3412 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3413 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3414 countW++;
Ryosei 27:dd9f27fce7d1 3415 }
Ryosei 27:dd9f27fce7d1 3416 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3417 tirePWM[TIRE_BL] = 20;
Ryosei 27:dd9f27fce7d1 3418 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3419 tirePWM[TIRE_FR] = -20;
Ryosei 27:dd9f27fce7d1 3420 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3421 break;
Ryosei 27:dd9f27fce7d1 3422 case 'N':
Ryosei 27:dd9f27fce7d1 3423 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3424 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3425 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3426 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3427 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3428 break;
Ryosei 27:dd9f27fce7d1 3429 default:
Ryosei 27:dd9f27fce7d1 3430 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3431 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3432 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3433 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3434 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3435 }
Ryosei 27:dd9f27fce7d1 3436
Ryosei 27:dd9f27fce7d1 3437 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3438 adj = 0;
Ryosei 27:dd9f27fce7d1 3439 } else {
Ryosei 27:dd9f27fce7d1 3440 adj = 0;
Ryosei 27:dd9f27fce7d1 3441 }
Ryosei 27:dd9f27fce7d1 3442
Ryosei 27:dd9f27fce7d1 3443 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3444 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3445 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3446 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3447 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei 27:dd9f27fce7d1 3448 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3449 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei 27:dd9f27fce7d1 3450 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3451
Ryosei 27:dd9f27fce7d1 3452 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 3453 lineCount++;
Ryosei 27:dd9f27fce7d1 3454 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 3455 }
Ryosei 27:dd9f27fce7d1 3456 if(countW == 1) {
Ryosei 27:dd9f27fce7d1 3457 countW = 0;
Ryosei 27:dd9f27fce7d1 3458 lineFase = 100;
Ryosei 27:dd9f27fce7d1 3459 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3460 lineCheck = false;
Ryosei 27:dd9f27fce7d1 3461 }
Ryosei 27:dd9f27fce7d1 3462 } else if(lineFase==100) {
Ryosei 27:dd9f27fce7d1 3463 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3464 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3465 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3466 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3467 if(LimitSw::IsPressed(LSW_UU)) {
Ryosei 27:dd9f27fce7d1 3468 motor[LIFT_U].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3469 motor[LIFT_U].pwm=100;
Ryosei 27:dd9f27fce7d1 3470 } else {
Ryosei 27:dd9f27fce7d1 3471 motor[LIFT_U].dir=BACK;
Ryosei 27:dd9f27fce7d1 3472 motor[LIFT_U].pwm=150;
Ryosei 27:dd9f27fce7d1 3473 }
Ryosei 27:dd9f27fce7d1 3474 if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
Ryosei 27:dd9f27fce7d1 3475 lineFase=101;
Ryosei 27:dd9f27fce7d1 3476 }
Ryosei 27:dd9f27fce7d1 3477 } else if(lineFase==101) {
Ryosei 27:dd9f27fce7d1 3478 //位置調整
Ryosei 27:dd9f27fce7d1 3479 //(P制御)
Ryosei 27:dd9f27fce7d1 3480 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3481 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 3482 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3483 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3484 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3485 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3486 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3487 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3488 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3489 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3490 lineFase=102;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 3491 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 3492 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 3493 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3494 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3495 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3496 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3497 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3498 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3499 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3500 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3501 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 3502 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3503 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3504 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3505 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3506 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3507 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3508 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3509 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3510 }
Ryosei 27:dd9f27fce7d1 3511 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 3512 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 3513 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3514 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3515 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3516 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3517 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3518 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3519 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3520 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3521 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 3522 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3523 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3524 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3525 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3526 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3527 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3528 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3529 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3530 }
Ryosei 27:dd9f27fce7d1 3531 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 3532 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 3533 if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3534 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3535 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3536 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3537 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3538 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3539 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3540 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3541 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3542 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3543 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3544 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3545 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3546 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3547 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3548 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3549 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3550 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3551 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3552 }
Ryosei 27:dd9f27fce7d1 3553 } else { //傾きが大きくなくて離れているとき
Ryosei 27:dd9f27fce7d1 3554 if((Ult_right+Ult_left)<=25) { //近すぎるとき
Ryosei 27:dd9f27fce7d1 3555 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3556 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3557 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3558 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3559 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 3560 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 3561 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 3562 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 3563 } else if((Ult_right+Ult_left)>=35) { //離れているとき
Ryosei 27:dd9f27fce7d1 3564 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3565 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3566 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3567 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3568 motor[TIRE_FL].pwm=20;
Ryosei 27:dd9f27fce7d1 3569 motor[TIRE_FR].pwm=20;
Ryosei 27:dd9f27fce7d1 3570 motor[TIRE_BL].pwm=20;
Ryosei 27:dd9f27fce7d1 3571 motor[TIRE_BR].pwm=20;
Ryosei 27:dd9f27fce7d1 3572 }
Ryosei 27:dd9f27fce7d1 3573 }
Ryosei 27:dd9f27fce7d1 3574
Ryosei 27:dd9f27fce7d1 3575 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3576 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3577 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3578 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3579 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3580 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3581 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3582 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3583 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3584 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3585 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3586 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3587 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3588 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3589 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3590 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3591 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3592 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3593 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3594 }
Ryosei 27:dd9f27fce7d1 3595 }
Ryosei 27:dd9f27fce7d1 3596 }
Ryosei 27:dd9f27fce7d1 3597
Ryosei 27:dd9f27fce7d1 3598 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 3599 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3600 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3601 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3602 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3603 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3604 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3605 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3606 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3607 }
Ryosei 27:dd9f27fce7d1 3608
Ryosei 27:dd9f27fce7d1 3609 } else if(lineFase==102) {
Ryosei 27:dd9f27fce7d1 3610 Air[CLOTHESPIN]=SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 3611 LedOut(1);
Ryosei 27:dd9f27fce7d1 3612 //リミットスイッチに当てる
Ryosei 27:dd9f27fce7d1 3613 static int count2=0;
Ryosei 27:dd9f27fce7d1 3614 if(count2==0) {
Ryosei 27:dd9f27fce7d1 3615 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3616 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {
Ryosei 27:dd9f27fce7d1 3617 if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
Ryosei 27:dd9f27fce7d1 3618 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 3619 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3620 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3621 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3622 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3623
Ryosei 27:dd9f27fce7d1 3624 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3625 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3626
Ryosei 27:dd9f27fce7d1 3627 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3628 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3629 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3630 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3631 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3632 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3633 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3634 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3635 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3636 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3637 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3638 }
Ryosei 27:dd9f27fce7d1 3639 } else if((Ult_left+Ult_right)<=25) {
Ryosei 27:dd9f27fce7d1 3640 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3641 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3642 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3643 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3644 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3645 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3646 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3647 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3648 } else if((Ult_left+Ult_right)>=35) {
Ryosei 27:dd9f27fce7d1 3649 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3650 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3651 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3652 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3653 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3654 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3655 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3656 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3657 }
Ryosei 27:dd9f27fce7d1 3658 } else {
Ryosei 27:dd9f27fce7d1 3659 if(Ult_left-Ult_right<=0) {
Ryosei 27:dd9f27fce7d1 3660 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3661 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3662 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3663 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3664
Ryosei 27:dd9f27fce7d1 3665 motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3666 motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3667
Ryosei 27:dd9f27fce7d1 3668 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3669 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3670 } else if(Ult_left-Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3671 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3672 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3673 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3674 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3675 motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3676 motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3677 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3678 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3679 }
Ryosei 27:dd9f27fce7d1 3680 }
Ryosei 27:dd9f27fce7d1 3681
Ryosei 27:dd9f27fce7d1 3682 } else {
Ryosei 27:dd9f27fce7d1 3683 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3684 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3685 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3686 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3687 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3688 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3689 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3690 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3691
Ryosei 27:dd9f27fce7d1 3692 }
Ryosei 27:dd9f27fce7d1 3693 if(LimitSw::IsPressed(LEFTlim)) {
Ryosei 27:dd9f27fce7d1 3694 count2=1;
Ryosei 27:dd9f27fce7d1 3695 }
Ryosei 27:dd9f27fce7d1 3696
Ryosei 27:dd9f27fce7d1 3697 } else if(count2==1) {
Ryosei 27:dd9f27fce7d1 3698 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3699 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3700 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3701 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3702 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 3703 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 3704 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 3705 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 3706 lineFase=103;
Ryosei 27:dd9f27fce7d1 3707 }
Ryosei 27:dd9f27fce7d1 3708 } else if(lineFase==103) {
Ryosei 27:dd9f27fce7d1 3709 if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei 27:dd9f27fce7d1 3710 if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei 27:dd9f27fce7d1 3711 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3712 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3713 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3714 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3715 motor[TIRE_FL].pwm=255;
Ryosei 27:dd9f27fce7d1 3716 motor[TIRE_FR].pwm=255;
Ryosei 27:dd9f27fce7d1 3717 motor[TIRE_BL].pwm=255;
Ryosei 27:dd9f27fce7d1 3718 motor[TIRE_BR].pwm=255;
Ryosei 27:dd9f27fce7d1 3719 lineFase=104;//system lineFase increasing
Ryosei 27:dd9f27fce7d1 3720 } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei 27:dd9f27fce7d1 3721 if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei 27:dd9f27fce7d1 3722 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3723 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3724 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3725 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3726 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3727 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 3728 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3729 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 3730 } else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei 27:dd9f27fce7d1 3731 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3732 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3733 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3734 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3735 motor[TIRE_FL].pwm=0;
Ryosei 27:dd9f27fce7d1 3736 motor[TIRE_FR].pwm=13;
Ryosei 27:dd9f27fce7d1 3737 motor[TIRE_BL].pwm=0;
Ryosei 27:dd9f27fce7d1 3738 motor[TIRE_BR].pwm=13;
Ryosei 27:dd9f27fce7d1 3739 }
Ryosei 27:dd9f27fce7d1 3740 } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei 27:dd9f27fce7d1 3741 if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei 27:dd9f27fce7d1 3742 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3743 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3744 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3745 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3746 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 3747 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3748 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 3749 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3750 } else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei 27:dd9f27fce7d1 3751 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3752 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3753 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3754 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3755 motor[TIRE_FL].pwm=13;
Ryosei 27:dd9f27fce7d1 3756 motor[TIRE_FR].pwm=0;
Ryosei 27:dd9f27fce7d1 3757 motor[TIRE_BL].pwm=13;
Ryosei 27:dd9f27fce7d1 3758 motor[TIRE_BR].pwm=0;
Ryosei 27:dd9f27fce7d1 3759 }
Ryosei 27:dd9f27fce7d1 3760 } else { //どっちもあってない場合
Ryosei 27:dd9f27fce7d1 3761 if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei 27:dd9f27fce7d1 3762 if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3763 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3764 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3765 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3766 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3767 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3768 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3769 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3770 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3771 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3772 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3773 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3774 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3775 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3776 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3777 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3778 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3779 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3780 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3781 }
Ryosei 27:dd9f27fce7d1 3782 }
Ryosei 27:dd9f27fce7d1 3783 } else { //さほど離れてはいないが傾きが大きいとき
Ryosei 27:dd9f27fce7d1 3784 if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3785 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3786 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3787 motor[TIRE_BL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3788 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3789 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3790 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3791 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3792 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3793 } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei 27:dd9f27fce7d1 3794 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3795 motor[TIRE_FR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3796 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3797 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3798 motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3799 motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3800 motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3801 motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei 27:dd9f27fce7d1 3802 }
Ryosei 27:dd9f27fce7d1 3803 }
Ryosei 27:dd9f27fce7d1 3804 }
Ryosei 27:dd9f27fce7d1 3805
Ryosei 27:dd9f27fce7d1 3806 } else {//データを受け取ってないとき
Ryosei 27:dd9f27fce7d1 3807 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3808 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3809 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3810 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3811 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3812 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3813 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3814 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3815 }
Ryosei 27:dd9f27fce7d1 3816 } else if(lineFase==104) {
Ryosei 27:dd9f27fce7d1 3817 static int count3=0;
Ryosei 27:dd9f27fce7d1 3818 static int loop=0;
Ryosei 27:dd9f27fce7d1 3819 pc.printf("%d\r\n",loop);
Ryosei 27:dd9f27fce7d1 3820 Air[CLOTHESPIN]=1;
Ryosei 27:dd9f27fce7d1 3821 if(count3==0) {
Ryosei 27:dd9f27fce7d1 3822 loop++;
Ryosei 27:dd9f27fce7d1 3823 if(loop==60) {
Ryosei 27:dd9f27fce7d1 3824 count3=1;
Ryosei 27:dd9f27fce7d1 3825 }
Ryosei 27:dd9f27fce7d1 3826 } else if(count3==1) {
Ryosei 27:dd9f27fce7d1 3827 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 3828 lineFase=105;
Ryosei 27:dd9f27fce7d1 3829
Ryosei 27:dd9f27fce7d1 3830 }
Ryosei 27:dd9f27fce7d1 3831 } else if(lineFase==105) {
Ryosei 27:dd9f27fce7d1 3832 LedOut(6);
Ryosei 27:dd9f27fce7d1 3833 Air[CLOTHESPIN]=0;
Ryosei 27:dd9f27fce7d1 3834 if(!(LimitSw::IsPressed(RIGHTlim))) {
Ryosei 27:dd9f27fce7d1 3835
Ryosei 27:dd9f27fce7d1 3836 //超音波
Ryosei 27:dd9f27fce7d1 3837 if(Ult_right>0) {
Ryosei 27:dd9f27fce7d1 3838 if(Ult_right<15) {
Ryosei 27:dd9f27fce7d1 3839 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3840 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3841 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3842 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3843 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3844 motor[TIRE_FR].pwm=25+(15-Ult_right);
Ryosei 27:dd9f27fce7d1 3845 motor[TIRE_BL].pwm=25+(15-Ult_right);
Ryosei 27:dd9f27fce7d1 3846 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3847 } else if(Ult_right>=15) {
Ryosei 27:dd9f27fce7d1 3848 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3849 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3850 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3851 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3852 motor[TIRE_FL].pwm=25+(Ult_right-15);
Ryosei 27:dd9f27fce7d1 3853 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3854 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3855 motor[TIRE_BR].pwm=25+(Ult_right-15);
Ryosei 27:dd9f27fce7d1 3856 }
Ryosei 27:dd9f27fce7d1 3857 } else {
Ryosei 27:dd9f27fce7d1 3858 motor[TIRE_FL].dir=FOR;
Ryosei 27:dd9f27fce7d1 3859 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3860 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3861 motor[TIRE_BR].dir=BACK;
Ryosei 27:dd9f27fce7d1 3862 motor[TIRE_FL].pwm=25;
Ryosei 27:dd9f27fce7d1 3863 motor[TIRE_FR].pwm=25;
Ryosei 27:dd9f27fce7d1 3864 motor[TIRE_BL].pwm=25;
Ryosei 27:dd9f27fce7d1 3865 motor[TIRE_BR].pwm=25;
Ryosei 27:dd9f27fce7d1 3866 }
Ryosei 27:dd9f27fce7d1 3867
Ryosei 27:dd9f27fce7d1 3868
Ryosei 27:dd9f27fce7d1 3869 } else {
Ryosei 27:dd9f27fce7d1 3870 motor[TIRE_FL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3871 motor[TIRE_FR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3872 motor[TIRE_BL].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3873 motor[TIRE_BR].dir=BRAKE;
Ryosei 27:dd9f27fce7d1 3874 motor[TIRE_FL].pwm=100;
Ryosei 27:dd9f27fce7d1 3875 motor[TIRE_FR].pwm=100;
Ryosei 27:dd9f27fce7d1 3876 motor[TIRE_BL].pwm=100;
Ryosei 27:dd9f27fce7d1 3877 motor[TIRE_BR].pwm=100;
Ryosei 27:dd9f27fce7d1 3878 lineFase=106;
Ryosei 27:dd9f27fce7d1 3879 }
Ryosei 27:dd9f27fce7d1 3880
Ryosei 27:dd9f27fce7d1 3881 } else if(lineFase==106) {
Ryosei 27:dd9f27fce7d1 3882 motor[TIRE_FL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3883 motor[TIRE_FR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3884 motor[TIRE_BL].dir=BACK;
Ryosei 27:dd9f27fce7d1 3885 motor[TIRE_BR].dir=FOR;
Ryosei 27:dd9f27fce7d1 3886 motor[TIRE_FL].pwm=15;
Ryosei 27:dd9f27fce7d1 3887 motor[TIRE_FR].pwm=15;
Ryosei 27:dd9f27fce7d1 3888 motor[TIRE_BL].pwm=15;
Ryosei 27:dd9f27fce7d1 3889 motor[TIRE_BR].pwm=15;
Ryosei 27:dd9f27fce7d1 3890 if(linePara[4]!='N') {
Ryosei 27:dd9f27fce7d1 3891 lineFase=19;
Ryosei 27:dd9f27fce7d1 3892 }
Ryosei 27:dd9f27fce7d1 3893 } else if(lineFase == 18) { // 前 低速
Ryosei 27:dd9f27fce7d1 3894 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3895 motor[TIRE_BL].dir = FOR;
Ryosei 27:dd9f27fce7d1 3896 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3897 motor[TIRE_FR].dir = BACK;
Ryosei 27:dd9f27fce7d1 3898 if(linePara[4] == 0) {
Ryosei 27:dd9f27fce7d1 3899 lineFase = 19;
Ryosei 27:dd9f27fce7d1 3900 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3901 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3902 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3903 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 3904 }
Ryosei 27:dd9f27fce7d1 3905 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3906 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 3907 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3908 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 3909 } else if(lineFase == 19) { // 右
Ryosei 27:dd9f27fce7d1 3910 switch(linePara[4]) {
Ryosei 27:dd9f27fce7d1 3911 case -2:
Ryosei 27:dd9f27fce7d1 3912 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3913 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3914 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3915 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3916 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3917 break;
Ryosei 27:dd9f27fce7d1 3918 case -3:
Ryosei 27:dd9f27fce7d1 3919 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3920 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 3921 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3922 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 3923 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3924 break;
Ryosei 27:dd9f27fce7d1 3925 case -1:
Ryosei 27:dd9f27fce7d1 3926 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3927 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 3928 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3929 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 3930 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3931 break;
Ryosei 27:dd9f27fce7d1 3932 case 0:
Ryosei 27:dd9f27fce7d1 3933 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3934 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3935 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3936 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3937 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3938 break;
Ryosei 27:dd9f27fce7d1 3939 case 1:
Ryosei 27:dd9f27fce7d1 3940 tirePWM[TIRE_FL] = 20;
Ryosei 27:dd9f27fce7d1 3941 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3942 tirePWM[TIRE_BR] = -20;
Ryosei 27:dd9f27fce7d1 3943 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3944 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3945 break;
Ryosei 27:dd9f27fce7d1 3946 case 3:
Ryosei 27:dd9f27fce7d1 3947 tirePWM[TIRE_FL] = 10;
Ryosei 27:dd9f27fce7d1 3948 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3949 tirePWM[TIRE_BR] = -10;
Ryosei 27:dd9f27fce7d1 3950 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3951 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3952 break;
Ryosei 27:dd9f27fce7d1 3953 case 2:
Ryosei 27:dd9f27fce7d1 3954 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3955 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3956 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3957 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3958 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3959 break;
Ryosei 27:dd9f27fce7d1 3960 case 'A':
Ryosei 27:dd9f27fce7d1 3961 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 3962 lineCheck = true;
Ryosei 27:dd9f27fce7d1 3963 lineCount = 0;
Ryosei 27:dd9f27fce7d1 3964 countW++;
Ryosei 27:dd9f27fce7d1 3965 }
Ryosei 27:dd9f27fce7d1 3966 tirePWM[TIRE_FL] = 30;
Ryosei 27:dd9f27fce7d1 3967 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 3968 tirePWM[TIRE_BR] = -30;
Ryosei 27:dd9f27fce7d1 3969 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 3970 adjAnable = true;
Ryosei 27:dd9f27fce7d1 3971 break;
Ryosei 27:dd9f27fce7d1 3972 case 'N':
Ryosei 27:dd9f27fce7d1 3973 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3974 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3975 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3976 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3977 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3978 break;
Ryosei 27:dd9f27fce7d1 3979 default:
Ryosei 27:dd9f27fce7d1 3980 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 3981 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 3982 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 3983 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 3984 adjAnable = false;
Ryosei 27:dd9f27fce7d1 3985 }
Ryosei 27:dd9f27fce7d1 3986
Ryosei 27:dd9f27fce7d1 3987 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 3988 if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
Ryosei 27:dd9f27fce7d1 3989 } else {
Ryosei 27:dd9f27fce7d1 3990 adj = 0;
Ryosei 27:dd9f27fce7d1 3991 }
Ryosei 27:dd9f27fce7d1 3992
Ryosei 27:dd9f27fce7d1 3993 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 3994 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3995 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 3996 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 3997 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 3998 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei 27:dd9f27fce7d1 3999 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4000 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei 27:dd9f27fce7d1 4001
Ryosei 27:dd9f27fce7d1 4002 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 4003 lineCount++;
Ryosei 27:dd9f27fce7d1 4004 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 4005 }
Ryosei 27:dd9f27fce7d1 4006
Ryosei 27:dd9f27fce7d1 4007 /*
Ryosei 27:dd9f27fce7d1 4008 if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
Ryosei 27:dd9f27fce7d1 4009 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4010 } else if(LimitSw::IsPressed(TOWEL1_SW)) {
Ryosei 27:dd9f27fce7d1 4011 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4012 } else {
Ryosei 27:dd9f27fce7d1 4013 targetCount = 2;
Ryosei 27:dd9f27fce7d1 4014 }
Ryosei 27:dd9f27fce7d1 4015 */
Ryosei 27:dd9f27fce7d1 4016
Ryosei 27:dd9f27fce7d1 4017 targetCount = 3;
Ryosei 27:dd9f27fce7d1 4018
Ryosei 27:dd9f27fce7d1 4019 if(countW == targetCount) {
Ryosei 27:dd9f27fce7d1 4020 countW = 0;
Ryosei 27:dd9f27fce7d1 4021 lineFase = 20;
Ryosei 27:dd9f27fce7d1 4022 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4023 lineCheck = false;
Ryosei 27:dd9f27fce7d1 4024 }
Ryosei 27:dd9f27fce7d1 4025 } else if(lineFase == 20) { // 右 低速
Ryosei 27:dd9f27fce7d1 4026 motor[TIRE_FL].dir = FOR;
Ryosei 27:dd9f27fce7d1 4027 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4028 motor[TIRE_BR].dir = BACK;
Ryosei 27:dd9f27fce7d1 4029 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4030 if (linePara[2] == 0) {
Ryosei 27:dd9f27fce7d1 4031 //(!!LimitSw::Ispressed(SHEETS_SW)) {
Ryosei 27:dd9f27fce7d1 4032 //ineFase = 20;
Ryosei 27:dd9f27fce7d1 4033 //else if(LimitSw::IsPressed(TOWEL2_SW) {
Ryosei 27:dd9f27fce7d1 4034 //ineFase = 14;
Ryosei 27:dd9f27fce7d1 4035 //else {
Ryosei 27:dd9f27fce7d1 4036 lineFase = 21;
Ryosei 27:dd9f27fce7d1 4037 //
Ryosei 27:dd9f27fce7d1 4038 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4039 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4040 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4041 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4042 }
Ryosei 27:dd9f27fce7d1 4043 motor[TIRE_FL].pwm = 16;
Ryosei 27:dd9f27fce7d1 4044 motor[TIRE_BL].pwm = 16;
Ryosei 27:dd9f27fce7d1 4045 motor[TIRE_BR].pwm = 16;
Ryosei 27:dd9f27fce7d1 4046 motor[TIRE_FR].pwm = 16;
Ryosei 27:dd9f27fce7d1 4047 } else if(lineFase == 21) {
Ryosei 27:dd9f27fce7d1 4048 switch(linePara[2]) {
Ryosei 27:dd9f27fce7d1 4049 case -2:
Ryosei 27:dd9f27fce7d1 4050 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4051 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4052 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4053 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4054 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4055 break;
Ryosei 27:dd9f27fce7d1 4056 case -3:
Ryosei 27:dd9f27fce7d1 4057 tirePWM[TIRE_FL] = -10;
Ryosei 27:dd9f27fce7d1 4058 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4059 tirePWM[TIRE_BR] = 10;
Ryosei 27:dd9f27fce7d1 4060 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4061 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4062 break;
Ryosei 27:dd9f27fce7d1 4063 case -1:
Ryosei 27:dd9f27fce7d1 4064 tirePWM[TIRE_FL] = -20;
Ryosei 27:dd9f27fce7d1 4065 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4066 tirePWM[TIRE_BR] = 20;
Ryosei 27:dd9f27fce7d1 4067 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4068 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4069 break;
Ryosei 27:dd9f27fce7d1 4070 case 0:
Ryosei 27:dd9f27fce7d1 4071 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4072 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4073 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4074 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4075 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4076 break;
Ryosei 27:dd9f27fce7d1 4077 case 1:
Ryosei 27:dd9f27fce7d1 4078 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4079 tirePWM[TIRE_BL] = -20;
Ryosei 27:dd9f27fce7d1 4080 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4081 tirePWM[TIRE_FR] = 20;
Ryosei 27:dd9f27fce7d1 4082 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4083 break;
Ryosei 27:dd9f27fce7d1 4084 case 3:
Ryosei 27:dd9f27fce7d1 4085 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4086 tirePWM[TIRE_BL] = -10;
Ryosei 27:dd9f27fce7d1 4087 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4088 tirePWM[TIRE_FR] = 10;
Ryosei 27:dd9f27fce7d1 4089 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4090 break;
Ryosei 27:dd9f27fce7d1 4091 case 2:
Ryosei 27:dd9f27fce7d1 4092 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4093 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4094 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4095 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4096 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4097 break;
Ryosei 27:dd9f27fce7d1 4098 case 'A':
Ryosei 27:dd9f27fce7d1 4099 if(lineCheck == false) {
Ryosei 27:dd9f27fce7d1 4100 lineCheck = true;
Ryosei 27:dd9f27fce7d1 4101 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4102 countW++;
Ryosei 27:dd9f27fce7d1 4103 }
Ryosei 27:dd9f27fce7d1 4104 tirePWM[TIRE_FL] = -30;
Ryosei 27:dd9f27fce7d1 4105 tirePWM[TIRE_BL] = -30;
Ryosei 27:dd9f27fce7d1 4106 tirePWM[TIRE_BR] = 30;
Ryosei 27:dd9f27fce7d1 4107 tirePWM[TIRE_FR] = 30;
Ryosei 27:dd9f27fce7d1 4108 adjAnable = true;
Ryosei 27:dd9f27fce7d1 4109 break;
Ryosei 27:dd9f27fce7d1 4110 case 'N':
Ryosei 27:dd9f27fce7d1 4111 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4112 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4113 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4114 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4115 adjAnable = false;
Ryosei 27:dd9f27fce7d1 4116 break;
Ryosei 27:dd9f27fce7d1 4117 default:
Ryosei 27:dd9f27fce7d1 4118 tirePWM[TIRE_FL] = 0;
Ryosei 27:dd9f27fce7d1 4119 tirePWM[TIRE_BL] = 0;
Ryosei 27:dd9f27fce7d1 4120 tirePWM[TIRE_BR] = 0;
Ryosei 27:dd9f27fce7d1 4121 tirePWM[TIRE_FR] = 0;
Ryosei 27:dd9f27fce7d1 4122 adjAnable = false;
Ryosei 27:dd9f27fce7d1 4123 }
Ryosei 27:dd9f27fce7d1 4124
Ryosei 27:dd9f27fce7d1 4125 if(adjAnable) {
Ryosei 27:dd9f27fce7d1 4126 if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
Ryosei 27:dd9f27fce7d1 4127 } else {
Ryosei 27:dd9f27fce7d1 4128 adj = 0;
Ryosei 27:dd9f27fce7d1 4129 }
Ryosei 27:dd9f27fce7d1 4130
Ryosei 27:dd9f27fce7d1 4131 motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 4132 motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 4133 motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4134 motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 4135 motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei 27:dd9f27fce7d1 4136 motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei 27:dd9f27fce7d1 4137 motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei 27:dd9f27fce7d1 4138 motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei 27:dd9f27fce7d1 4139
Ryosei 27:dd9f27fce7d1 4140 if(lineCheck == true) {
Ryosei 27:dd9f27fce7d1 4141 lineCount++;
Ryosei 27:dd9f27fce7d1 4142 if(lineCount > 20) lineCheck = false;
Ryosei 27:dd9f27fce7d1 4143 }
Ryosei 27:dd9f27fce7d1 4144 if(countW == 2) {
Ryosei 27:dd9f27fce7d1 4145 countW = 0;
Ryosei 27:dd9f27fce7d1 4146 lineFase = 22;
Ryosei 27:dd9f27fce7d1 4147 lineCount = 0;
Ryosei 27:dd9f27fce7d1 4148 lineCheck = false;
Ryosei 27:dd9f27fce7d1 4149 }
Ryosei 27:dd9f27fce7d1 4150 } else if(lineFase == 22) {
Ryosei 27:dd9f27fce7d1 4151 motor[TIRE_FL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4152 motor[TIRE_BL].dir = BACK;
Ryosei 27:dd9f27fce7d1 4153 motor[TIRE_BR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4154 motor[TIRE_FR].dir = FOR;
Ryosei 27:dd9f27fce7d1 4155 if (linePara[0] == 'N') {
Ryosei 27:dd9f27fce7d1 4156 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4157 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4158 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4159 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4160 }
Ryosei 27:dd9f27fce7d1 4161 motor[TIRE_FL].pwm = 30;
Ryosei 27:dd9f27fce7d1 4162 motor[TIRE_BL].pwm = 30;
Ryosei 27:dd9f27fce7d1 4163 motor[TIRE_BR].pwm = 30;
Ryosei 27:dd9f27fce7d1 4164 motor[TIRE_FR].pwm = 30;
Ryosei 27:dd9f27fce7d1 4165 } else {
Ryosei 27:dd9f27fce7d1 4166 motor[TIRE_FL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4167 motor[TIRE_BL].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4168 motor[TIRE_BR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4169 motor[TIRE_FR].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4170 }
Ryosei 27:dd9f27fce7d1 4171
kishibekairohan 13:b6e02d6261d7 4172 }
kishibekairohan 13:b6e02d6261d7 4173 #endif
kishibekairohan 13:b6e02d6261d7 4174
kishibekairohan 13:b6e02d6261d7 4175 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 4176 static void Process6()
kishibekairohan 13:b6e02d6261d7 4177 {
Ryosei 27:dd9f27fce7d1 4178
kishibekairohan 13:b6e02d6261d7 4179 }
kishibekairohan 13:b6e02d6261d7 4180 #endif
kishibekairohan 13:b6e02d6261d7 4181
kishibekairohan 13:b6e02d6261d7 4182 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 4183 static void Process7()
kishibekairohan 13:b6e02d6261d7 4184 {
Ryosei 27:dd9f27fce7d1 4185 static int SW_flag=0;
Ryosei 27:dd9f27fce7d1 4186 static int Processflag=0;
Ryosei 27:dd9f27fce7d1 4187 if(Processflag==0) {
Ryosei 27:dd9f27fce7d1 4188 if(Limitphase==0) {
Ryosei 27:dd9f27fce7d1 4189 //下待機
Ryosei 27:dd9f27fce7d1 4190 Limitphase=2;
Ryosei 27:dd9f27fce7d1 4191 } else if(Limitphase==1) {
Ryosei 27:dd9f27fce7d1 4192 } else if(Limitphase==2) {
Ryosei 27:dd9f27fce7d1 4193 //下→上とタオル展開
Ryosei 27:dd9f27fce7d1 4194 pc.printf("%d\r\n",SW_flag);
Ryosei 27:dd9f27fce7d1 4195
Ryosei 27:dd9f27fce7d1 4196 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4197 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4198 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4199 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4200
Ryosei 27:dd9f27fce7d1 4201 if(SW_flag==0) {
Ryosei 27:dd9f27fce7d1 4202 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4203 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4204 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4205 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4206 if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4207 SW_flag=1;
Ryosei 27:dd9f27fce7d1 4208 }
Ryosei 27:dd9f27fce7d1 4209 } else if(SW_flag==1) {
Ryosei 27:dd9f27fce7d1 4210 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4211 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4212 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4213 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4214 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4215 SW_flag=2;
Ryosei 27:dd9f27fce7d1 4216 }
Ryosei 27:dd9f27fce7d1 4217 } else if(SW_flag==2) {
Ryosei 27:dd9f27fce7d1 4218 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4219 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4220 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4221 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4222 if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4223 SW_flag=3;
Ryosei 27:dd9f27fce7d1 4224 }
Ryosei 27:dd9f27fce7d1 4225 } else if(SW_flag==3) {
Ryosei 27:dd9f27fce7d1 4226 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4227 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4228 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4229 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4230 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4231 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4232 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4233 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4234 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4235 Limitphase=4;
Ryosei 27:dd9f27fce7d1 4236 }
Ryosei 27:dd9f27fce7d1 4237 }
Ryosei 27:dd9f27fce7d1 4238 } else if(Limitphase==3) {
Ryosei 27:dd9f27fce7d1 4239 } else if(Limitphase==4) {
Ryosei 27:dd9f27fce7d1 4240 //上段待機
Ryosei 27:dd9f27fce7d1 4241 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4242 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4243 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4244 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4245 current=0;
Ryosei 27:dd9f27fce7d1 4246 startFlag=true;
Ryosei 27:dd9f27fce7d1 4247 } else {
Ryosei 27:dd9f27fce7d1 4248 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4249 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4250 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4251 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4252 startFlag=true;
Ryosei 27:dd9f27fce7d1 4253 current=0;
Ryosei 27:dd9f27fce7d1 4254 }
Ryosei 27:dd9f27fce7d1 4255 if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)) {
Ryosei 27:dd9f27fce7d1 4256 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4257 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4258 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4259 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4260 Air[TOWEL0]=1;
Ryosei 27:dd9f27fce7d1 4261 SW_flag=0;
Ryosei 27:dd9f27fce7d1 4262 Limitphase=0;
Ryosei 27:dd9f27fce7d1 4263 current=0;
Ryosei 27:dd9f27fce7d1 4264 startFlag=true;
Ryosei 27:dd9f27fce7d1 4265 Processflag=1;
Ryosei 27:dd9f27fce7d1 4266 }
Ryosei 27:dd9f27fce7d1 4267 } else if(Processflag==1) {
Ryosei 27:dd9f27fce7d1 4268 motor[LIFT_LB].dir = FOR;
Ryosei 27:dd9f27fce7d1 4269 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4270 motor[LIFT_RB].dir = BACK;
Ryosei 27:dd9f27fce7d1 4271 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4272 if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei 27:dd9f27fce7d1 4273 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4274 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4275 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4276 motor[LIFT_RB].pwm = 180;
Ryosei 27:dd9f27fce7d1 4277 current=0;
Ryosei 27:dd9f27fce7d1 4278 startFlag=true;
Ryosei 27:dd9f27fce7d1 4279 Processflag=1;
Ryosei 27:dd9f27fce7d1 4280 }
Ryosei 27:dd9f27fce7d1 4281 if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)){
Ryosei 27:dd9f27fce7d1 4282 motor[LIFT_LB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4283 motor[LIFT_LB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4284 motor[LIFT_RB].dir = BRAKE;
Ryosei 27:dd9f27fce7d1 4285 motor[LIFT_RB].pwm = 200;
Ryosei 27:dd9f27fce7d1 4286 Air[TOWEL0]=1;
Ryosei 27:dd9f27fce7d1 4287 SW_flag=0;
Ryosei 27:dd9f27fce7d1 4288 Limitphase=0;
Ryosei 27:dd9f27fce7d1 4289 current=0;
Ryosei 27:dd9f27fce7d1 4290 startFlag=true;
Ryosei 27:dd9f27fce7d1 4291 Processflag=1;
Ryosei 27:dd9f27fce7d1 4292 }
Ryosei 27:dd9f27fce7d1 4293 }
kishibekairohan 12:c09b3e08a316 4294 }
Ryosei 27:dd9f27fce7d1 4295
Ryosei 27:dd9f27fce7d1 4296
Ryosei 27:dd9f27fce7d1 4297
kishibekairohan 12:c09b3e08a316 4298 #endif
kishibekairohan 12:c09b3e08a316 4299
Ryosei 27:dd9f27fce7d1 4300 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 4301 static void Process8()
kishibekairohan 12:c09b3e08a316 4302 {
Ryosei 27:dd9f27fce7d1 4303 if(controller->Button.A) {
Ryosei 27:dd9f27fce7d1 4304 rotaconSampling.start();
Ryosei 27:dd9f27fce7d1 4305 PIDflag = true;
Ryosei 27:dd9f27fce7d1 4306
Ryosei 27:dd9f27fce7d1 4307 //linePara_U = LineHub::GetPara(0);
Ryosei 27:dd9f27fce7d1 4308 //linePara_B = LineHub::GetPara(3);
Ryosei 27:dd9f27fce7d1 4309
Ryosei 27:dd9f27fce7d1 4310
Ryosei 27:dd9f27fce7d1 4311 pulsePV[FL] = encoder[FL].getPulses();
Ryosei 27:dd9f27fce7d1 4312 pulsePV[BL] = encoder[BL].getPulses();
Ryosei 27:dd9f27fce7d1 4313 pulsePV[BR] = encoder[BR].getPulses();
Ryosei 27:dd9f27fce7d1 4314 pulsePV[FR] = encoder[FR].getPulses();
Ryosei 27:dd9f27fce7d1 4315
Ryosei 27:dd9f27fce7d1 4316
Ryosei 27:dd9f27fce7d1 4317 for (int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4318 timeCV[i] = timePV[i];
Ryosei 27:dd9f27fce7d1 4319 timePV[i] = rotaconSampling.read();
Ryosei 27:dd9f27fce7d1 4320 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
Ryosei 27:dd9f27fce7d1 4321 pulseCV[i] = pulsePV[i];
Ryosei 27:dd9f27fce7d1 4322 }
Ryosei 27:dd9f27fce7d1 4323
Ryosei 27:dd9f27fce7d1 4324 move.Vx = 0.5;
Ryosei 27:dd9f27fce7d1 4325 move.Vy = 0.5;
Ryosei 27:dd9f27fce7d1 4326 move.Va = 0;
Ryosei 27:dd9f27fce7d1 4327
Ryosei 27:dd9f27fce7d1 4328 correction_LT.Vx = 0; //0.1 * linePara_U;
Ryosei 27:dd9f27fce7d1 4329 correction_LT.Vy = 0;
Ryosei 27:dd9f27fce7d1 4330 correction_LT.Va = 0;
Ryosei 27:dd9f27fce7d1 4331
Ryosei 27:dd9f27fce7d1 4332 synthetic.Vx = move.Vx + correction_LT.Vx;
Ryosei 27:dd9f27fce7d1 4333 synthetic.Vy = move.Vy + correction_LT.Vy;
Ryosei 27:dd9f27fce7d1 4334 synthetic.Va = move.Va + correction_LT.Va;
Ryosei 27:dd9f27fce7d1 4335
Ryosei 27:dd9f27fce7d1 4336 sita = 0;
Ryosei 27:dd9f27fce7d1 4337
Ryosei 27:dd9f27fce7d1 4338 //タイヤの目標速度算出
Ryosei 27:dd9f27fce7d1 4339 float sinR = 0.7071 * (float)sin(sita);
Ryosei 27:dd9f27fce7d1 4340 float cosR = 0.7071 * (float)cos(sita);
Ryosei 27:dd9f27fce7d1 4341 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
Ryosei 27:dd9f27fce7d1 4342 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4343 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4344 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4345 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei 27:dd9f27fce7d1 4346
Ryosei 27:dd9f27fce7d1 4347 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
Ryosei 27:dd9f27fce7d1 4348
Ryosei 27:dd9f27fce7d1 4349 //PIDによるPWM算出
Ryosei 27:dd9f27fce7d1 4350
Ryosei 27:dd9f27fce7d1 4351 //モータの駆動
Ryosei 27:dd9f27fce7d1 4352 for (int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4353 if (tirePWM[i] > 255) {
Ryosei 27:dd9f27fce7d1 4354 tirePWM[i] = 255;
Ryosei 27:dd9f27fce7d1 4355 } else if (tirePWM[i] < -255) {
Ryosei 27:dd9f27fce7d1 4356 tirePWM[i] = -255;
Ryosei 27:dd9f27fce7d1 4357 }
Ryosei 27:dd9f27fce7d1 4358 }
Ryosei 27:dd9f27fce7d1 4359
Ryosei 27:dd9f27fce7d1 4360 for(int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4361 motor[i].dir = SetStatus(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4362 motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4363 }
Ryosei 27:dd9f27fce7d1 4364 } else {
Ryosei 27:dd9f27fce7d1 4365 PIDflag = false;
Ryosei 27:dd9f27fce7d1 4366 rotaconSampling.stop();
Ryosei 27:dd9f27fce7d1 4367 rotaconSampling.reset();
Ryosei 27:dd9f27fce7d1 4368 for(int i = 0; i < 4; i++) {
Ryosei 27:dd9f27fce7d1 4369 encoder[i].reset();
Ryosei 27:dd9f27fce7d1 4370 pulsePV[i] = 0;
Ryosei 27:dd9f27fce7d1 4371 pulseCV[i] = 0;
Ryosei 27:dd9f27fce7d1 4372 timePV[i] = 0;
Ryosei 27:dd9f27fce7d1 4373 timeCV[i] = 0;
Ryosei 27:dd9f27fce7d1 4374 tirePWM[i] = 0;
Ryosei 27:dd9f27fce7d1 4375 motor[i].dir = SetStatus(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4376 motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei 27:dd9f27fce7d1 4377 }
Ryosei 27:dd9f27fce7d1 4378 }
kishibekairohan 12:c09b3e08a316 4379 }
kishibekairohan 12:c09b3e08a316 4380 #endif
kishibekairohan 12:c09b3e08a316 4381
kishibekairohan 13:b6e02d6261d7 4382 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 4383 static void Process9()
kishibekairohan 12:c09b3e08a316 4384 {
Ryosei 27:dd9f27fce7d1 4385
t_yamamoto 0:669ef71cba68 4386 }
t_yamamoto 0:669ef71cba68 4387 #endif
t_yamamoto 0:669ef71cba68 4388 #endif
t_yamamoto 0:669ef71cba68 4389 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 4390
t_yamamoto 0:669ef71cba68 4391 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 4392 {
t_yamamoto 0:669ef71cba68 4393
Ryosei 27:dd9f27fce7d1 4394 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 4395 solenoid.all = ALL_SOLENOID_OFF;
Ryosei 27:dd9f27fce7d1 4396 #endif
t_yamamoto 0:669ef71cba68 4397
Ryosei 27:dd9f27fce7d1 4398 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 4399 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
Ryosei 27:dd9f27fce7d1 4400 motor[i].dir = FREE;
Ryosei 27:dd9f27fce7d1 4401 motor[i].pwm = 0;
Ryosei 27:dd9f27fce7d1 4402 }
Ryosei 27:dd9f27fce7d1 4403 #endif
t_yamamoto 0:669ef71cba68 4404 }
t_yamamoto 0:669ef71cba68 4405
Ryosei 27:dd9f27fce7d1 4406 void BuzzerTimer_func()
Ryosei 27:dd9f27fce7d1 4407 {
kishibekairohan 16:3f2c2d89372b 4408 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 4409 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 4410 }
kishibekairohan 11:028a150943b5 4411
Ryosei 27:dd9f27fce7d1 4412 void TapeLedEms_func()
Ryosei 27:dd9f27fce7d1 4413 {
kishibekairohan 16:3f2c2d89372b 4414 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 4415 }
kishibekairohan 16:3f2c2d89372b 4416
kishibekairohan 16:3f2c2d89372b 4417 #pragma region USER-DEFINED-FUNCTIONS
Ryosei 27:dd9f27fce7d1 4418 void tirePID()
Ryosei 27:dd9f27fce7d1 4419 {
Ryosei 27:dd9f27fce7d1 4420 if(PIDflag == true) {
Ryosei 27:dd9f27fce7d1 4421 //加算するPID値の算出
Ryosei 27:dd9f27fce7d1 4422 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
Ryosei 27:dd9f27fce7d1 4423 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
Ryosei 27:dd9f27fce7d1 4424 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
Ryosei 27:dd9f27fce7d1 4425 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
Ryosei 27:dd9f27fce7d1 4426 //PID値の加算
Ryosei 27:dd9f27fce7d1 4427 tirePWM[FL] += rotaconPID[0].GetMV();
Ryosei 27:dd9f27fce7d1 4428 tirePWM[BL] += rotaconPID[1].GetMV();
Ryosei 27:dd9f27fce7d1 4429 tirePWM[FR] += rotaconPID[2].GetMV();
Ryosei 27:dd9f27fce7d1 4430 tirePWM[BR] += rotaconPID[3].GetMV();
Ryosei 27:dd9f27fce7d1 4431 }
M_souta 22:7d93f79a3686 4432 }
kishibekairohan 16:3f2c2d89372b 4433
Ryosei 27:dd9f27fce7d1 4434 int lineCast(char k)
Ryosei 27:dd9f27fce7d1 4435 {
Ryosei 27:dd9f27fce7d1 4436 int l;
Ryosei 27:dd9f27fce7d1 4437 switch(k) {
Ryosei 27:dd9f27fce7d1 4438 case 255:
Ryosei 27:dd9f27fce7d1 4439 l = -1;
Ryosei 27:dd9f27fce7d1 4440 break;
Ryosei 27:dd9f27fce7d1 4441 case 254:
Ryosei 27:dd9f27fce7d1 4442 l = -2;
Ryosei 27:dd9f27fce7d1 4443 break;
Ryosei 27:dd9f27fce7d1 4444 case 253:
Ryosei 27:dd9f27fce7d1 4445 l = -3;
Ryosei 27:dd9f27fce7d1 4446 break;
Ryosei 27:dd9f27fce7d1 4447 default:
Ryosei 27:dd9f27fce7d1 4448 l = k;
Ryosei 27:dd9f27fce7d1 4449 }
Ryosei 27:dd9f27fce7d1 4450 return l;
M_souta 22:7d93f79a3686 4451 }
kishibekairohan 16:3f2c2d89372b 4452
t_yamamoto 0:669ef71cba68 4453 #pragma endregion