Miya Miyagawa
lknds
Dependencies: mbed TrapezoidControl Pulse QEI
Diff: System/Process/Process.cpp
- Revision:
- 4:ba9df71868df
- Parent:
- 3:e10d8736fd22
- Child:
- 6:10e22bc327ce
--- a/System/Process/Process.cpp Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/Process.cpp Mon Oct 01 13:47:19 2018 +0000
@@ -1,6 +1,8 @@
#include "mbed.h"
#include "Process.h"
+#include "QEI.h"
+#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
@@ -17,6 +19,8 @@
using namespace SWITCH;
using namespace COLORSENSOR;
using namespace ACCELERATIONSENSOR;
+using namespace PID_SPACE;
+using namespace ROTARYENCODER;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -55,13 +59,13 @@
return result;
}
-#define TILE_FR 0 //足回り前右
-#define TILE_FL 1 //足回り前左
-#define TILE_BR 2 //足回り後右
-#define TILE_BL 3 //足回り後左
+#define TIRE_FR 0 //足回り前右
+#define TIRE_FL 1 //足回り前左
+#define TIRE_BR 2 //足回り後右
+#define TIRE_BL 3 //足回り後左
-#define Anguladjust_R 4 //角度調節右
-#define Anguladjust_L 5 //角度調節左
+#define Angul_R 4 //角度調節右
+#define Angul_L 5 //角度調節左
const int mecanum[15][15]=
{
@@ -105,32 +109,33 @@
//************ライントレース変数*******************
int Point[3] = {234, 466, 590};//赤，緑，青
- int startP = 150;
- int downP = 70;
-
- bool compA = false;
- bool compB = false;
- bool compC = false;
- bool compD = false;
-
- bool invationA = false;
- bool invationB = false;
- bool invationC = false;
- bool invationD = false;
+ int startP = 35;
+ int downP = 5;
//************ライントレース変数*******************
+//ROタコン
+ QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
+
+ Ticker get_rpm;
+ int rpm;
+ int palse;
+ double goalpoint = 3000.0000;
+
+ PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
+
+//
-int averageR_0;
-int averageG_0;
-int averageB_0;
-int averageR_1;
-int averageG_1;
-int averageB_1;
-int averageR_2;
-int averageG_2;
-int averageB_2;
-int averageR_3;
-int averageG_3;
-int averageB_3;
+int averageR_A;
+int averageG_A;
+int averageB_A;
+int averageR_B;
+int averageG_B;
+int averageB_B;
+int averageR_C;
+int averageG_C;
+int averageB_C;
+int averageR_D;
+int averageG_D;
+int averageB_D;
void ColorDetection();
@@ -256,10 +261,15 @@
SystemProcessInitialize();
while(1)
- {
- float x = 0, y= 0, z = 0;
-
- pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
+ {
+ /*wait(0.1);
+ //wheel.getPulses()...どちらの方向にどれだけ回ったか
+ pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+ //軸が何回転したか
+ pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
+ */
+
+ /* float x = 0, y= 0, z = 0;
x = acc[0]*1000;
y = acc[1]*1000;
@@ -269,8 +279,12 @@
float rotateX = (x - 306)/2.22 - 90;
float rotateY = (y - 305)/2.21 - 90;
- pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
+ float rotateZ = (z - 300)/1.18 - 90;
+ pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
wait_ms(50);
+ */
+ /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
+ else LED_DEBUG0 = LED_OFF;*/
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
@@ -312,36 +326,62 @@
static void Process0()
{
ColorDetection();
+ pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]);
+ pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]);
+ pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]);
+ pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]);
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
- motor[0].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]) * 0.8;
- motor[1].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]) * 0.8;
- motor[2].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]) * 0.8;
- motor[3].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]) * 0.8;
+
+
+ motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
+ motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
- motor[0].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
- motor[1].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
- motor[2].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
- motor[3].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
+ motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
+ motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
+ motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
+ motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
- motor[0].pwm = motor[0].pwm * 1.3;
- motor[1].pwm = motor[1].pwm * 1.3;
+ motor[TIRE_FR].pwm = motor[0].pwm * 1.3;
+ motor[TIRE_FL].pwm = motor[1].pwm * 1.3;
+ }
+
- }
+ //wheel.getPulses()...どちらの方向にどれだけ回ったか
+ pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+ //軸が何回転したか
+ pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
}
#endif
-bool buttoncomp = false;
#if USE_PROCESS_NUM>2
static void Process2()
{
- /*ColorDetection();
+ static bool color_flag = false;
+
+ static bool traceon = false;//fase1
+ static bool yokofla = false;//fase2
+ static bool boxslip = false;//fase3
+ static bool compA = false;
+ static bool compB = false;
+ static bool compC = false;
+ static bool compD = false;
+
+ static bool invationA = false;
+ static bool invationB = false;
+ static bool invationC = false;
+ static bool invationD = false;
+
+ ColorDetection();
+ //
if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
{
invationA ^= 1;//start false,over true
@@ -349,21 +389,203 @@
}
else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
- if(controller->Button.A && buttoncomp = false)
+ if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
+ {
+ invationB ^= 1;//start false,over true
+ compB = true;//on true,noon false
+ }
+ else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
+
+ if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
+ {
+ invationC ^= 1;//start false,over true
+ compC = true;//on true,noon false
+ }
+ else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
+
+ if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
{
- motor[0].dir = dir;
- motor[0].pwm = startP;
+ invationD ^= 1;//start false,over true
+ compD = true;//on true,noon false
+ }
+ else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
+
+
+ //
+
+ if(controller->Button.B && !color_flag)
+ {
+ traceon ^= 1;
+ color_flag = true;
}
+ else if(!controller->Button.B)color_flag = false;
- if(invationA)
+ if(traceon && !yokofla && !boxslip)
{
- motor[0].PWM = startP
+ if(!invationA && !compA && !invationB && !compB)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(invationA && compA && !invationB && !compB)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP - downP;
+ motor[TIRE_FL].pwm = startP - downP;
+ motor[TIRE_BR].pwm = startP - downP;
+ motor[TIRE_BL].pwm = startP - downP;
+ }
+ else if(invationA && !compA && !invationB && !compB)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ wait(2);
+
+ yokofla = true;
+ traceon = false;
+ }
+ else{
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+ }
+
+ if(!traceon && yokofla && !boxslip)
+ {
+ if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ wait(2);
+
+ boxslip = true;
+ yokofla = false;
+ }
+ else if(invationA && !compA && invationB)
+ {
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(!invationA && !compB && !invationB)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
- }*/
-
-
-
-
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(invationA && compA && !invationB && !compB)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(compB && invationB)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ }
+
+ if(!traceon && !yokofla && boxslip)
+ {
+ if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4))
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+ }
+ /*////
+ motor[0].dir = BACK;
+ motor[1].dir = BACK;
+ motor[2].dir = FOR;
+ motor[3].dir = FOR;
+
+ motor[0].pwm = startP;
+ motor[1].pwm = startP;
+ motor[2].pwm = startP;
+ motor[3].pwm = startP;
+ else if()
+ {
+ motor[0].dir = BRAKE;
+ motor[1].dir = BRAKE;
+ motor[2].dir = BRAKE;
+ motor[3].dir = BRAKE;
+
+ motor[0].pwm = 255;
+ motor[1].pwm = 255;
+ motor[2].pwm = 255;
+ motor[3].pwm = 255;
+ }*///////
}
#endif
@@ -371,68 +593,129 @@
static void Process3()
{
if(controller->Button.R){
- motor[4].dir = FOR;
- motor[5].dir = BACK;
- motor[4].pwm = 150;
- motor[5].pwm = 150;
+ motor[Angul_R].dir = FOR;
+ motor[Angul_L].dir = BACK;
+ motor[Angul_R].pwm = 150;
+ motor[Angul_L].pwm = 150;
}else if(controller->Button.L){
- motor[4].dir = BACK;
- motor[5].dir = FOR;
- motor[4].pwm = 150;
- motor[5].pwm = 150;
+ motor[Angul_R].dir = BACK;
+ motor[Angul_L].dir = FOR;
+ motor[Angul_R].pwm = 150;
+ motor[Angul_L].pwm = 150;
}else{
- motor[4].dir = BRAKE;
- motor[5].dir = BRAKE;
- }
- if(LimitSw::IsPressed(0)){
- motor[4].dir = BRAKE;
- motor[5].dir = BRAKE;
- }else if(LimitSw::IsPressed(1)){
- motor[4].dir = BRAKE;
- motor[5].dir = BRAKE;
- }
+ motor[Angul_R].dir = BRAKE;
+ motor[Angul_L].dir = BRAKE;
+ }
+
+ if(LimitSw::IsPressed(0) && motor[4].dir == FOR && motor[5].dir == BACK){
+ motor[Angul_R].dir = BRAKE;
+ motor[Angul_L].dir = BRAKE;
+
+ motor[Angul_R].pwm = 255;
+ motor[Angul_L].pwm = 255;
+ }else if(LimitSw::IsPressed(1) && motor[4].dir == BACK && motor[5].dir == FOR){
+ motor[Angul_R].dir = BRAKE;
+ motor[Angul_L].dir = BRAKE;
+
+ motor[Angul_R].pwm = 255;
+ motor[Angul_L].pwm = 255;
+ }
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
- //ColorDetection();
+ static bool color_flag = false;
+
+ static bool traceon = false;//fase1
+ static bool yokofla = false;//fase2
+ static bool boxslip = false;//fase3
+
+ static bool compA = false;
+ static bool compB = false;
+ static bool compC = false;
+ static bool compD = false;
+
+ static bool invationA = false;
+ static bool invationB = false;
+ static bool invationC = false;
+ static bool invationD = false;
+
+ ColorDetection();
+ //
+ if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+ {
+ invationA ^= 1;//start false,over true
+ compA = true;//on true,noon false
+ }
+ else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
+
+ if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
+ {
+ invationB ^= 1;//start false,over true
+ compB = true;//on true,noon false
+ }
+ else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
+
+ if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
+ {
+ invationC ^= 1;//start false,over true
+ compC = true;//on true,noon false
+ }
+ else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
- for(int i=0;i<=10;i++)
- {
- ColorDetection();
-
- averageR_0 += Color_A[0];
- averageG_0 += Color_A[1];
- averageB_0 += Color_A[2];
- averageR_1 += Color_B[0];
- averageG_1 += Color_B[1];
- averageB_1 += Color_B[2];
- averageR_2 += Color_C[0];
- averageG_2 += Color_C[1];
- averageB_2 += Color_C[2];
- averageR_3 += Color_D[0];
- averageG_3 += Color_D[1];
- averageB_3 += Color_D[2];
- }
- pc.printf("AR_0:%d, AG_0:%d ,AB_0:%d \r\n",averageR_0 / 10 ,averageG_0 / 10, averageB_0 / 10);
- pc.printf("AR_1:%d, AG_1:%d ,AB_1:%d \r\n",averageR_1 / 10 ,averageG_1 / 10, averageB_1 / 10);
- pc.printf("AR_2:%d, AG_2:%d ,AB_2:%d \r\n",averageR_2 / 10 ,averageG_2 / 10, averageB_2 / 10);
- pc.printf("AR_3:%d, AG_3:%d ,AB_3:%d \r\n",averageR_3 / 10 ,averageG_3 / 10, averageB_3 / 10);
+ if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
+ {
+ invationD ^= 1;//start false,over true
+ compD = true;//on true,noon false
+ }
+ else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
+
+
+ //
+
+ if(controller->Button.B && !color_flag)
+ {
+ traceon ^= 1;
+ color_flag = true;
+ }
+ else if(!controller->Button.B)color_flag = false;
- averageR_0 = 0;
- averageG_0 = 0;
- averageB_0 = 0;
- averageR_1 = 0;
- averageG_1 = 0;
- averageB_1 = 0;
- averageR_2 = 0;
- averageG_2 = 0;
- averageB_2 = 0;
- averageR_3 = 0;
- averageG_3 = 0;
- averageB_3 = 0;
+ if(traceon)
+ {
+ if(compA && compD)
+ {
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else if(compB && compC)
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+
+ motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FL].pwm = startP;
+ motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = startP;
+ }
+ else
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+ }
}
#endif
@@ -450,14 +733,30 @@
#if USE_PROCESS_NUM>6
static void Process6()
{
-
+ float x = 0, y= 0, z = 0;
+
+ pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
+
+ x = acc[0]*1000;
+ y = acc[1]*1000;
+ z = acc[2]*1000;
+
+ pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
+
+ float rotateX = (x - 306)/2.22 - 90;
+ float rotateY = (y - 305)/2.21 - 90;
+ pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
+ wait_ms(50);
+
+ /*void Anglecontrol(){
+ if(rotateX>) && */
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+
}
#endif
@@ -515,17 +814,11 @@
wait_us(3);
Color_A[2] = ColorIn(0); //緑
- //pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d",Color_A[0],Color_A[1],Color_A[2]);
- //pc.printf("\r\n");
-
Color_B[0] = ColorIn(1);
wait_us(3);
Color_B[1] = ColorIn(1);
wait_us(3);
Color_B[2] = ColorIn(1);
-
- //pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d",Color_B[0],Color_B[1],Color_B[2]);
- //pc.printf("\r\n");
Color_C[0] = ColorIn(2);
wait_us(3);
@@ -533,16 +826,10 @@
wait_us(3);
Color_C[2] = ColorIn(2);
- /*pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d",Color_C[0],Color_C[1],Color_C[2]);
- pc.printf("\r\n");*/
-
Color_D[0] = ColorIn(3);
wait_us(3);
Color_D[1] = ColorIn(3);
wait_us(3);
Color_D[2] = ColorIn(3);
-
- /*pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d",Color_D[0],Color_D[1],Color_D[2]);
- pc.printf("\r\n");*/
}
#pragma endregion