Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_final

aaaaaaaaa

Dependencies:   mbed QEI

System/Process/Process.cpp@14:93776ca449a4, 2018-10-25 (annotated)

Committer:
7ka884
Date:
Thu Oct 25 09:00:19 2018 +0000
Revision:
14:93776ca449a4
Parent:
13:b6e02d6261d7 
okok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
7ka884 4:ba9df71868df 4 #include "QEI.h"
t_yamamoto 0:669ef71cba68 5
7ka884 4:ba9df71868df 6 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.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"
7ka884 1:b1219d8ca117 11 #include "../../Input/ColorSensor/ColorSensor.h"
7ka884 1:b1219d8ca117 12 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto 0:669ef71cba68 13 #include "../../Input/Potentiometer/Potentiometer.h"
7ka884 1:b1219d8ca117 14 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto 0:669ef71cba68 15 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 16 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 17 #include "../Using.h"
t_yamamoto 0:669ef71cba68 18
kishibekairohan 2:c015739085d3 19
t_yamamoto 0:669ef71cba68 20 using namespace SWITCH;
kishibekairohan 2:c015739085d3 21 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 22 using namespace ACCELERATIONSENSOR;
7ka884 4:ba9df71868df 23 using namespace PID_SPACE;
7ka884 4:ba9df71868df 24 using namespace ROTARYENCODER;
t_yamamoto 0:669ef71cba68 25
t_yamamoto 0:669ef71cba68 26 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 27 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 28 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 29
t_yamamoto 0:669ef71cba68 30 static bool lock;
t_yamamoto 0:669ef71cba68 31 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 32 static int current;
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 35
t_yamamoto 0:669ef71cba68 36 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 37 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 38 #endif
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 41
t_yamamoto 0:669ef71cba68 42 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 43
kishibekairohan 2:c015739085d3 44 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 45
kishibekairohan 7:e88c5d47a3be 46 //************メカナム********************
kishibekairohan 2:c015739085d3 47
kishibekairohan 2:c015739085d3 48 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 49 {
kishibekairohan 2:c015739085d3 50 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 51 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 52 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 53 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 54 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 55 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 56 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 57 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 58 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 59 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 60 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 61 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 62 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 63 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 64 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 65 };
kishibekairohan 2:c015739085d3 66
kishibekairohan 2:c015739085d3 67 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 68 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 69 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 70 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 71 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 72 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 73 else return BRAKE;
kishibekairohan 2:c015739085d3 74 }
kishibekairohan 2:c015739085d3 75 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 76 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 77 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 78 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 79 }
kishibekairohan 2:c015739085d3 80
7ka884 14:93776ca449a4 81 //****************************************************************************************************
kishibekairohan 7:e88c5d47a3be 82
kishibekairohan 7:e88c5d47a3be 83 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 84
kishibekairohan 2:c015739085d3 85 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 86 int Color_B[3];
kishibekairohan 2:c015739085d3 87 int Color_C[3];
kishibekairohan 2:c015739085d3 88 int Color_D[3];
kishibekairohan 2:c015739085d3 89 int intergration = 50;
kishibekairohan 2:c015739085d3 90
kishibekairohan 12:c09b3e08a316 91 int Avecolor_A[3];
kishibekairohan 12:c09b3e08a316 92 int Avecolor_B[3];
kishibekairohan 12:c09b3e08a316 93 int Avecolor_C[3];
kishibekairohan 12:c09b3e08a316 94 int Avecolor_D[3];
kishibekairohan 12:c09b3e08a316 95
kishibekairohan 12:c09b3e08a316 96 void ColorIn();
kishibekairohan 7:e88c5d47a3be 97 void ColorDetection();
kishibekairohan 12:c09b3e08a316 98 void getcolor();
kishibekairohan 13:b6e02d6261d7 99
7ka884 14:93776ca449a4 100 //***************************************************************************************************************
kishibekairohan 7:e88c5d47a3be 101
kishibekairohan 2:c015739085d3 102 //************ライントレース変数*******************
7ka884 14:93776ca449a4 103 int PointA[3] = {350, 600, 800};//赤，緑，青
7ka884 14:93776ca449a4 104 int PointB[3] = {350, 600, 800};//赤，緑，青
kishibekairohan 13:b6e02d6261d7 105 int PointC[3] = {1000, 1700, 2400};//赤，緑，青
kishibekairohan 8:6fb3723f7747 106
kishibekairohan 13:b6e02d6261d7 107 bool compA = false;
kishibekairohan 13:b6e02d6261d7 108 bool compB = false;
kishibekairohan 13:b6e02d6261d7 109 bool compC = false;
kishibekairohan 13:b6e02d6261d7 110 bool compD = false;
kishibekairohan 13:b6e02d6261d7 111
kishibekairohan 13:b6e02d6261d7 112 bool invationA = false;
kishibekairohan 13:b6e02d6261d7 113 bool invationB = false;
kishibekairohan 13:b6e02d6261d7 114 bool invationC = false;
kishibekairohan 13:b6e02d6261d7 115 bool invationD = false;
kishibekairohan 8:6fb3723f7747 116
kishibekairohan 8:6fb3723f7747 117 Ticker Color_T;
kishibekairohan 13:b6e02d6261d7 118
kishibekairohan 13:b6e02d6261d7 119 void ColorDetection();
7ka884 14:93776ca449a4 120 void Color_changeflagA();
7ka884 14:93776ca449a4 121 void Color_changeflagB();
7ka884 14:93776ca449a4 122 //*****************************************************************************************************
kishibekairohan 9:f93fc79a49ea 123
kishibekairohan 10:1295d39fec3a 124 //************ROタコン******************
kishibekairohan 9:f93fc79a49ea 125 QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 9:f93fc79a49ea 126 QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 8:6fb3723f7747 127 Ticker get_rpm;
7ka884 14:93776ca449a4 128 PID Rt_X = PID(0.03, -255, 255, 0.15, 0, 0);
7ka884 14:93776ca449a4 129 PID Rt_Y = PID(0.03, -255, 255, 0.17, 0, 0);
7ka884 14:93776ca449a4 130 double rpmX = 0, rpmXB =0;
7ka884 14:93776ca449a4 131 double rpmY = 0, rpmYB =0;
7ka884 14:93776ca449a4 132 double disX =0,disXB =0;
7ka884 14:93776ca449a4 133 double disY =0,disYB =0;
kishibekairohan 8:6fb3723f7747 134 int palseX;
kishibekairohan 8:6fb3723f7747 135 int palseY;
7ka884 14:93776ca449a4 136 int RtpwmX =0,RtpwmXB =0;
7ka884 14:93776ca449a4 137 int RtpwmY =0,RtpwmYB =0;
7ka884 14:93776ca449a4 138 double goalAX = 1000.000;
7ka884 14:93776ca449a4 139 double goalAY = 850.000;
7ka884 14:93776ca449a4 140 double goalBX = -430.000;
7ka884 14:93776ca449a4 141 double goalBY = 1000.000;
kishibekairohan 8:6fb3723f7747 142 void filip();
7ka884 14:93776ca449a4 143 void filipB();
kishibekairohan 12:c09b3e08a316 144
kishibekairohan 10:1295d39fec3a 145 //************ROタコン******************
kishibekairohan 2:c015739085d3 146
kishibekairohan 7:e88c5d47a3be 147 //************ジャイロ*******************
kishibekairohan 9:f93fc79a49ea 148 bool Angle_flagI = false;
kishibekairohan 11:028a150943b5 149 int Angle;
kishibekairohan 7:e88c5d47a3be 150 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan 7:e88c5d47a3be 151 float rotateY;
kishibekairohan 11:028a150943b5 152 //初期値 -5
kishibekairohan 11:028a150943b5 153 int AngletargetX = 4;
kishibekairohan 11:028a150943b5 154 int AngletargetY = -12;
kishibekairohan 12:c09b3e08a316 155 int AngletargetI = -5;
kishibekairohan 7:e88c5d47a3be 156 //************ジャイロ*******************
kishibekairohan 2:c015739085d3 157
kishibekairohan 9:f93fc79a49ea 158 //************Buzzer******************
kishibekairohan 11:028a150943b5 159 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 11:028a150943b5 160 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 161 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 162 Ticker BuzzerTimer;
7ka884 14:93776ca449a4 163 Ticker LostTimer;
kishibekairohan 9:f93fc79a49ea 164 bool Emsflag = false;
kishibekairohan 9:f93fc79a49ea 165 //************Buzzer******************
kishibekairohan 11:028a150943b5 166
kishibekairohan 11:028a150943b5 167 //************TapeLed*****************
kishibekairohan 11:028a150943b5 168 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 169 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 170 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 171 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 172 Ticker tapeLedTimer;
7ka884 14:93776ca449a4 173 Ticker LostLedTimer;
7ka884 14:93776ca449a4 174 //void LostLed_func();
kishibekairohan 11:028a150943b5 175 //************TapaLed*****************
t_yamamoto 0:669ef71cba68 176 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 177
t_yamamoto 0:669ef71cba68 178 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 179 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 180 static void Process0(void);
t_yamamoto 0:669ef71cba68 181 #endif
t_yamamoto 0:669ef71cba68 182 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 183 static void Process1(void);
t_yamamoto 0:669ef71cba68 184 #endif
t_yamamoto 0:669ef71cba68 185 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 186 static void Process2(void);
t_yamamoto 0:669ef71cba68 187 #endif
t_yamamoto 0:669ef71cba68 188 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 189 static void Process3(void);
t_yamamoto 0:669ef71cba68 190 #endif
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 192 static void Process4(void);
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 195 static void Process5(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 198 static void Process6(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 201 static void Process7(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 204 static void Process8(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 207 static void Process9(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #endif
t_yamamoto 0:669ef71cba68 210
t_yamamoto 0:669ef71cba68 211 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 212 {
t_yamamoto 0:669ef71cba68 213 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 11:028a150943b5 214 /*Replace here with the initialization code of your variables.*/
kishibekairohan 9:f93fc79a49ea 215 get_rpm.attach_us(&filip,100);
kishibekairohan 11:028a150943b5 216 buzzer.period(1.0/800);
t_yamamoto 0:669ef71cba68 217
t_yamamoto 0:669ef71cba68 218 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 219
t_yamamoto 0:669ef71cba68 220 lock = true;
t_yamamoto 0:669ef71cba68 221 processChangeComp = true;
t_yamamoto 0:669ef71cba68 222 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 223
t_yamamoto 0:669ef71cba68 224 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 225 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 226 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 227 #endif
t_yamamoto 0:669ef71cba68 228 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 229 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 230 #endif
t_yamamoto 0:669ef71cba68 231 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 232 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 233 #endif
t_yamamoto 0:669ef71cba68 234 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 235 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 236 #endif
t_yamamoto 0:669ef71cba68 237 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 238 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 239 #endif
t_yamamoto 0:669ef71cba68 240 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 241 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 242 #endif
t_yamamoto 0:669ef71cba68 243 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 244 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 245 #endif
t_yamamoto 0:669ef71cba68 246 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 247 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 248 #endif
t_yamamoto 0:669ef71cba68 249 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 250 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 251 #endif
t_yamamoto 0:669ef71cba68 252 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 253 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 254 #endif
t_yamamoto 0:669ef71cba68 255 #endif
t_yamamoto 0:669ef71cba68 256 }
t_yamamoto 0:669ef71cba68 257
t_yamamoto 0:669ef71cba68 258 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 259 {
t_yamamoto 0:669ef71cba68 260 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 261 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 262
t_yamamoto 0:669ef71cba68 263 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 264 {
t_yamamoto 0:669ef71cba68 265 current++;
t_yamamoto 0:669ef71cba68 266 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 267 processChangeComp = false;
t_yamamoto 0:669ef71cba68 268 }
t_yamamoto 0:669ef71cba68 269 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 270 {
t_yamamoto 0:669ef71cba68 271 current--;
t_yamamoto 0:669ef71cba68 272 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 273 processChangeComp = false;
t_yamamoto 0:669ef71cba68 274 }
t_yamamoto 0:669ef71cba68 275 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 276 #endif
t_yamamoto 0:669ef71cba68 277
t_yamamoto 0:669ef71cba68 278 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 279 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 280 #endif
t_yamamoto 0:669ef71cba68 281
t_yamamoto 0:669ef71cba68 282 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 283 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 284 #endif
t_yamamoto 0:669ef71cba68 285
t_yamamoto 0:669ef71cba68 286 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 287 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 288 #endif
t_yamamoto 0:669ef71cba68 289
t_yamamoto 0:669ef71cba68 290 }
t_yamamoto 0:669ef71cba68 291
kishibekairohan 2:c015739085d3 292
kishibekairohan 2:c015739085d3 293
t_yamamoto 0:669ef71cba68 294 void SystemProcess()
t_yamamoto 0:669ef71cba68 295 {
t_yamamoto 0:669ef71cba68 296 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 297
t_yamamoto 0:669ef71cba68 298 while(1)
7ka884 14:93776ca449a4 299 {
7ka884 14:93776ca449a4 300 /*
kishibekairohan 12:c09b3e08a316 301 getcolor();
kishibekairohan 12:c09b3e08a316 302 pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
kishibekairohan 12:c09b3e08a316 303 pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
kishibekairohan 12:c09b3e08a316 304 pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
kishibekairohan 12:c09b3e08a316 305 pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
7ka884 14:93776ca449a4 306
7ka884 14:93776ca449a4 307 palseX = RtX.getPulses();
7ka884 14:93776ca449a4 308 palseY = RtY.getPulses();
7ka884 14:93776ca449a4 309
7ka884 14:93776ca449a4 310 pc.printf("X:%d",palseX);
7ka884 14:93776ca449a4 311 pc.printf("Y:%d",palseY);
kishibekairohan 13:b6e02d6261d7 312 */
7ka884 14:93776ca449a4 313 //wheel.getPulses()...どちらの方向にどれだけ回ったか
7ka884 14:93776ca449a4 314 //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
7ka884 14:93776ca449a4 315 //軸が何回転したか
7ka884 14:93776ca449a4 316 //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
t_yamamoto 0:669ef71cba68 317 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 318 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 319 #endif
t_yamamoto 0:669ef71cba68 320
t_yamamoto 0:669ef71cba68 321 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 322 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 323 {
t_yamamoto 0:669ef71cba68 324 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 325 AllActuatorReset();
t_yamamoto 0:669ef71cba68 326 lock = true;
7ka884 14:93776ca449a4 327 //BuzzerTimer.attach(BuzzerTimer_func, 0.5);
7ka884 14:93776ca449a4 328 //LostLedTimer.attach(LostLed_func, 0.5);
7ka884 14:93776ca449a4 329 //sendLedData.code = (uint32_t)Yellow;
7ka884 14:93776ca449a4 330 //buzzer = 0.5;
7ka884 14:93776ca449a4 331 //LostTimer.attach(BuzzerTimer_func, 1.2);
t_yamamoto 0:669ef71cba68 332 }
t_yamamoto 0:669ef71cba68 333 else
t_yamamoto 0:669ef71cba68 334 #endif
t_yamamoto 0:669ef71cba68 335 {
t_yamamoto 0:669ef71cba68 336
t_yamamoto 0:669ef71cba68 337 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 338 if(!lock)
t_yamamoto 0:669ef71cba68 339 {
t_yamamoto 0:669ef71cba68 340 Process[current]();
t_yamamoto 0:669ef71cba68 341 }
t_yamamoto 0:669ef71cba68 342 else
t_yamamoto 0:669ef71cba68 343 #endif
t_yamamoto 0:669ef71cba68 344 {
t_yamamoto 0:669ef71cba68 345 //ロック時の処理
t_yamamoto 0:669ef71cba68 346 }
t_yamamoto 0:669ef71cba68 347 }
t_yamamoto 0:669ef71cba68 348
kishibekairohan 10:1295d39fec3a 349 if ((EMS_0 || EMS_1) && !Emsflag){
kishibekairohan 11:028a150943b5 350 buzzer = 0.5;
kishibekairohan 9:f93fc79a49ea 351 BuzzerTimer.attach(BuzzerTimer_func, 1.2);
kishibekairohan 9:f93fc79a49ea 352 Emsflag = true;
kishibekairohan 11:028a150943b5 353 ledMode = EMS;
kishibekairohan 11:028a150943b5 354 current = 0;
kishibekairohan 11:028a150943b5 355 tapeLedTimer.attach(TapeLedEms_func, 1.2);
kishibekairohan 11:028a150943b5 356 sendLedData.code = (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 357 }
kishibekairohan 9:f93fc79a49ea 358
kishibekairohan 9:f93fc79a49ea 359 if(!EMS_0 && !EMS_1) {
kishibekairohan 9:f93fc79a49ea 360 buzzer = 0;
kishibekairohan 9:f93fc79a49ea 361 BuzzerTimer.detach();
kishibekairohan 9:f93fc79a49ea 362 Emsflag = false;
kishibekairohan 11:028a150943b5 363 if(ledMode == EMS) ledMode = Normal;
kishibekairohan 11:028a150943b5 364 tapeLedTimer.detach();
kishibekairohan 11:028a150943b5 365 }
kishibekairohan 11:028a150943b5 366
kishibekairohan 11:028a150943b5 367 switch(ledMode)
kishibekairohan 11:028a150943b5 368 {
kishibekairohan 11:028a150943b5 369 case EMS :
kishibekairohan 11:028a150943b5 370 break;
kishibekairohan 11:028a150943b5 371
kishibekairohan 11:028a150943b5 372 case Normal :
kishibekairohan 11:028a150943b5 373 sendLedData.code = tapeLED.code;
kishibekairohan 11:028a150943b5 374
kishibekairohan 11:028a150943b5 375 default:
kishibekairohan 11:028a150943b5 376 break;
kishibekairohan 9:f93fc79a49ea 377 }
kishibekairohan 9:f93fc79a49ea 378
t_yamamoto 0:669ef71cba68 379 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 380 }
t_yamamoto 0:669ef71cba68 381 }
t_yamamoto 0:669ef71cba68 382
kishibekairohan 2:c015739085d3 383
kishibekairohan 2:c015739085d3 384
kishibekairohan 2:c015739085d3 385
t_yamamoto 0:669ef71cba68 386 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 387 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 388 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 389 static void Process0()
kishibekairohan 10:1295d39fec3a 390 {
kishibekairohan 12:c09b3e08a316 391 tapeLED.code = (uint32_t)Green;
kishibekairohan 13:b6e02d6261d7 392 if(RedSW){
7ka884 14:93776ca449a4 393 current = 6;
kishibekairohan 13:b6e02d6261d7 394 }
kishibekairohan 13:b6e02d6261d7 395 if(BlueSW){
7ka884 14:93776ca449a4 396 current = 5;
kishibekairohan 13:b6e02d6261d7 397 }
t_yamamoto 0:669ef71cba68 398 }
t_yamamoto 0:669ef71cba68 399 #endif
t_yamamoto 0:669ef71cba68 400
t_yamamoto 0:669ef71cba68 401 #if USE_PROCESS_NUM>1
kishibekairohan 12:c09b3e08a316 402 static void Process1() //手動
t_yamamoto 0:669ef71cba68 403 {
kishibekairohan 12:c09b3e08a316 404 tapeLED.code = (uint32_t)Orange;
7ka884 14:93776ca449a4 405
7ka884 4:ba9df71868df 406 motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 407 motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 408 motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 409 motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 2:c015739085d3 410
kishibekairohan 7:e88c5d47a3be 411 motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 412 motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 413 motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 7:e88c5d47a3be 414 motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 2:c015739085d3 415
kishibekairohan 2:c015739085d3 416 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 7:e88c5d47a3be 417 motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
kishibekairohan 7:e88c5d47a3be 418 motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
7ka884 4:ba9df71868df 419 }
7ka884 4:ba9df71868df 420
kishibekairohan 12:c09b3e08a316 421 if(controller->Button.R){
kishibekairohan 12:c09b3e08a316 422 motor[Angle_R].dir = FOR;
kishibekairohan 12:c09b3e08a316 423 motor[Angle_L].dir = BACK;
kishibekairohan 12:c09b3e08a316 424 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 425 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 426 }else if(controller->Button.L){
kishibekairohan 12:c09b3e08a316 427 motor[Angle_R].dir = BACK;
kishibekairohan 12:c09b3e08a316 428 motor[Angle_L].dir = FOR;
kishibekairohan 12:c09b3e08a316 429 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 430 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 431 }else{
kishibekairohan 12:c09b3e08a316 432 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 433 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 434 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 435 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 436 }
kishibekairohan 12:c09b3e08a316 437
kishibekairohan 12:c09b3e08a316 438 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 12:c09b3e08a316 439 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 440 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 441 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 442 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 443 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 12:c09b3e08a316 444 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 445 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 446 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 447 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 448 }
kishibekairohan 12:c09b3e08a316 449 /*for(int i = 0;i<20;i++){
kishibekairohan 12:c09b3e08a316 450 float y = 0;
kishibekairohan 12:c09b3e08a316 451 y = acc[1]*1000;
kishibekairohan 12:c09b3e08a316 452 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 12:c09b3e08a316 453 Angle += rotateY;
kishibekairohan 12:c09b3e08a316 454 }
kishibekairohan 12:c09b3e08a316 455 Angle = Angle/20;
kishibekairohan 12:c09b3e08a316 456 pc.printf("Y:%d \r\n",Angle);*/
kishibekairohan 2:c015739085d3 457
kishibekairohan 12:c09b3e08a316 458 //wheel.getPulses()...どちらの方向にどれだけ回ったか
kishibekairohan 12:c09b3e08a316 459 //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
kishibekairohan 12:c09b3e08a316 460 //軸が何回転したか
kishibekairohan 12:c09b3e08a316 461 //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
7ka884 14:93776ca449a4 462
7ka884 14:93776ca449a4 463 //filip();
7ka884 14:93776ca449a4 464 //pc.printf("AKApwmX:%d , AKApwmY:%d\r\n",RtpwmX,RtpwmY);
7ka884 14:93776ca449a4 465
7ka884 14:93776ca449a4 466 //filipB();
7ka884 14:93776ca449a4 467 //pc.printf("BLUEpwmX:%d , BLUEpwmY:%d \r\n",RtpwmXB,RtpwmYB);
t_yamamoto 0:669ef71cba68 468 }
t_yamamoto 0:669ef71cba68 469 #endif
t_yamamoto 0:669ef71cba68 470
7ka884 14:93776ca449a4 471 #if USE_PROCESS_NUM>2 //Blue Zone
kishibekairohan 13:b6e02d6261d7 472 static void Process2() //trace
kishibekairohan 13:b6e02d6261d7 473 {
7ka884 14:93776ca449a4 474 /*//tapeLED.code = (uint32_t)Blue;
kishibekairohan 13:b6e02d6261d7 475 static bool traceon = false;//fase1
kishibekairohan 13:b6e02d6261d7 476 static bool yokofla = false;//fase2
kishibekairohan 13:b6e02d6261d7 477 static bool boxslip = false;//fase3
kishibekairohan 13:b6e02d6261d7 478
7ka884 14:93776ca449a4 479 static bool Rt_flagX = false;
7ka884 14:93776ca449a4 480 static bool Rt_flagY = false;
7ka884 14:93776ca449a4 481
kishibekairohan 13:b6e02d6261d7 482
kishibekairohan 13:b6e02d6261d7 483 ColorDetection();
7ka884 14:93776ca449a4 484 Color_changeflagA();
kishibekairohan 13:b6e02d6261d7 485
7ka884 14:93776ca449a4 486 /*if(controller->Button.B && !color_nopush)
kishibekairohan 13:b6e02d6261d7 487 {
kishibekairohan 13:b6e02d6261d7 488 traceon = true;
7ka884 14:93776ca449a4 489 color_nopush = true;
kishibekairohan 13:b6e02d6261d7 490 }
7ka884 14:93776ca449a4 491 traceon = true;
7ka884 14:93776ca449a4 492
7ka884 14:93776ca449a4 493 //else if(!controller->Button.B)color_nopush = false;
kishibekairohan 13:b6e02d6261d7 494
kishibekairohan 13:b6e02d6261d7 495 if(traceon)
kishibekairohan 13:b6e02d6261d7 496 {
7ka884 14:93776ca449a4 497 Color_changeflagA();
7ka884 14:93776ca449a4 498 if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
kishibekairohan 13:b6e02d6261d7 499 {
7ka884 14:93776ca449a4 500 motor[TIRE_FR].dir = BACK;
7ka884 14:93776ca449a4 501 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 502 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 503 motor[TIRE_BL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 504
7ka884 14:93776ca449a4 505 motor[TIRE_FR].pwm = 42;
kishibekairohan 13:b6e02d6261d7 506 motor[TIRE_FL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 507 motor[TIRE_BR].pwm = startP;
kishibekairohan 13:b6e02d6261d7 508 motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 509 }
7ka884 14:93776ca449a4 510 else if(invationB || compB || invationA || compA || invationC || compC)
kishibekairohan 13:b6e02d6261d7 511 {
kishibekairohan 13:b6e02d6261d7 512 for(int i = 0; i<1000; i++){
kishibekairohan 13:b6e02d6261d7 513 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 514 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 515 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 516 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 517
kishibekairohan 13:b6e02d6261d7 518 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 519 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 520 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 521 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 522 }
kishibekairohan 13:b6e02d6261d7 523
kishibekairohan 13:b6e02d6261d7 524 yokofla = true;
kishibekairohan 13:b6e02d6261d7 525 traceon = false;
kishibekairohan 13:b6e02d6261d7 526 }
kishibekairohan 13:b6e02d6261d7 527 }
kishibekairohan 13:b6e02d6261d7 528
kishibekairohan 13:b6e02d6261d7 529
kishibekairohan 13:b6e02d6261d7 530 if(yokofla && !traceon)
kishibekairohan 13:b6e02d6261d7 531 {
kishibekairohan 13:b6e02d6261d7 532 //pointcalculation();
7ka884 14:93776ca449a4 533 Color_changeflagA();
7ka884 14:93776ca449a4 534 if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
7ka884 14:93776ca449a4 535 {
7ka884 14:93776ca449a4 536 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 537 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 538 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 539 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 540
7ka884 14:93776ca449a4 541 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 542 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 543 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 544 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 545
7ka884 14:93776ca449a4 546
7ka884 14:93776ca449a4 547 boxslip = true;
7ka884 14:93776ca449a4 548 yokofla = false;
7ka884 14:93776ca449a4 549 }
7ka884 14:93776ca449a4 550 else{
7ka884 14:93776ca449a4 551 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 552 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 553 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 554 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 555
7ka884 14:93776ca449a4 556 motor[TIRE_FR].pwm = startP;
7ka884 14:93776ca449a4 557 motor[TIRE_FL].pwm = startP;
7ka884 14:93776ca449a4 558 motor[TIRE_BR].pwm = startP;
7ka884 14:93776ca449a4 559 motor[TIRE_BL].pwm = 47;
7ka884 14:93776ca449a4 560 }
7ka884 14:93776ca449a4 561 }
7ka884 14:93776ca449a4 562
7ka884 14:93776ca449a4 563 if(boxslip)
7ka884 14:93776ca449a4 564 {
7ka884 14:93776ca449a4 565 for(int i = 0; i<500; i++){
7ka884 14:93776ca449a4 566 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 567 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 568 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 569 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 570
7ka884 14:93776ca449a4 571 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 572 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 573 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 574 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 575 }
7ka884 14:93776ca449a4 576
7ka884 14:93776ca449a4 577 RtX.reset();
7ka884 14:93776ca449a4 578 RtY.reset();
7ka884 14:93776ca449a4 579
7ka884 14:93776ca449a4 580 Rt_flagX = true;
7ka884 14:93776ca449a4 581 boxslip = false;
7ka884 14:93776ca449a4 582 }
7ka884 14:93776ca449a4 583
7ka884 14:93776ca449a4 584 filipB();
7ka884 14:93776ca449a4 585 if(Rt_flagX && !boxslip)
7ka884 14:93776ca449a4 586 {
7ka884 14:93776ca449a4 587 filipB();
7ka884 14:93776ca449a4 588 if(disX > -goalAX + 5){
7ka884 14:93776ca449a4 589 motor[TIRE_FR].dir = BACK;
7ka884 14:93776ca449a4 590 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 591 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 592 motor[TIRE_BL].dir = FOR;
7ka884 14:93776ca449a4 593 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884 14:93776ca449a4 594 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 595 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 596 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 597 }
7ka884 14:93776ca449a4 598 else if(disX < -goalAX + 5){
7ka884 14:93776ca449a4 599 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 600 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 601 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 602 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 603
7ka884 14:93776ca449a4 604 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 605 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 606 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 607 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 608
7ka884 14:93776ca449a4 609 Rt_flagY = true;
7ka884 14:93776ca449a4 610 Rt_flagX = false;
7ka884 14:93776ca449a4 611 }
7ka884 14:93776ca449a4 612 }
7ka884 14:93776ca449a4 613
7ka884 14:93776ca449a4 614
7ka884 14:93776ca449a4 615 if(Rt_flagY && !Rt_flagX){
7ka884 14:93776ca449a4 616 filipB();
7ka884 14:93776ca449a4 617 if(disY < goalAY - 5){
7ka884 14:93776ca449a4 618 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
7ka884 14:93776ca449a4 619 motor[TIRE_FL].dir = SetStatus(RtpwmY);
7ka884 14:93776ca449a4 620 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
7ka884 14:93776ca449a4 621 motor[TIRE_BL].dir = SetStatus(RtpwmY);
7ka884 14:93776ca449a4 622 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 623 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 624 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 625 motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
7ka884 14:93776ca449a4 626 }
7ka884 14:93776ca449a4 627 else if(disY > goalAY - 5)
7ka884 14:93776ca449a4 628 {
7ka884 14:93776ca449a4 629 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 630 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 631 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 632 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 633 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 634 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 635 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 636 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 637 }
7ka884 14:93776ca449a4 638 }*/
7ka884 14:93776ca449a4 639 }
7ka884 14:93776ca449a4 640 #endif
7ka884 14:93776ca449a4 641
7ka884 14:93776ca449a4 642 #if USE_PROCESS_NUM>3
7ka884 14:93776ca449a4 643 static void Process3() //Red Zone
7ka884 14:93776ca449a4 644 {
7ka884 14:93776ca449a4 645 /*tapeLED.code = (uint32_t)Red;
7ka884 14:93776ca449a4 646
7ka884 14:93776ca449a4 647 //static bool color_nopush = false;
7ka884 14:93776ca449a4 648
7ka884 14:93776ca449a4 649 static bool traceon = false;//fase1
7ka884 14:93776ca449a4 650 static bool yokofla = false;//fase2
7ka884 14:93776ca449a4 651 static bool boxslip = false;//fase3
7ka884 14:93776ca449a4 652
7ka884 14:93776ca449a4 653 static bool Rt_flagX = false;
7ka884 14:93776ca449a4 654 static bool Rt_flagY = false;
7ka884 14:93776ca449a4 655
7ka884 14:93776ca449a4 656 //static bool syu = false;
7ka884 14:93776ca449a4 657
7ka884 14:93776ca449a4 658 ColorDetection();
7ka884 14:93776ca449a4 659 Color_changeflagA();
7ka884 14:93776ca449a4 660
7ka884 14:93776ca449a4 661 /*if(controller->Button.B && !color_nopush)
7ka884 14:93776ca449a4 662 {
7ka884 14:93776ca449a4 663 traceon = true;
7ka884 14:93776ca449a4 664 color_nopush = true;
7ka884 14:93776ca449a4 665 }
7ka884 14:93776ca449a4 666 //else if(!controller->Button.B)color_nopush = false;
7ka884 14:93776ca449a4 667 traceon = true;
7ka884 14:93776ca449a4 668
7ka884 14:93776ca449a4 669 if(traceon)
7ka884 14:93776ca449a4 670 {
7ka884 14:93776ca449a4 671 Color_changeflagA();
7ka884 14:93776ca449a4 672 if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
7ka884 14:93776ca449a4 673 {
7ka884 14:93776ca449a4 674 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 675 motor[TIRE_FL].dir = FOR;
7ka884 14:93776ca449a4 676 motor[TIRE_BR].dir = BACK;
7ka884 14:93776ca449a4 677 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 678
7ka884 14:93776ca449a4 679 motor[TIRE_FR].pwm = startP;
7ka884 14:93776ca449a4 680 motor[TIRE_FL].pwm = startP;
7ka884 14:93776ca449a4 681 motor[TIRE_BR].pwm = startP;
7ka884 14:93776ca449a4 682 motor[TIRE_BL].pwm = startP;
7ka884 14:93776ca449a4 683 }
7ka884 14:93776ca449a4 684 else if(invationA || compA || invationB || compB || invationC || compC)
7ka884 14:93776ca449a4 685 {
7ka884 14:93776ca449a4 686 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 687 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 688 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 689 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 690
7ka884 14:93776ca449a4 691 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 692 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 693 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 694 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 695
7ka884 14:93776ca449a4 696 yokofla = true;
7ka884 14:93776ca449a4 697 traceon = false;
7ka884 14:93776ca449a4 698 }
7ka884 14:93776ca449a4 699 }
7ka884 14:93776ca449a4 700
7ka884 14:93776ca449a4 701
7ka884 14:93776ca449a4 702 if(yokofla && !traceon)
7ka884 14:93776ca449a4 703 {
7ka884 14:93776ca449a4 704 //pointcalculation();
7ka884 14:93776ca449a4 705 Color_changeflagA();
7ka884 14:93776ca449a4 706 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
7ka884 14:93776ca449a4 707 {
7ka884 14:93776ca449a4 708 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 709 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 710 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 711 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 712
7ka884 14:93776ca449a4 713 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 714 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 715 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 716 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 717
7ka884 14:93776ca449a4 718 boxslip = true;
7ka884 14:93776ca449a4 719 yokofla = false;
7ka884 14:93776ca449a4 720 }
7ka884 14:93776ca449a4 721 else{
7ka884 14:93776ca449a4 722 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 723 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 724 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 725 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 726
7ka884 14:93776ca449a4 727 motor[TIRE_FR].pwm = startP;
7ka884 14:93776ca449a4 728 motor[TIRE_FL].pwm = startP;
7ka884 14:93776ca449a4 729 motor[TIRE_BR].pwm = startP;
7ka884 14:93776ca449a4 730 motor[TIRE_BL].pwm = startP;
7ka884 14:93776ca449a4 731 }
7ka884 14:93776ca449a4 732 }
7ka884 14:93776ca449a4 733
7ka884 14:93776ca449a4 734
7ka884 14:93776ca449a4 735 /********************************
7ka884 14:93776ca449a4 736 if(boxslip && !yokofla)
7ka884 14:93776ca449a4 737 {
7ka884 14:93776ca449a4 738 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 739 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 740 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 741 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 742
7ka884 14:93776ca449a4 743 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 744 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 745 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 746 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 747
7ka884 14:93776ca449a4 748
7ka884 14:93776ca449a4 749 RtX.reset();
7ka884 14:93776ca449a4 750 RtY.reset();
7ka884 14:93776ca449a4 751
7ka884 14:93776ca449a4 752 Rt_flagX = true;
7ka884 14:93776ca449a4 753 boxslip = false;
7ka884 14:93776ca449a4 754 }
7ka884 14:93776ca449a4 755
7ka884 14:93776ca449a4 756 if(Rt_flagX && !boxslip)
7ka884 14:93776ca449a4 757 {
7ka884 14:93776ca449a4 758 filip();
7ka884 14:93776ca449a4 759 if(disX < goalAX - 5){
7ka884 14:93776ca449a4 760
7ka884 14:93776ca449a4 761 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 762 motor[TIRE_FL].dir = FOR;
7ka884 14:93776ca449a4 763 motor[TIRE_BR].dir = BACK;
7ka884 14:93776ca449a4 764 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 765 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884 14:93776ca449a4 766 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 767 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 768 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 769 }
7ka884 14:93776ca449a4 770 else if(disX > goalAX - 5){
7ka884 14:93776ca449a4 771
7ka884 14:93776ca449a4 772 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 773 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 774 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 775 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 776
7ka884 14:93776ca449a4 777 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 778 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 779 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 780 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 781
7ka884 14:93776ca449a4 782 Rt_flagY = true;
7ka884 14:93776ca449a4 783 Rt_flagX = false;
7ka884 14:93776ca449a4 784 }
7ka884 14:93776ca449a4 785 }
7ka884 14:93776ca449a4 786
7ka884 14:93776ca449a4 787
7ka884 14:93776ca449a4 788 if(Rt_flagY && !Rt_flagX){
7ka884 14:93776ca449a4 789 filip();
7ka884 14:93776ca449a4 790 if(disY < goalAY - 5){
7ka884 14:93776ca449a4 791 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
7ka884 14:93776ca449a4 792 motor[TIRE_FL].dir = SetStatus(RtpwmY);
7ka884 14:93776ca449a4 793 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
7ka884 14:93776ca449a4 794 motor[TIRE_BL].dir = SetStatus(RtpwmY);
7ka884 14:93776ca449a4 795 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 796 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 797 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 798 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 799 }
7ka884 14:93776ca449a4 800 else if(disY > goalAY - 5)
7ka884 14:93776ca449a4 801 {
7ka884 14:93776ca449a4 802 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 803 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 804 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 805 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 806 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 807 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 808 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 809 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 810 }
7ka884 14:93776ca449a4 811 }*/
7ka884 14:93776ca449a4 812 }
7ka884 14:93776ca449a4 813 #endif
7ka884 14:93776ca449a4 814
7ka884 14:93776ca449a4 815 #if USE_PROCESS_NUM>4
7ka884 14:93776ca449a4 816 static void Process4()
7ka884 14:93776ca449a4 817 {
7ka884 14:93776ca449a4 818
7ka884 14:93776ca449a4 819 }
7ka884 14:93776ca449a4 820 #endif
7ka884 14:93776ca449a4 821
7ka884 14:93776ca449a4 822 #if USE_PROCESS_NUM>5
7ka884 14:93776ca449a4 823 static void Process5()//traceB
7ka884 14:93776ca449a4 824 {
7ka884 14:93776ca449a4 825 static bool traceonB = true;//fase1
7ka884 14:93776ca449a4 826 static bool yokoflaB = false;//fase2
7ka884 14:93776ca449a4 827 static bool boxslipB = false;//fase3
7ka884 14:93776ca449a4 828
7ka884 14:93776ca449a4 829 static bool Rt_flagXB = false;
7ka884 14:93776ca449a4 830 static bool Rt_flagYB = false;
7ka884 14:93776ca449a4 831
7ka884 14:93776ca449a4 832 ColorDetection();
7ka884 14:93776ca449a4 833 Color_changeflagB();
7ka884 14:93776ca449a4 834
7ka884 14:93776ca449a4 835 if(traceonB && !Rt_flagXB && !Rt_flagYB && !boxslipB)
7ka884 14:93776ca449a4 836 {
7ka884 14:93776ca449a4 837 Color_changeflagB();
7ka884 14:93776ca449a4 838 if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
7ka884 14:93776ca449a4 839 {
7ka884 14:93776ca449a4 840 motor[TIRE_FR].dir = BACK;
7ka884 14:93776ca449a4 841 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 842 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 843 motor[TIRE_BL].dir = FOR;
7ka884 14:93776ca449a4 844
7ka884 14:93776ca449a4 845 motor[TIRE_FR].pwm = 48;
7ka884 14:93776ca449a4 846 motor[TIRE_FL].pwm = 43;
7ka884 14:93776ca449a4 847 motor[TIRE_BR].pwm = 43;
7ka884 14:93776ca449a4 848 motor[TIRE_BL].pwm = 43;
7ka884 14:93776ca449a4 849 }
7ka884 14:93776ca449a4 850 else if(invationA || compA || invationB || compB || invationC || compC)
7ka884 14:93776ca449a4 851 {
7ka884 14:93776ca449a4 852 motor[TIRE_FR].dir = BRAKE;
7ka884 14:93776ca449a4 853 motor[TIRE_FL].dir = BRAKE;
7ka884 14:93776ca449a4 854 motor[TIRE_BR].dir = BRAKE;
7ka884 14:93776ca449a4 855 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 856
7ka884 14:93776ca449a4 857 motor[TIRE_FR].pwm = 255;
7ka884 14:93776ca449a4 858 motor[TIRE_FL].pwm = 255;
7ka884 14:93776ca449a4 859 motor[TIRE_BR].pwm = 255;
7ka884 14:93776ca449a4 860 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 861
7ka884 14:93776ca449a4 862 yokoflaB = true;
7ka884 14:93776ca449a4 863 traceonB = false;
7ka884 14:93776ca449a4 864 }
7ka884 14:93776ca449a4 865 }
7ka884 14:93776ca449a4 866
7ka884 14:93776ca449a4 867
7ka884 14:93776ca449a4 868 if(yokoflaB && !traceonB)
7ka884 14:93776ca449a4 869 {
7ka884 14:93776ca449a4 870 Color_changeflagB();
7ka884 14:93776ca449a4 871 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 872 {
kishibekairohan 13:b6e02d6261d7 873 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 874 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 875 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 876 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 877
kishibekairohan 13:b6e02d6261d7 878 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 879 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 880 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 881 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 882
7ka884 14:93776ca449a4 883 boxslipB = true;
7ka884 14:93776ca449a4 884 yokoflaB = false;
kishibekairohan 13:b6e02d6261d7 885 }
7ka884 14:93776ca449a4 886 else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 887 {
7ka884 14:93776ca449a4 888 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 889 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 890 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 891 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 892
7ka884 14:93776ca449a4 893 motor[TIRE_FR].pwm = 55;
7ka884 14:93776ca449a4 894 motor[TIRE_FL].pwm = 55;
7ka884 14:93776ca449a4 895 motor[TIRE_BR].pwm = 55;
7ka884 14:93776ca449a4 896 motor[TIRE_BL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 897 }
7ka884 14:93776ca449a4 898 else if(invationA || invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
kishibekairohan 13:b6e02d6261d7 899 {
kishibekairohan 13:b6e02d6261d7 900 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 901 motor[TIRE_FL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 902 motor[TIRE_BR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 903 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 904
7ka884 14:93776ca449a4 905 motor[TIRE_FR].pwm = 55;
7ka884 14:93776ca449a4 906 //motor[TIRE_FL].pwm = 30;
7ka884 14:93776ca449a4 907 //motor[TIRE_BR].pwm = 30;
7ka884 14:93776ca449a4 908 motor[TIRE_BL].pwm = 55;
7ka884 14:93776ca449a4 909 }
7ka884 14:93776ca449a4 910 else if(!invationA || !invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
7ka884 14:93776ca449a4 911 {
7ka884 14:93776ca449a4 912 motor[TIRE_FR].dir = FREE;
7ka884 14:93776ca449a4 913 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 914 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 915 motor[TIRE_BL].dir = FREE;
7ka884 14:93776ca449a4 916
7ka884 14:93776ca449a4 917 //motor[TIRE_FR].pwm = 30;
7ka884 14:93776ca449a4 918 motor[TIRE_FL].pwm = 55;
7ka884 14:93776ca449a4 919 motor[TIRE_BR].pwm = 55;
7ka884 14:93776ca449a4 920 //motor[TIRE_BL].pwm = 30;
kishibekairohan 13:b6e02d6261d7 921 }
7ka884 14:93776ca449a4 922 else
7ka884 14:93776ca449a4 923 {
7ka884 14:93776ca449a4 924 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 925 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 926 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 927 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 928
7ka884 14:93776ca449a4 929 motor[TIRE_FR].pwm = 50;
7ka884 14:93776ca449a4 930 motor[TIRE_FL].pwm = 50;
7ka884 14:93776ca449a4 931 motor[TIRE_BR].pwm = 50;
7ka884 14:93776ca449a4 932 motor[TIRE_BL].pwm = 50;
7ka884 14:93776ca449a4 933 }
7ka884 14:93776ca449a4 934 }
7ka884 14:93776ca449a4 935
7ka884 14:93776ca449a4 936
7ka884 14:93776ca449a4 937 /*********************************/
7ka884 14:93776ca449a4 938 if(boxslipB && !yokoflaB && !traceonB)
7ka884 14:93776ca449a4 939 {
7ka884 14:93776ca449a4 940 Color_changeflagB();
7ka884 14:93776ca449a4 941 if((compA && compB) || compC){
7ka884 14:93776ca449a4 942
7ka884 14:93776ca449a4 943 RtX.reset();
7ka884 14:93776ca449a4 944 RtY.reset();
7ka884 14:93776ca449a4 945
7ka884 14:93776ca449a4 946 Rt_flagXB = true;
7ka884 14:93776ca449a4 947 boxslipB = false;
7ka884 14:93776ca449a4 948 }
7ka884 14:93776ca449a4 949 else if(invationA || invationB){
7ka884 14:93776ca449a4 950 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 951 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 952 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 953 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 954
7ka884 14:93776ca449a4 955 motor[TIRE_FR].pwm = 25;
7ka884 14:93776ca449a4 956 motor[TIRE_FL].pwm = 25;
7ka884 14:93776ca449a4 957 motor[TIRE_BR].pwm = 25;
7ka884 14:93776ca449a4 958 motor[TIRE_BL].pwm = 25;
kishibekairohan 13:b6e02d6261d7 959 }
7ka884 14:93776ca449a4 960 else if(!invationA || !invationB){
kishibekairohan 13:b6e02d6261d7 961 motor[TIRE_FR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 962 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 963 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 964 motor[TIRE_BL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 965
7ka884 14:93776ca449a4 966 motor[TIRE_FR].pwm = 25;
7ka884 14:93776ca449a4 967 motor[TIRE_FL].pwm = 25;
7ka884 14:93776ca449a4 968 motor[TIRE_BR].pwm = 25;
7ka884 14:93776ca449a4 969 motor[TIRE_BL].pwm = 25;
kishibekairohan 13:b6e02d6261d7 970 }
kishibekairohan 13:b6e02d6261d7 971 }
7ka884 14:93776ca449a4 972
7ka884 14:93776ca449a4 973
7ka884 14:93776ca449a4 974
7ka884 14:93776ca449a4 975 filipB();
7ka884 14:93776ca449a4 976 if(Rt_flagXB && !boxslipB)
kishibekairohan 13:b6e02d6261d7 977 {
7ka884 14:93776ca449a4 978 filipB();
7ka884 14:93776ca449a4 979 if(disXB > goalBX + 5){
7ka884 14:93776ca449a4 980 motor[TIRE_FR].dir = BACK;
7ka884 14:93776ca449a4 981 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 982 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 983 motor[TIRE_BL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 984 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
kishibekairohan 13:b6e02d6261d7 985 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 986 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 987 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan 13:b6e02d6261d7 988 }
7ka884 14:93776ca449a4 989 else if(disXB < goalBX + 5){
kishibekairohan 13:b6e02d6261d7 990 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 991 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 992 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 993 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 994
kishibekairohan 13:b6e02d6261d7 995 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 996 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 997 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 998 motor[TIRE_BL].pwm = 255;
7ka884 14:93776ca449a4 999
7ka884 14:93776ca449a4 1000 wait(1);
kishibekairohan 13:b6e02d6261d7 1001
7ka884 14:93776ca449a4 1002 Rt_flagYB = true;
7ka884 14:93776ca449a4 1003 Rt_flagXB = false;
kishibekairohan 13:b6e02d6261d7 1004 }
kishibekairohan 13:b6e02d6261d7 1005 }
kishibekairohan 13:b6e02d6261d7 1006
kishibekairohan 13:b6e02d6261d7 1007
7ka884 14:93776ca449a4 1008 if(Rt_flagYB && !Rt_flagXB){
7ka884 14:93776ca449a4 1009 filipB();
7ka884 14:93776ca449a4 1010 if(disYB < goalBY - 5){
kishibekairohan 13:b6e02d6261d7 1011 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1012 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1013 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1014 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1015 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1016 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1017 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884 14:93776ca449a4 1018 motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
kishibekairohan 13:b6e02d6261d7 1019 }
7ka884 14:93776ca449a4 1020 else if(disYB > goalBY - 5)
kishibekairohan 13:b6e02d6261d7 1021 {
kishibekairohan 13:b6e02d6261d7 1022 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1023 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1024 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1025 motor[TIRE_BL].dir = BRAKE;
7ka884 14:93776ca449a4 1026 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1027 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1028 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1029 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1030 }
kishibekairohan 13:b6e02d6261d7 1031 }
kishibekairohan 13:b6e02d6261d7 1032 }
kishibekairohan 13:b6e02d6261d7 1033 #endif
kishibekairohan 13:b6e02d6261d7 1034
7ka884 14:93776ca449a4 1035 #if USE_PROCESS_NUM>6//traceA
kishibekairohan 13:b6e02d6261d7 1036 static void Process6()
7ka884 14:93776ca449a4 1037 {
7ka884 14:93776ca449a4 1038 static bool traceon = true;//fase1
kishibekairohan 13:b6e02d6261d7 1039 static bool yokofla = false;//fase2
kishibekairohan 13:b6e02d6261d7 1040 static bool boxslip = false;//fase3
kishibekairohan 13:b6e02d6261d7 1041
kishibekairohan 13:b6e02d6261d7 1042 static bool Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 1043 static bool Rt_flagY = false;
kishibekairohan 13:b6e02d6261d7 1044
7ka884 14:93776ca449a4 1045 ColorDetection();
7ka884 14:93776ca449a4 1046 Color_changeflagA();
kishibekairohan 13:b6e02d6261d7 1047
7ka884 14:93776ca449a4 1048 if(traceon && !Rt_flagX && !Rt_flagY && !boxslip )
kishibekairohan 13:b6e02d6261d7 1049 {
7ka884 14:93776ca449a4 1050 Color_changeflagA();
7ka884 14:93776ca449a4 1051 if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
kishibekairohan 13:b6e02d6261d7 1052 {
kishibekairohan 13:b6e02d6261d7 1053 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1054 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1055 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1056 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1057
7ka884 14:93776ca449a4 1058 motor[TIRE_FR].pwm = 42;
7ka884 14:93776ca449a4 1059 motor[TIRE_FL].pwm = 42;
7ka884 14:93776ca449a4 1060 motor[TIRE_BR].pwm = 42;
7ka884 14:93776ca449a4 1061 motor[TIRE_BL].pwm = 42;
kishibekairohan 13:b6e02d6261d7 1062 }
7ka884 14:93776ca449a4 1063 else if(invationA || compA || invationB || compB || invationC || compC)
kishibekairohan 13:b6e02d6261d7 1064 {
kishibekairohan 13:b6e02d6261d7 1065 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1066 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1067 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1068 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1069
kishibekairohan 13:b6e02d6261d7 1070 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1071 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1072 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1073 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1074
kishibekairohan 13:b6e02d6261d7 1075 yokofla = true;
kishibekairohan 13:b6e02d6261d7 1076 traceon = false;
kishibekairohan 13:b6e02d6261d7 1077 }
kishibekairohan 13:b6e02d6261d7 1078 }
kishibekairohan 13:b6e02d6261d7 1079
kishibekairohan 13:b6e02d6261d7 1080
kishibekairohan 13:b6e02d6261d7 1081 if(yokofla && !traceon)
kishibekairohan 13:b6e02d6261d7 1082 {
7ka884 14:93776ca449a4 1083 Color_changeflagA();
7ka884 14:93776ca449a4 1084 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 1085 {
kishibekairohan 13:b6e02d6261d7 1086 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1087 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1088 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1089 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1090
kishibekairohan 13:b6e02d6261d7 1091 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1092 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1093 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1094 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1095
kishibekairohan 13:b6e02d6261d7 1096 boxslip = true;
kishibekairohan 13:b6e02d6261d7 1097 yokofla = false;
kishibekairohan 13:b6e02d6261d7 1098 }
7ka884 14:93776ca449a4 1099 else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 1100 {
7ka884 14:93776ca449a4 1101 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1102 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1103 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1104 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1105
7ka884 14:93776ca449a4 1106 motor[TIRE_FR].pwm = 55;
7ka884 14:93776ca449a4 1107 motor[TIRE_FL].pwm = 55;
7ka884 14:93776ca449a4 1108 motor[TIRE_BR].pwm = 55;
7ka884 14:93776ca449a4 1109 motor[TIRE_BL].pwm = 55;
kishibekairohan 13:b6e02d6261d7 1110 }
7ka884 14:93776ca449a4 1111 else if((invationA || invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 1112 {
kishibekairohan 13:b6e02d6261d7 1113 motor[TIRE_FR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 1114 motor[TIRE_FL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1115 motor[TIRE_BR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1116 motor[TIRE_BL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 1117
kishibekairohan 13:b6e02d6261d7 1118 //motor[TIRE_FR].pwm = startP;
7ka884 14:93776ca449a4 1119 motor[TIRE_FL].pwm = 55;
7ka884 14:93776ca449a4 1120 motor[TIRE_BR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 1121 //motor[TIRE_BL].pwm = startP;
kishibekairohan 13:b6e02d6261d7 1122 }
7ka884 14:93776ca449a4 1123 else if((!invationA || !invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
kishibekairohan 13:b6e02d6261d7 1124 {
kishibekairohan 13:b6e02d6261d7 1125 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1126 motor[TIRE_FL].dir = FREE;
kishibekairohan 13:b6e02d6261d7 1127 motor[TIRE_BR].dir = FREE;
kishibekairohan 13:b6e02d6261d7 1128 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1129
7ka884 14:93776ca449a4 1130 motor[TIRE_FR].pwm = 55;
kishibekairohan 13:b6e02d6261d7 1131 //motor[TIRE_FL].pwm = startP;
7ka884 14:93776ca449a4 1132 //motor[TIRE_BR].pwm = startP;
7ka884 14:93776ca449a4 1133 motor[TIRE_BL].pwm = 55;
7ka884 14:93776ca449a4 1134 }
7ka884 14:93776ca449a4 1135 else
7ka884 14:93776ca449a4 1136 {
7ka884 14:93776ca449a4 1137 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 1138 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 1139 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 1140 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 1141
7ka884 14:93776ca449a4 1142 motor[TIRE_FR].pwm = 50;
7ka884 14:93776ca449a4 1143 motor[TIRE_FL].pwm = 50;
7ka884 14:93776ca449a4 1144 motor[TIRE_BR].pwm = 50;
7ka884 14:93776ca449a4 1145 motor[TIRE_BL].pwm = 50;
kishibekairohan 13:b6e02d6261d7 1146 }
7ka884 14:93776ca449a4 1147 }
7ka884 14:93776ca449a4 1148
7ka884 14:93776ca449a4 1149
7ka884 14:93776ca449a4 1150 /*********************************/
7ka884 14:93776ca449a4 1151 if(boxslip && !yokofla && !traceon)
7ka884 14:93776ca449a4 1152 {
7ka884 14:93776ca449a4 1153 Color_changeflagA();
7ka884 14:93776ca449a4 1154 if((compA && compB) || compC){
7ka884 14:93776ca449a4 1155
7ka884 14:93776ca449a4 1156 RtX.reset();
7ka884 14:93776ca449a4 1157 RtY.reset();
7ka884 14:93776ca449a4 1158
7ka884 14:93776ca449a4 1159 Rt_flagX = true;
7ka884 14:93776ca449a4 1160 boxslip = false;
7ka884 14:93776ca449a4 1161 }
7ka884 14:93776ca449a4 1162 else if(invationA || invationB && !compC && !(compA && compB)){
7ka884 14:93776ca449a4 1163 motor[TIRE_FR].dir = BACK;
7ka884 14:93776ca449a4 1164 motor[TIRE_FL].dir = BACK;
7ka884 14:93776ca449a4 1165 motor[TIRE_BR].dir = FOR;
7ka884 14:93776ca449a4 1166 motor[TIRE_BL].dir = FOR;
7ka884 14:93776ca449a4 1167
7ka884 14:93776ca449a4 1168 motor[TIRE_FR].pwm = 25;
7ka884 14:93776ca449a4 1169 motor[TIRE_FL].pwm = 25;
7ka884 14:93776ca449a4 1170 motor[TIRE_BR].pwm = 25;
7ka884 14:93776ca449a4 1171 motor[TIRE_BL].pwm = 25;
7ka884 14:93776ca449a4 1172 }
7ka884 14:93776ca449a4 1173 else if(!invationA || !invationB && !compC && !(compA && compB)){
7ka884 14:93776ca449a4 1174 motor[TIRE_FR].dir = FOR;
7ka884 14:93776ca449a4 1175 motor[TIRE_FL].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1176 motor[TIRE_BR].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1177 motor[TIRE_BL].dir = BACK;
kishibekairohan 13:b6e02d6261d7 1178
7ka884 14:93776ca449a4 1179 motor[TIRE_FR].pwm = 25;
7ka884 14:93776ca449a4 1180 motor[TIRE_FL].pwm = 25;
7ka884 14:93776ca449a4 1181 motor[TIRE_BR].pwm = 25;
7ka884 14:93776ca449a4 1182 motor[TIRE_BL].pwm = 25;
7ka884 14:93776ca449a4 1183 }
7ka884 14:93776ca449a4 1184 }
7ka884 14:93776ca449a4 1185
7ka884 14:93776ca449a4 1186
7ka884 14:93776ca449a4 1187 filip();
7ka884 14:93776ca449a4 1188 if(Rt_flagX && !boxslip)
7ka884 14:93776ca449a4 1189 {
7ka884 14:93776ca449a4 1190 filip();
7ka884 14:93776ca449a4 1191 if(disX < goalAX - 5){
kishibekairohan 13:b6e02d6261d7 1192
kishibekairohan 13:b6e02d6261d7 1193 motor[TIRE_FR].dir = FOR;
kishibekairohan 13:b6e02d6261d7 1194 motor[TIRE_FL].dir = FOR;
7ka884 14:93776ca449a4 1195 motor[TIRE_BR].dir = BACK;
7ka884 14:93776ca449a4 1196 motor[TIRE_BL].dir = BACK;
7ka884 14:93776ca449a4 1197 motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884 14:93776ca449a4 1198 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 1199 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 1200 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884 14:93776ca449a4 1201 }
7ka884 14:93776ca449a4 1202 else if(disX > goalAX - 5){
kishibekairohan 13:b6e02d6261d7 1203
kishibekairohan 13:b6e02d6261d7 1204 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1205 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1206 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1207 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1208
kishibekairohan 13:b6e02d6261d7 1209 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1210 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1211 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1212 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1213
7ka884 14:93776ca449a4 1214 wait(1);
kishibekairohan 13:b6e02d6261d7 1215
kishibekairohan 13:b6e02d6261d7 1216 Rt_flagY = true;
kishibekairohan 13:b6e02d6261d7 1217 Rt_flagX = false;
kishibekairohan 13:b6e02d6261d7 1218 }
kishibekairohan 13:b6e02d6261d7 1219 }
kishibekairohan 13:b6e02d6261d7 1220
kishibekairohan 13:b6e02d6261d7 1221
kishibekairohan 13:b6e02d6261d7 1222 if(Rt_flagY && !Rt_flagX){
kishibekairohan 13:b6e02d6261d7 1223 filip();
7ka884 14:93776ca449a4 1224 if(disY < goalAY - 5){
kishibekairohan 13:b6e02d6261d7 1225 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1226 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1227 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 13:b6e02d6261d7 1228 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1229 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1230 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1231 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1232 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan 13:b6e02d6261d7 1233 }
7ka884 14:93776ca449a4 1234 else if(disY > goalAY - 5)
kishibekairohan 13:b6e02d6261d7 1235 {
kishibekairohan 13:b6e02d6261d7 1236 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1237 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1238 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1239 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 13:b6e02d6261d7 1240 motor[TIRE_FR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1241 motor[TIRE_FL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1242 motor[TIRE_BR].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1243 motor[TIRE_BL].pwm = 255;
kishibekairohan 13:b6e02d6261d7 1244 }
kishibekairohan 13:b6e02d6261d7 1245 }
kishibekairohan 13:b6e02d6261d7 1246 }
kishibekairohan 13:b6e02d6261d7 1247 #endif
kishibekairohan 13:b6e02d6261d7 1248
kishibekairohan 13:b6e02d6261d7 1249 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 1250 static void Process7()
kishibekairohan 13:b6e02d6261d7 1251 {
kishibekairohan 12:c09b3e08a316 1252 tapeLED.code = (uint32_t)Hotpink;
kishibekairohan 12:c09b3e08a316 1253 static bool Xnopush = false;
kishibekairohan 12:c09b3e08a316 1254 static bool Ynopush = false;
kishibekairohan 12:c09b3e08a316 1255 static bool Inopush = false;
kishibekairohan 12:c09b3e08a316 1256
kishibekairohan 12:c09b3e08a316 1257 static bool Angle_flagX = false;
kishibekairohan 12:c09b3e08a316 1258 static bool Angle_flagY = false;
kishibekairohan 12:c09b3e08a316 1259 static bool ANgle_flagI = false;
kishibekairohan 12:c09b3e08a316 1260
kishibekairohan 12:c09b3e08a316 1261 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 12:c09b3e08a316 1262 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1263 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1264 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1265 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1266 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 12:c09b3e08a316 1267 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1268 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1269 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1270 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1271 }
kishibekairohan 12:c09b3e08a316 1272 for(int i = 0;i<20;i++){
kishibekairohan 12:c09b3e08a316 1273 float y = 0;
kishibekairohan 12:c09b3e08a316 1274 y = acc[1]*1000;
kishibekairohan 12:c09b3e08a316 1275 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 12:c09b3e08a316 1276 Angle += rotateY;
kishibekairohan 12:c09b3e08a316 1277 }
kishibekairohan 12:c09b3e08a316 1278 Angle = Angle /20;
kishibekairohan 12:c09b3e08a316 1279
kishibekairohan 12:c09b3e08a316 1280 int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan 12:c09b3e08a316 1281 int gyropwmY = gyro.SetPV(Angle,AngletargetY);
kishibekairohan 12:c09b3e08a316 1282 int gyropwmI = gyro.SetPV(Angle,AngletargetI);
kishibekairohan 12:c09b3e08a316 1283
kishibekairohan 12:c09b3e08a316 1284 if(controller->Button.X && !Xnopush){
kishibekairohan 12:c09b3e08a316 1285 Angle_flagX = true;
kishibekairohan 12:c09b3e08a316 1286 Xnopush = true;
kishibekairohan 12:c09b3e08a316 1287 }else if(!controller->Button.X)Xnopush = false;
kishibekairohan 12:c09b3e08a316 1288
kishibekairohan 12:c09b3e08a316 1289 if(controller->Button.Y && !Ynopush){
kishibekairohan 12:c09b3e08a316 1290 Angle_flagY = true;
kishibekairohan 12:c09b3e08a316 1291 Ynopush = true;
kishibekairohan 12:c09b3e08a316 1292 }else if(!controller->Button.Y)Ynopush = false;
kishibekairohan 12:c09b3e08a316 1293
kishibekairohan 12:c09b3e08a316 1294 if(controller->Button.A && !Inopush){
kishibekairohan 12:c09b3e08a316 1295 Angle_flagI = true;
kishibekairohan 12:c09b3e08a316 1296 Inopush = true;
kishibekairohan 12:c09b3e08a316 1297 }else if(!controller->Button.A)Inopush = false;
kishibekairohan 12:c09b3e08a316 1298
kishibekairohan 12:c09b3e08a316 1299 if (Angle_flagX){
kishibekairohan 12:c09b3e08a316 1300 motor[Angle_R].dir = SetStatus(gyropwmX);
kishibekairohan 12:c09b3e08a316 1301 motor[Angle_L].dir = SetStatus(-gyropwmX);
kishibekairohan 12:c09b3e08a316 1302 motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan 12:c09b3e08a316 1303 motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan 12:c09b3e08a316 1304 if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan 12:c09b3e08a316 1305 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1306 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1307 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1308 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1309 Angle_flagX = false;
kishibekairohan 12:c09b3e08a316 1310 }
kishibekairohan 12:c09b3e08a316 1311 }
kishibekairohan 12:c09b3e08a316 1312
kishibekairohan 12:c09b3e08a316 1313 if (Angle_flagY){
kishibekairohan 12:c09b3e08a316 1314 motor[Angle_R].dir = SetStatus(-gyropwmY);
kishibekairohan 12:c09b3e08a316 1315 motor[Angle_L].dir = SetStatus(gyropwmY);
kishibekairohan 12:c09b3e08a316 1316 motor[Angle_R].pwm = SetPWM(gyropwmY);
kishibekairohan 12:c09b3e08a316 1317 motor[Angle_L].pwm = SetPWM(gyropwmY);
kishibekairohan 12:c09b3e08a316 1318 if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
kishibekairohan 12:c09b3e08a316 1319 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1320 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1321 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1322 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1323 Angle_flagY = false;
kishibekairohan 12:c09b3e08a316 1324 }
kishibekairohan 12:c09b3e08a316 1325 }
kishibekairohan 12:c09b3e08a316 1326
kishibekairohan 12:c09b3e08a316 1327 if (Angle_flagI){
kishibekairohan 12:c09b3e08a316 1328 if(Angle < 0)
kishibekairohan 12:c09b3e08a316 1329 {
kishibekairohan 12:c09b3e08a316 1330 motor[Angle_R].dir = SetStatus(-gyropwmI);
kishibekairohan 12:c09b3e08a316 1331 motor[Angle_L].dir = SetStatus(gyropwmI);
kishibekairohan 12:c09b3e08a316 1332 motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1333 motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1334
kishibekairohan 12:c09b3e08a316 1335 if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan 12:c09b3e08a316 1336 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1337 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1338 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1339 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1340 Angle_flagI = false;
kishibekairohan 12:c09b3e08a316 1341 }
kishibekairohan 12:c09b3e08a316 1342 }
kishibekairohan 12:c09b3e08a316 1343 else if(Angle > 0)
kishibekairohan 12:c09b3e08a316 1344 {
kishibekairohan 12:c09b3e08a316 1345 motor[Angle_R].dir = FOR;
kishibekairohan 12:c09b3e08a316 1346 motor[Angle_L].dir = BACK;
kishibekairohan 12:c09b3e08a316 1347 motor[Angle_R].pwm = 150;
kishibekairohan 12:c09b3e08a316 1348 motor[Angle_L].pwm = 150;
kishibekairohan 12:c09b3e08a316 1349
kishibekairohan 12:c09b3e08a316 1350 if(Angle < 0){
kishibekairohan 12:c09b3e08a316 1351 motor[Angle_R].dir = SetStatus(gyropwmI);
kishibekairohan 12:c09b3e08a316 1352 motor[Angle_L].dir = SetStatus(-gyropwmI);
kishibekairohan 12:c09b3e08a316 1353 motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1354 motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan 12:c09b3e08a316 1355
kishibekairohan 12:c09b3e08a316 1356 if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan 12:c09b3e08a316 1357 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1358 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1359 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1360 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1361 Angle_flagI = false;
kishibekairohan 12:c09b3e08a316 1362 }
kishibekairohan 12:c09b3e08a316 1363 }
kishibekairohan 12:c09b3e08a316 1364 }
kishibekairohan 12:c09b3e08a316 1365 }
kishibekairohan 12:c09b3e08a316 1366 else{
kishibekairohan 12:c09b3e08a316 1367 motor[Angle_R].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1368 motor[Angle_L].dir = BRAKE;
kishibekairohan 12:c09b3e08a316 1369 motor[Angle_R].pwm = 255;
kishibekairohan 12:c09b3e08a316 1370 motor[Angle_L].pwm = 255;
kishibekairohan 12:c09b3e08a316 1371 }
kishibekairohan 12:c09b3e08a316 1372 }
kishibekairohan 12:c09b3e08a316 1373 #endif
kishibekairohan 12:c09b3e08a316 1374
kishibekairohan 13:b6e02d6261d7 1375 #if USE_PROCESS_NUM>8 //kakudo
kishibekairohan 13:b6e02d6261d7 1376 static void Process8()
kishibekairohan 12:c09b3e08a316 1377 {
kishibekairohan 12:c09b3e08a316 1378 }
kishibekairohan 12:c09b3e08a316 1379 #endif
kishibekairohan 12:c09b3e08a316 1380
kishibekairohan 13:b6e02d6261d7 1381 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 1382 static void Process9()
kishibekairohan 12:c09b3e08a316 1383 {
kishibekairohan 2:c015739085d3 1384
t_yamamoto 0:669ef71cba68 1385 }
t_yamamoto 0:669ef71cba68 1386 #endif
t_yamamoto 0:669ef71cba68 1387 #endif
t_yamamoto 0:669ef71cba68 1388 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1389
t_yamamoto 0:669ef71cba68 1390 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1391 {
t_yamamoto 0:669ef71cba68 1392
t_yamamoto 0:669ef71cba68 1393 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 1394 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 1395 #endif
t_yamamoto 0:669ef71cba68 1396
t_yamamoto 0:669ef71cba68 1397 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 1398 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 1399 {
t_yamamoto 0:669ef71cba68 1400 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 1401 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 1402 }
t_yamamoto 0:669ef71cba68 1403 #endif
t_yamamoto 0:669ef71cba68 1404 }
t_yamamoto 0:669ef71cba68 1405
t_yamamoto 0:669ef71cba68 1406 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 8:6fb3723f7747 1407
kishibekairohan 8:6fb3723f7747 1408 void filip(){
kishibekairohan 8:6fb3723f7747 1409 palseX = RtX.getPulses();
kishibekairohan 8:6fb3723f7747 1410 palseY = RtY.getPulses();
7ka884 6:10e22bc327ce 1411
kishibekairohan 9:f93fc79a49ea 1412 rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1413 rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1414
kishibekairohan 10:1295d39fec3a 1415 disX = 48*3.141*rpmX;
kishibekairohan 9:f93fc79a49ea 1416 disY = 48*3.141*rpmY;
kishibekairohan 9:f93fc79a49ea 1417
7ka884 14:93776ca449a4 1418 RtpwmX = (int)Rt_X.SetPV(disX , goalAX);
7ka884 14:93776ca449a4 1419 RtpwmY = (int)Rt_Y.SetPV(disY , goalAY);
7ka884 14:93776ca449a4 1420 }
7ka884 14:93776ca449a4 1421
7ka884 14:93776ca449a4 1422 void filipB(){
7ka884 14:93776ca449a4 1423 palseX = RtX.getPulses();
7ka884 14:93776ca449a4 1424 palseY = RtY.getPulses();
7ka884 14:93776ca449a4 1425
7ka884 14:93776ca449a4 1426 rpmXB = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
7ka884 14:93776ca449a4 1427 rpmYB = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
7ka884 14:93776ca449a4 1428
7ka884 14:93776ca449a4 1429 disXB = 48*3.141*rpmXB;
7ka884 14:93776ca449a4 1430 disYB = 48*3.141*rpmYB;
7ka884 14:93776ca449a4 1431
7ka884 14:93776ca449a4 1432 RtpwmXB = (int)Rt_X.SetPV(disXB , goalBX);
7ka884 14:93776ca449a4 1433 RtpwmYB = (int)Rt_Y.SetPV(disYB , goalBY);
kishibekairohan 8:6fb3723f7747 1434 }
7ka884 6:10e22bc327ce 1435
kishibekairohan 12:c09b3e08a316 1436 unsigned long ColorIn(int index)
kishibekairohan 12:c09b3e08a316 1437 {
kishibekairohan 12:c09b3e08a316 1438 int result = 0;
kishibekairohan 12:c09b3e08a316 1439 bool rtn = false;
kishibekairohan 12:c09b3e08a316 1440 for(int i=0; i<12; i++)
kishibekairohan 12:c09b3e08a316 1441 {
kishibekairohan 12:c09b3e08a316 1442 CK[index] = 1;
kishibekairohan 12:c09b3e08a316 1443 rtn = DOUT[index];
kishibekairohan 12:c09b3e08a316 1444 CK[index] = 0;
kishibekairohan 12:c09b3e08a316 1445 if(rtn)
kishibekairohan 12:c09b3e08a316 1446 {
kishibekairohan 12:c09b3e08a316 1447 result|=(1 << i);
kishibekairohan 12:c09b3e08a316 1448 }
kishibekairohan 12:c09b3e08a316 1449 }
kishibekairohan 12:c09b3e08a316 1450 return result;
kishibekairohan 12:c09b3e08a316 1451 }
7ka884 6:10e22bc327ce 1452
kishibekairohan 2:c015739085d3 1453 void ColorDetection(){
kishibekairohan 2:c015739085d3 1454 GATE = 0;
kishibekairohan 2:c015739085d3 1455
kishibekairohan 2:c015739085d3 1456 CK[0] = 0;
kishibekairohan 2:c015739085d3 1457 CK[1] = 0;
kishibekairohan 2:c015739085d3 1458 CK[2] = 0;
kishibekairohan 2:c015739085d3 1459 CK[3] = 0;
kishibekairohan 2:c015739085d3 1460
kishibekairohan 2:c015739085d3 1461 RANGE = 1;
kishibekairohan 2:c015739085d3 1462
kishibekairohan 2:c015739085d3 1463 GATE = 1;
kishibekairohan 2:c015739085d3 1464 wait_ms(intergration);
kishibekairohan 2:c015739085d3 1465 GATE = 0;
kishibekairohan 2:c015739085d3 1466 wait_us(4);
kishibekairohan 2:c015739085d3 1467
kishibekairohan 2:c015739085d3 1468 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 1469 wait_us(3);
kishibekairohan 2:c015739085d3 1470 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 1471 wait_us(3);
kishibekairohan 2:c015739085d3 1472 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 1473
kishibekairohan 2:c015739085d3 1474 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 1475 wait_us(3);
kishibekairohan 2:c015739085d3 1476 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 1477 wait_us(3);
kishibekairohan 2:c015739085d3 1478 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 1479
kishibekairohan 2:c015739085d3 1480 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 1481 wait_us(3);
kishibekairohan 2:c015739085d3 1482 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 1483 wait_us(3);
kishibekairohan 2:c015739085d3 1484 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 1485
kishibekairohan 2:c015739085d3 1486 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 1487 wait_us(3);
kishibekairohan 2:c015739085d3 1488 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 1489 wait_us(3);
kishibekairohan 2:c015739085d3 1490 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 1491 }
kishibekairohan 12:c09b3e08a316 1492
7ka884 14:93776ca449a4 1493 void Color_changeflagA(){
kishibekairohan 13:b6e02d6261d7 1494 ColorDetection();
kishibekairohan 13:b6e02d6261d7 1495
kishibekairohan 13:b6e02d6261d7 1496 if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
kishibekairohan 13:b6e02d6261d7 1497 {
kishibekairohan 13:b6e02d6261d7 1498 compA = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1499 }
7ka884 14:93776ca449a4 1500 else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
kishibekairohan 13:b6e02d6261d7 1501
kishibekairohan 13:b6e02d6261d7 1502 if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
kishibekairohan 13:b6e02d6261d7 1503 {
kishibekairohan 13:b6e02d6261d7 1504 compB = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1505 }
7ka884 14:93776ca449a4 1506 else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
kishibekairohan 13:b6e02d6261d7 1507
kishibekairohan 13:b6e02d6261d7 1508 if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
kishibekairohan 13:b6e02d6261d7 1509 {
kishibekairohan 13:b6e02d6261d7 1510 compC = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1511 }
7ka884 14:93776ca449a4 1512 else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
kishibekairohan 13:b6e02d6261d7 1513 /*
kishibekairohan 13:b6e02d6261d7 1514 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan 13:b6e02d6261d7 1515 {
kishibekairohan 13:b6e02d6261d7 1516 invationD ^= 1;//start false,over true
kishibekairohan 13:b6e02d6261d7 1517 compD = true;//on true,noon false
kishibekairohan 13:b6e02d6261d7 1518 }
kishibekairohan 13:b6e02d6261d7 1519 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
kishibekairohan 13:b6e02d6261d7 1520 */
7ka884 14:93776ca449a4 1521
7ka884 14:93776ca449a4 1522 if(compA && !compB)
7ka884 14:93776ca449a4 1523 {
7ka884 14:93776ca449a4 1524 invationA = false;
7ka884 14:93776ca449a4 1525 }
7ka884 14:93776ca449a4 1526 else if(!compA && compB)
7ka884 14:93776ca449a4 1527 {
7ka884 14:93776ca449a4 1528 invationA = true;
7ka884 14:93776ca449a4 1529 }
7ka884 14:93776ca449a4 1530
7ka884 14:93776ca449a4 1531 if(!invationA && !compA && !compB){
7ka884 14:93776ca449a4 1532 invationB = false;
7ka884 14:93776ca449a4 1533 }
7ka884 14:93776ca449a4 1534 else if(invationA && !compA && !compB){
7ka884 14:93776ca449a4 1535 invationB = true;
7ka884 14:93776ca449a4 1536 }
7ka884 14:93776ca449a4 1537 }
7ka884 14:93776ca449a4 1538
7ka884 14:93776ca449a4 1539 void Color_changeflagB(){
7ka884 14:93776ca449a4 1540 ColorDetection();
7ka884 14:93776ca449a4 1541
7ka884 14:93776ca449a4 1542 if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
7ka884 14:93776ca449a4 1543 {
7ka884 14:93776ca449a4 1544 compA = true;//on true,noon false
7ka884 14:93776ca449a4 1545 }
7ka884 14:93776ca449a4 1546 else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
7ka884 14:93776ca449a4 1547
7ka884 14:93776ca449a4 1548 if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
7ka884 14:93776ca449a4 1549 {
7ka884 14:93776ca449a4 1550 compB = true;//on true,noon false
7ka884 14:93776ca449a4 1551 }
7ka884 14:93776ca449a4 1552 else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
7ka884 14:93776ca449a4 1553
7ka884 14:93776ca449a4 1554 if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
7ka884 14:93776ca449a4 1555 {
7ka884 14:93776ca449a4 1556 compC = true;//on true,noon false
7ka884 14:93776ca449a4 1557 }
7ka884 14:93776ca449a4 1558 else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
7ka884 14:93776ca449a4 1559 /*
7ka884 14:93776ca449a4 1560 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
7ka884 14:93776ca449a4 1561 {
7ka884 14:93776ca449a4 1562 invationD ^= 1;//start false,over true
7ka884 14:93776ca449a4 1563 compD = true;//on true,noon false
7ka884 14:93776ca449a4 1564 }
7ka884 14:93776ca449a4 1565 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
7ka884 14:93776ca449a4 1566 */
7ka884 14:93776ca449a4 1567
7ka884 14:93776ca449a4 1568 if(compB && !compA)
7ka884 14:93776ca449a4 1569 {
7ka884 14:93776ca449a4 1570 invationB = false;
7ka884 14:93776ca449a4 1571 }
7ka884 14:93776ca449a4 1572 else if(!compB && compA)
7ka884 14:93776ca449a4 1573 {
7ka884 14:93776ca449a4 1574 invationB = true;
7ka884 14:93776ca449a4 1575 }
7ka884 14:93776ca449a4 1576
7ka884 14:93776ca449a4 1577 if(!invationB && !compA && !compB){
7ka884 14:93776ca449a4 1578 invationA = false;
7ka884 14:93776ca449a4 1579 }
7ka884 14:93776ca449a4 1580 else if(invationB && !compA && !compB){
7ka884 14:93776ca449a4 1581 invationA = true;
7ka884 14:93776ca449a4 1582 }
kishibekairohan 13:b6e02d6261d7 1583 }
kishibekairohan 13:b6e02d6261d7 1584
kishibekairohan 12:c09b3e08a316 1585 void getcolor(){
kishibekairohan 13:b6e02d6261d7 1586 for(int i=0;i<10;i++){
kishibekairohan 12:c09b3e08a316 1587 ColorDetection();
kishibekairohan 12:c09b3e08a316 1588
kishibekairohan 12:c09b3e08a316 1589 Avecolor_A[0] += Color_A[0];
kishibekairohan 12:c09b3e08a316 1590 Avecolor_A[1] += Color_A[1];
kishibekairohan 12:c09b3e08a316 1591 Avecolor_A[2] += Color_A[2];
kishibekairohan 12:c09b3e08a316 1592 Avecolor_B[0] += Color_B[0];
kishibekairohan 12:c09b3e08a316 1593 Avecolor_B[1] += Color_B[1];
kishibekairohan 12:c09b3e08a316 1594 Avecolor_B[2] += Color_B[2];
kishibekairohan 12:c09b3e08a316 1595 Avecolor_C[0] += Color_C[0];
kishibekairohan 12:c09b3e08a316 1596 Avecolor_C[1] += Color_C[1];
kishibekairohan 12:c09b3e08a316 1597 Avecolor_C[2] += Color_C[2];
kishibekairohan 12:c09b3e08a316 1598 Avecolor_D[0] += Color_D[0];
kishibekairohan 12:c09b3e08a316 1599 Avecolor_D[1] += Color_D[1];
kishibekairohan 12:c09b3e08a316 1600 Avecolor_D[2] += Color_D[2];
kishibekairohan 12:c09b3e08a316 1601 }
kishibekairohan 12:c09b3e08a316 1602
kishibekairohan 13:b6e02d6261d7 1603 Avecolor_A[0] = Avecolor_A[0]/10;
kishibekairohan 13:b6e02d6261d7 1604 Avecolor_A[1] = Avecolor_A[1]/10;
kishibekairohan 13:b6e02d6261d7 1605 Avecolor_A[2] = Avecolor_A[2]/10;
kishibekairohan 13:b6e02d6261d7 1606 Avecolor_B[0] = Avecolor_B[0]/10;
kishibekairohan 13:b6e02d6261d7 1607 Avecolor_B[1] = Avecolor_B[1]/10;
kishibekairohan 13:b6e02d6261d7 1608 Avecolor_B[2] = Avecolor_B[2]/10;
kishibekairohan 13:b6e02d6261d7 1609 Avecolor_C[0] = Avecolor_C[0]/10;
kishibekairohan 13:b6e02d6261d7 1610 Avecolor_C[1] = Avecolor_C[1]/10;
kishibekairohan 13:b6e02d6261d7 1611 Avecolor_C[2] = Avecolor_C[2]/10;
kishibekairohan 13:b6e02d6261d7 1612 Avecolor_D[0] = Avecolor_D[0]/10;
kishibekairohan 13:b6e02d6261d7 1613 Avecolor_D[1] = Avecolor_D[1]/10;
kishibekairohan 13:b6e02d6261d7 1614 Avecolor_D[2] = Avecolor_D[2]/10;
kishibekairohan 13:b6e02d6261d7 1615
kishibekairohan 12:c09b3e08a316 1616 }
kishibekairohan 12:c09b3e08a316 1617
kishibekairohan 9:f93fc79a49ea 1618 void BuzzerTimer_func() {
kishibekairohan 11:028a150943b5 1619 if(buzzer == 0.5){
kishibekairohan 11:028a150943b5 1620 buzzer = 0;
kishibekairohan 11:028a150943b5 1621 }
kishibekairohan 11:028a150943b5 1622 else if(buzzer == 0){
kishibekairohan 11:028a150943b5 1623 buzzer = 0.5;
kishibekairohan 11:028a150943b5 1624 }
kishibekairohan 11:028a150943b5 1625 }
kishibekairohan 11:028a150943b5 1626
kishibekairohan 11:028a150943b5 1627 void TapeLedEms_func() {
kishibekairohan 11:028a150943b5 1628 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1629 }
7ka884 14:93776ca449a4 1630
7ka884 14:93776ca449a4 1631 void LostLed_func(){
7ka884 14:93776ca449a4 1632 sendLedData.code = sendLedData.code == (uint32_t)Yellow ? (uint32_t)Black : (uint32_t)Yellow;
7ka884 14:93776ca449a4 1633 }
t_yamamoto 0:669ef71cba68 1634 #pragma endregion