大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_16

da

Dependencies:   mbed TrapezoidControl QEI

System/Process/Process.cpp@11:028a150943b5, 2018-10-06 (annotated)

Committer:
kishibekairohan
Date:
Sat Oct 06 08:30:58 2018 +0000
Revision:
11:028a150943b5
Parent:
10:1295d39fec3a
Child:
12:c09b3e08a316
ll

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"
7ka884 4:ba9df71868df 3 #include "QEI.h"
t_yamamoto 0:669ef71cba68 4
7ka884 4:ba9df71868df 5 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 6 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
t_yamamoto 0:669ef71cba68 7 #include "../../Communication/Controller/Controller.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/Switch/Switch.h"
7ka884 1:b1219d8ca117 10 #include "../../Input/ColorSensor/ColorSensor.h"
7ka884 1:b1219d8ca117 11 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Input/Potentiometer/Potentiometer.h"
7ka884 1:b1219d8ca117 13 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto 0:669ef71cba68 14 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 15 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 16 #include "../Using.h"
t_yamamoto 0:669ef71cba68 17
kishibekairohan 2:c015739085d3 18
t_yamamoto 0:669ef71cba68 19 using namespace SWITCH;
kishibekairohan 2:c015739085d3 20 using namespace COLORSENSOR;
kishibekairohan 2:c015739085d3 21 using namespace ACCELERATIONSENSOR;
7ka884 4:ba9df71868df 22 using namespace PID_SPACE;
7ka884 4:ba9df71868df 23 using namespace ROTARYENCODER;
t_yamamoto 0:669ef71cba68 24
t_yamamoto 0:669ef71cba68 25 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 27 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 static bool lock;
t_yamamoto 0:669ef71cba68 30 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 31 static int current;
t_yamamoto 0:669ef71cba68 32
t_yamamoto 0:669ef71cba68 33 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 36 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 37 #endif
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 40
t_yamamoto 0:669ef71cba68 41 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 42
kishibekairohan 2:c015739085d3 43 Serial pc(USBTX, USBRX);
kishibekairohan 2:c015739085d3 44
kishibekairohan 9:f93fc79a49ea 45 #define TIRE_FR 0 //足回り前右
kishibekairohan 9:f93fc79a49ea 46 #define TIRE_FL 1 //足回り前左
7ka884 4:ba9df71868df 47 #define TIRE_BR 2 //足回り後右
7ka884 4:ba9df71868df 48 #define TIRE_BL 3 //足回り後左
kishibekairohan 2:c015739085d3 49
kishibekairohan 9:f93fc79a49ea 50 #define Angle_R 4 //角度調節右
kishibekairohan 9:f93fc79a49ea 51 #define Angle_L 5 //角度調節左
kishibekairohan 8:6fb3723f7747 52
kishibekairohan 7:e88c5d47a3be 53 #define Lim_AR 3 //角度調節右
kishibekairohan 7:e88c5d47a3be 54 #define Lim_AL 4 //角度調節左
kishibekairohan 7:e88c5d47a3be 55 #define Lim_R 0 //センター右
kishibekairohan 7:e88c5d47a3be 56 #define Lim_L 1 //センター左
kishibekairohan 9:f93fc79a49ea 57 #define EMS_0 LimitSw::IsPressed(8)
kishibekairohan 9:f93fc79a49ea 58 #define EMS_1 LimitSw::IsPressed(9)
kishibekairohan 11:028a150943b5 59 #define LS LimitSw::IsPressed(7) //赤ゾーン用スイッチ
kishibekairohan 11:028a150943b5 60 #define BS LimitSw::IsPressed(6) //青ゾーン用スイッチ
kishibekairohan 7:e88c5d47a3be 61 //************メカナム********************
kishibekairohan 2:c015739085d3 62
kishibekairohan 2:c015739085d3 63 const int mecanum[15][15]=
kishibekairohan 2:c015739085d3 64 {
kishibekairohan 2:c015739085d3 65 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 66 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan 2:c015739085d3 67 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan 2:c015739085d3 68 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan 2:c015739085d3 69 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan 2:c015739085d3 70 {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan 2:c015739085d3 71 {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan 2:c015739085d3 72 {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan 2:c015739085d3 73 {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan 2:c015739085d3 74 {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan 2:c015739085d3 75 {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan 2:c015739085d3 76 {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan 2:c015739085d3 77 {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan 2:c015739085d3 78 {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan 2:c015739085d3 79 {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan 2:c015739085d3 80 };
kishibekairohan 2:c015739085d3 81
kishibekairohan 2:c015739085d3 82 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan 2:c015739085d3 83 uint8_t SetStatus(int);
kishibekairohan 2:c015739085d3 84 uint8_t SetStatus(int pwmVal){
kishibekairohan 2:c015739085d3 85 if(pwmVal < 0) return BACK;
kishibekairohan 2:c015739085d3 86 else if(pwmVal > 0) return FOR;
kishibekairohan 2:c015739085d3 87 else if(pwmVal == 0) return BRAKE;
kishibekairohan 2:c015739085d3 88 else return BRAKE;
kishibekairohan 2:c015739085d3 89 }
kishibekairohan 2:c015739085d3 90 uint8_t SetPWM(int);
kishibekairohan 2:c015739085d3 91 uint8_t SetPWM(int pwmVal){
kishibekairohan 2:c015739085d3 92 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
kishibekairohan 2:c015739085d3 93 else return abs(pwmVal);
kishibekairohan 2:c015739085d3 94 }
kishibekairohan 2:c015739085d3 95
kishibekairohan 7:e88c5d47a3be 96 //************メカナム********************
kishibekairohan 7:e88c5d47a3be 97
kishibekairohan 7:e88c5d47a3be 98 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 99
kishibekairohan 2:c015739085d3 100 int Color_A[3]; //[赤,緑,青]
kishibekairohan 2:c015739085d3 101 int Color_B[3];
kishibekairohan 2:c015739085d3 102 int Color_C[3];
kishibekairohan 2:c015739085d3 103 int Color_D[3];
kishibekairohan 2:c015739085d3 104 int intergration = 50;
kishibekairohan 2:c015739085d3 105
kishibekairohan 11:028a150943b5 106 unsigned long ColorIn(int index)
kishibekairohan 11:028a150943b5 107 {
kishibekairohan 11:028a150943b5 108 int result = 0;
kishibekairohan 11:028a150943b5 109 bool rtn = false;
kishibekairohan 11:028a150943b5 110 for(int i=0; i<12; i++)
kishibekairohan 11:028a150943b5 111 {
kishibekairohan 11:028a150943b5 112 CK[index] = 1;
kishibekairohan 11:028a150943b5 113 rtn = DOUT[index];
kishibekairohan 11:028a150943b5 114 CK[index] = 0;
kishibekairohan 11:028a150943b5 115 if(rtn)
kishibekairohan 11:028a150943b5 116 {
kishibekairohan 11:028a150943b5 117 result|=(1 << i);
kishibekairohan 11:028a150943b5 118 }
kishibekairohan 11:028a150943b5 119 }
kishibekairohan 11:028a150943b5 120 return result;
kishibekairohan 11:028a150943b5 121 }
kishibekairohan 7:e88c5d47a3be 122 void ColorDetection();
kishibekairohan 7:e88c5d47a3be 123
kishibekairohan 7:e88c5d47a3be 124 //************カラーセンサ********************
kishibekairohan 7:e88c5d47a3be 125
kishibekairohan 2:c015739085d3 126 //************ライントレース変数*******************
kishibekairohan 7:e88c5d47a3be 127 int Point[3] = {234, 466, 590};//赤，緑，青
kishibekairohan 2:c015739085d3 128
kishibekairohan 7:e88c5d47a3be 129 int startP = 35;
kishibekairohan 7:e88c5d47a3be 130 int downP = 5;
kishibekairohan 8:6fb3723f7747 131
kishibekairohan 8:6fb3723f7747 132 int Asasult = 0;
kishibekairohan 8:6fb3723f7747 133 int Bsasult = 0;
kishibekairohan 8:6fb3723f7747 134 int Csasult = 0;
kishibekairohan 8:6fb3723f7747 135 int Dsasult = 0;
kishibekairohan 8:6fb3723f7747 136
kishibekairohan 8:6fb3723f7747 137 void pointcalculation();
kishibekairohan 8:6fb3723f7747 138
kishibekairohan 8:6fb3723f7747 139 Ticker Color_T;
kishibekairohan 2:c015739085d3 140 //************ライントレース変数*******************
kishibekairohan 9:f93fc79a49ea 141
kishibekairohan 9:f93fc79a49ea 142
kishibekairohan 10:1295d39fec3a 143 //************ROタコン******************
kishibekairohan 9:f93fc79a49ea 144 QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 9:f93fc79a49ea 145 QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan 8:6fb3723f7747 146 Ticker get_rpm;
kishibekairohan 8:6fb3723f7747 147 PID Rt_X = PID(0.03, -255, 255, 0.1, 0, 0);
kishibekairohan 8:6fb3723f7747 148 PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
kishibekairohan 9:f93fc79a49ea 149 double rpmX;
kishibekairohan 9:f93fc79a49ea 150 double rpmY;
kishibekairohan 9:f93fc79a49ea 151 double disX;
kishibekairohan 9:f93fc79a49ea 152 double disY;
kishibekairohan 8:6fb3723f7747 153 int palseX;
kishibekairohan 8:6fb3723f7747 154 int palseY;
kishibekairohan 8:6fb3723f7747 155 int RtpwmX;
kishibekairohan 8:6fb3723f7747 156 int RtpwmY;
kishibekairohan 8:6fb3723f7747 157 double goalX = 1200.000;
kishibekairohan 8:6fb3723f7747 158 double goalY = 900.000;
kishibekairohan 8:6fb3723f7747 159 void filip();
kishibekairohan 8:6fb3723f7747 160 //PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
7ka884 4:ba9df71868df 161
kishibekairohan 10:1295d39fec3a 162 //************ROタコン******************
kishibekairohan 2:c015739085d3 163
kishibekairohan 7:e88c5d47a3be 164 //************ジャイロ*******************
kishibekairohan 9:f93fc79a49ea 165
kishibekairohan 9:f93fc79a49ea 166 bool Angle_flagI = false;
kishibekairohan 11:028a150943b5 167 int Angle;
kishibekairohan 7:e88c5d47a3be 168 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan 7:e88c5d47a3be 169 float rotateY;
kishibekairohan 11:028a150943b5 170 //初期値 -5
kishibekairohan 11:028a150943b5 171 int AngletargetX = 4;
kishibekairohan 11:028a150943b5 172 int AngletargetY = -12;
kishibekairohan 7:e88c5d47a3be 173 int Angle_I = -5;
kishibekairohan 7:e88c5d47a3be 174 //************ジャイロ*******************
kishibekairohan 2:c015739085d3 175
kishibekairohan 9:f93fc79a49ea 176 //************Buzzer******************
kishibekairohan 11:028a150943b5 177 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 11:028a150943b5 178 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 179 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 180 Ticker BuzzerTimer;
kishibekairohan 9:f93fc79a49ea 181 bool Emsflag = false;
kishibekairohan 9:f93fc79a49ea 182 //************Buzzer******************
kishibekairohan 11:028a150943b5 183
kishibekairohan 11:028a150943b5 184 //************TapeLed*****************
kishibekairohan 11:028a150943b5 185 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 186 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 187 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 188 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 189 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 190 //************TapaLed*****************
t_yamamoto 0:669ef71cba68 191 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 192
t_yamamoto 0:669ef71cba68 193 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 195 static void Process0(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 198 static void Process1(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 201 static void Process2(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 204 static void Process3(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 207 static void Process4(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 210 static void Process5(void);
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 213 static void Process6(void);
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 216 static void Process7(void);
t_yamamoto 0:669ef71cba68 217 #endif
t_yamamoto 0:669ef71cba68 218 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 219 static void Process8(void);
t_yamamoto 0:669ef71cba68 220 #endif
t_yamamoto 0:669ef71cba68 221 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 222 static void Process9(void);
t_yamamoto 0:669ef71cba68 223 #endif
t_yamamoto 0:669ef71cba68 224 #endif
t_yamamoto 0:669ef71cba68 225
t_yamamoto 0:669ef71cba68 226 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 227 {
t_yamamoto 0:669ef71cba68 228 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 11:028a150943b5 229 /*Replace here with the initialization code of your variables.*/
kishibekairohan 9:f93fc79a49ea 230 get_rpm.attach_us(&filip,100);
kishibekairohan 11:028a150943b5 231 buzzer.period(1.0/800);
t_yamamoto 0:669ef71cba68 232
t_yamamoto 0:669ef71cba68 233 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 234
t_yamamoto 0:669ef71cba68 235 lock = true;
t_yamamoto 0:669ef71cba68 236 processChangeComp = true;
t_yamamoto 0:669ef71cba68 237 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 238
t_yamamoto 0:669ef71cba68 239 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 240 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 241 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 242 #endif
t_yamamoto 0:669ef71cba68 243 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 244 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 245 #endif
t_yamamoto 0:669ef71cba68 246 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 247 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 248 #endif
t_yamamoto 0:669ef71cba68 249 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 250 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 251 #endif
t_yamamoto 0:669ef71cba68 252 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 253 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 254 #endif
t_yamamoto 0:669ef71cba68 255 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 256 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 257 #endif
t_yamamoto 0:669ef71cba68 258 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 259 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 260 #endif
t_yamamoto 0:669ef71cba68 261 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 262 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 263 #endif
t_yamamoto 0:669ef71cba68 264 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 265 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 266 #endif
t_yamamoto 0:669ef71cba68 267 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 268 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 269 #endif
t_yamamoto 0:669ef71cba68 270 #endif
t_yamamoto 0:669ef71cba68 271 }
t_yamamoto 0:669ef71cba68 272
t_yamamoto 0:669ef71cba68 273 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 274 {
t_yamamoto 0:669ef71cba68 275 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 276 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 277
t_yamamoto 0:669ef71cba68 278 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 279 {
t_yamamoto 0:669ef71cba68 280 current++;
t_yamamoto 0:669ef71cba68 281 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 282 processChangeComp = false;
t_yamamoto 0:669ef71cba68 283 }
t_yamamoto 0:669ef71cba68 284 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 285 {
t_yamamoto 0:669ef71cba68 286 current--;
t_yamamoto 0:669ef71cba68 287 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 288 processChangeComp = false;
t_yamamoto 0:669ef71cba68 289 }
t_yamamoto 0:669ef71cba68 290 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 291 #endif
t_yamamoto 0:669ef71cba68 292
t_yamamoto 0:669ef71cba68 293 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 294 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 295 #endif
t_yamamoto 0:669ef71cba68 296
t_yamamoto 0:669ef71cba68 297 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 298 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 299 #endif
t_yamamoto 0:669ef71cba68 300
t_yamamoto 0:669ef71cba68 301 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 302 ACTUATORHUB::ActuatorHub::Update();
t_yamamoto 0:669ef71cba68 303 #endif
t_yamamoto 0:669ef71cba68 304
t_yamamoto 0:669ef71cba68 305 }
t_yamamoto 0:669ef71cba68 306
kishibekairohan 2:c015739085d3 307
kishibekairohan 2:c015739085d3 308
t_yamamoto 0:669ef71cba68 309 void SystemProcess()
t_yamamoto 0:669ef71cba68 310 {
t_yamamoto 0:669ef71cba68 311 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 312
t_yamamoto 0:669ef71cba68 313 while(1)
kishibekairohan 9:f93fc79a49ea 314 {
t_yamamoto 0:669ef71cba68 315 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 316 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 317 #endif
t_yamamoto 0:669ef71cba68 318
t_yamamoto 0:669ef71cba68 319 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 320 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 321 {
t_yamamoto 0:669ef71cba68 322 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 323 AllActuatorReset();
t_yamamoto 0:669ef71cba68 324 lock = true;
t_yamamoto 0:669ef71cba68 325 }
t_yamamoto 0:669ef71cba68 326 else
t_yamamoto 0:669ef71cba68 327 #endif
t_yamamoto 0:669ef71cba68 328 {
t_yamamoto 0:669ef71cba68 329
t_yamamoto 0:669ef71cba68 330 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 331 if(!lock)
t_yamamoto 0:669ef71cba68 332 {
t_yamamoto 0:669ef71cba68 333 Process[current]();
t_yamamoto 0:669ef71cba68 334 }
t_yamamoto 0:669ef71cba68 335 else
t_yamamoto 0:669ef71cba68 336 #endif
t_yamamoto 0:669ef71cba68 337 {
t_yamamoto 0:669ef71cba68 338 //ロック時の処理
t_yamamoto 0:669ef71cba68 339 }
t_yamamoto 0:669ef71cba68 340 }
t_yamamoto 0:669ef71cba68 341
kishibekairohan 10:1295d39fec3a 342 if ((EMS_0 || EMS_1) && !Emsflag){
kishibekairohan 11:028a150943b5 343 buzzer = 0.5;
kishibekairohan 9:f93fc79a49ea 344 BuzzerTimer.attach(BuzzerTimer_func, 1.2);
kishibekairohan 9:f93fc79a49ea 345 Emsflag = true;
kishibekairohan 11:028a150943b5 346 ledMode = EMS;
kishibekairohan 11:028a150943b5 347 current = 0;
kishibekairohan 11:028a150943b5 348 tapeLedTimer.attach(TapeLedEms_func, 1.2);
kishibekairohan 11:028a150943b5 349 sendLedData.code = (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 350 }
kishibekairohan 9:f93fc79a49ea 351
kishibekairohan 9:f93fc79a49ea 352 if(!EMS_0 && !EMS_1) {
kishibekairohan 9:f93fc79a49ea 353 buzzer = 0;
kishibekairohan 9:f93fc79a49ea 354 BuzzerTimer.detach();
kishibekairohan 9:f93fc79a49ea 355 Emsflag = false;
kishibekairohan 11:028a150943b5 356 if(ledMode == EMS) ledMode = Normal;
kishibekairohan 11:028a150943b5 357 tapeLedTimer.detach();
kishibekairohan 11:028a150943b5 358 }
kishibekairohan 11:028a150943b5 359
kishibekairohan 11:028a150943b5 360 switch(ledMode)
kishibekairohan 11:028a150943b5 361 {
kishibekairohan 11:028a150943b5 362 case EMS :
kishibekairohan 11:028a150943b5 363 break;
kishibekairohan 11:028a150943b5 364
kishibekairohan 11:028a150943b5 365 case Normal :
kishibekairohan 11:028a150943b5 366 sendLedData.code = tapeLED.code;
kishibekairohan 11:028a150943b5 367
kishibekairohan 11:028a150943b5 368 default:
kishibekairohan 11:028a150943b5 369 break;
kishibekairohan 9:f93fc79a49ea 370 }
kishibekairohan 9:f93fc79a49ea 371
t_yamamoto 0:669ef71cba68 372 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 373 }
t_yamamoto 0:669ef71cba68 374 }
t_yamamoto 0:669ef71cba68 375
kishibekairohan 2:c015739085d3 376
kishibekairohan 2:c015739085d3 377
kishibekairohan 2:c015739085d3 378
t_yamamoto 0:669ef71cba68 379 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 380 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 381 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 382 static void Process0()
kishibekairohan 10:1295d39fec3a 383 {
kishibekairohan 10:1295d39fec3a 384 static bool Xnopush = false;
kishibekairohan 10:1295d39fec3a 385 static bool Angle_flagX = false;
kishibekairohan 10:1295d39fec3a 386
kishibekairohan 7:e88c5d47a3be 387 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 388 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 389 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 390 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 391 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 392 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 393 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 394 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 395 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 396 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 397 }
kishibekairohan 7:e88c5d47a3be 398 for(int i = 0;i<20;i++){
kishibekairohan 7:e88c5d47a3be 399 float y = 0;
kishibekairohan 7:e88c5d47a3be 400 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 401 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 402 Angle += rotateY;
kishibekairohan 7:e88c5d47a3be 403 }
kishibekairohan 7:e88c5d47a3be 404 Angle = Angle /20;
kishibekairohan 10:1295d39fec3a 405 int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan 10:1295d39fec3a 406
kishibekairohan 10:1295d39fec3a 407 if(controller->Button.X && !Xnopush){
kishibekairohan 10:1295d39fec3a 408 Angle_flagX = true;
kishibekairohan 10:1295d39fec3a 409 Xnopush = true;
kishibekairohan 10:1295d39fec3a 410 }else if(!controller->Button.X)Xnopush = false;
kishibekairohan 7:e88c5d47a3be 411
kishibekairohan 10:1295d39fec3a 412
kishibekairohan 10:1295d39fec3a 413 if (Angle_flagX){
kishibekairohan 10:1295d39fec3a 414 motor[Angle_R].dir = SetStatus(-gyropwmX);
kishibekairohan 10:1295d39fec3a 415 motor[Angle_L].dir = SetStatus(gyropwmX);
kishibekairohan 10:1295d39fec3a 416 motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan 10:1295d39fec3a 417 motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan 10:1295d39fec3a 418 if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan 7:e88c5d47a3be 419 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 420 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 421 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 422 motor[Angle_L].pwm = 255;
kishibekairohan 10:1295d39fec3a 423 Angle_flagX = false;
kishibekairohan 7:e88c5d47a3be 424 }
kishibekairohan 7:e88c5d47a3be 425 }
kishibekairohan 10:1295d39fec3a 426
kishibekairohan 11:028a150943b5 427 else{
kishibekairohan 11:028a150943b5 428 motor[Angle_R].dir = BRAKE;
kishibekairohan 11:028a150943b5 429 motor[Angle_L].dir = BRAKE;
kishibekairohan 11:028a150943b5 430 motor[Angle_R].pwm = 255;
kishibekairohan 11:028a150943b5 431 motor[Angle_L].pwm = 255;
kishibekairohan 11:028a150943b5 432 }
t_yamamoto 0:669ef71cba68 433 }
t_yamamoto 0:669ef71cba68 434 #endif
t_yamamoto 0:669ef71cba68 435
t_yamamoto 0:669ef71cba68 436 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 437 static void Process1()
t_yamamoto 0:669ef71cba68 438 {
7ka884 4:ba9df71868df 439
7ka884 4:ba9df71868df 440
7ka884 4:ba9df71868df 441 motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 442 motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 443 motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
7ka884 4:ba9df71868df 444 motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan 2:c015739085d3 445
kishibekairohan 7:e88c5d47a3be 446 motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 447 motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
kishibekairohan 7:e88c5d47a3be 448 motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 7:e88c5d47a3be 449 motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan 2:c015739085d3 450
kishibekairohan 2:c015739085d3 451 if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan 7:e88c5d47a3be 452 motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
kishibekairohan 7:e88c5d47a3be 453 motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
7ka884 4:ba9df71868df 454 }
7ka884 4:ba9df71868df 455
kishibekairohan 2:c015739085d3 456
7ka884 4:ba9df71868df 457 //wheel.getPulses()...どちらの方向にどれだけ回ったか
kishibekairohan 8:6fb3723f7747 458 //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
7ka884 4:ba9df71868df 459 //軸が何回転したか
kishibekairohan 9:f93fc79a49ea 460 //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
t_yamamoto 0:669ef71cba68 461 }
t_yamamoto 0:669ef71cba68 462 #endif
t_yamamoto 0:669ef71cba68 463
t_yamamoto 0:669ef71cba68 464 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 465 static void Process2()
t_yamamoto 0:669ef71cba68 466 {
7ka884 4:ba9df71868df 467 static bool color_flag = false;
7ka884 4:ba9df71868df 468
7ka884 4:ba9df71868df 469 static bool traceon = false;//fase1
7ka884 4:ba9df71868df 470 static bool yokofla = false;//fase2
7ka884 4:ba9df71868df 471 static bool boxslip = false;//fase3
kishibekairohan 2:c015739085d3 472
7ka884 4:ba9df71868df 473 static bool compA = false;
7ka884 4:ba9df71868df 474 static bool compB = false;
7ka884 4:ba9df71868df 475 static bool compC = false;
7ka884 4:ba9df71868df 476 static bool compD = false;
7ka884 4:ba9df71868df 477
7ka884 4:ba9df71868df 478 static bool invationA = false;
7ka884 4:ba9df71868df 479 static bool invationB = false;
7ka884 4:ba9df71868df 480 static bool invationC = false;
7ka884 4:ba9df71868df 481 static bool invationD = false;
7ka884 4:ba9df71868df 482
7ka884 4:ba9df71868df 483 ColorDetection();
7ka884 4:ba9df71868df 484 //
kishibekairohan 2:c015739085d3 485 if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 2:c015739085d3 486 {
kishibekairohan 2:c015739085d3 487 invationA ^= 1;//start false,over true
kishibekairohan 2:c015739085d3 488 compA = true;//on true,noon false
kishibekairohan 2:c015739085d3 489 }
kishibekairohan 2:c015739085d3 490 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
kishibekairohan 2:c015739085d3 491
7ka884 4:ba9df71868df 492 if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
7ka884 4:ba9df71868df 493 {
7ka884 4:ba9df71868df 494 invationB ^= 1;//start false,over true
7ka884 4:ba9df71868df 495 compB = true;//on true,noon false
7ka884 4:ba9df71868df 496 }
7ka884 4:ba9df71868df 497 else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
kishibekairohan 8:6fb3723f7747 498 /*
7ka884 4:ba9df71868df 499 if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
7ka884 4:ba9df71868df 500 {
7ka884 4:ba9df71868df 501 invationC ^= 1;//start false,over true
7ka884 4:ba9df71868df 502 compC = true;//on true,noon false
7ka884 4:ba9df71868df 503 }
7ka884 4:ba9df71868df 504 else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
7ka884 4:ba9df71868df 505
7ka884 4:ba9df71868df 506 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan 2:c015739085d3 507 {
7ka884 4:ba9df71868df 508 invationD ^= 1;//start false,over true
7ka884 4:ba9df71868df 509 compD = true;//on true,noon false
7ka884 4:ba9df71868df 510 }
7ka884 4:ba9df71868df 511 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
kishibekairohan 8:6fb3723f7747 512 */
7ka884 4:ba9df71868df 513
7ka884 4:ba9df71868df 514 //
7ka884 4:ba9df71868df 515
7ka884 4:ba9df71868df 516 if(controller->Button.B && !color_flag)
7ka884 4:ba9df71868df 517 {
7ka884 4:ba9df71868df 518 traceon ^= 1;
7ka884 4:ba9df71868df 519 color_flag = true;
kishibekairohan 2:c015739085d3 520 }
7ka884 4:ba9df71868df 521 else if(!controller->Button.B)color_flag = false;
kishibekairohan 2:c015739085d3 522
7ka884 4:ba9df71868df 523 if(traceon && !yokofla && !boxslip)
kishibekairohan 2:c015739085d3 524 {
7ka884 4:ba9df71868df 525 if(!invationA && !compA && !invationB && !compB)
7ka884 4:ba9df71868df 526 {
7ka884 4:ba9df71868df 527 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 528 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 529 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 530 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 531
7ka884 4:ba9df71868df 532 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 533 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 534 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 535 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 536 }
7ka884 4:ba9df71868df 537 else if(invationA && compA && !invationB && !compB)
7ka884 4:ba9df71868df 538 {
7ka884 4:ba9df71868df 539 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 540 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 541 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 542 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 543
7ka884 4:ba9df71868df 544 motor[TIRE_FR].pwm = startP - downP;
7ka884 4:ba9df71868df 545 motor[TIRE_FL].pwm = startP - downP;
7ka884 4:ba9df71868df 546 motor[TIRE_BR].pwm = startP - downP;
7ka884 4:ba9df71868df 547 motor[TIRE_BL].pwm = startP - downP;
7ka884 4:ba9df71868df 548 }
7ka884 4:ba9df71868df 549 else if(invationA && !compA && !invationB && !compB)
7ka884 4:ba9df71868df 550 {
7ka884 4:ba9df71868df 551 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 552 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 553 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 554 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 555
7ka884 4:ba9df71868df 556 wait(2);
7ka884 4:ba9df71868df 557
7ka884 4:ba9df71868df 558 yokofla = true;
7ka884 4:ba9df71868df 559 traceon = false;
7ka884 4:ba9df71868df 560 }
7ka884 4:ba9df71868df 561 else{
7ka884 4:ba9df71868df 562 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 563 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 564 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 565 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 566 }
7ka884 4:ba9df71868df 567 }
7ka884 4:ba9df71868df 568
7ka884 4:ba9df71868df 569 if(!traceon && yokofla && !boxslip)
7ka884 4:ba9df71868df 570 {
kishibekairohan 7:e88c5d47a3be 571 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
7ka884 4:ba9df71868df 572 {
7ka884 4:ba9df71868df 573 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 574 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 575 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 576 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 577
7ka884 4:ba9df71868df 578 wait(2);
7ka884 4:ba9df71868df 579
7ka884 4:ba9df71868df 580 boxslip = true;
7ka884 4:ba9df71868df 581 yokofla = false;
7ka884 4:ba9df71868df 582 }
7ka884 4:ba9df71868df 583 else if(invationA && !compA && invationB)
7ka884 4:ba9df71868df 584 {
7ka884 4:ba9df71868df 585 motor[TIRE_FR].dir = BACK;
7ka884 4:ba9df71868df 586 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 587 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 588 motor[TIRE_BL].dir = FOR;
7ka884 4:ba9df71868df 589
7ka884 4:ba9df71868df 590 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 591 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 592 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 593 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 594 }
7ka884 4:ba9df71868df 595 else if(!invationA && !compB && !invationB)
7ka884 4:ba9df71868df 596 {
7ka884 4:ba9df71868df 597 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 598 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 599 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 600 motor[TIRE_BL].dir = BACK;
kishibekairohan 2:c015739085d3 601
7ka884 4:ba9df71868df 602 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 603 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 604 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 605 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 606 }
7ka884 4:ba9df71868df 607 else if(invationA && compA && !invationB && !compB)
7ka884 4:ba9df71868df 608 {
7ka884 4:ba9df71868df 609 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 610 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 611 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 612 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 613
7ka884 4:ba9df71868df 614 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 615 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 616 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 617 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 618 }
7ka884 4:ba9df71868df 619 else if(compB && invationB)
7ka884 4:ba9df71868df 620 {
7ka884 4:ba9df71868df 621 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 622 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 623 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 624 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 625
7ka884 4:ba9df71868df 626 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 627 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 628 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 629 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 630 }
7ka884 4:ba9df71868df 631 else
7ka884 4:ba9df71868df 632 {
7ka884 4:ba9df71868df 633 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 634 motor[TIRE_FL].dir = BACK;
7ka884 4:ba9df71868df 635 motor[TIRE_BR].dir = FOR;
7ka884 4:ba9df71868df 636 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 637
7ka884 4:ba9df71868df 638 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 639 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 640 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 641 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 642 }
7ka884 4:ba9df71868df 643 }
7ka884 4:ba9df71868df 644
7ka884 4:ba9df71868df 645 if(!traceon && !yokofla && boxslip)
7ka884 4:ba9df71868df 646 {
kishibekairohan 7:e88c5d47a3be 647 if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
7ka884 4:ba9df71868df 648 {
7ka884 4:ba9df71868df 649 motor[TIRE_FR].dir = FOR;
7ka884 4:ba9df71868df 650 motor[TIRE_FL].dir = FOR;
7ka884 4:ba9df71868df 651 motor[TIRE_BR].dir = BACK;
7ka884 4:ba9df71868df 652 motor[TIRE_BL].dir = BACK;
7ka884 4:ba9df71868df 653
7ka884 4:ba9df71868df 654 motor[TIRE_FR].pwm = startP;
7ka884 4:ba9df71868df 655 motor[TIRE_FL].pwm = startP;
7ka884 4:ba9df71868df 656 motor[TIRE_BR].pwm = startP;
7ka884 4:ba9df71868df 657 motor[TIRE_BL].pwm = startP;
7ka884 4:ba9df71868df 658 }
kishibekairohan 7:e88c5d47a3be 659 else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
7ka884 4:ba9df71868df 660 {
7ka884 4:ba9df71868df 661 motor[TIRE_FR].dir = BRAKE;
7ka884 4:ba9df71868df 662 motor[TIRE_FL].dir = BRAKE;
7ka884 4:ba9df71868df 663 motor[TIRE_BR].dir = BRAKE;
7ka884 4:ba9df71868df 664 motor[TIRE_BL].dir = BRAKE;
7ka884 4:ba9df71868df 665 }
7ka884 4:ba9df71868df 666 }
7ka884 4:ba9df71868df 667 /*////
7ka884 4:ba9df71868df 668 motor[0].dir = BACK;
7ka884 4:ba9df71868df 669 motor[1].dir = BACK;
7ka884 4:ba9df71868df 670 motor[2].dir = FOR;
7ka884 4:ba9df71868df 671 motor[3].dir = FOR;
7ka884 4:ba9df71868df 672
7ka884 4:ba9df71868df 673 motor[0].pwm = startP;
7ka884 4:ba9df71868df 674 motor[1].pwm = startP;
7ka884 4:ba9df71868df 675 motor[2].pwm = startP;
7ka884 4:ba9df71868df 676 motor[3].pwm = startP;
7ka884 4:ba9df71868df 677 else if()
7ka884 4:ba9df71868df 678 {
7ka884 4:ba9df71868df 679 motor[0].dir = BRAKE;
7ka884 4:ba9df71868df 680 motor[1].dir = BRAKE;
7ka884 4:ba9df71868df 681 motor[2].dir = BRAKE;
7ka884 4:ba9df71868df 682 motor[3].dir = BRAKE;
7ka884 4:ba9df71868df 683
7ka884 4:ba9df71868df 684 motor[0].pwm = 255;
7ka884 4:ba9df71868df 685 motor[1].pwm = 255;
7ka884 4:ba9df71868df 686 motor[2].pwm = 255;
7ka884 4:ba9df71868df 687 motor[3].pwm = 255;
7ka884 4:ba9df71868df 688 }*///////
t_yamamoto 0:669ef71cba68 689 }
t_yamamoto 0:669ef71cba68 690 #endif
t_yamamoto 0:669ef71cba68 691
t_yamamoto 0:669ef71cba68 692 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 693 static void Process3()
t_yamamoto 0:669ef71cba68 694 {
kishibekairohan 2:c015739085d3 695 if(controller->Button.R){
kishibekairohan 7:e88c5d47a3be 696 motor[Angle_R].dir = FOR;
kishibekairohan 7:e88c5d47a3be 697 motor[Angle_L].dir = BACK;
kishibekairohan 7:e88c5d47a3be 698 motor[Angle_R].pwm = 150;
kishibekairohan 7:e88c5d47a3be 699 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 700 }else if(controller->Button.L){
kishibekairohan 7:e88c5d47a3be 701 motor[Angle_R].dir = BACK;
kishibekairohan 7:e88c5d47a3be 702 motor[Angle_L].dir = FOR;
kishibekairohan 7:e88c5d47a3be 703 motor[Angle_R].pwm = 150;
kishibekairohan 7:e88c5d47a3be 704 motor[Angle_L].pwm = 150;
kishibekairohan 2:c015739085d3 705 }else{
kishibekairohan 7:e88c5d47a3be 706 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 707 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 708 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 709 motor[Angle_L].pwm = 255;
7ka884 4:ba9df71868df 710 }
7ka884 4:ba9df71868df 711
kishibekairohan 7:e88c5d47a3be 712 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 713 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 714 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 715 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 716 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 717 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 718 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 719 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 720 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 721 motor[Angle_L].pwm = 255;
7ka884 4:ba9df71868df 722 }
kishibekairohan 9:f93fc79a49ea 723 for(int i = 0;i<20;i++){
kishibekairohan 9:f93fc79a49ea 724 float y = 0;
kishibekairohan 9:f93fc79a49ea 725 y = acc[1]*1000;
kishibekairohan 9:f93fc79a49ea 726 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 9:f93fc79a49ea 727 Angle += rotateY;
kishibekairohan 9:f93fc79a49ea 728 }
kishibekairohan 11:028a150943b5 729 Angle = Angle/20;
kishibekairohan 10:1295d39fec3a 730 pc.printf("Y:%d \r\n",Angle);
t_yamamoto 0:669ef71cba68 731 }
t_yamamoto 0:669ef71cba68 732 #endif
t_yamamoto 0:669ef71cba68 733
t_yamamoto 0:669ef71cba68 734 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 735 static void Process4()
t_yamamoto 0:669ef71cba68 736 {
kishibekairohan 9:f93fc79a49ea 737 static bool Xnopush = false;
kishibekairohan 9:f93fc79a49ea 738 static bool Ynopush = false;
kishibekairohan 9:f93fc79a49ea 739
kishibekairohan 9:f93fc79a49ea 740 static bool Angle_flagX = false;
kishibekairohan 9:f93fc79a49ea 741 static bool Angle_flagY = false;
kishibekairohan 9:f93fc79a49ea 742
7ka884 4:ba9df71868df 743
kishibekairohan 7:e88c5d47a3be 744 if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan 7:e88c5d47a3be 745 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 746 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 747 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 748 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 749 }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan 7:e88c5d47a3be 750 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 751 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 752 motor[Angle_R].pwm = 255;
kishibekairohan 7:e88c5d47a3be 753 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 754 }
kishibekairohan 7:e88c5d47a3be 755 for(int i = 0;i<20;i++){
kishibekairohan 7:e88c5d47a3be 756 float y = 0;
kishibekairohan 7:e88c5d47a3be 757 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 758 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 759 Angle += rotateY;
kishibekairohan 7:e88c5d47a3be 760 }
kishibekairohan 7:e88c5d47a3be 761 Angle = Angle /20;
kishibekairohan 10:1295d39fec3a 762
kishibekairohan 7:e88c5d47a3be 763 int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan 7:e88c5d47a3be 764 int gyropwmY = gyro.SetPV(Angle,AngletargetY);
kishibekairohan 7:e88c5d47a3be 765
kishibekairohan 9:f93fc79a49ea 766 if(controller->Button.X && !Xnopush){
kishibekairohan 7:e88c5d47a3be 767 Angle_flagX = true;
kishibekairohan 9:f93fc79a49ea 768 Xnopush = true;
kishibekairohan 10:1295d39fec3a 769 }else if(!controller->Button.X)Xnopush = false;
kishibekairohan 9:f93fc79a49ea 770
kishibekairohan 9:f93fc79a49ea 771 if(controller->Button.Y && !Ynopush){
kishibekairohan 7:e88c5d47a3be 772 Angle_flagY = true;
kishibekairohan 9:f93fc79a49ea 773 Ynopush = true;
kishibekairohan 10:1295d39fec3a 774 }else if(!controller->Button.Y)Ynopush = false;
kishibekairohan 7:e88c5d47a3be 775
kishibekairohan 7:e88c5d47a3be 776 if (Angle_flagX){
kishibekairohan 7:e88c5d47a3be 777 motor[Angle_R].dir = SetStatus(gyropwmX);
kishibekairohan 7:e88c5d47a3be 778 motor[Angle_L].dir = SetStatus(-gyropwmX);
kishibekairohan 7:e88c5d47a3be 779 motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan 7:e88c5d47a3be 780 motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan 7:e88c5d47a3be 781 if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan 7:e88c5d47a3be 782 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 783 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 784 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 785 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 786 Angle_flagX = false;
kishibekairohan 7:e88c5d47a3be 787 }
kishibekairohan 7:e88c5d47a3be 788 }
kishibekairohan 7:e88c5d47a3be 789
kishibekairohan 7:e88c5d47a3be 790 if (Angle_flagY){
kishibekairohan 10:1295d39fec3a 791 motor[Angle_R].dir = SetStatus(-gyropwmY);
kishibekairohan 10:1295d39fec3a 792 motor[Angle_L].dir = SetStatus(gyropwmY);
kishibekairohan 7:e88c5d47a3be 793 motor[Angle_R].pwm = SetPWM(gyropwmY);
kishibekairohan 7:e88c5d47a3be 794 motor[Angle_L].pwm = SetPWM(gyropwmY);
kishibekairohan 7:e88c5d47a3be 795 if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
kishibekairohan 7:e88c5d47a3be 796 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 797 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 798 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 799 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 800 Angle_flagY = false;
kishibekairohan 7:e88c5d47a3be 801 }
kishibekairohan 7:e88c5d47a3be 802 }
kishibekairohan 7:e88c5d47a3be 803 /*float y = 0;
kishibekairohan 7:e88c5d47a3be 804 y = acc[1]*1000;
kishibekairohan 7:e88c5d47a3be 805 float rotateY = (y - 305)/2.21 - 90;
kishibekairohan 7:e88c5d47a3be 806 int gyropwm = gyro.SetPV(rotateY , Angletarget);
kishibekairohan 7:e88c5d47a3be 807
kishibekairohan 7:e88c5d47a3be 808 if(controller->Button.X){
kishibekairohan 7:e88c5d47a3be 809 Angle_flag = true;
kishibekairohan 7:e88c5d47a3be 810 }
kishibekairohan 7:e88c5d47a3be 811 if (Angle_flag){
kishibekairohan 7:e88c5d47a3be 812 motor[Angle_R].dir = SetStatus(gyropwm);
kishibekairohan 7:e88c5d47a3be 813 motor[Angle_L].dir = SetStatus(-gyropwm);
kishibekairohan 7:e88c5d47a3be 814 motor[Angle_R].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 815 motor[Angle_L].pwm = SetPWM(gyropwm);
kishibekairohan 7:e88c5d47a3be 816 if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
kishibekairohan 7:e88c5d47a3be 817 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 818 motor[Angle_L].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 819 Angle_flag = false;
kishibekairohan 7:e88c5d47a3be 820 }
kishibekairohan 7:e88c5d47a3be 821 }*/
kishibekairohan 7:e88c5d47a3be 822 else{
kishibekairohan 7:e88c5d47a3be 823 motor[Angle_R].dir = BRAKE;
kishibekairohan 7:e88c5d47a3be 824 motor[Angle_L].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 825 motor[Angle_R].pwm = 255;
kishibekairohan 10:1295d39fec3a 826 motor[Angle_L].pwm = 255;
kishibekairohan 7:e88c5d47a3be 827 }
kishibekairohan 10:1295d39fec3a 828 //pc.printf("%d \r\n",gyropwmY);
t_yamamoto 0:669ef71cba68 829 }
t_yamamoto 0:669ef71cba68 830 #endif
t_yamamoto 0:669ef71cba68 831
t_yamamoto 0:669ef71cba68 832 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 833 static void Process5()
t_yamamoto 0:669ef71cba68 834 {
kishibekairohan 9:f93fc79a49ea 835 static bool nopushed = false;
kishibekairohan 9:f93fc79a49ea 836 static bool Rt_flagY = false;
kishibekairohan 9:f93fc79a49ea 837 /* wait(0.1);
kishibekairohan 9:f93fc79a49ea 838 //RtX.getPulses();//...どちらの方向にどれだけ回ったか
kishibekairohan 8:6fb3723f7747 839 //RtY.getPulses();
kishibekairohan 8:6fb3723f7747 840 pc.printf("Pulses:%07d \r\n",RtX.getPulses());
kishibekairohan 8:6fb3723f7747 841 pc.printf("Pulses:%07d \r\n",RtY.getPulses());
kishibekairohan 8:6fb3723f7747 842 //軸が何回転したか
kishibekairohan 9:f93fc79a49ea 843 pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
kishibekairohan 8:6fb3723f7747 844 */
kishibekairohan 7:e88c5d47a3be 845
kishibekairohan 8:6fb3723f7747 846
kishibekairohan 9:f93fc79a49ea 847 if(controller->Button.B && !nopushed){
kishibekairohan 9:f93fc79a49ea 848 Rt_flagY = true;
kishibekairohan 9:f93fc79a49ea 849 nopushed = true;
kishibekairohan 10:1295d39fec3a 850 }else if(!controller->Button.B)nopushed = false;
kishibekairohan 9:f93fc79a49ea 851
kishibekairohan 8:6fb3723f7747 852
kishibekairohan 9:f93fc79a49ea 853 if (Rt_flagY && SetPWM(RtpwmY) > 0){
kishibekairohan 9:f93fc79a49ea 854 filip();
kishibekairohan 9:f93fc79a49ea 855
kishibekairohan 9:f93fc79a49ea 856 motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan 8:6fb3723f7747 857 motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan 8:6fb3723f7747 858 motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan 9:f93fc79a49ea 859 motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan 8:6fb3723f7747 860 motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan 8:6fb3723f7747 861 motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan 8:6fb3723f7747 862 motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan 8:6fb3723f7747 863 motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan 8:6fb3723f7747 864 }
kishibekairohan 9:f93fc79a49ea 865 else if(Rt_flagY && SetPWM(RtpwmY) < 0)
kishibekairohan 9:f93fc79a49ea 866 {
kishibekairohan 9:f93fc79a49ea 867 filip();
kishibekairohan 8:6fb3723f7747 868 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 869 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 870 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 871 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 872 motor[TIRE_FR].pwm = 255;
kishibekairohan 10:1295d39fec3a 873 motor[TIRE_FL].pwm = 255;
kishibekairohan 10:1295d39fec3a 874 motor[TIRE_BR].pwm = 255;
kishibekairohan 10:1295d39fec3a 875 motor[TIRE_BL].pwm = 255;
kishibekairohan 10:1295d39fec3a 876
kishibekairohan 8:6fb3723f7747 877 }
kishibekairohan 9:f93fc79a49ea 878 else
kishibekairohan 9:f93fc79a49ea 879 {
kishibekairohan 8:6fb3723f7747 880 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 881 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 882 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 883 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 884 motor[TIRE_FR].pwm = 255;
kishibekairohan 10:1295d39fec3a 885 motor[TIRE_FL].pwm = 255;
kishibekairohan 10:1295d39fec3a 886 motor[TIRE_BR].pwm = 255;
kishibekairohan 10:1295d39fec3a 887 motor[TIRE_BL].pwm = 255;
kishibekairohan 9:f93fc79a49ea 888 }
kishibekairohan 9:f93fc79a49ea 889
kishibekairohan 9:f93fc79a49ea 890 //pc.printf("RtpwmX:%f \r\n", RtpwmX);
kishibekairohan 9:f93fc79a49ea 891
kishibekairohan 9:f93fc79a49ea 892
kishibekairohan 10:1295d39fec3a 893 //pc.printf("PWM:%d \r\n", RtpwmY);
kishibekairohan 10:1295d39fec3a 894 //pc.printf("回転数:%f \r\n" ,rpmY);
kishibekairohan 10:1295d39fec3a 895 //pc.printf("距離:%f \r\n", disY);
kishibekairohan 9:f93fc79a49ea 896
kishibekairohan 9:f93fc79a49ea 897
t_yamamoto 0:669ef71cba68 898 }
t_yamamoto 0:669ef71cba68 899 #endif
t_yamamoto 0:669ef71cba68 900
t_yamamoto 0:669ef71cba68 901 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 902 static void Process6()
t_yamamoto 0:669ef71cba68 903 {
kishibekairohan 8:6fb3723f7747 904 /*static bool color_flag = false;
kishibekairohan 8:6fb3723f7747 905
kishibekairohan 8:6fb3723f7747 906 static bool traceon = false;//fase1
kishibekairohan 8:6fb3723f7747 907 static bool yokofla = false;//fase2
kishibekairohan 8:6fb3723f7747 908 static bool boxslip = false;//fase3
kishibekairohan 8:6fb3723f7747 909
kishibekairohan 8:6fb3723f7747 910 static bool syu = false;
kishibekairohan 8:6fb3723f7747 911
kishibekairohan 8:6fb3723f7747 912 static bool compA = false;
kishibekairohan 8:6fb3723f7747 913 static bool compB = false;
kishibekairohan 8:6fb3723f7747 914 static bool compC = false;
kishibekairohan 8:6fb3723f7747 915 static bool compD = false;
kishibekairohan 8:6fb3723f7747 916
kishibekairohan 8:6fb3723f7747 917 static bool invationA = false;
kishibekairohan 8:6fb3723f7747 918 static bool invationB = false;
kishibekairohan 8:6fb3723f7747 919 static bool invationC = false;
kishibekairohan 8:6fb3723f7747 920 static bool invationD = false;
kishibekairohan 8:6fb3723f7747 921
kishibekairohan 8:6fb3723f7747 922
kishibekairohan 8:6fb3723f7747 923 ColorDetection();
kishibekairohan 8:6fb3723f7747 924
kishibekairohan 8:6fb3723f7747 925 //
kishibekairohan 8:6fb3723f7747 926 if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 8:6fb3723f7747 927 {
kishibekairohan 8:6fb3723f7747 928 invationA ^= 1;//start false,over true
kishibekairohan 8:6fb3723f7747 929 compA = true;//on true,noon false
kishibekairohan 8:6fb3723f7747 930 }
kishibekairohan 8:6fb3723f7747 931 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
kishibekairohan 8:6fb3723f7747 932
kishibekairohan 8:6fb3723f7747 933 if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
kishibekairohan 8:6fb3723f7747 934 {
kishibekairohan 8:6fb3723f7747 935 invationB ^= 1;//start false,over true
kishibekairohan 8:6fb3723f7747 936 compB = true;//on true,noon false
kishibekairohan 8:6fb3723f7747 937 }
kishibekairohan 8:6fb3723f7747 938 else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
kishibekairohan 8:6fb3723f7747 939 /*
kishibekairohan 8:6fb3723f7747 940 if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
kishibekairohan 8:6fb3723f7747 941 {
kishibekairohan 8:6fb3723f7747 942 invationC ^= 1;//start false,over true
kishibekairohan 8:6fb3723f7747 943 compC = true;//on true,noon false
kishibekairohan 8:6fb3723f7747 944 }
kishibekairohan 8:6fb3723f7747 945 else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
kishibekairohan 8:6fb3723f7747 946
kishibekairohan 8:6fb3723f7747 947 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan 8:6fb3723f7747 948 {
kishibekairohan 8:6fb3723f7747 949 invationD ^= 1;//start false,over true
kishibekairohan 8:6fb3723f7747 950 compD = true;//on true,noon false
kishibekairohan 8:6fb3723f7747 951 }
kishibekairohan 8:6fb3723f7747 952 else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
kishibekairohan 8:6fb3723f7747 953 */
kishibekairohan 7:e88c5d47a3be 954
kishibekairohan 8:6fb3723f7747 955 //
kishibekairohan 8:6fb3723f7747 956 /*
kishibekairohan 8:6fb3723f7747 957 if(controller->Button.B && !color_flag)
kishibekairohan 8:6fb3723f7747 958 {
kishibekairohan 8:6fb3723f7747 959 traceon ^= 1;
kishibekairohan 8:6fb3723f7747 960 color_flag = true;
kishibekairohan 8:6fb3723f7747 961 }
kishibekairohan 8:6fb3723f7747 962 else if(!controller->Button.B)color_flag = false;
kishibekairohan 8:6fb3723f7747 963
kishibekairohan 8:6fb3723f7747 964 if(traceon && !yokofla && !boxslip && !syu)
kishibekairohan 8:6fb3723f7747 965 {
kishibekairohan 8:6fb3723f7747 966 if(!invationA && !compA && !invationB && !compB)
kishibekairohan 8:6fb3723f7747 967 {
kishibekairohan 8:6fb3723f7747 968 motor[TIRE_FR].dir = FOR;
kishibekairohan 8:6fb3723f7747 969 motor[TIRE_FL].dir = FOR;
kishibekairohan 8:6fb3723f7747 970 motor[TIRE_BR].dir = BACK;
kishibekairohan 8:6fb3723f7747 971 motor[TIRE_BL].dir = BACK;
kishibekairohan 8:6fb3723f7747 972
kishibekairohan 8:6fb3723f7747 973 motor[TIRE_FR].pwm = startP;
kishibekairohan 8:6fb3723f7747 974 motor[TIRE_FL].pwm = startP;
kishibekairohan 8:6fb3723f7747 975 motor[TIRE_BR].pwm = startP;
kishibekairohan 8:6fb3723f7747 976 motor[TIRE_BL].pwm = startP;
kishibekairohan 8:6fb3723f7747 977 }
kishibekairohan 8:6fb3723f7747 978 else if(invationA && compA && !invationB && !compB)
kishibekairohan 8:6fb3723f7747 979 {
kishibekairohan 8:6fb3723f7747 980 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 981 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 982 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 983 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 984
kishibekairohan 8:6fb3723f7747 985 wait(2);
kishibekairohan 8:6fb3723f7747 986
kishibekairohan 8:6fb3723f7747 987 syu = true;
kishibekairohan 8:6fb3723f7747 988 yokofla = false;
kishibekairohan 8:6fb3723f7747 989 traceon = false;
kishibekairohan 8:6fb3723f7747 990 }
kishibekairohan 8:6fb3723f7747 991 else{
kishibekairohan 8:6fb3723f7747 992 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 993 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 994 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 995 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 996 }
kishibekairohan 8:6fb3723f7747 997 }
kishibekairohan 8:6fb3723f7747 998
kishibekairohan 8:6fb3723f7747 999 pointcalculation();
kishibekairohan 8:6fb3723f7747 1000
kishibekairohan 8:6fb3723f7747 1001 if(syu && !traceon && !yokofla && !boxslip)
kishibekairohan 8:6fb3723f7747 1002 {
kishibekairohan 8:6fb3723f7747 1003 if(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)
kishibekairohan 8:6fb3723f7747 1004 {
kishibekairohan 8:6fb3723f7747 1005 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1006 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1007 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1008 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1009
kishibekairohan 8:6fb3723f7747 1010 wait(2);
kishibekairohan 8:6fb3723f7747 1011
kishibekairohan 8:6fb3723f7747 1012 yokofla = true;
kishibekairohan 8:6fb3723f7747 1013 traceon = false;
kishibekairohan 8:6fb3723f7747 1014 syu = false;
kishibekairohan 8:6fb3723f7747 1015 }
kishibekairohan 8:6fb3723f7747 1016 else if(Asasult < Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
kishibekairohan 8:6fb3723f7747 1017 {
kishibekairohan 8:6fb3723f7747 1018 motor[TIRE_FR].dir = FOR;
kishibekairohan 8:6fb3723f7747 1019 motor[TIRE_FL].dir = FOR;
kishibekairohan 8:6fb3723f7747 1020 motor[TIRE_BR].dir = FOR;
kishibekairohan 8:6fb3723f7747 1021 motor[TIRE_BL].dir = FOR;
kishibekairohan 8:6fb3723f7747 1022
kishibekairohan 8:6fb3723f7747 1023 motor[TIRE_FR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1024 motor[TIRE_FL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1025 motor[TIRE_BR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1026 motor[TIRE_BL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1027 }
kishibekairohan 8:6fb3723f7747 1028 else if(Asasult > Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
kishibekairohan 8:6fb3723f7747 1029 {
kishibekairohan 8:6fb3723f7747 1030 motor[TIRE_FR].dir = BACK;
kishibekairohan 8:6fb3723f7747 1031 motor[TIRE_FL].dir = BACK;
kishibekairohan 8:6fb3723f7747 1032 motor[TIRE_BR].dir = BACK;
kishibekairohan 8:6fb3723f7747 1033 motor[TIRE_BL].dir = BACK;
kishibekairohan 8:6fb3723f7747 1034
kishibekairohan 8:6fb3723f7747 1035 motor[TIRE_FR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1036 motor[TIRE_FL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1037 motor[TIRE_BR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1038 motor[TIRE_BL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1039 }
kishibekairohan 8:6fb3723f7747 1040 else
kishibekairohan 8:6fb3723f7747 1041 {
kishibekairohan 8:6fb3723f7747 1042 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1043 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1044 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1045 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1046 }
kishibekairohan 8:6fb3723f7747 1047 }
kishibekairohan 8:6fb3723f7747 1048
kishibekairohan 8:6fb3723f7747 1049 if(!syu && !traceon && yokofla && !boxslip)
kishibekairohan 8:6fb3723f7747 1050 {
kishibekairohan 8:6fb3723f7747 1051 motor[TIRE_FR].dir = FOR;
kishibekairohan 8:6fb3723f7747 1052 motor[TIRE_FL].dir = BACK;
kishibekairohan 8:6fb3723f7747 1053 motor[TIRE_BR].dir = FOR;
kishibekairohan 8:6fb3723f7747 1054 motor[TIRE_BL].dir = BACK;
kishibekairohan 8:6fb3723f7747 1055
kishibekairohan 8:6fb3723f7747 1056 motor[TIRE_FR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1057 motor[TIRE_FL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1058 motor[TIRE_BR].pwm = startP;
kishibekairohan 8:6fb3723f7747 1059 motor[TIRE_BL].pwm = startP;
kishibekairohan 8:6fb3723f7747 1060 }
kishibekairohan 8:6fb3723f7747 1061 else if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan 8:6fb3723f7747 1062 {
kishibekairohan 8:6fb3723f7747 1063 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1064 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1065 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1066 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1067
kishibekairohan 8:6fb3723f7747 1068 wait(2);
kishibekairohan 8:6fb3723f7747 1069
kishibekairohan 8:6fb3723f7747 1070 boxslip = true;
kishibekairohan 8:6fb3723f7747 1071 yokofla = false;
kishibekairohan 8:6fb3723f7747 1072 }
kishibekairohan 8:6fb3723f7747 1073 else
kishibekairohan 8:6fb3723f7747 1074 {
kishibekairohan 8:6fb3723f7747 1075 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1076 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1077 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1078 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 8:6fb3723f7747 1079 }
kishibekairohan 8:6fb3723f7747 1080 */
t_yamamoto 0:669ef71cba68 1081 }
t_yamamoto 0:669ef71cba68 1082 #endif
t_yamamoto 0:669ef71cba68 1083
t_yamamoto 0:669ef71cba68 1084 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 1085 static void Process7()
t_yamamoto 0:669ef71cba68 1086 {
kishibekairohan 10:1295d39fec3a 1087 static bool nopushed = false;
kishibekairohan 10:1295d39fec3a 1088 static bool Rt_flagX = false;
kishibekairohan 9:f93fc79a49ea 1089
kishibekairohan 10:1295d39fec3a 1090 if(controller->Button.A && !nopushed){
kishibekairohan 10:1295d39fec3a 1091 Rt_flagX = true;
kishibekairohan 10:1295d39fec3a 1092 nopushed = true;
kishibekairohan 10:1295d39fec3a 1093 }else if(!controller->Button.A)nopushed = false;
kishibekairohan 10:1295d39fec3a 1094
kishibekairohan 9:f93fc79a49ea 1095
kishibekairohan 10:1295d39fec3a 1096 if (Rt_flagX && SetPWM(RtpwmX) > 0){
kishibekairohan 10:1295d39fec3a 1097 filip();
kishibekairohan 10:1295d39fec3a 1098
kishibekairohan 10:1295d39fec3a 1099 motor[TIRE_FR].dir = SetStatus(-RtpwmX);
kishibekairohan 10:1295d39fec3a 1100 motor[TIRE_FL].dir = SetStatus(-RtpwmX);
kishibekairohan 10:1295d39fec3a 1101 motor[TIRE_BR].dir = SetStatus(RtpwmX);
kishibekairohan 10:1295d39fec3a 1102 motor[TIRE_BL].dir = SetStatus(RtpwmX);
kishibekairohan 10:1295d39fec3a 1103 motor[TIRE_FR].pwm = SetPWM(RtpwmX);
kishibekairohan 10:1295d39fec3a 1104 motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan 10:1295d39fec3a 1105 motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan 10:1295d39fec3a 1106 motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan 10:1295d39fec3a 1107 }
kishibekairohan 10:1295d39fec3a 1108 else if(Rt_flagX && SetPWM(RtpwmX) < 0)
kishibekairohan 10:1295d39fec3a 1109 {
kishibekairohan 10:1295d39fec3a 1110 filip();
kishibekairohan 10:1295d39fec3a 1111 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1112 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1113 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1114 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1115
kishibekairohan 10:1295d39fec3a 1116 motor[TIRE_FR].pwm = 255;
kishibekairohan 10:1295d39fec3a 1117 motor[TIRE_FL].pwm = 255;
kishibekairohan 10:1295d39fec3a 1118 motor[TIRE_BR].pwm = 255;
kishibekairohan 10:1295d39fec3a 1119 motor[TIRE_BL].pwm = 255;
kishibekairohan 10:1295d39fec3a 1120
kishibekairohan 10:1295d39fec3a 1121 }
kishibekairohan 10:1295d39fec3a 1122 else
kishibekairohan 10:1295d39fec3a 1123 {
kishibekairohan 10:1295d39fec3a 1124 motor[TIRE_FR].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1125 motor[TIRE_FL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1126 motor[TIRE_BR].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1127 motor[TIRE_BL].dir = BRAKE;
kishibekairohan 10:1295d39fec3a 1128
kishibekairohan 10:1295d39fec3a 1129 motor[TIRE_FR].pwm = 255;
kishibekairohan 10:1295d39fec3a 1130 motor[TIRE_FL].pwm = 255;
kishibekairohan 10:1295d39fec3a 1131 motor[TIRE_BR].pwm = 255;
kishibekairohan 10:1295d39fec3a 1132 motor[TIRE_BL].pwm = 255;
kishibekairohan 10:1295d39fec3a 1133 }
kishibekairohan 11:028a150943b5 1134 //pc.printf("X:%d \r\n",RtpwmX);
t_yamamoto 0:669ef71cba68 1135 }
t_yamamoto 0:669ef71cba68 1136 #endif
t_yamamoto 0:669ef71cba68 1137
t_yamamoto 0:669ef71cba68 1138 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 1139 static void Process8()
t_yamamoto 0:669ef71cba68 1140 {
t_yamamoto 0:669ef71cba68 1141
t_yamamoto 0:669ef71cba68 1142 }
t_yamamoto 0:669ef71cba68 1143 #endif
t_yamamoto 0:669ef71cba68 1144
t_yamamoto 0:669ef71cba68 1145 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 1146 static void Process9()
t_yamamoto 0:669ef71cba68 1147 {
t_yamamoto 0:669ef71cba68 1148
t_yamamoto 0:669ef71cba68 1149 }
t_yamamoto 0:669ef71cba68 1150 #endif
t_yamamoto 0:669ef71cba68 1151 #endif
t_yamamoto 0:669ef71cba68 1152 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 1153
t_yamamoto 0:669ef71cba68 1154 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 1155 {
t_yamamoto 0:669ef71cba68 1156
t_yamamoto 0:669ef71cba68 1157 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 1158 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 1159 #endif
t_yamamoto 0:669ef71cba68 1160
t_yamamoto 0:669ef71cba68 1161 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 1162 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 1163 {
t_yamamoto 0:669ef71cba68 1164 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 1165 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 1166 }
t_yamamoto 0:669ef71cba68 1167 #endif
t_yamamoto 0:669ef71cba68 1168 }
t_yamamoto 0:669ef71cba68 1169
t_yamamoto 0:669ef71cba68 1170 #pragma region USER-DEFINED-FUNCTIONS
kishibekairohan 8:6fb3723f7747 1171 void pointcalculation(){
kishibekairohan 8:6fb3723f7747 1172 ColorDetection();
kishibekairohan 8:6fb3723f7747 1173 /*if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
kishibekairohan 8:6fb3723f7747 1174 {
kishibekairohan 8:6fb3723f7747 1175 invationA ^= 1;//start false,over true
kishibekairohan 8:6fb3723f7747 1176 compA = true;//on true,noon false
kishibekairohan 8:6fb3723f7747 1177 }
kishibekairohan 8:6fb3723f7747 1178 else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶*/
kishibekairohan 8:6fb3723f7747 1179 for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];}
kishibekairohan 8:6fb3723f7747 1180 for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];}
kishibekairohan 8:6fb3723f7747 1181 for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];}
kishibekairohan 8:6fb3723f7747 1182 for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];}
kishibekairohan 8:6fb3723f7747 1183 }
kishibekairohan 8:6fb3723f7747 1184
kishibekairohan 8:6fb3723f7747 1185 void filip(){
kishibekairohan 8:6fb3723f7747 1186 palseX = RtX.getPulses();
kishibekairohan 8:6fb3723f7747 1187 palseY = RtY.getPulses();
7ka884 6:10e22bc327ce 1188
kishibekairohan 9:f93fc79a49ea 1189 rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1190 rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan 9:f93fc79a49ea 1191
kishibekairohan 10:1295d39fec3a 1192 disX = 48*3.141*rpmX;
kishibekairohan 9:f93fc79a49ea 1193 disY = 48*3.141*rpmY;
kishibekairohan 9:f93fc79a49ea 1194
kishibekairohan 10:1295d39fec3a 1195 RtpwmX = (int)Rt_X.SetPV(disX , goalX);
kishibekairohan 9:f93fc79a49ea 1196 RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
kishibekairohan 8:6fb3723f7747 1197 }
7ka884 6:10e22bc327ce 1198
7ka884 6:10e22bc327ce 1199
kishibekairohan 2:c015739085d3 1200 void ColorDetection(){
kishibekairohan 2:c015739085d3 1201 GATE = 0;
kishibekairohan 2:c015739085d3 1202
kishibekairohan 2:c015739085d3 1203 CK[0] = 0;
kishibekairohan 2:c015739085d3 1204 CK[1] = 0;
kishibekairohan 2:c015739085d3 1205 CK[2] = 0;
kishibekairohan 2:c015739085d3 1206 CK[3] = 0;
kishibekairohan 2:c015739085d3 1207
kishibekairohan 2:c015739085d3 1208 RANGE = 1;
kishibekairohan 2:c015739085d3 1209
kishibekairohan 2:c015739085d3 1210 GATE = 1;
kishibekairohan 2:c015739085d3 1211 wait_ms(intergration);
kishibekairohan 2:c015739085d3 1212 GATE = 0;
kishibekairohan 2:c015739085d3 1213 wait_us(4);
kishibekairohan 2:c015739085d3 1214
kishibekairohan 2:c015739085d3 1215 Color_A[0] = ColorIn(0); //赤
kishibekairohan 2:c015739085d3 1216 wait_us(3);
kishibekairohan 2:c015739085d3 1217 Color_A[1] = ColorIn(0); //青
kishibekairohan 2:c015739085d3 1218 wait_us(3);
kishibekairohan 2:c015739085d3 1219 Color_A[2] = ColorIn(0); //緑
kishibekairohan 2:c015739085d3 1220
kishibekairohan 2:c015739085d3 1221 Color_B[0] = ColorIn(1);
kishibekairohan 2:c015739085d3 1222 wait_us(3);
kishibekairohan 2:c015739085d3 1223 Color_B[1] = ColorIn(1);
kishibekairohan 2:c015739085d3 1224 wait_us(3);
kishibekairohan 2:c015739085d3 1225 Color_B[2] = ColorIn(1);
kishibekairohan 2:c015739085d3 1226
kishibekairohan 2:c015739085d3 1227 Color_C[0] = ColorIn(2);
kishibekairohan 2:c015739085d3 1228 wait_us(3);
kishibekairohan 2:c015739085d3 1229 Color_C[1] = ColorIn(2);
kishibekairohan 2:c015739085d3 1230 wait_us(3);
kishibekairohan 2:c015739085d3 1231 Color_C[2] = ColorIn(2);
kishibekairohan 2:c015739085d3 1232
kishibekairohan 2:c015739085d3 1233 Color_D[0] = ColorIn(3);
kishibekairohan 2:c015739085d3 1234 wait_us(3);
kishibekairohan 2:c015739085d3 1235 Color_D[1] = ColorIn(3);
kishibekairohan 2:c015739085d3 1236 wait_us(3);
kishibekairohan 2:c015739085d3 1237 Color_D[2] = ColorIn(3);
kishibekairohan 2:c015739085d3 1238 }
kishibekairohan 9:f93fc79a49ea 1239 void BuzzerTimer_func() {
kishibekairohan 11:028a150943b5 1240 if(buzzer == 0.5){
kishibekairohan 11:028a150943b5 1241 buzzer = 0;
kishibekairohan 11:028a150943b5 1242 }
kishibekairohan 11:028a150943b5 1243 else if(buzzer == 0){
kishibekairohan 11:028a150943b5 1244 buzzer = 0.5;
kishibekairohan 11:028a150943b5 1245 }
kishibekairohan 11:028a150943b5 1246 }
kishibekairohan 11:028a150943b5 1247
kishibekairohan 11:028a150943b5 1248 void TapeLedEms_func() {
kishibekairohan 11:028a150943b5 1249 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 1250 }
t_yamamoto 0:669ef71cba68 1251 #pragma endregion