![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp@4:ba9df71868df, 2018-10-01 (annotated)
- Committer:
- 7ka884
- Date:
- Mon Oct 01 13:47:19 2018 +0000
- Revision:
- 4:ba9df71868df
- Parent:
- 3:e10d8736fd22
- Child:
- 6:10e22bc327ce
huhuhuh
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:c015739085d3 | 45 | unsigned long ColorIn(int index) |
kishibekairohan | 2:c015739085d3 | 46 | { |
kishibekairohan | 2:c015739085d3 | 47 | int result = 0; |
kishibekairohan | 2:c015739085d3 | 48 | bool rtn = false; |
kishibekairohan | 2:c015739085d3 | 49 | for(int i=0; i<12; i++) |
kishibekairohan | 2:c015739085d3 | 50 | { |
kishibekairohan | 2:c015739085d3 | 51 | CK[index] = 1; |
kishibekairohan | 2:c015739085d3 | 52 | rtn = DOUT[index]; |
kishibekairohan | 2:c015739085d3 | 53 | CK[index] = 0; |
kishibekairohan | 2:c015739085d3 | 54 | if(rtn) |
kishibekairohan | 2:c015739085d3 | 55 | { |
kishibekairohan | 2:c015739085d3 | 56 | result|=(1 << i); |
kishibekairohan | 2:c015739085d3 | 57 | } |
kishibekairohan | 2:c015739085d3 | 58 | } |
kishibekairohan | 2:c015739085d3 | 59 | return result; |
kishibekairohan | 2:c015739085d3 | 60 | } |
kishibekairohan | 2:c015739085d3 | 61 | |
7ka884 | 4:ba9df71868df | 62 | #define TIRE_FR 0 //足回り前右 |
7ka884 | 4:ba9df71868df | 63 | #define TIRE_FL 1 //足回り前左 |
7ka884 | 4:ba9df71868df | 64 | #define TIRE_BR 2 //足回り後右 |
7ka884 | 4:ba9df71868df | 65 | #define TIRE_BL 3 //足回り後左 |
kishibekairohan | 2:c015739085d3 | 66 | |
7ka884 | 4:ba9df71868df | 67 | #define Angul_R 4 //角度調節右 |
7ka884 | 4:ba9df71868df | 68 | #define Angul_L 5 //角度調節左 |
kishibekairohan | 2:c015739085d3 | 69 | |
kishibekairohan | 2:c015739085d3 | 70 | const int mecanum[15][15]= |
kishibekairohan | 2:c015739085d3 | 71 | { |
kishibekairohan | 2:c015739085d3 | 72 | { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255}, |
kishibekairohan | 2:c015739085d3 | 73 | { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255}, |
kishibekairohan | 2:c015739085d3 | 74 | { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255}, |
kishibekairohan | 2:c015739085d3 | 75 | { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255}, |
kishibekairohan | 2:c015739085d3 | 76 | { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255}, |
kishibekairohan | 2:c015739085d3 | 77 | {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255}, |
kishibekairohan | 2:c015739085d3 | 78 | {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255}, |
kishibekairohan | 2:c015739085d3 | 79 | {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255}, |
kishibekairohan | 2:c015739085d3 | 80 | {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187}, |
kishibekairohan | 2:c015739085d3 | 81 | {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130}, |
kishibekairohan | 2:c015739085d3 | 82 | {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83}, |
kishibekairohan | 2:c015739085d3 | 83 | {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47}, |
kishibekairohan | 2:c015739085d3 | 84 | {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21}, |
kishibekairohan | 2:c015739085d3 | 85 | {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5}, |
kishibekairohan | 2:c015739085d3 | 86 | {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0} |
kishibekairohan | 2:c015739085d3 | 87 | }; |
kishibekairohan | 2:c015739085d3 | 88 | |
kishibekairohan | 2:c015739085d3 | 89 | const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204}; |
kishibekairohan | 2:c015739085d3 | 90 | uint8_t SetStatus(int); |
kishibekairohan | 2:c015739085d3 | 91 | uint8_t SetStatus(int pwmVal){ |
kishibekairohan | 2:c015739085d3 | 92 | if(pwmVal < 0) return BACK; |
kishibekairohan | 2:c015739085d3 | 93 | else if(pwmVal > 0) return FOR; |
kishibekairohan | 2:c015739085d3 | 94 | else if(pwmVal == 0) return BRAKE; |
kishibekairohan | 2:c015739085d3 | 95 | else return BRAKE; |
kishibekairohan | 2:c015739085d3 | 96 | } |
kishibekairohan | 2:c015739085d3 | 97 | uint8_t SetPWM(int); |
kishibekairohan | 2:c015739085d3 | 98 | uint8_t SetPWM(int pwmVal){ |
kishibekairohan | 2:c015739085d3 | 99 | if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; |
kishibekairohan | 2:c015739085d3 | 100 | else return abs(pwmVal); |
kishibekairohan | 2:c015739085d3 | 101 | } |
kishibekairohan | 2:c015739085d3 | 102 | |
kishibekairohan | 2:c015739085d3 | 103 | int Color_A[3]; //[赤,緑,青] |
kishibekairohan | 2:c015739085d3 | 104 | int Color_B[3]; |
kishibekairohan | 2:c015739085d3 | 105 | int Color_C[3]; |
kishibekairohan | 2:c015739085d3 | 106 | int Color_D[3]; |
kishibekairohan | 2:c015739085d3 | 107 | int intergration = 50; |
kishibekairohan | 2:c015739085d3 | 108 | |
kishibekairohan | 2:c015739085d3 | 109 | //************ライントレース変数******************* |
kishibekairohan | 2:c015739085d3 | 110 | int Point[3] = {234, 466, 590};//赤,緑,青 |
kishibekairohan | 2:c015739085d3 | 111 | |
7ka884 | 4:ba9df71868df | 112 | int startP = 35; |
7ka884 | 4:ba9df71868df | 113 | int downP = 5; |
kishibekairohan | 2:c015739085d3 | 114 | //************ライントレース変数******************* |
7ka884 | 4:ba9df71868df | 115 | //ROタコン |
7ka884 | 4:ba9df71868df | 116 | QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING); |
7ka884 | 4:ba9df71868df | 117 | |
7ka884 | 4:ba9df71868df | 118 | Ticker get_rpm; |
7ka884 | 4:ba9df71868df | 119 | int rpm; |
7ka884 | 4:ba9df71868df | 120 | int palse; |
7ka884 | 4:ba9df71868df | 121 | double goalpoint = 3000.0000; |
7ka884 | 4:ba9df71868df | 122 | |
7ka884 | 4:ba9df71868df | 123 | PID startup = PID(0.03, -255, 255, 0.3, 0, 0); |
7ka884 | 4:ba9df71868df | 124 | |
7ka884 | 4:ba9df71868df | 125 | // |
kishibekairohan | 2:c015739085d3 | 126 | |
7ka884 | 4:ba9df71868df | 127 | int averageR_A; |
7ka884 | 4:ba9df71868df | 128 | int averageG_A; |
7ka884 | 4:ba9df71868df | 129 | int averageB_A; |
7ka884 | 4:ba9df71868df | 130 | int averageR_B; |
7ka884 | 4:ba9df71868df | 131 | int averageG_B; |
7ka884 | 4:ba9df71868df | 132 | int averageB_B; |
7ka884 | 4:ba9df71868df | 133 | int averageR_C; |
7ka884 | 4:ba9df71868df | 134 | int averageG_C; |
7ka884 | 4:ba9df71868df | 135 | int averageB_C; |
7ka884 | 4:ba9df71868df | 136 | int averageR_D; |
7ka884 | 4:ba9df71868df | 137 | int averageG_D; |
7ka884 | 4:ba9df71868df | 138 | int averageB_D; |
kishibekairohan | 2:c015739085d3 | 139 | |
kishibekairohan | 2:c015739085d3 | 140 | void ColorDetection(); |
kishibekairohan | 2:c015739085d3 | 141 | |
t_yamamoto | 0:669ef71cba68 | 142 | #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE |
t_yamamoto | 0:669ef71cba68 | 143 | |
t_yamamoto | 0:669ef71cba68 | 144 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 145 | #if USE_PROCESS_NUM>0 |
t_yamamoto | 0:669ef71cba68 | 146 | static void Process0(void); |
t_yamamoto | 0:669ef71cba68 | 147 | #endif |
t_yamamoto | 0:669ef71cba68 | 148 | #if USE_PROCESS_NUM>1 |
t_yamamoto | 0:669ef71cba68 | 149 | static void Process1(void); |
t_yamamoto | 0:669ef71cba68 | 150 | #endif |
t_yamamoto | 0:669ef71cba68 | 151 | #if USE_PROCESS_NUM>2 |
t_yamamoto | 0:669ef71cba68 | 152 | static void Process2(void); |
t_yamamoto | 0:669ef71cba68 | 153 | #endif |
t_yamamoto | 0:669ef71cba68 | 154 | #if USE_PROCESS_NUM>3 |
t_yamamoto | 0:669ef71cba68 | 155 | static void Process3(void); |
t_yamamoto | 0:669ef71cba68 | 156 | #endif |
t_yamamoto | 0:669ef71cba68 | 157 | #if USE_PROCESS_NUM>4 |
t_yamamoto | 0:669ef71cba68 | 158 | static void Process4(void); |
t_yamamoto | 0:669ef71cba68 | 159 | #endif |
t_yamamoto | 0:669ef71cba68 | 160 | #if USE_PROCESS_NUM>5 |
t_yamamoto | 0:669ef71cba68 | 161 | static void Process5(void); |
t_yamamoto | 0:669ef71cba68 | 162 | #endif |
t_yamamoto | 0:669ef71cba68 | 163 | #if USE_PROCESS_NUM>6 |
t_yamamoto | 0:669ef71cba68 | 164 | static void Process6(void); |
t_yamamoto | 0:669ef71cba68 | 165 | #endif |
t_yamamoto | 0:669ef71cba68 | 166 | #if USE_PROCESS_NUM>7 |
t_yamamoto | 0:669ef71cba68 | 167 | static void Process7(void); |
t_yamamoto | 0:669ef71cba68 | 168 | #endif |
t_yamamoto | 0:669ef71cba68 | 169 | #if USE_PROCESS_NUM>8 |
t_yamamoto | 0:669ef71cba68 | 170 | static void Process8(void); |
t_yamamoto | 0:669ef71cba68 | 171 | #endif |
t_yamamoto | 0:669ef71cba68 | 172 | #if USE_PROCESS_NUM>9 |
t_yamamoto | 0:669ef71cba68 | 173 | static void Process9(void); |
t_yamamoto | 0:669ef71cba68 | 174 | #endif |
t_yamamoto | 0:669ef71cba68 | 175 | #endif |
t_yamamoto | 0:669ef71cba68 | 176 | |
t_yamamoto | 0:669ef71cba68 | 177 | void SystemProcessInitialize() |
t_yamamoto | 0:669ef71cba68 | 178 | { |
t_yamamoto | 0:669ef71cba68 | 179 | #pragma region USER-DEFINED_VARIABLE_INIT |
t_yamamoto | 0:669ef71cba68 | 180 | |
t_yamamoto | 0:669ef71cba68 | 181 | /*Replace here with the initialization code of your variables.*/ |
t_yamamoto | 0:669ef71cba68 | 182 | |
t_yamamoto | 0:669ef71cba68 | 183 | #pragma endregion USER-DEFINED_VARIABLE_INIT |
t_yamamoto | 0:669ef71cba68 | 184 | |
t_yamamoto | 0:669ef71cba68 | 185 | lock = true; |
t_yamamoto | 0:669ef71cba68 | 186 | processChangeComp = true; |
t_yamamoto | 0:669ef71cba68 | 187 | current = DEFAULT_PROCESS; |
t_yamamoto | 0:669ef71cba68 | 188 | |
t_yamamoto | 0:669ef71cba68 | 189 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 190 | #if USE_PROCESS_NUM>0 |
t_yamamoto | 0:669ef71cba68 | 191 | Process[0] = Process0; |
t_yamamoto | 0:669ef71cba68 | 192 | #endif |
t_yamamoto | 0:669ef71cba68 | 193 | #if USE_PROCESS_NUM>1 |
t_yamamoto | 0:669ef71cba68 | 194 | Process[1] = Process1; |
t_yamamoto | 0:669ef71cba68 | 195 | #endif |
t_yamamoto | 0:669ef71cba68 | 196 | #if USE_PROCESS_NUM>2 |
t_yamamoto | 0:669ef71cba68 | 197 | Process[2] = Process2; |
t_yamamoto | 0:669ef71cba68 | 198 | #endif |
t_yamamoto | 0:669ef71cba68 | 199 | #if USE_PROCESS_NUM>3 |
t_yamamoto | 0:669ef71cba68 | 200 | Process[3] = Process3; |
t_yamamoto | 0:669ef71cba68 | 201 | #endif |
t_yamamoto | 0:669ef71cba68 | 202 | #if USE_PROCESS_NUM>4 |
t_yamamoto | 0:669ef71cba68 | 203 | Process[4] = Process4; |
t_yamamoto | 0:669ef71cba68 | 204 | #endif |
t_yamamoto | 0:669ef71cba68 | 205 | #if USE_PROCESS_NUM>5 |
t_yamamoto | 0:669ef71cba68 | 206 | Process[5] = Process5; |
t_yamamoto | 0:669ef71cba68 | 207 | #endif |
t_yamamoto | 0:669ef71cba68 | 208 | #if USE_PROCESS_NUM>6 |
t_yamamoto | 0:669ef71cba68 | 209 | Process[6] = Process6; |
t_yamamoto | 0:669ef71cba68 | 210 | #endif |
t_yamamoto | 0:669ef71cba68 | 211 | #if USE_PROCESS_NUM>7 |
t_yamamoto | 0:669ef71cba68 | 212 | Process[7] = Process7; |
t_yamamoto | 0:669ef71cba68 | 213 | #endif |
t_yamamoto | 0:669ef71cba68 | 214 | #if USE_PROCESS_NUM>8 |
t_yamamoto | 0:669ef71cba68 | 215 | Process[8] = Process8; |
t_yamamoto | 0:669ef71cba68 | 216 | #endif |
t_yamamoto | 0:669ef71cba68 | 217 | #if USE_PROCESS_NUM>9 |
t_yamamoto | 0:669ef71cba68 | 218 | Process[9] = Process9; |
t_yamamoto | 0:669ef71cba68 | 219 | #endif |
t_yamamoto | 0:669ef71cba68 | 220 | #endif |
t_yamamoto | 0:669ef71cba68 | 221 | } |
t_yamamoto | 0:669ef71cba68 | 222 | |
t_yamamoto | 0:669ef71cba68 | 223 | static void SystemProcessUpdate() |
t_yamamoto | 0:669ef71cba68 | 224 | { |
t_yamamoto | 0:669ef71cba68 | 225 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 226 | if(controller->Button.HOME) lock = false; |
t_yamamoto | 0:669ef71cba68 | 227 | |
t_yamamoto | 0:669ef71cba68 | 228 | if(controller->Button.START && processChangeComp) |
t_yamamoto | 0:669ef71cba68 | 229 | { |
t_yamamoto | 0:669ef71cba68 | 230 | current++; |
t_yamamoto | 0:669ef71cba68 | 231 | if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; |
t_yamamoto | 0:669ef71cba68 | 232 | processChangeComp = false; |
t_yamamoto | 0:669ef71cba68 | 233 | } |
t_yamamoto | 0:669ef71cba68 | 234 | else if(controller->Button.SELECT && processChangeComp) |
t_yamamoto | 0:669ef71cba68 | 235 | { |
t_yamamoto | 0:669ef71cba68 | 236 | current--; |
t_yamamoto | 0:669ef71cba68 | 237 | if (current < 0) current = 0; |
t_yamamoto | 0:669ef71cba68 | 238 | processChangeComp = false; |
t_yamamoto | 0:669ef71cba68 | 239 | } |
t_yamamoto | 0:669ef71cba68 | 240 | else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; |
t_yamamoto | 0:669ef71cba68 | 241 | #endif |
t_yamamoto | 0:669ef71cba68 | 242 | |
t_yamamoto | 0:669ef71cba68 | 243 | #ifdef USE_MOTOR |
t_yamamoto | 0:669ef71cba68 | 244 | ACTUATORHUB::MOTOR::Motor::Update(motor); |
t_yamamoto | 0:669ef71cba68 | 245 | #endif |
t_yamamoto | 0:669ef71cba68 | 246 | |
t_yamamoto | 0:669ef71cba68 | 247 | #ifdef USE_SOLENOID |
t_yamamoto | 0:669ef71cba68 | 248 | ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); |
t_yamamoto | 0:669ef71cba68 | 249 | #endif |
t_yamamoto | 0:669ef71cba68 | 250 | |
t_yamamoto | 0:669ef71cba68 | 251 | #ifdef USE_RS485 |
t_yamamoto | 0:669ef71cba68 | 252 | ACTUATORHUB::ActuatorHub::Update(); |
t_yamamoto | 0:669ef71cba68 | 253 | #endif |
t_yamamoto | 0:669ef71cba68 | 254 | |
t_yamamoto | 0:669ef71cba68 | 255 | } |
t_yamamoto | 0:669ef71cba68 | 256 | |
kishibekairohan | 2:c015739085d3 | 257 | |
kishibekairohan | 2:c015739085d3 | 258 | |
t_yamamoto | 0:669ef71cba68 | 259 | void SystemProcess() |
t_yamamoto | 0:669ef71cba68 | 260 | { |
t_yamamoto | 0:669ef71cba68 | 261 | SystemProcessInitialize(); |
t_yamamoto | 0:669ef71cba68 | 262 | |
t_yamamoto | 0:669ef71cba68 | 263 | while(1) |
7ka884 | 4:ba9df71868df | 264 | { |
7ka884 | 4:ba9df71868df | 265 | /*wait(0.1); |
7ka884 | 4:ba9df71868df | 266 | //wheel.getPulses()...どちらの方向にどれだけ回ったか |
7ka884 | 4:ba9df71868df | 267 | pc.printf("Pulses:%07d \r\n",wheel.getPulses()); |
7ka884 | 4:ba9df71868df | 268 | //軸が何回転したか |
7ka884 | 4:ba9df71868df | 269 | pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2)); |
7ka884 | 4:ba9df71868df | 270 | */ |
7ka884 | 4:ba9df71868df | 271 | |
7ka884 | 4:ba9df71868df | 272 | /* float x = 0, y= 0, z = 0; |
kishibekairohan | 2:c015739085d3 | 273 | |
kishibekairohan | 2:c015739085d3 | 274 | x = acc[0]*1000; |
kishibekairohan | 2:c015739085d3 | 275 | y = acc[1]*1000; |
kishibekairohan | 2:c015739085d3 | 276 | z = acc[2]*1000; |
kishibekairohan | 2:c015739085d3 | 277 | |
kishibekairohan | 2:c015739085d3 | 278 | pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z); |
kishibekairohan | 2:c015739085d3 | 279 | |
kishibekairohan | 2:c015739085d3 | 280 | float rotateX = (x - 306)/2.22 - 90; |
kishibekairohan | 2:c015739085d3 | 281 | float rotateY = (y - 305)/2.21 - 90; |
7ka884 | 4:ba9df71868df | 282 | float rotateZ = (z - 300)/1.18 - 90; |
7ka884 | 4:ba9df71868df | 283 | pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ); |
kishibekairohan | 2:c015739085d3 | 284 | wait_ms(50); |
7ka884 | 4:ba9df71868df | 285 | */ |
7ka884 | 4:ba9df71868df | 286 | /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON; |
7ka884 | 4:ba9df71868df | 287 | else LED_DEBUG0 = LED_OFF;*/ |
kishibekairohan | 2:c015739085d3 | 288 | |
t_yamamoto | 0:669ef71cba68 | 289 | #ifdef USE_MU |
t_yamamoto | 0:669ef71cba68 | 290 | controller = CONTROLLER::Controller::GetData(); |
t_yamamoto | 0:669ef71cba68 | 291 | #endif |
t_yamamoto | 0:669ef71cba68 | 292 | |
t_yamamoto | 0:669ef71cba68 | 293 | #ifdef USE_ERRORCHECK |
t_yamamoto | 0:669ef71cba68 | 294 | if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) |
t_yamamoto | 0:669ef71cba68 | 295 | { |
t_yamamoto | 0:669ef71cba68 | 296 | CONTROLLER::Controller::DataReset(); |
t_yamamoto | 0:669ef71cba68 | 297 | AllActuatorReset(); |
t_yamamoto | 0:669ef71cba68 | 298 | lock = true; |
t_yamamoto | 0:669ef71cba68 | 299 | } |
t_yamamoto | 0:669ef71cba68 | 300 | else |
t_yamamoto | 0:669ef71cba68 | 301 | #endif |
t_yamamoto | 0:669ef71cba68 | 302 | { |
t_yamamoto | 0:669ef71cba68 | 303 | |
t_yamamoto | 0:669ef71cba68 | 304 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 305 | if(!lock) |
t_yamamoto | 0:669ef71cba68 | 306 | { |
t_yamamoto | 0:669ef71cba68 | 307 | Process[current](); |
t_yamamoto | 0:669ef71cba68 | 308 | } |
t_yamamoto | 0:669ef71cba68 | 309 | else |
t_yamamoto | 0:669ef71cba68 | 310 | #endif |
t_yamamoto | 0:669ef71cba68 | 311 | { |
t_yamamoto | 0:669ef71cba68 | 312 | //ロック時の処理 |
t_yamamoto | 0:669ef71cba68 | 313 | } |
t_yamamoto | 0:669ef71cba68 | 314 | } |
t_yamamoto | 0:669ef71cba68 | 315 | |
t_yamamoto | 0:669ef71cba68 | 316 | SystemProcessUpdate(); |
t_yamamoto | 0:669ef71cba68 | 317 | } |
t_yamamoto | 0:669ef71cba68 | 318 | } |
t_yamamoto | 0:669ef71cba68 | 319 | |
kishibekairohan | 2:c015739085d3 | 320 | |
kishibekairohan | 2:c015739085d3 | 321 | |
kishibekairohan | 2:c015739085d3 | 322 | |
t_yamamoto | 0:669ef71cba68 | 323 | #pragma region PROCESS |
t_yamamoto | 0:669ef71cba68 | 324 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 325 | #if USE_PROCESS_NUM>0 |
t_yamamoto | 0:669ef71cba68 | 326 | static void Process0() |
t_yamamoto | 0:669ef71cba68 | 327 | { |
kishibekairohan | 2:c015739085d3 | 328 | ColorDetection(); |
7ka884 | 4:ba9df71868df | 329 | pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]); |
7ka884 | 4:ba9df71868df | 330 | pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]); |
7ka884 | 4:ba9df71868df | 331 | pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]); |
7ka884 | 4:ba9df71868df | 332 | pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]); |
t_yamamoto | 0:669ef71cba68 | 333 | } |
t_yamamoto | 0:669ef71cba68 | 334 | #endif |
t_yamamoto | 0:669ef71cba68 | 335 | |
t_yamamoto | 0:669ef71cba68 | 336 | #if USE_PROCESS_NUM>1 |
t_yamamoto | 0:669ef71cba68 | 337 | static void Process1() |
t_yamamoto | 0:669ef71cba68 | 338 | { |
7ka884 | 4:ba9df71868df | 339 | |
7ka884 | 4:ba9df71868df | 340 | |
7ka884 | 4:ba9df71868df | 341 | motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); |
7ka884 | 4:ba9df71868df | 342 | motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); |
7ka884 | 4:ba9df71868df | 343 | motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); |
7ka884 | 4:ba9df71868df | 344 | motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); |
kishibekairohan | 2:c015739085d3 | 345 | |
7ka884 | 4:ba9df71868df | 346 | motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]); |
7ka884 | 4:ba9df71868df | 347 | motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]); |
7ka884 | 4:ba9df71868df | 348 | motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]); |
7ka884 | 4:ba9df71868df | 349 | motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]); |
kishibekairohan | 2:c015739085d3 | 350 | |
kishibekairohan | 2:c015739085d3 | 351 | if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){ |
7ka884 | 4:ba9df71868df | 352 | motor[TIRE_FR].pwm = motor[0].pwm * 1.3; |
7ka884 | 4:ba9df71868df | 353 | motor[TIRE_FL].pwm = motor[1].pwm * 1.3; |
7ka884 | 4:ba9df71868df | 354 | } |
7ka884 | 4:ba9df71868df | 355 | |
kishibekairohan | 2:c015739085d3 | 356 | |
7ka884 | 4:ba9df71868df | 357 | //wheel.getPulses()...どちらの方向にどれだけ回ったか |
7ka884 | 4:ba9df71868df | 358 | pc.printf("Pulses:%07d \r\n",wheel.getPulses()); |
7ka884 | 4:ba9df71868df | 359 | //軸が何回転したか |
7ka884 | 4:ba9df71868df | 360 | pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2)); |
t_yamamoto | 0:669ef71cba68 | 361 | } |
t_yamamoto | 0:669ef71cba68 | 362 | #endif |
t_yamamoto | 0:669ef71cba68 | 363 | |
t_yamamoto | 0:669ef71cba68 | 364 | #if USE_PROCESS_NUM>2 |
t_yamamoto | 0:669ef71cba68 | 365 | static void Process2() |
t_yamamoto | 0:669ef71cba68 | 366 | { |
7ka884 | 4:ba9df71868df | 367 | static bool color_flag = false; |
7ka884 | 4:ba9df71868df | 368 | |
7ka884 | 4:ba9df71868df | 369 | static bool traceon = false;//fase1 |
7ka884 | 4:ba9df71868df | 370 | static bool yokofla = false;//fase2 |
7ka884 | 4:ba9df71868df | 371 | static bool boxslip = false;//fase3 |
kishibekairohan | 2:c015739085d3 | 372 | |
7ka884 | 4:ba9df71868df | 373 | static bool compA = false; |
7ka884 | 4:ba9df71868df | 374 | static bool compB = false; |
7ka884 | 4:ba9df71868df | 375 | static bool compC = false; |
7ka884 | 4:ba9df71868df | 376 | static bool compD = false; |
7ka884 | 4:ba9df71868df | 377 | |
7ka884 | 4:ba9df71868df | 378 | static bool invationA = false; |
7ka884 | 4:ba9df71868df | 379 | static bool invationB = false; |
7ka884 | 4:ba9df71868df | 380 | static bool invationC = false; |
7ka884 | 4:ba9df71868df | 381 | static bool invationD = false; |
7ka884 | 4:ba9df71868df | 382 | |
7ka884 | 4:ba9df71868df | 383 | ColorDetection(); |
7ka884 | 4:ba9df71868df | 384 | // |
kishibekairohan | 2:c015739085d3 | 385 | if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白 |
kishibekairohan | 2:c015739085d3 | 386 | { |
kishibekairohan | 2:c015739085d3 | 387 | invationA ^= 1;//start false,over true |
kishibekairohan | 2:c015739085d3 | 388 | compA = true;//on true,noon false |
kishibekairohan | 2:c015739085d3 | 389 | } |
kishibekairohan | 2:c015739085d3 | 390 | else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶 |
kishibekairohan | 2:c015739085d3 | 391 | |
7ka884 | 4:ba9df71868df | 392 | if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白 |
7ka884 | 4:ba9df71868df | 393 | { |
7ka884 | 4:ba9df71868df | 394 | invationB ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 395 | compB = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 396 | } |
7ka884 | 4:ba9df71868df | 397 | else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶 |
7ka884 | 4:ba9df71868df | 398 | |
7ka884 | 4:ba9df71868df | 399 | if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白 |
7ka884 | 4:ba9df71868df | 400 | { |
7ka884 | 4:ba9df71868df | 401 | invationC ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 402 | compC = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 403 | } |
7ka884 | 4:ba9df71868df | 404 | else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶 |
7ka884 | 4:ba9df71868df | 405 | |
7ka884 | 4:ba9df71868df | 406 | if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白 |
kishibekairohan | 2:c015739085d3 | 407 | { |
7ka884 | 4:ba9df71868df | 408 | invationD ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 409 | compD = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 410 | } |
7ka884 | 4:ba9df71868df | 411 | else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶 |
7ka884 | 4:ba9df71868df | 412 | |
7ka884 | 4:ba9df71868df | 413 | |
7ka884 | 4:ba9df71868df | 414 | // |
7ka884 | 4:ba9df71868df | 415 | |
7ka884 | 4:ba9df71868df | 416 | if(controller->Button.B && !color_flag) |
7ka884 | 4:ba9df71868df | 417 | { |
7ka884 | 4:ba9df71868df | 418 | traceon ^= 1; |
7ka884 | 4:ba9df71868df | 419 | color_flag = true; |
kishibekairohan | 2:c015739085d3 | 420 | } |
7ka884 | 4:ba9df71868df | 421 | else if(!controller->Button.B)color_flag = false; |
kishibekairohan | 2:c015739085d3 | 422 | |
7ka884 | 4:ba9df71868df | 423 | if(traceon && !yokofla && !boxslip) |
kishibekairohan | 2:c015739085d3 | 424 | { |
7ka884 | 4:ba9df71868df | 425 | if(!invationA && !compA && !invationB && !compB) |
7ka884 | 4:ba9df71868df | 426 | { |
7ka884 | 4:ba9df71868df | 427 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 428 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 429 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 430 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 431 | |
7ka884 | 4:ba9df71868df | 432 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 433 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 434 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 435 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 436 | } |
7ka884 | 4:ba9df71868df | 437 | else if(invationA && compA && !invationB && !compB) |
7ka884 | 4:ba9df71868df | 438 | { |
7ka884 | 4:ba9df71868df | 439 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 440 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 441 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 442 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 443 | |
7ka884 | 4:ba9df71868df | 444 | motor[TIRE_FR].pwm = startP - downP; |
7ka884 | 4:ba9df71868df | 445 | motor[TIRE_FL].pwm = startP - downP; |
7ka884 | 4:ba9df71868df | 446 | motor[TIRE_BR].pwm = startP - downP; |
7ka884 | 4:ba9df71868df | 447 | motor[TIRE_BL].pwm = startP - downP; |
7ka884 | 4:ba9df71868df | 448 | } |
7ka884 | 4:ba9df71868df | 449 | else if(invationA && !compA && !invationB && !compB) |
7ka884 | 4:ba9df71868df | 450 | { |
7ka884 | 4:ba9df71868df | 451 | motor[TIRE_FR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 452 | motor[TIRE_FL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 453 | motor[TIRE_BR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 454 | motor[TIRE_BL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 455 | |
7ka884 | 4:ba9df71868df | 456 | wait(2); |
7ka884 | 4:ba9df71868df | 457 | |
7ka884 | 4:ba9df71868df | 458 | yokofla = true; |
7ka884 | 4:ba9df71868df | 459 | traceon = false; |
7ka884 | 4:ba9df71868df | 460 | } |
7ka884 | 4:ba9df71868df | 461 | else{ |
7ka884 | 4:ba9df71868df | 462 | motor[TIRE_FR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 463 | motor[TIRE_FL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 464 | motor[TIRE_BR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 465 | motor[TIRE_BL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 466 | } |
7ka884 | 4:ba9df71868df | 467 | } |
7ka884 | 4:ba9df71868df | 468 | |
7ka884 | 4:ba9df71868df | 469 | if(!traceon && yokofla && !boxslip) |
7ka884 | 4:ba9df71868df | 470 | { |
7ka884 | 4:ba9df71868df | 471 | if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4)) |
7ka884 | 4:ba9df71868df | 472 | { |
7ka884 | 4:ba9df71868df | 473 | motor[TIRE_FR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 474 | motor[TIRE_FL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 475 | motor[TIRE_BR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 476 | motor[TIRE_BL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 477 | |
7ka884 | 4:ba9df71868df | 478 | wait(2); |
7ka884 | 4:ba9df71868df | 479 | |
7ka884 | 4:ba9df71868df | 480 | boxslip = true; |
7ka884 | 4:ba9df71868df | 481 | yokofla = false; |
7ka884 | 4:ba9df71868df | 482 | } |
7ka884 | 4:ba9df71868df | 483 | else if(invationA && !compA && invationB) |
7ka884 | 4:ba9df71868df | 484 | { |
7ka884 | 4:ba9df71868df | 485 | motor[TIRE_FR].dir = BACK; |
7ka884 | 4:ba9df71868df | 486 | motor[TIRE_FL].dir = BACK; |
7ka884 | 4:ba9df71868df | 487 | motor[TIRE_BR].dir = FOR; |
7ka884 | 4:ba9df71868df | 488 | motor[TIRE_BL].dir = FOR; |
7ka884 | 4:ba9df71868df | 489 | |
7ka884 | 4:ba9df71868df | 490 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 491 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 492 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 493 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 494 | } |
7ka884 | 4:ba9df71868df | 495 | else if(!invationA && !compB && !invationB) |
7ka884 | 4:ba9df71868df | 496 | { |
7ka884 | 4:ba9df71868df | 497 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 498 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 499 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 500 | motor[TIRE_BL].dir = BACK; |
kishibekairohan | 2:c015739085d3 | 501 | |
7ka884 | 4:ba9df71868df | 502 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 503 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 504 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 505 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 506 | } |
7ka884 | 4:ba9df71868df | 507 | else if(invationA && compA && !invationB && !compB) |
7ka884 | 4:ba9df71868df | 508 | { |
7ka884 | 4:ba9df71868df | 509 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 510 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 511 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 512 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 513 | |
7ka884 | 4:ba9df71868df | 514 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 515 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 516 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 517 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 518 | } |
7ka884 | 4:ba9df71868df | 519 | else if(compB && invationB) |
7ka884 | 4:ba9df71868df | 520 | { |
7ka884 | 4:ba9df71868df | 521 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 522 | motor[TIRE_FL].dir = BACK; |
7ka884 | 4:ba9df71868df | 523 | motor[TIRE_BR].dir = FOR; |
7ka884 | 4:ba9df71868df | 524 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 525 | |
7ka884 | 4:ba9df71868df | 526 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 527 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 528 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 529 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 530 | } |
7ka884 | 4:ba9df71868df | 531 | else |
7ka884 | 4:ba9df71868df | 532 | { |
7ka884 | 4:ba9df71868df | 533 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 534 | motor[TIRE_FL].dir = BACK; |
7ka884 | 4:ba9df71868df | 535 | motor[TIRE_BR].dir = FOR; |
7ka884 | 4:ba9df71868df | 536 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 537 | |
7ka884 | 4:ba9df71868df | 538 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 539 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 540 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 541 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 542 | } |
7ka884 | 4:ba9df71868df | 543 | } |
7ka884 | 4:ba9df71868df | 544 | |
7ka884 | 4:ba9df71868df | 545 | if(!traceon && !yokofla && boxslip) |
7ka884 | 4:ba9df71868df | 546 | { |
7ka884 | 4:ba9df71868df | 547 | if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4)) |
7ka884 | 4:ba9df71868df | 548 | { |
7ka884 | 4:ba9df71868df | 549 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 550 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 551 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 552 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 553 | |
7ka884 | 4:ba9df71868df | 554 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 555 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 556 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 557 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 558 | } |
7ka884 | 4:ba9df71868df | 559 | else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)) |
7ka884 | 4:ba9df71868df | 560 | { |
7ka884 | 4:ba9df71868df | 561 | motor[TIRE_FR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 562 | motor[TIRE_FL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 563 | motor[TIRE_BR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 564 | motor[TIRE_BL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 565 | } |
7ka884 | 4:ba9df71868df | 566 | } |
7ka884 | 4:ba9df71868df | 567 | /*//// |
7ka884 | 4:ba9df71868df | 568 | motor[0].dir = BACK; |
7ka884 | 4:ba9df71868df | 569 | motor[1].dir = BACK; |
7ka884 | 4:ba9df71868df | 570 | motor[2].dir = FOR; |
7ka884 | 4:ba9df71868df | 571 | motor[3].dir = FOR; |
7ka884 | 4:ba9df71868df | 572 | |
7ka884 | 4:ba9df71868df | 573 | motor[0].pwm = startP; |
7ka884 | 4:ba9df71868df | 574 | motor[1].pwm = startP; |
7ka884 | 4:ba9df71868df | 575 | motor[2].pwm = startP; |
7ka884 | 4:ba9df71868df | 576 | motor[3].pwm = startP; |
7ka884 | 4:ba9df71868df | 577 | else if() |
7ka884 | 4:ba9df71868df | 578 | { |
7ka884 | 4:ba9df71868df | 579 | motor[0].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 580 | motor[1].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 581 | motor[2].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 582 | motor[3].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 583 | |
7ka884 | 4:ba9df71868df | 584 | motor[0].pwm = 255; |
7ka884 | 4:ba9df71868df | 585 | motor[1].pwm = 255; |
7ka884 | 4:ba9df71868df | 586 | motor[2].pwm = 255; |
7ka884 | 4:ba9df71868df | 587 | motor[3].pwm = 255; |
7ka884 | 4:ba9df71868df | 588 | }*/////// |
t_yamamoto | 0:669ef71cba68 | 589 | } |
t_yamamoto | 0:669ef71cba68 | 590 | #endif |
t_yamamoto | 0:669ef71cba68 | 591 | |
t_yamamoto | 0:669ef71cba68 | 592 | #if USE_PROCESS_NUM>3 |
t_yamamoto | 0:669ef71cba68 | 593 | static void Process3() |
t_yamamoto | 0:669ef71cba68 | 594 | { |
kishibekairohan | 2:c015739085d3 | 595 | if(controller->Button.R){ |
7ka884 | 4:ba9df71868df | 596 | motor[Angul_R].dir = FOR; |
7ka884 | 4:ba9df71868df | 597 | motor[Angul_L].dir = BACK; |
7ka884 | 4:ba9df71868df | 598 | motor[Angul_R].pwm = 150; |
7ka884 | 4:ba9df71868df | 599 | motor[Angul_L].pwm = 150; |
kishibekairohan | 2:c015739085d3 | 600 | }else if(controller->Button.L){ |
7ka884 | 4:ba9df71868df | 601 | motor[Angul_R].dir = BACK; |
7ka884 | 4:ba9df71868df | 602 | motor[Angul_L].dir = FOR; |
7ka884 | 4:ba9df71868df | 603 | motor[Angul_R].pwm = 150; |
7ka884 | 4:ba9df71868df | 604 | motor[Angul_L].pwm = 150; |
kishibekairohan | 2:c015739085d3 | 605 | }else{ |
7ka884 | 4:ba9df71868df | 606 | motor[Angul_R].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 607 | motor[Angul_L].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 608 | } |
7ka884 | 4:ba9df71868df | 609 | |
7ka884 | 4:ba9df71868df | 610 | if(LimitSw::IsPressed(0) && motor[4].dir == FOR && motor[5].dir == BACK){ |
7ka884 | 4:ba9df71868df | 611 | motor[Angul_R].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 612 | motor[Angul_L].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 613 | |
7ka884 | 4:ba9df71868df | 614 | motor[Angul_R].pwm = 255; |
7ka884 | 4:ba9df71868df | 615 | motor[Angul_L].pwm = 255; |
7ka884 | 4:ba9df71868df | 616 | }else if(LimitSw::IsPressed(1) && motor[4].dir == BACK && motor[5].dir == FOR){ |
7ka884 | 4:ba9df71868df | 617 | motor[Angul_R].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 618 | motor[Angul_L].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 619 | |
7ka884 | 4:ba9df71868df | 620 | motor[Angul_R].pwm = 255; |
7ka884 | 4:ba9df71868df | 621 | motor[Angul_L].pwm = 255; |
7ka884 | 4:ba9df71868df | 622 | } |
t_yamamoto | 0:669ef71cba68 | 623 | } |
t_yamamoto | 0:669ef71cba68 | 624 | #endif |
t_yamamoto | 0:669ef71cba68 | 625 | |
t_yamamoto | 0:669ef71cba68 | 626 | #if USE_PROCESS_NUM>4 |
t_yamamoto | 0:669ef71cba68 | 627 | static void Process4() |
t_yamamoto | 0:669ef71cba68 | 628 | { |
7ka884 | 4:ba9df71868df | 629 | static bool color_flag = false; |
7ka884 | 4:ba9df71868df | 630 | |
7ka884 | 4:ba9df71868df | 631 | static bool traceon = false;//fase1 |
7ka884 | 4:ba9df71868df | 632 | static bool yokofla = false;//fase2 |
7ka884 | 4:ba9df71868df | 633 | static bool boxslip = false;//fase3 |
7ka884 | 4:ba9df71868df | 634 | |
7ka884 | 4:ba9df71868df | 635 | static bool compA = false; |
7ka884 | 4:ba9df71868df | 636 | static bool compB = false; |
7ka884 | 4:ba9df71868df | 637 | static bool compC = false; |
7ka884 | 4:ba9df71868df | 638 | static bool compD = false; |
7ka884 | 4:ba9df71868df | 639 | |
7ka884 | 4:ba9df71868df | 640 | static bool invationA = false; |
7ka884 | 4:ba9df71868df | 641 | static bool invationB = false; |
7ka884 | 4:ba9df71868df | 642 | static bool invationC = false; |
7ka884 | 4:ba9df71868df | 643 | static bool invationD = false; |
7ka884 | 4:ba9df71868df | 644 | |
7ka884 | 4:ba9df71868df | 645 | ColorDetection(); |
7ka884 | 4:ba9df71868df | 646 | // |
7ka884 | 4:ba9df71868df | 647 | if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白 |
7ka884 | 4:ba9df71868df | 648 | { |
7ka884 | 4:ba9df71868df | 649 | invationA ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 650 | compA = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 651 | } |
7ka884 | 4:ba9df71868df | 652 | else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶 |
7ka884 | 4:ba9df71868df | 653 | |
7ka884 | 4:ba9df71868df | 654 | if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白 |
7ka884 | 4:ba9df71868df | 655 | { |
7ka884 | 4:ba9df71868df | 656 | invationB ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 657 | compB = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 658 | } |
7ka884 | 4:ba9df71868df | 659 | else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶 |
7ka884 | 4:ba9df71868df | 660 | |
7ka884 | 4:ba9df71868df | 661 | if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白 |
7ka884 | 4:ba9df71868df | 662 | { |
7ka884 | 4:ba9df71868df | 663 | invationC ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 664 | compC = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 665 | } |
7ka884 | 4:ba9df71868df | 666 | else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶 |
kishibekairohan | 2:c015739085d3 | 667 | |
7ka884 | 4:ba9df71868df | 668 | if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白 |
7ka884 | 4:ba9df71868df | 669 | { |
7ka884 | 4:ba9df71868df | 670 | invationD ^= 1;//start false,over true |
7ka884 | 4:ba9df71868df | 671 | compD = true;//on true,noon false |
7ka884 | 4:ba9df71868df | 672 | } |
7ka884 | 4:ba9df71868df | 673 | else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶 |
7ka884 | 4:ba9df71868df | 674 | |
7ka884 | 4:ba9df71868df | 675 | |
7ka884 | 4:ba9df71868df | 676 | // |
7ka884 | 4:ba9df71868df | 677 | |
7ka884 | 4:ba9df71868df | 678 | if(controller->Button.B && !color_flag) |
7ka884 | 4:ba9df71868df | 679 | { |
7ka884 | 4:ba9df71868df | 680 | traceon ^= 1; |
7ka884 | 4:ba9df71868df | 681 | color_flag = true; |
7ka884 | 4:ba9df71868df | 682 | } |
7ka884 | 4:ba9df71868df | 683 | else if(!controller->Button.B)color_flag = false; |
kishibekairohan | 2:c015739085d3 | 684 | |
7ka884 | 4:ba9df71868df | 685 | if(traceon) |
7ka884 | 4:ba9df71868df | 686 | { |
7ka884 | 4:ba9df71868df | 687 | if(compA && compD) |
7ka884 | 4:ba9df71868df | 688 | { |
7ka884 | 4:ba9df71868df | 689 | motor[TIRE_FR].dir = BACK; |
7ka884 | 4:ba9df71868df | 690 | motor[TIRE_FL].dir = BACK; |
7ka884 | 4:ba9df71868df | 691 | motor[TIRE_BR].dir = BACK; |
7ka884 | 4:ba9df71868df | 692 | motor[TIRE_BL].dir = BACK; |
7ka884 | 4:ba9df71868df | 693 | |
7ka884 | 4:ba9df71868df | 694 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 695 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 696 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 697 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 698 | } |
7ka884 | 4:ba9df71868df | 699 | else if(compB && compC) |
7ka884 | 4:ba9df71868df | 700 | { |
7ka884 | 4:ba9df71868df | 701 | motor[TIRE_FR].dir = FOR; |
7ka884 | 4:ba9df71868df | 702 | motor[TIRE_FL].dir = FOR; |
7ka884 | 4:ba9df71868df | 703 | motor[TIRE_BR].dir = FOR; |
7ka884 | 4:ba9df71868df | 704 | motor[TIRE_BL].dir = FOR; |
7ka884 | 4:ba9df71868df | 705 | |
7ka884 | 4:ba9df71868df | 706 | motor[TIRE_FR].pwm = startP; |
7ka884 | 4:ba9df71868df | 707 | motor[TIRE_FL].pwm = startP; |
7ka884 | 4:ba9df71868df | 708 | motor[TIRE_BR].pwm = startP; |
7ka884 | 4:ba9df71868df | 709 | motor[TIRE_BL].pwm = startP; |
7ka884 | 4:ba9df71868df | 710 | } |
7ka884 | 4:ba9df71868df | 711 | else |
7ka884 | 4:ba9df71868df | 712 | { |
7ka884 | 4:ba9df71868df | 713 | motor[TIRE_FR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 714 | motor[TIRE_FL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 715 | motor[TIRE_BR].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 716 | motor[TIRE_BL].dir = BRAKE; |
7ka884 | 4:ba9df71868df | 717 | } |
7ka884 | 4:ba9df71868df | 718 | } |
t_yamamoto | 0:669ef71cba68 | 719 | } |
t_yamamoto | 0:669ef71cba68 | 720 | #endif |
t_yamamoto | 0:669ef71cba68 | 721 | |
t_yamamoto | 0:669ef71cba68 | 722 | #if USE_PROCESS_NUM>5 |
t_yamamoto | 0:669ef71cba68 | 723 | static void Process5() |
t_yamamoto | 0:669ef71cba68 | 724 | { |
kishibekairohan | 2:c015739085d3 | 725 | pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read()); |
kishibekairohan | 2:c015739085d3 | 726 | //int rotateX = (acc[0].read()-)/ -90; |
kishibekairohan | 2:c015739085d3 | 727 | //int rotateY = (acc[1].read()-)/ -90; |
kishibekairohan | 2:c015739085d3 | 728 | //pc.printf("X:%d ,Y:%d", rotateX, rotateY); |
kishibekairohan | 2:c015739085d3 | 729 | wait_ms(50); |
t_yamamoto | 0:669ef71cba68 | 730 | } |
t_yamamoto | 0:669ef71cba68 | 731 | #endif |
t_yamamoto | 0:669ef71cba68 | 732 | |
t_yamamoto | 0:669ef71cba68 | 733 | #if USE_PROCESS_NUM>6 |
t_yamamoto | 0:669ef71cba68 | 734 | static void Process6() |
t_yamamoto | 0:669ef71cba68 | 735 | { |
7ka884 | 4:ba9df71868df | 736 | float x = 0, y= 0, z = 0; |
7ka884 | 4:ba9df71868df | 737 | |
7ka884 | 4:ba9df71868df | 738 | pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read()); |
7ka884 | 4:ba9df71868df | 739 | |
7ka884 | 4:ba9df71868df | 740 | x = acc[0]*1000; |
7ka884 | 4:ba9df71868df | 741 | y = acc[1]*1000; |
7ka884 | 4:ba9df71868df | 742 | z = acc[2]*1000; |
7ka884 | 4:ba9df71868df | 743 | |
7ka884 | 4:ba9df71868df | 744 | pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z); |
7ka884 | 4:ba9df71868df | 745 | |
7ka884 | 4:ba9df71868df | 746 | float rotateX = (x - 306)/2.22 - 90; |
7ka884 | 4:ba9df71868df | 747 | float rotateY = (y - 305)/2.21 - 90; |
7ka884 | 4:ba9df71868df | 748 | pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY); |
7ka884 | 4:ba9df71868df | 749 | wait_ms(50); |
7ka884 | 4:ba9df71868df | 750 | |
7ka884 | 4:ba9df71868df | 751 | /*void Anglecontrol(){ |
7ka884 | 4:ba9df71868df | 752 | if(rotateX>) && */ |
t_yamamoto | 0:669ef71cba68 | 753 | } |
t_yamamoto | 0:669ef71cba68 | 754 | #endif |
t_yamamoto | 0:669ef71cba68 | 755 | |
t_yamamoto | 0:669ef71cba68 | 756 | #if USE_PROCESS_NUM>7 |
t_yamamoto | 0:669ef71cba68 | 757 | static void Process7() |
t_yamamoto | 0:669ef71cba68 | 758 | { |
7ka884 | 4:ba9df71868df | 759 | |
t_yamamoto | 0:669ef71cba68 | 760 | } |
t_yamamoto | 0:669ef71cba68 | 761 | #endif |
t_yamamoto | 0:669ef71cba68 | 762 | |
t_yamamoto | 0:669ef71cba68 | 763 | #if USE_PROCESS_NUM>8 |
t_yamamoto | 0:669ef71cba68 | 764 | static void Process8() |
t_yamamoto | 0:669ef71cba68 | 765 | { |
t_yamamoto | 0:669ef71cba68 | 766 | |
t_yamamoto | 0:669ef71cba68 | 767 | } |
t_yamamoto | 0:669ef71cba68 | 768 | #endif |
t_yamamoto | 0:669ef71cba68 | 769 | |
t_yamamoto | 0:669ef71cba68 | 770 | #if USE_PROCESS_NUM>9 |
t_yamamoto | 0:669ef71cba68 | 771 | static void Process9() |
t_yamamoto | 0:669ef71cba68 | 772 | { |
t_yamamoto | 0:669ef71cba68 | 773 | |
t_yamamoto | 0:669ef71cba68 | 774 | } |
t_yamamoto | 0:669ef71cba68 | 775 | #endif |
t_yamamoto | 0:669ef71cba68 | 776 | #endif |
t_yamamoto | 0:669ef71cba68 | 777 | #pragma endregion PROCESS |
t_yamamoto | 0:669ef71cba68 | 778 | |
t_yamamoto | 0:669ef71cba68 | 779 | static void AllActuatorReset() |
t_yamamoto | 0:669ef71cba68 | 780 | { |
t_yamamoto | 0:669ef71cba68 | 781 | |
t_yamamoto | 0:669ef71cba68 | 782 | #ifdef USE_SOLENOID |
t_yamamoto | 0:669ef71cba68 | 783 | solenoid.all = ALL_SOLENOID_OFF; |
t_yamamoto | 0:669ef71cba68 | 784 | #endif |
t_yamamoto | 0:669ef71cba68 | 785 | |
t_yamamoto | 0:669ef71cba68 | 786 | #ifdef USE_MOTOR |
t_yamamoto | 0:669ef71cba68 | 787 | for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) |
t_yamamoto | 0:669ef71cba68 | 788 | { |
t_yamamoto | 0:669ef71cba68 | 789 | motor[i].dir = FREE; |
t_yamamoto | 0:669ef71cba68 | 790 | motor[i].pwm = 0; |
t_yamamoto | 0:669ef71cba68 | 791 | } |
t_yamamoto | 0:669ef71cba68 | 792 | #endif |
t_yamamoto | 0:669ef71cba68 | 793 | } |
t_yamamoto | 0:669ef71cba68 | 794 | |
t_yamamoto | 0:669ef71cba68 | 795 | #pragma region USER-DEFINED-FUNCTIONS |
kishibekairohan | 2:c015739085d3 | 796 | void ColorDetection(){ |
kishibekairohan | 2:c015739085d3 | 797 | GATE = 0; |
kishibekairohan | 2:c015739085d3 | 798 | |
kishibekairohan | 2:c015739085d3 | 799 | CK[0] = 0; |
kishibekairohan | 2:c015739085d3 | 800 | CK[1] = 0; |
kishibekairohan | 2:c015739085d3 | 801 | CK[2] = 0; |
kishibekairohan | 2:c015739085d3 | 802 | CK[3] = 0; |
kishibekairohan | 2:c015739085d3 | 803 | |
kishibekairohan | 2:c015739085d3 | 804 | RANGE = 1; |
kishibekairohan | 2:c015739085d3 | 805 | |
kishibekairohan | 2:c015739085d3 | 806 | GATE = 1; |
kishibekairohan | 2:c015739085d3 | 807 | wait_ms(intergration); |
kishibekairohan | 2:c015739085d3 | 808 | GATE = 0; |
kishibekairohan | 2:c015739085d3 | 809 | wait_us(4); |
kishibekairohan | 2:c015739085d3 | 810 | |
kishibekairohan | 2:c015739085d3 | 811 | Color_A[0] = ColorIn(0); //赤 |
kishibekairohan | 2:c015739085d3 | 812 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 813 | Color_A[1] = ColorIn(0); //青 |
kishibekairohan | 2:c015739085d3 | 814 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 815 | Color_A[2] = ColorIn(0); //緑 |
kishibekairohan | 2:c015739085d3 | 816 | |
kishibekairohan | 2:c015739085d3 | 817 | Color_B[0] = ColorIn(1); |
kishibekairohan | 2:c015739085d3 | 818 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 819 | Color_B[1] = ColorIn(1); |
kishibekairohan | 2:c015739085d3 | 820 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 821 | Color_B[2] = ColorIn(1); |
kishibekairohan | 2:c015739085d3 | 822 | |
kishibekairohan | 2:c015739085d3 | 823 | Color_C[0] = ColorIn(2); |
kishibekairohan | 2:c015739085d3 | 824 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 825 | Color_C[1] = ColorIn(2); |
kishibekairohan | 2:c015739085d3 | 826 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 827 | Color_C[2] = ColorIn(2); |
kishibekairohan | 2:c015739085d3 | 828 | |
kishibekairohan | 2:c015739085d3 | 829 | Color_D[0] = ColorIn(3); |
kishibekairohan | 2:c015739085d3 | 830 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 831 | Color_D[1] = ColorIn(3); |
kishibekairohan | 2:c015739085d3 | 832 | wait_us(3); |
kishibekairohan | 2:c015739085d3 | 833 | Color_D[2] = ColorIn(3); |
kishibekairohan | 2:c015739085d3 | 834 | } |
t_yamamoto | 0:669ef71cba68 | 835 | #pragma endregion |