Akihiro Nakabayashi
/
MainBoard2018_Auto_Master_A_awasetekudsai
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_neww
by 
Taiki Maruyama
System/Process/Process.cpp
- Committer:
- 7ka884
- Date:
- 2018-10-01
- Revision:
- 4:ba9df71868df
- Parent:
- 3:e10d8736fd22
- Child:
- 6:10e22bc327ce
File content as of revision 4:ba9df71868df:
#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"
#include "../../Input/Switch/Switch.h"
#include "../../Input/ColorSensor/ColorSensor.h"
#include "../../Input/AccelerationSensor/AccelerationSensor.h"
#include "../../Input/Potentiometer/Potentiometer.h"
#include "../../Input/Rotaryencoder/Rotaryencoder.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
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];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
static bool lock;
static bool processChangeComp;
static int current;
static void AllActuatorReset();
#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif
#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
/*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 TIRE_FR 0 //足回り前右
#define TIRE_FL 1 //足回り前左
#define TIRE_BR 2 //足回り後右
#define TIRE_BL 3 //足回り後左
#define Angul_R 4 //角度調節右
#define Angul_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 = 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_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();
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
processChangeComp = true;
current = DEFAULT_PROCESS;
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
Process[0] = Process0;
#endif
#if USE_PROCESS_NUM>1
Process[1] = Process1;
#endif
#if USE_PROCESS_NUM>2
Process[2] = Process2;
#endif
#if USE_PROCESS_NUM>3
Process[3] = Process3;
#endif
#if USE_PROCESS_NUM>4
Process[4] = Process4;
#endif
#if USE_PROCESS_NUM>5
Process[5] = Process5;
#endif
#if USE_PROCESS_NUM>6
Process[6] = Process6;
#endif
#if USE_PROCESS_NUM>7
Process[7] = Process7;
#endif
#if USE_PROCESS_NUM>8
Process[8] = Process8;
#endif
#if USE_PROCESS_NUM>9
Process[9] = Process9;
#endif
#endif
}
static void SystemProcessUpdate()
{
#ifdef USE_SUBPROCESS
if(controller->Button.HOME) lock = false;
if(controller->Button.START && processChangeComp)
{
current++;
if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
processChangeComp = false;
}
else if(controller->Button.SELECT && processChangeComp)
{
current--;
if (current < 0) current = 0;
processChangeComp = false;
}
else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
#endif
#ifdef USE_MOTOR
ACTUATORHUB::MOTOR::Motor::Update(motor);
#endif
#ifdef USE_SOLENOID
ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
#endif
#ifdef USE_RS485
ACTUATORHUB::ActuatorHub::Update();
#endif
}
void SystemProcess()
{
SystemProcessInitialize();
while(1)
{
/*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;
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;
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();
#endif
#ifdef USE_ERRORCHECK
if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
{
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
}
else
#endif
{
#ifdef USE_SUBPROCESS
if(!lock)
{
Process[current]();
}
else
#endif
{
//ロック時の処理
}
}
SystemProcessUpdate();
}
}
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
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[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[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[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
#if USE_PROCESS_NUM>2
static void Process2()
{
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;//茶
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;
if(traceon && !yokofla && !boxslip)
{
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
#if USE_PROCESS_NUM>3
static void Process3()
{
if(controller->Button.R){
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[Angul_R].dir = BACK;
motor[Angul_L].dir = FOR;
motor[Angul_R].pwm = 150;
motor[Angul_L].pwm = 150;
}else{
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()
{
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;//茶
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;
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
#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
#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
#if USE_PROCESS_NUM>8
static void Process8()
{
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
}
#endif
#endif
#pragma endregion PROCESS
static void AllActuatorReset()
{
#ifdef USE_SOLENOID
solenoid.all = ALL_SOLENOID_OFF;
#endif
#ifdef USE_MOTOR
for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
{
motor[i].dir = FREE;
motor[i].pwm = 0;
}
#endif
}
#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); //緑
Color_B[0] = ColorIn(1);
wait_us(3);
Color_B[1] = ColorIn(1);
wait_us(3);
Color_B[2] = ColorIn(1);
Color_C[0] = ColorIn(2);
wait_us(3);
Color_C[1] = ColorIn(2);
wait_us(3);
Color_C[2] = ColorIn(2);
Color_D[0] = ColorIn(3);
wait_us(3);
Color_D[1] = ColorIn(3);
wait_us(3);
Color_D[2] = ColorIn(3);
}
#pragma endregion