Taiki Maruyama
/
MainBoard2018_Auto_Master_A
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
MainBoard2018_Auto_Master_A
by 
Akihiro Nakabayashi
Diff: System/Process/Process.cpp
- Revision:
- 2:c015739085d3
- Parent:
- 1:b1219d8ca117
- Child:
- 3:e10d8736fd22
diff -r b1219d8ca117 -r c015739085d3 System/Process/Process.cpp
--- a/System/Process/Process.cpp Mon Sep 10 01:39:15 2018 +0000
+++ b/System/Process/Process.cpp Sat Sep 22 10:55:22 2018 +0000
@@ -13,7 +13,10 @@
#include "../../Safty/Safty.h"
#include "../Using.h"
+
using namespace SWITCH;
+using namespace COLORSENSOR;
+using namespace ACCELERATIONSENSOR;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -33,6 +36,104 @@
/*Replace here with the definition code of your variables.*/
+Serial pc(USBTX, USBRX);
+
+unsigned long ColorIn(int index)
+{
+ int result = 0;
+ bool rtn = false;
+ for(int i=0; i<12; i++)
+ {
+ CK[index] = 1;
+ rtn = DOUT[index];
+ CK[index] = 0;
+ if(rtn)
+ {
+ result|=(1 << i);
+ }
+ }
+ return result;
+}
+
+#define TILE_FR 0 //足回り前右
+#define TILE_FL 1 //足回り前左
+#define TILE_BR 2 //足回り後右
+#define TILE_BL 3 //足回り後左
+
+#define Anguladjust_R 4 //角度調節右
+#define Anguladjust_L 5 //角度調節左
+
+const int mecanum[15][15]=
+{
+ { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
+ { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
+ { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
+ { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
+ { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
+ {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
+ {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
+ {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
+ {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
+ {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
+ {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
+ {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
+ {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
+ {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
+ {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
+};
+
+const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
+uint8_t SetStatus(int);
+uint8_t SetStatus(int pwmVal){
+ if(pwmVal < 0) return BACK;
+ else if(pwmVal > 0) return FOR;
+ else if(pwmVal == 0) return BRAKE;
+ else return BRAKE;
+}
+uint8_t SetPWM(int);
+uint8_t SetPWM(int pwmVal){
+ if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
+ else return abs(pwmVal);
+}
+
+int Color_A[3]; //[赤,緑,青]
+int Color_B[3];
+int Color_C[3];
+int Color_D[3];
+int intergration = 50;
+
+//************ライントレース変数*******************
+ 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 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;
+
+void ColorDetection();
+
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -148,12 +249,29 @@
}
+
+
void SystemProcess()
{
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());
+
+ 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);
+
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
@@ -185,47 +303,147 @@
}
}
+
+
+
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
-
+ ColorDetection();
}
#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[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]);
+
+ 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;
+
+ }
}
#endif
+bool buttoncomp = false;
#if USE_PROCESS_NUM>2
static void Process2()
{
-
+ 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(controller->Button.A && buttoncomp = false)
+ {
+ motor[0].dir = dir;
+ motor[0].pwm = startP;
+ }
+
+ if(invationA)
+ {
+ motor[0].PWM = startP
+
+ }
+
+
+
+
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
-
+ if(controller->Button.R){
+ motor[4].dir = FOR;
+ motor[5].dir = BACK;
+ motor[4].pwm = 150;
+ motor[5].pwm = 150;
+ }else if(controller->Button.L){
+ motor[4].dir = BACK;
+ motor[5].dir = FOR;
+ motor[4].pwm = 150;
+ motor[5].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;
+ }
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
-
+ //ColorDetection();
+
+ 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);
+
+ 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;
}
#endif
#if USE_PROCESS_NUM>5
static void Process5()
{
-
+ pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
+ //int rotateX = (acc[0].read()-)/ -90;
+ //int rotateY = (acc[1].read()-)/ -90;
+ //pc.printf("X:%d ,Y:%d", rotateX, rotateY);
+ wait_ms(50);
}
#endif
@@ -276,5 +494,55 @@
}
#pragma region USER-DEFINED-FUNCTIONS
-
+void ColorDetection(){
+ GATE = 0;
+
+ CK[0] = 0;
+ CK[1] = 0;
+ CK[2] = 0;
+ CK[3] = 0;
+
+ RANGE = 1;
+
+ GATE = 1;
+ wait_ms(intergration);
+ GATE = 0;
+ wait_us(4);
+
+ Color_A[0] = ColorIn(0); //赤
+ wait_us(3);
+ Color_A[1] = ColorIn(0); //青
+ 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);
+ Color_C[1] = ColorIn(2);
+ 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