大季 矢花
/
MB2019_main_11_17_home
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.
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-09-22
- Revision:
- 2:c015739085d3
- Parent:
- 1:b1219d8ca117
- Child:
- 3:e10d8736fd22
File content as of revision 2:c015739085d3:
#include "mbed.h"
#include "Process.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;
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 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
#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)
{
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
#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();
}
#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
#if USE_PROCESS_NUM>6
static void Process6()
{
}
#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); //緑
//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